#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include <string.h>
#include "utils.h"
#include "main.h"
#include "uart.h"

#define POWER_MIN   0
#define POWER_MAX   400
#define SERVO_STEPS 125

#define BUFSIZE     10

#define DEBUG2
// #define DEBUG_UART

volatile uint16_t syscounter = 0;
uint8_t data_count = 0;
char data_in[BUFSIZE];
volatile uint16_t current_pulse_width = 250;

void reset_input_buffer(void) {
    data_count = 0;
    memset(data_in, 0, BUFSIZE);
}

static void timer_init(void) {
    // set Timer 1 to fast PWM mode, with prescale of 64, clear OC1A as soon as
    // TCNT1 reaches OCR1A, set OC1A as soon as it reaches ICR1
    // NOTE: one tick is equivalent to 8 usecs
    TCCR1A = _BV(COM1A1) | _BV(WGM11); 
    TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11) | _BV(CS10);
    ICR1  = 2500;        // period of 20 msecs
    OCR1A = 250;         // inital pulse width of 2 msecs (10% duty cycle)
    TIMSK |= _BV(TOIE1); // enable overflow interrupt

    // CTC Mode for Timer 0 (8Bit) with prescale of 1024
    TCCR0B |= _BV(CS02) | _BV(CS00); // prescaler
    TCCR0A |= _BV(WGM01);            // CTC mode
    TIMSK  |= _BV(OCIE0A);           // enable timer interrupt

    OCR0A = 78; // gives us ~100ms interval
}

static void ports_init(void) {
    DDRB |= _BV(PB3);
}

static void work_uart() {
    static uint16_t power;
    uint16_t c = uart_getc();

    if ( !(c & UART_NO_DATA) ) {
        char cur = c & 0xff;

        data_in[data_count] = cur;
        //uart_print_uint8(cur);

        data_count++;
        if(data_count >= BUFSIZE) { // buffer overflow
            reset_input_buffer();
        }

#ifdef DEBUG1
        for(uint8_t i=0;i<BUFSIZE;i++) {
            uart_print_uint8(data_in[i]);
            uart_putc('\r');
            uart_putc('\n');
        }
        uart_puts_P(" -- \r\n");
#endif

        if (cur == 13 || cur == '\n') {
            uint16_t new_power = atoi(data_in);
            new_power = new_power <= POWER_MAX ? new_power : POWER_MAX;

#ifdef DEBUG_UART
            uint16_t diff = new_power > power ? new_power - power : power - new_power;
            if(diff > 50) {
                uart_puts_P("WARNING! Deviation greater than 50! ");
            }
            uart_puts_P("Transmitted power = ");
            uart_print_uint16(new_power);
            uart_puts_P("\r\n");
#endif
            power = new_power;
            set_servo(power);
            reset_input_buffer();
        }
    }
}

/**
 *  \brief  set the servo to a position calculated to given power
 *
 *  \param  display     The power value from 0 to 400 (including bounds)
 */
void set_servo(uint16_t display) {
    // invert value and ensure that we don't exceed the limit
    display = POWER_MAX - (display < POWER_MAX ? display : POWER_MAX);
    // *10 otherwise we need float
    display = (display * 10u) / ((POWER_MAX * 10u) / SERVO_STEPS) + SERVO_STEPS;

#ifdef DEBUG
    uart_puts_P("display = ");
    uart_print_uint16(display);
    uart_puts_P("\r\n");
#endif

    cli();
    current_pulse_width = display;
    sei();
}

/**
 * \brief   the method moves the servo one complete cycle
 */
static void demo_display(void) {
    set_servo(0);
    wait(100);
    set_servo(400);
    wait(100);
    set_servo(0);
    wait(100);
}

int main(void) {
    sei();

    ports_init();
    timer_init();
    uart_init(UART_BAUD_SELECT(38400,F_CPU));

    reset_input_buffer();
    // demo_display();

    while(1) {
        work_uart();

        if(syscounter >= 10) {
            reset_input_buffer();
            uart_putc('a');     // send a to receive values from master box
            syscounter = 0;

#ifdef DEBUG
            uart_puts_P("OCR1A = ");
            uart_print_uint16(OCR1A);
            uart_puts_P("\r\n");
#endif
        }
    }

    return(0);
}

ISR(TIMER1_OVF_vect) {
    OCR1A = current_pulse_width;
}

ISR(TIMER0_COMPA_vect) {
    syscounter++;
}