#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
#include "serial.h"
#include "writepin.h"
#include "WirelessRelay.h"
#include "pwm.h"
#include "rgbled.h"
#include "eeprom.h"

#define MAX_RELAYS 16
#define COMMAND_BUFFER_SIZE 32
#define SNPRINTF_BUFFER_SIZE 64

char buffer[SNPRINTF_BUFFER_SIZE];

struct WirelessRelayStore
{
    WirelessRelay array[MAX_RELAYS];
    bool states[MAX_RELAYS]={};
    bool printState = true;
    uint8_t count;
};

WirelessRelayStore relays;

inline static void printHelp(Serial* serial)
{
    
    serial->write_p(PSTR("Available Commands: \n\
                help                       :  Show this prompt.\n\
                relay add [id]             :  Add Wireless Relay.\n\
                relay delete               :  Delete last Relay.\n\
                relay list                 :  Print list of saved Wireless Relays.\n\
                relay machine [on/off]     :  Get machine readable relay state when state changes.\n\
                relay [on/off] [nn]        :  Turn on/off nth relay.\n\
                state                      :  Get machine readable state.\n\
                erase                      :  Erase epprom.\n\
                rgb [on/off]               :  Turn on/off RGB leds at current value.\n\
                rgb print                  :  Print current RGB value.\n\
                rgb set [RRR] [GGG] [BBB]  :  Set RGB value, pattern must be 0.\n\
                rgb pattern [id]           :  RGB pattern.\n\
                rgb preset [id]            :  set preset color.\n\
                rgb fade [on/off]          :  turn Colorfade on or off.\n\
                aux [on/off]               :  Turn on/off PWM Channel.\n\
                aux set [VAL]              :  Set PWM value, pattern must be 0.\n"));
}

void writeState( WirelessRelayStore* relays, Pwm16b* auxPwm, Serial* serial )
{
    serial->write("ST");
    serial->write(auxPwm->isOn() ? auxPwm->getValueA() >> 8 : 0);
    serial->putChar(',');
    serial->write(relays->count);
    for(uint8_t i = 0; i < relays->count; i++) 
    {
        serial->putChar(',');
        relays->array[i].getExpectedState() ? serial->putChar('1') : serial->putChar('0');
    }
    serial->putChar('\n');
}

void relayDispatch(WirelessRelayStore* relays, Pwm16b* auxPwm,  char* inBuffer, Serial* serial)
{
    if( strncmp(inBuffer, "add", 3) == 0 && inBuffer[3] != '\0')
    {
        uint16_t id = strtol( inBuffer+3, nullptr, 16 );
        if(id != 0 && relays->count < MAX_RELAYS) 
        {
            relays->array[relays->count].init(id);
            
            EEPROM_write_char( 0, relays->count+1 );
            EEPROM_write_string( relays->count*2+1, (char*) &id, 2 );
            
            uint8_t size = snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Relay saved! NUMBER: %u ID: %X \n", relays->count, id);
            serial->write(buffer, size);
            
            relays->count++;
        }
        else serial->write_p(PSTR("Usage: relay add [id]    [id] being a 16bit hex nummber\n"));
    }
    else if( strncmp(inBuffer, "delete", 6) == 0 && inBuffer[5] != '\0')
    {
        serial->write_p(PSTR("Relay: "));
        serial->write(relays->count);
        serial->write_p(PSTR(" deleted\n"));
        relays->count--;
    }
    else if( strncmp(inBuffer, "list", 4) == 0)
    {
        serial->write_p(PSTR("Relays:\n"));
        for(uint8_t i = 0; i < relays->count; i++)
        {
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %X\n", i, relays->array[i].getId());
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
        }
        serial->putChar('\n');
    }
    else if( strncmp(inBuffer, "on", 2) == 0 && inBuffer[2] != '\0')
    {
        uint8_t selected = atoi(inBuffer+2);
        if (selected <= relays->count)
        {
            relays->array[selected].on();
            
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "RELAY: %u turned on\n", selected);
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
            if(relays->printState) writeState(relays, auxPwm, serial);
        }
        else serial->write("No sutch Relay\n");
    }
    else if( strncmp(inBuffer, "off", 3) == 0 && inBuffer[3] != '\0')
    {
        uint8_t selected = atoi(inBuffer+3);
        if (selected <= relays->count)
        {
            relays->array[selected].off();
            
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "RELAY: %u turned off\n", selected);
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
            if(relays->printState) writeState(relays, auxPwm, serial);
        }
        else serial->write("No sutch Relay\n");
    }
    else if( strncmp(inBuffer, "machine on", 7) == 0 )
    {
        relays->printState = true;
        serial->write_p(PSTR("Turned on State printing\n"));
    }
    else if( strncmp(inBuffer, "machine off", 8) == 0 )
    {
        relays->printState = false;
        serial->write("Turned off State printing\n");
    }
    else 
    { 
        serial->write(inBuffer, COMMAND_BUFFER_SIZE-5);
        serial->write_p(PSTR(" is not a valid subcommand: relay [add/delete/list/on/off/state/machine]\n"));
    }
}

