#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 BUFSIZE 40
#define CURRENT_MAX     30000       // 30 Ampere is max
#define POWER_MAX       300
#define SERVO_NEUTRAL   2312
#define SERVO_MIN       2312
#define SERVO_MAX       2312


volatile uint16_t syscounter = 0;

// values send over uart from powerboard
uint16_t voltage = 0;
int16_t current_in = 0;
int16_t current_out = 0;
uint8_t dumpsw = 0;
uint8_t loadsw = 0;
uint8_t gensw = 0;
uint16_t display = 0;

uint16_t power_gen = 0;
uint16_t power_load = 0;

unsigned char data_count = 0;
unsigned char data_in[BUFSIZE];
char command_in[BUFSIZE];


static void timer_init(void) {
	// clock is 8MHz
	TCCR1B |= _BV(WGM12) | _BV(CS11) | _BV(CS10);       // CTC Mode for Timer 1 (16Bit) with prescale of 64
    OCR1A = 2312;                           // Neutralposition ((2500-2312)*0.008ms)=1,5ms)
	TIMSK = _BV(OCIE1A);
	TCCR1A = (1<<COM1A0);                   // Togglen bei Compare Match

    sei();	// enable interrupts
}

static void ports_init(void) {

}

static void process_command() {
    if(strstr(command_in,"A") != NULL) {
        // we have an A and B (from check in work_uart()
        // so our message should be complete and consist of:
        // A$voltage,$current_in,$current_out,$power_in,$power_out,loadsw,dumpsw,gensw\n

        //A12.5,65464,00000,00000,00000,1,0,1B

        char *token;
        uint8_t tokencounter = 0;

        char *start = strrchr(command_in, 'A');

        // remove first (B is ignored by atoi)
        start++;

        token = strtok(start, ",");

        while( token ) {
            switch(tokencounter) {
                case 0:
                    voltage = atoi(token);
                    break;
                case 1:
                    current_in = atoi(token);
                    break;
                case 2:
                    current_out = atoi(token);
                    break;
                case 3:
                    power_gen = atoi(token);
                    break;
                case 4:
                    power_load = atoi(token);
                    break;
                case 5:
                    if(atoi(token) == 1) loadsw = 1;
                    else loadsw = 0;
                    break;
                case 6:
                    if(atoi(token) == 1) dumpsw = 1;
                    else dumpsw = 0;
                    break;
                case 7:
                    if(atoi(token) == 1) gensw = 1;
                    else gensw = 0;
                    break;
            }

            tokencounter++;
            token = strtok(NULL, ",");
        }
    }
}

static void work_uart() {

    uint8_t c = uart_getc();

    if ( !(c & UART_NO_DATA) ) {
        data_in[data_count] = c;

        if (data_in[data_count] == 'B') {
            data_count = 0;

            memcpy(command_in, data_in, BUFSIZE);
            memset(data_in, 0, BUFSIZE);

            process_command();
        } else {
            data_count++;
        }
    }
}

static void set_servo(uint8_t power) {

    uint8_t display = 0;

    display = power * 10;     // shift, since we have to divide by 2,4 (24)
    display = display / 24;   // instead of dividing by 2,4
    display = display + 125;

    OCR1A = 2500-display;
}


static void demo_display(void) {

	for(uint8_t i = 0; i< 30;i++) {
		set_servo(i*10);
		wait(5);
	}

	for(uint8_t i = 30; i> 0;i--) {
        set_servo(i*10);
    	wait(5);
    }

}

int main(void) {

	ports_init();
	timer_init();
    uart_init(UART_BAUD_SELECT(19200,F_CPU));
    memset(data_in, 0, BUFSIZE);

	demo_display();

	while(1) {
	    work_uart();

        if(syscounter >= 10) {
            uart_putc('a'); // send a to receive values

            set_servo(power_gen);
            syscounter = 0;
        }
	}
	
	return(0);
}

SIGNAL(TIMER1_COMPA_vect) {
	syscounter++;

    OCR1A = 2500-OCR1A;         // Das Servosignal wird aus der Differenz von
                                // Periodenlänge (2500*0,008ms=20ms) und letztem
                                // Vergleichswert (OCR1A) gebildet
}