#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

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

   
void reset_input_buffer(void) {
	data_count = 0;
	memset(data_in, 0, BUFSIZE);
}
   
static void timer_init(void) {
	// CTC Mode for Timer 1 (16Bit) with prescale of 64
	TCCR1B |= _BV(WGM12) | _BV(CS11) | _BV(CS10);       
	OCR1A = 2250;           // set Servo to max. position
	TIMSK |= _BV(OCIE1A);	// enable timer interrupt
	TCCR1A |= _BV(COM1A0);   // toggle OCR1A on overflow
    
    // 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() {
	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') {
			
            power = atol(data_in);
            if(power > POWER_MAX) power = POWER_MAX;

#ifdef DEBUG
            uart_puts_P("power = ");
            uart_print_uint16(power);
            uart_puts_P("\r\n");
#endif
            if(power > 0) {
                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) {
	int diff;

	if( display > POWER_MAX ) display = POWER_MAX;

	display = POWER_MAX-display;        // invert the value / servo

	display = display * 10;				// *10 otherwise we need float
	display = display / ((POWER_MAX * 10) / SERVO_STEPS);
	display = display + SERVO_STEPS;

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

    if( display < 125 ) display = 125;	// just make sure, the timer
	if( display > 250 ) display = 250;  // is never out ouf 20ms grid

    
    // check if timer is currently in the small pulse, then sleep here 2ms
    // and do again
    
    
    if(OCR1A <= 2250) {
        _delay_ms(10);
    }

	//cli();								// read and write atomic
	TIMSK &= ~(_BV(OCIE1A));
	diff = OCR1A - (2500-display);
    if(diff <=20 && diff >= -20) {
    	OCR1A = 2500-display;
	}
	else {
		if(diff <=20) {
			OCR1A = OCR1A+5;
		}
		else if ( diff >= -20 ) {
			OCR1A = OCR1A-5;
		}
	}	
    //sei();
    TIMSK |= _BV(OCIE1A);
}


/**
 * \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);
}

/**
 * \brief	this is our timer for PWM generation and system clock
 * 			the system clock varies a bit, but this does not matter
 */
ISR(TIMER1_COMPA_vect) {
	OCR1A = 2500-OCR1A;	// Das Servosignal wird aus der Differenz von
						// Periodenlänge (2500*0,008ms=20ms) und letztem
						// Vergleichswert (OCR1A) gebildet	
}

ISR(TIMER0_COMPA_vect) {
    syscounter++;
}