void rgbDispatch(RgbLed* rgbled, char* inBuffer, Serial* serial)
{
    if( strncmp(inBuffer, "on", 2) == 0 )
    {
        rgbled->on();
        serial->write_p(PSTR("RGB lights on\n"));
    }
    else if( strncmp(inBuffer, "off", 3) == 0 )
    {
        rgbled->off();
        serial->write_p(PSTR("RGB lights off\n"));
    }
    else if( strncmp(inBuffer, "print", 5) == 0 )
    {
        
        snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Current RGB values:\nR: %u G: %u B: %u\n", rgbled->getR(), rgbled->getG(), rgbled->getB());
        serial->write(buffer, SNPRINTF_BUFFER_SIZE);
    }
    else if( strncmp(inBuffer, "set", 3) == 0 )
    {
        uint8_t i = 0;
        for(; inBuffer[i] != '\0' && i < COMMAND_BUFFER_SIZE-4; i++ );
        if( i > 14)
        {
            inBuffer[7] = '\0';
            inBuffer[11] = '\0';
            rgbled->setSolidColor(atoi(inBuffer+4), atoi(inBuffer+8), atoi(inBuffer+12));
            serial->write_p(PSTR("Set RGB values\n"));
        }
        else serial->write_p(PSTR("Usage: rgb set [RRR] [GGG] [BBB]\n"));
    }
    else if( strncmp(inBuffer, "pattern", 7) == 0 )
    {
        rgbled->setPattern(atoi(inBuffer+8));
        serial->write_p(PSTR("Set Pattern\n"));
    }
    else if( strncmp(inBuffer, "preset", 6) == 0 )
    {
        rgbled->setPreset(atoi(inBuffer+7));
        serial->write_p(PSTR("Set Preset\n"));
    }
    else if( strncmp(inBuffer, "fade on", 7) == 0 )
    {
        rgbled->setFade(true);
        serial->write_p(PSTR("Turned on Fade\n"));
    }
    else if( strncmp(inBuffer, "fade off", 8) == 0 )
    {
        rgbled->setFade(false);
        serial->write_p(PSTR("Turned off Fade\n"));
    }
    else 
    {
        serial->write(inBuffer, COMMAND_BUFFER_SIZE-4);
        serial->write_p(PSTR(" is not a valid subcommand: rgb [on/off/print/set/pattern/preset]\n"));
    }
}

void auxDispatch(Pwm16b* auxPwm, char* inBuffer, Serial* serial)
{
    if( strncmp(inBuffer, "on", 2) == 0 )
    {
        auxPwm->on();
        serial->write_p(PSTR("Aux pwm on\n"));
    }
    else if( strncmp(inBuffer, "off", 3) == 0 )
    {
        auxPwm->off();
        serial->write_p(PSTR("Aux pwm off\n"));
    }
    else if( strncmp(inBuffer, "set", 3) == 0 )
    {
        uint8_t i = 0;
        for(; inBuffer[i] != '\0' && i < COMMAND_BUFFER_SIZE-4; i++ );
        if( i > 4)
        {
            auxPwm->setDutyA(atoi(inBuffer+4) << 8);
            serial->write_p(PSTR("Set PWM value\n"));
        }
        else serial->write_p(PSTR("Usage: aux set [VALUE]\n"));
        
    }
    else 
    {
        serial->write(inBuffer, COMMAND_BUFFER_SIZE-4);
        serial->write_p(PSTR(" is not a valid subcommand: aux [on/off/set]\n"));
    }
}

