#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "avr-hd44780/lcd.h"
#include "i2c_peter/i2cmaster.h"
#include <avr/eeprom.h>

uint8_t pgm = 0;
uint8_t pvw = 0;

typedef struct {
    uint8_t cam;
    uint8_t front_enabled;
    uint8_t pvw_enabled;
} settings_t;

uint8_t setting_limits[sizeof(settings_t)] = {
    32,
    2,
    2
};

volatile union{
    settings_t settings;
    uint8_t arr[sizeof(settings_t)];
} menu;

void* EEMEM settings_saved;

volatile uint8_t menu_state = 0;
volatile uint8_t update = 1;

#define SDI_SHIELD_ADDR 0x84

#define VF_BACK_PVW_TALLY_PIN 0
#define VF_BACK_PGM_TALLY_PIN 1
#define LENS_PGM_TALLY_PIN 2
#define VF_FRONT_PGM_TALLY_PIN 3

void enc_init(){
    PORTC |= (1<<2) | (1 << 3);
    _delay_ms(10);
    PCMSK1 |= (1<<PCINT10);
    PCICR |= (1<<PCIE1);
}

void btn_init(){
    PORTB |= 1<<6; //PullUp
    PCMSK0 |= (1<<PCINT6);
    PCICR |= (1<<PCIE0);
}

void sdi_shield_write(uint16_t addr, uint8_t val){
    i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
    i2c_write(addr & 0xFF);
    i2c_write(addr >> 8);
    i2c_write(val);
    i2c_stop();
}

uint8_t sdi_shield_read(uint16_t addr){
    i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
    i2c_write(addr & 0xFF);
    i2c_write(addr >> 8);
    i2c_rep_start(SDI_SHIELD_ADDR + I2C_READ);       // set device address and read mode
    uint8_t data = i2c_readNak();
    i2c_stop();
    return data;
}

int main(void){
    DDRB |= 1<<VF_BACK_PVW_TALLY_PIN;
    DDRB |= 1<<VF_BACK_PGM_TALLY_PIN;
    DDRB |= 1<<LENS_PGM_TALLY_PIN;
    DDRB |= 1<<VF_FRONT_PGM_TALLY_PIN;

    DDRD |= 1<<7; //enable backlight

    eeprom_read_block(menu.arr, &settings_saved, sizeof(settings_t));
    if(menu.settings.cam > setting_limits[0])
	menu.settings.cam = 0;
    enc_init();
    btn_init();

    lcd_init();
    lcd_on();
    lcd_clear();

    i2c_init();                             // initialize I2C library

    DDRC |= 1<<1;
    PORTB |= (1<<VF_BACK_PGM_TALLY_PIN);

    _delay_ms(100);

    lcd_printf("sdishield: ");
    i2c_start_wait(SDI_SHIELD_ADDR + I2C_WRITE);     // set device address and write mode
    i2c_write(0x00);
    i2c_write(0x00);
    i2c_rep_start(SDI_SHIELD_ADDR + I2C_READ);       // set device address and read mode
    for(uint8_t i = 0; i < 4; i++){
        unsigned char ret = i2c_readAck();                    // read one byte from EEPROM
        lcd_printf("%c", ret);
    }
    unsigned char hw0 = i2c_readAck();                    // read one byte from EEPROM
    unsigned char hw1 = i2c_readAck();                    // read one byte from EEPROM

    unsigned char fw0 = i2c_readAck();
    unsigned char fw1 = i2c_readNak();

    i2c_stop();

    lcd_set_cursor(0,1);
    lcd_printf("hw: %d.%d", hw0, hw1);
    lcd_printf(" fw: %d.%d", fw0, fw1);

    DDRC |= 1<<0;
    PORTB |= (1<<VF_BACK_PVW_TALLY_PIN);
    PORTB |= (1<<VF_FRONT_PGM_TALLY_PIN);

    _delay_ms(500);
    lcd_clear();

    // Turn LEDs off
    DDRC &= ~(1<<0);
    DDRC &= ~(1<<1);
    PORTB &= ~(1<<VF_FRONT_PGM_TALLY_PIN);
    PORTB &= ~(1<<VF_BACK_PVW_TALLY_PIN);

    /*
    for(uint8_t i=0; i<255; i++){
	uint8_t res = i2c_start(i);
	i2c_stop();
	if(!res)
	    lcd_printf("%x ", i);
    }
    */

    sei();

    while(1){
	sdi_shield_write(0x5000, 1);
 
	uint8_t data_length = sdi_shield_read(0x5001);
	if(data_length){
	    uint8_t tally = sdi_shield_read(0x5100 + menu.settings.cam);
	    pgm = !!(tally & (1<<0));
	    pvw = !!(tally & (1<<1));
	}

	if(pgm){
	    DDRC |= 1<<1;
	    if(menu.settings.front_enabled){
		PORTB |= (1<<LENS_PGM_TALLY_PIN);
		PORTB |= (1<<VF_FRONT_PGM_TALLY_PIN);
	    }
	    PORTB |= (1<<VF_BACK_PGM_TALLY_PIN);
	}
	else{
	    DDRC &= ~(1<<1);
	    PORTB &= ~(1<<VF_BACK_PGM_TALLY_PIN);
	}
	if((!pgm != !menu.settings.front_enabled)){
	    PORTB &= ~(1<<LENS_PGM_TALLY_PIN);
	    PORTB &= ~(1<<VF_FRONT_PGM_TALLY_PIN);
	}

	if(pvw & menu.settings.pvw_enabled){
	    DDRC |= 1<<0;
	    PORTB |= (1<<VF_BACK_PVW_TALLY_PIN);
	}
	if(!pvw | !menu.settings.pvw_enabled){
	    DDRC &= ~(1<<0);
	    PORTB &= ~(1<<VF_BACK_PVW_TALLY_PIN);
	}

	if(update){
	    update = 0;
	    lcd_set_cursor(0,0);
	    lcd_printf("cam: %d  ", menu.settings.cam+1);
	    lcd_set_cursor(0,1);
	    lcd_printf("front: %d pvw: %d", menu.settings.front_enabled, menu.settings.pvw_enabled);

	    switch(menu_state){
		case 1:
		    lcd_set_cursor(5,0);
		    break;
		case 2:
		    lcd_set_cursor(7, 1);
		    break;
		case 3:
		    lcd_set_cursor(14,1);
		    break;
	    }
	    if(menu_state){
		lcd_enable_cursor();
		lcd_enable_blinking();
	    }else{
		lcd_disable_cursor();
		lcd_disable_blinking();
	    }
	}
	_delay_ms(10);
    }

}

ISR(PCINT0_vect){
    if(!(PINB & (1<<6))){
	menu_state=(menu_state+1)%4;

	if(!menu_state)
	    eeprom_update_block(menu.arr, &settings_saved, sizeof(settings_t));

	update=1;
    }
}

ISR(PCINT1_vect){
    if(menu_state){
	uint8_t dir = PINC & (1<<2)?1:-1;

	if(PINC & (1<<3))
	    menu.arr[menu_state-1]-=dir;
	else
	    menu.arr[menu_state-1]+=dir;
	menu.arr[menu_state-1]%=setting_limits[menu_state-1];

	update=1;
    }
}