#include "servos.hpp"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdint.h>

#define MIN_PULSE 999
#define MAX_PULSE 4999

static void reorderServos();
static void initTimer1();

struct Servo
{
    volatile uint8_t * const port;  // the PORT register connected to the servo
    volatile uint8_t * const ddr;   // the DDR register connected to the servo
    const uint8_t pinMask;          // the pin mask for the servo
    uint16_t tempValue;             // stores a temporary value before it's written to the servo
    uint16_t value;                 // current pulse width value
    int16_t angle;                  // the angle in arc-minutes
};

static Servo servos[NUM_SERVOS] = {
    { &PORTC, &DDRC, (1 << PB0), 0, 0 },
    { &PORTC, &DDRC, (1 << PB1), 0, 0 },
    { &PORTC, &DDRC, (1 << PB2), 0, 0 }
};

static uint16_t orderedValues[NUM_SERVOS];  // current servo values in ascending order
static uint8_t orderedValueIndex = 0;       // index in orderedValues array
static bool updateFlag = false;             // used to flag when the servo values have been updated

void initServos()
{
    for (Servo &servo : servos)
    {
        *servo.ddr |= servo.pinMask;
    }
    
    initTimer1();
}

void setServoAngle(uint8_t servo, int16_t arcMin)
{
    if (0 <= servo && servo < NUM_SERVOS)
    {
        if (arcMin < -180 * 60)
            arcMin = -180 * 60;
        else if (arcMin > 180 * 60)
            arcMin = 180 * 60;
        
        servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
        servos[servo].angle = arcMin;
    }
}

int16_t getServoAngle(uint8_t servo)
{
    if (0 <= servo && servo < NUM_SERVOS)
        return servos[servo].angle;
    else
        return 0;
}

void updateServos()
{
    updateFlag = true;
}

static void reorderServos()
{
    uint16_t minValue, maxValue = 0;
    
    for (Servo &servo : servos)
        servo.value = servo.tempValue;
    
    for (uint16_t &value : orderedValues)
    {
        minValue = (uint16_t) -1;
        
        for (Servo servo : servos)
        {
            if (servo.value < minValue && servo.value > maxValue)
                minValue = servo.value;
        }
        
        maxValue = minValue;
        value = maxValue;
    }
}

static void initTimer1()
{
    cli();
    
    TCCR1A = 0;
    TCCR1B = (1 << WGM12)   // CTC mode, TOP = OCR1A
           | (1 << CS11);   // 8th prescaler 
    TCCR1C = 0;
    
    OCR1A = 39999;                          // set TOP for 50 Hz 
    TIMSK1 = (1 << OCIE1A) | (1 << OCIE1B); // enable interrupts
    
    sei();
}

ISR(TIMER1_COMPA_vect)
{
    if (updateFlag)
    {
        updateFlag = false;
        reorderServos();
    }
    
    for (Servo &servo : servos)
        *servo.port |= (servo.pinMask); // set servo pin HIGH
    
    orderedValueIndex = 0;
    OCR1B = orderedValues[orderedValueIndex];
}

ISR(TIMER1_COMPB_vect)
{
    uint16_t currValue = orderedValues[orderedValueIndex];
    
    for (Servo &servo : servos)
    {
        if (servo.value == currValue)
            *servo.port &= ~(servo.pinMask); // set servo pin LOW
    }
    
    orderedValueIndex++;
    
    if (orderedValueIndex < NUM_SERVOS)
        OCR1B = orderedValues[orderedValueIndex];
}