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

#define MAX_RELAYS 32
#define COMMAND_BUFFER_SIZE 32

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

inline static void printHelp(Serial* serial)
{
    serial->write("Available Commands: \n\
                help                       :  Show this prompt \n\
                relay add [id]             :  Add Wireless 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\
                relay state                :  Get machine readable relay 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\
                version                    :  Print Version.\n");
}

void writeState( WirelessRelayStore* relays, Serial* serial )
{
    serial->write("ST");
    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, char* inBuffer, Serial* serial)
{
    if( strncmp(inBuffer, "add", 3) == 0 && inBuffer[3] != '\0')
    {
        uint16_t id = strtol( inBuffer+4, nullptr, 16 );
        if(id != 0 && relays->count < MAX_RELAYS) 
        {
            relays->array[relays->count].init(&PORTB, PB5, id);
            char buffer[64];
            
            EEPROM_write_char( 0, relays->count+1 );
            EEPROM_write_string( relays->count*2+1, (char*) &id, 2 );
            
            sprintf(buffer, "Relay saved! NUMBER: %u ID: %X\n", relays->count, id);
            serial->write(buffer, 64);
            
            relays->count++;
        }
        else serial->write("Usage: relay add [id]    [id] being a 16bit hex nummber\n");
    }
    else if( strncmp(inBuffer, "list", 4) == 0)
    {
        serial->write("Relays:\n");
        for(uint8_t i = 0; i < relays->count; i++)
        {
            char buffer[64];
            sprintf(buffer, "NUMBER: %u ID: %X\n", i, relays->array[i].getId());
            serial->write(buffer, 64);
        }
        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();
            
            
            char buffer[64];
            sprintf(buffer, "RELAY: %u turned on\n", selected);
            serial->write(buffer, 64);
            if(relays->printState) writeState(relays, 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();
            
            char buffer[64];
            sprintf(buffer, "RELAY: %u turned off\n", selected);
            serial->write(buffer, 64);
            if(relays->printState) writeState(relays, serial);
        }
        else serial->write("No sutch Relay\n");
    }
    else if( strncmp(inBuffer, "state", 5) == 0 && inBuffer[3] != '\0')
    {
        writeState(relays, serial);
    }
    else if( strncmp(inBuffer, "machine on", 7) == 0 )
    {
        relays->printState = true;
        serial->write("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(" is not a valid subcommand: relay [add/list/on/off]\n");
    }
}

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

void serialDispatch(Serial* serial , WirelessRelayStore* relays, RgbLed* rgbled)
{
    
    if(serial->dataIsWaiting())
    {
        char buffer[COMMAND_BUFFER_SIZE];
        unsigned int length = serial->getString(buffer, COMMAND_BUFFER_SIZE);
        if(length != 0)
        {
            if(length > 4 && strncmp(buffer, "relay", 5) == 0)
            {
                relayDispatch(relays, buffer+6, serial);
            }
            else if(length > 2 && strncmp(buffer, "rgb", 3) == 0)
            {
                rgbDispatch(rgbled, buffer+4, serial);
            }
            else if(length > 4 && strncmp(buffer, "state", 3) == 0)
            {
                rgbDispatch(rgbled, buffer+4, 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("EEPROM erased\n");
                relays->count = 0;
            }
            else if(buffer[0] == '?' || ( length > 3 && strncmp(buffer, "help", 4) == 0))
            {
                printHelp(serial);
            }
            else
            {
                serial->write("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(&PORTB, PB5, id); 
    }
}


int main() 
{
    DDRB =  (1 << PB5) | ( 1 << PB1); 
    DDRD =  (1 << PD3) | (1 << PD5)| (1 << PD6);
     
    PORTB  = 0x00;
    
    sei();
    Serial serial;
    
    /*
    Pwm16b pwmA;
    pwmA.setDuty(10);
    pwmA.on();
    */
    
    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 );
    
    WirelessRelayStore relays;
    relays.count=0;    
    restore_relays(&relays);
    
    serial.write("RGBController v0.4 starting\n");
    while(true)
    {
        serialDispatch(&serial, &relays, &rgbled);
        rgbled.logic();
        _delay_ms(2);
    }

    return 0; //master interupt.
}