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

volatile uint16_t syscounter = 0;
volatile uint8_t digitbuffer[6] = { 0,0,0,0,0,0 };
volatile uint8_t leddigitbuffer[4] = { 0,0,0,0 };
uint8_t digit = 0;
uint8_t leddigit = 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; //TODO: make bitfield
uint8_t loadsw = 0; //TODO: make bitfield
uint8_t gensw = 0;  //TODO: make bitfield

uint16_t power_gen = 0;
uint16_t power_load = 0;

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

const uint8_t digit_translate[10] = {
    63, 6, 91, 79, 102, 109, 125, 7, 127, 111
};


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   = 250;    // 500Hz
	TIMSK   = _BV(OCIE1A);
	sei();	// enable interrupts
}

static void ports_init(void) {
    // make column / digit driver pins to output
    LEDDIG_DDR |= _BV(LEDS_MID1) | _BV(LEDS_MID2) | _BV(LEDS_LOAD) | _BV(LEDS_GEN);
    SEVENSEGDIG_DDR |= _BV(DIG0) | _BV(DIG1) | _BV(DIG2) | _BV(DIG3) | _BV(DIG4) | _BV(DIG5);

    // make data ports to output
    LED_DDR = 0xff;
    SEVENSEG_DDR = 0xff;

    SEVENSEGDIG_PORT = 0;
    SEVENSEG_PORT = 0;

    LEDDIG_PORT = 0;
    LED_PORT = 0;
}


static void print_sevenseg(uint8_t display, uint16_t value) {
    uint8_t d[3];
    d[2] = (value % 1000 / 100 );
    d[1] = (value % 100 / 10 );
    d[0] = (value % 10);

    if(display == 0) {
        for(uint8_t i = 0; i< 3; i++) {
           digitbuffer[i] = digit_translate[d[i]];
        }
    } else {
        for(uint8_t i = 0; i< 3; i++) {
           digitbuffer[i+3] = digit_translate[d[i]];
        }
    }
}

static void receive_values_from_powerboard(void) {
    uint16_t uart_char;
    static char buffer[20];
    // send a to request data from powerboard
    uart_putc('a');

    // input format is:
    // A$voltage,$current_in,$current_out,$power_in,$power_out,loadsw,dumpsw,gensw\n

    uart_char = uart_getc();

    if(uart_char != UART_NO_DATA) {



    }

    voltage = 12200;
    current_in = 1500;
    current_out = 1600;
    power_gen += 1;
    power_load += 2;

}

/*
void process_command() {
    if(strstr(command_in,"a") != NULL) {
        //

    if(strcasestr(command_in,"?") != NULL)
    print_value("goto", variable_goto);
    else
    variable_goto = parse_assignment(command_in);
    }
    else if(strcasestr(command_in,"A") != NULL){
    if(strcasestr(command_in,"?") != NULL)
    print_value("A", variable_A);
    else
    variable_A = parse_assignment(command_in);
    }
}   */

void work_uart(){
  unsigned int c = uart_getc();

  if ( !(c & UART_NO_DATA) ) {

    data_in[data_count] = c;

    if (data_in[data_count] == '\n') {  // finish reading when newline is received
      data_count = 0;

      // copy_command
      memcpy(command_in, data_in, 20);
      // Now clear data_in, the UART can reuse it now
      memset(data_in, 0, 20);

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




int main(void) {
	ports_init();
	timer_init();
    uart_init(UART_BAUD_SELECT(19200,F_CPU));

	while(1) {

	    work_uart();

        if(syscounter == 200) {

            receive_values_from_powerboard();

            print_sevenseg(0, power_gen);
            print_sevenseg(1, power_load);

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

// system timer
SIGNAL(TIMER1_COMPA_vect) {
	syscounter++;

	// output to sevensegment and leds
	// make this here to reduce display flicker
    digit++;
    if(digit >5) digit = 0;
    leddigit++;
    if(leddigit >3) leddigit = 0;

    SEVENSEG_PORT = digitbuffer[digit];
    SEVENSEGDIG_PORT = _BV(digit+DIG0);

    LED_PORT = leddigitbuffer[leddigit];
    LEDDIG_PORT = _BV(leddigit);
}


