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

#define COMMAND_BUFFER_SIZE 64
#define SNPRINTF_BUFFER_SIZE 64

#define MAX_RELAYS 16
#define RELAY_VECTOR_EEPROM_ADDR 32

char buffer[SNPRINTF_BUFFER_SIZE];

SVector<WirelessRelay, MAX_RELAYS> relays;

inline static void printHelp(Serial* serial)
{
    
    serial->write_p(PSTR("Available Commands: \n\
                help                       :  Show this prompt.\n\
                relay add [id] [name]      :  Add Wireless Relay.\n\
                relay delete               :  Delete last Relay.\n\
                relay list                 :  Print list of saved Wireless Relays.\n\
                relay [on/off] [nn]        :  Turn on/off nth relay.\n\
                state                      :  Get machine readable state.\n\
                erase                      :  Erase epprom.\n\
                dump                       :  Dump 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.\n"));
}

void save()
{
    EEPROM_write_class< SVector<WirelessRelay, MAX_RELAYS> > (RELAY_VECTOR_EEPROM_ADDR, relays);
}

void load()
{
    EEPROM_read_class< SVector<WirelessRelay, MAX_RELAYS> > (RELAY_VECTOR_EEPROM_ADDR, &relays);
}

void writeState( SVector<WirelessRelay, MAX_RELAYS>* 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->at(i).getExpectedState() ? serial->putChar('1') : serial->putChar('0');
    }
    serial->putChar('\n');
}

void relayDispatch(SVector<WirelessRelay, MAX_RELAYS>* relays, Pwm16b* auxPwm, char* token, Serial* serial)
{
    if( strcmp(token, "add") == 0 )
    {
        token = strtok(NULL, " \n");
        uint16_t id = strtol(token, nullptr, 2 );
        if(id != 0 && relays->remainingCapacity() > 0) 
        {
            id = id << 4;
            token = strtok(NULL, "\0");
            if( token == NULL )
            {
                char name[] = "";
                WirelessRelay relay(id, name);
                relays->push_back(relay);
            }
            else
            {
                WirelessRelay relay(id, token);
                relays->push_back(relay);
            }
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Adding relay NUMBER: %u NAME: %s\n", relays->count()-1, relays->back().getName());
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
            save();
        }
        else if(relays->remainingCapacity() == 0) serial->write_p(PSTR("Relay storage full.\n"));
        else serial->write_p(PSTR("Usage: relay add [id] [name]\n [id] being a 16bit binary nummber and [name] an optional string\n"));
    }
    else if( strcmp(token, "delete") == 0 )
    {
        relays->erase(relays->count());
        snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Deleting relay NUMBER: %u NAME: %s\n", relays->count(), relays->at(relays->count()).getName());
        serial->write(buffer, SNPRINTF_BUFFER_SIZE);
        save();
    }
    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->at(i).getId();
            snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NUMBER: %u ID: %s%s%s NAME: %s\n", i, 
                    bit_rep[ id           >> 12], 
                    bit_rep[(id & 0x0F00) >> 8 ], 
                    bit_rep[(id & 0x00F0) >> 4 ],
                    relays->at(i).getName());
            serial->write(buffer, SNPRINTF_BUFFER_SIZE);
        }
        serial->write_p(PSTR("EOL\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->at(selected).on();
                
                snprintf(buffer, SNPRINTF_BUFFER_SIZE, "RELAY: %u turned on\n", selected);
                serial->write(buffer, SNPRINTF_BUFFER_SIZE);
                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->at(selected).off();
                
                snprintf(buffer, SNPRINTF_BUFFER_SIZE, "RELAY: %u turned off\n", selected);
                serial->write(buffer, SNPRINTF_BUFFER_SIZE);
                writeState(relays, auxPwm, serial);
            }
            else serial->write("No sutch Relay\n");
        }
    }
    else 
    { 
        serial->write(token);
        serial->write_p(PSTR(" is not a valid subcommand: relay [add/delete/list/on/off]\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, SVector<WirelessRelay, MAX_RELAYS>* 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);
                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 < sizeof(*relays)+RELAY_VECTOR_EEPROM_ADDR; i++) EEPROM_write_char(i, 0);
                serial->write_p(PSTR("EEPROM erased\n"));
                load();
            }
            else if(strcmp(token, "dump") == 0)
            {
                for(uint16_t i = 0; i < sizeof(*relays)+RELAY_VECTOR_EEPROM_ADDR; i++) 
                {
                    if(i != 0) serial->putChar(',');
                    serial->write((uint16_t)EEPROM_read_char(i));
                }
                serial->putChar('\n');
            }
            else if(strcmp(token, "help") == 0)
            {
                printHelp(serial);
            }
            else
            {
                serial->write_p(PSTR("Not a valid command\n"));
            }
        }
    }
}


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);
    
    serial.write_p(PSTR("RGBController v0.7 starting\n"));
    
    load();
    
    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;
}