void serialDispatch(Serial* serial , WirelessRelayStore* relays, RgbLed* rgbled, Pwm16b* auxPwm)
{
    
    if(serial->dataIsWaiting())
    {
        char buffer[COMMAND_BUFFER_SIZE];
        unsigned int length = serial->getString(buffer, COMMAND_BUFFER_SIZE);
        if(length > 2)
        {
            if(length > 4 && strncmp(buffer, "relay", 5) == 0)
            {
                relayDispatch(relays, auxPwm, buffer+6, serial);
            }
            else if(length > 2 && strncmp(buffer, "rgb", 3) == 0)
            {
                rgbDispatch(rgbled, buffer+4, serial);
            }
            else if(length > 2 && strncmp(buffer, "aux", 3) == 0)
            {
                auxDispatch(auxPwm, buffer+4, serial);
            }
            else if(length > 4 && strncmp(buffer, "state", 5) == 0)
            {
                writeState(relays, auxPwm, serial);
            }
            else if(length > 4 && strncmp(buffer, "erase", 5) == 0)
            {
                for(uint16_t i = 0; i < MAX_RELAYS*2+1; i++) EEPROM_write_char(i, 0);
                serial->write_p(PSTR("EEPROM erased\n"));
                relays->count = 0;
            }
            else if(length > 3 && strncmp(buffer, "help", 4) == 0)
            {
                printHelp(serial);
            }
            else
            {
                serial->write_p(PSTR("Not a valid command\n"));
            }
        }
    }
}

void restore_relays(WirelessRelayStore* relays)
{
    relays->count = EEPROM_read_char(0);
    if(relays->count > MAX_RELAYS ) 
    {
        for(uint16_t i = 0; i < MAX_RELAYS*2+1; i++) EEPROM_write_char(i, 0);
        relays->count = 0;
    }
    for(uint8_t i = 0; i <= relays->count; i++)
    {
        uint16_t id;
        EEPROM_read_string(1+i*2, (char*) &id, 2);
        relays->array[i].init(id); 
    }
}


int main() 
{
    DDRB =  (1 << PB5) | ( 1 << PB1); 
    DDRD =  (1 << PD3) | (1 << PD5)| (1 << PD6);
     
    //door watcher
    PORTB  = (1<< PB3) | (1<< PB4); //Enable pull up on door watcher pins;
    
    bool doorOne = readPin(&PINB, PB3);
    bool doorTow = readPin(&PINB, PB4);
    uint16_t openCount = 0;
    
    sei();
    
    Serial serial;
    
    Pwm8b pwmTc0( &TCCR0A, &TCCR0B, &OCR0A, &OCR0B, 0b00000011, true, true );
    Pwm8b pwmTc2( &TCCR2A, &TCCR2B, &OCR2A, &OCR2B, 0b00000101, false, true );
    pwmTc0.off();
    pwmTc2.off();
    
    RgbLed rgbled( &pwmTc0, &pwmTc2 );
    
    Pwm16b pwmTc1 ( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001, true, false);
    
    relays.count=0;    

    restore_relays(&relays);
    
    serial.write_p(PSTR("RGBController v0.6 starting\n"));
    while(true)
    {
        serialDispatch(&serial, &relays, &rgbled, &pwmTc1);
        rgbled.logic();
        
        if(doorOne != readPin(&PINB, PB3))
        {
            _delay_ms(10);
            if(doorOne != readPin(&PINB, PB3))
            {
                doorOne = readPin(&PINB, PB3);
                serial.write("D1");
                doorOne ? serial.write("O\n") : serial.write("C\n");
            }
        }
        
        if(doorTow != readPin(&PINB, PB4))
        {
            _delay_ms(10);
            if(doorTow != readPin(&PINB, PB4))
            {
                doorTow = readPin(&PINB, PB4);
                serial.write("D2");
                doorTow ? serial.write("O\n") : serial.write("C\n");
            }
        }
        if(doorTow) openCount++;
        if(openCount > 60000) 
        {
            serial.write_p(PSTR("Door Open Warning\n"));
            openCount = 0;
        }
        
        _delay_ms(2);
    }

    return 0;
}