#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"
#include "bitrep.h"

#define MAX_RELAYS 32
#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* token, Serial* serial)
{
    if( strcmp(token, "add") == 0 )
    {
        char* token = strtok(NULL, " \n");
        uint16_t id = strtol(token, nullptr, 2 );
        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 binary nummber\n"));
    }
    else if( strcmp(token, "delete") == 0 )
    {
        serial->write_p(PSTR("Relay: "));
        serial->write(relays->count);
        serial->write_p(PSTR(" deleted\n"));
        relays->count--;
    }
    else if( strcmp(token, "list")  == 0 )
    {
        serial->write_p(PSTR("Relays:\n"));
        for(uint8_t i = 0; i < relays->count; i++)
        {
            uint16_t id = relays->array[i].getId();
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %s%s%s%s\n", i, 
                    bit_rep[ id           >> 12], 
                    bit_rep[(id & 0x0F00) >> 8 ], 
                    bit_rep[(id & 0x00F0) >> 4 ], 
                    bit_rep[ id & 0x000F       ]);
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
        }
        serial->putChar('\n');
    }
    else if( strcmp(token, "on") == 0 )
    {
        char* token = strtok(NULL, " \n");
        if( token != NULL)
        {
            uint8_t selected = atoi(token);
            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( strcmp(token, "off") == 0 )
    {
        char* token = strtok(NULL, " \n");
        if( token != NULL)
        {
            uint8_t selected = atoi(token);
            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( strcmp(token, "machine") == 0 )
    {
        char* token = strtok(NULL, " \n");
        if( token != NULL)
        {
            if( strcmp(token, "on") == 0 )
            {
                relays->printState = true;
                serial->write_p(PSTR("Turned on State printing\n"));
            }
            else
            {
                relays->printState = false;
            serial->write("Turned off State printing\n");
            }
        }
        else serial->write_p(PSTR("Usage: relay machine [on/off]\n"));
    }
    else 
    { 
        serial->write(token);
        serial->write_p(PSTR(" is not a valid subcommand: relay [add/delete/list/on/off/state/machine]\n"));
    }
}

void rgbDispatch(RgbLed* rgbled, char* token, Serial* serial)
{
    if( strcmp(token, "on") == 0 )
    {
        rgbled->on();
        serial->write_p(PSTR("RGB lights on\n"));
    }
    else if( strcmp(token, "off") == 0 )
    {
        rgbled->off();
        serial->write_p(PSTR("RGB lights off\n"));
    }
    else if( strcmp(token, "print") == 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( strcmp(token, "set") == 0 )
    {
        char* rToken = strtok(NULL, " \n");
        char* gToken = strtok(NULL, " \n");
        char* bToken = strtok(NULL, " \n");
        if(rToken != NULL && gToken != NULL && bToken != NULL)
        {
            rgbled->setSolidColor(atoi(rToken), atoi(gToken), atoi(bToken));
            serial->write_p(PSTR("Set RGB values\n"));
        }
        else serial->write_p(PSTR("Usage: rgb set [RRR] [GGG] [BBB]\n"));
    }
    else if( strcmp(token, "pattern") == 0 )
    {
        token = strtok(NULL, " \n");
        if( token != NULL )
        {
            rgbled->setPattern(atoi(token));
            serial->write_p(PSTR("Set Pattern\n"));
        }
        else serial->write_p(PSTR("Usage: rgb pattern [id]\n"));
    }
    else if( strcmp(token, "preset") == 0 )
    {
        token = strtok(NULL, " \n");
        if( token != NULL )
        {
            rgbled->setPreset(atoi(token));
            serial->write_p(PSTR("Set Preset\n"));
        }
        else serial->write_p(PSTR("Usage: rgb preset [ID]\n"));
    }
    else if( strcmp(token, "fade") == 0 )
    {
        token = strtok(NULL, " \n");
        if( token != NULL )
        {
            if( strcmp(token, "on") == 0 )
            {
                rgbled->setFade(true);
                serial->write_p(PSTR("Turned on Fade\n"));
            }
            else
            {
                rgbled->setFade(false);
                serial->write_p(PSTR("Turned off Fade\n"));
            }
        }
        else serial->write_p(PSTR("Usage: rgb fade [on/off]\n"));
    }
    else 
    {
        serial->write(token);
        serial->write_p(PSTR(" is not a valid subcommand: rgb [on/off/print/set/pattern/preset/fade]\n"));
    }
}

void auxDispatch(Pwm16b* auxPwm, char* token, Serial* serial)
{
    if(strcmp(token, "on") == 0 )
    {
        auxPwm->on();
        serial->write_p(PSTR("Aux pwm on\n"));
    }
    else if(strcmp(token, "off") == 0 )
    {
        auxPwm->off();
        serial->write_p(PSTR("Aux pwm off\n"));
    }
    else if(strcmp(token, "set") == 0 )
    {
        token = strtok(NULL, " \n");
        if(token != NULL)
        {
            auxPwm->setDutyA(atoi(token) << 8);
            serial->write_p(PSTR("Set PWM value\n"));
        }
        else serial->write_p(PSTR("Usage: aux set [VALUE]\n"));
    }
    else 
    {
        serial->write(token, 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)
        {
            char* token = strtok(buffer, " \n");
            if(strcmp(token, "relay") == 0)
            {
                relayDispatch(relays, auxPwm, strtok(NULL, " \n"), serial);
            }
            else if(strcmp(token, "rgb") == 0)
            {
                rgbDispatch(rgbled, strtok(NULL, " \n"), serial);
            }
            else if(strcmp(token, "aux") == 0)
            {
                auxDispatch(auxPwm, strtok(NULL, " \n"), serial);
                if(relays->printState == true) writeState(relays, auxPwm, serial);
            }
            else if(strcmp(token, "state") == 0)
            {
                writeState(relays, auxPwm, serial);
            }
            else if(strcmp(token, "erase") == 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(strcmp(token, "help") == 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;
}