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

#define COMMAND_BUFFER_SIZE 64
#define SNPRINTF_BUFFER_SIZE 96

#define MAX_RELAYS 32
#define RELAY_VECTOR_EEPROM_ADDR 32

char buffer[SNPRINTF_BUFFER_SIZE];

SVector<WirelessRelay, MAX_RELAYS> relays;

bool sensorsPaused = false;

static volatile bool resendNow = false;
static volatile uint8_t resendCounter = 0;
static bool resendEnabled = false;

ISR(PCINT1_vect)
{
    W433DataReciver::staticInterrupt();
}

ISR(WDT_vect)
{
	if(++resendCounter > 225)
	{ 
		resendCounter = 0;
		if(resendEnabled)resendNow = true;
	}
}

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 [n]           :  Delete n'th Relay.\n\
                relay [on/off] [nn]        :  Turn on/off nth relay.\n\
                relay resend [on/off]      :  Turn on/off periodic auto resend.\n\
                state                      :  Get machine readable state.\n\
                erase                      :  Erase epprom.\n\
                dump                       :  Dump epprom.\n\
                free                       :  show free ram.\n\
                pause                      :  pause sensor output.\n\
                resume                     :  resume sensor output.\n\
                rgb fade [on/off]          :  turn Colorfade on or off.\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 set [VAL]              :  Set PWM value.\n"));
}

int freeRAM()
{
	extern int __heap_start, *__brkval;
	int v;
	return (int) &v - (__brkval == 0 ? (int) &__heap_start: (int) __brkval);
}

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

void loadRGB(RgbLed* rgbled)
{
	rgbled->setSolidColor(EEPROM_read_char(1), EEPROM_read_char(2), EEPROM_read_char(3));
}

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

void writeRelayState(Serial* serial, WirelessRelay* relay, uint8_t number)
{
	uint16_t id = relay->getId();
	snprintf(buffer, SNPRINTF_BUFFER_SIZE, "RELAY NUMBER: %u ID: %s%s%s STATE: %u NAME: %s\n", number, 
                    bit_rep[ id           >> 12], 
                    bit_rep[(id & 0x0F00) >> 8 ], 
                    bit_rep[(id & 0x00F0) >> 4 ],
                    relay->getExpectedState(),
					relay->getName()
			);
	serial->write(buffer, SNPRINTF_BUFFER_SIZE);
}

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);
            }
            writeRelayState(serial, &relays->back(), relays->count()-1);
            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 )
    {
		token = strtok(NULL, " \n");
		if(relays->count() > 0)
		{
			uint16_t index = relays->count();
			if( token != NULL) index = atoi(token);
			snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Deleting relay NUMBER: %u NAME: %s\n", index, relays->at(index).getName());
			serial->write(buffer, SNPRINTF_BUFFER_SIZE);
			relays->erase(index);
			save();
		}
    }
    else if( strcmp(token, "on") == 0 )
    {
        char* token = strtok(NULL, " \n");
        if( token != NULL)
        {
            uint8_t selected = strtol(token, nullptr, 10);
            if (selected < relays->count())
            {
				relays->at(selected).on();
				
                writeRelayState(serial, &relays->at(selected), selected);
            }
            else serial->write_p(PSTR("No sutch Relay\n"));
        }
        else serial->write_p(PSTR("Usage: relay on [nn]\n"));
    }
    else if( strcmp(token, "off") == 0 )
    {
        char* token = strtok(NULL, " \n");
        if( token != NULL)
        {
            uint8_t selected = strtol(token, nullptr, 10);
            if (selected < relays->count())
            {
                relays->at(selected).off();
                
                writeRelayState(serial, &relays->at(selected), selected);
            }
            else serial->write_p(PSTR("No sutch Relay\n"));
        }
        else serial->write_p(PSTR("Usage: relay off [nn]\n"));
    }
    else if( strcmp(token, "resend") == 0 )
    {
        char* token = strtok(NULL, " \n");
		serial->write_p(PSTR("Resend every 30 min is "));
        if( token != NULL )
        {
			serial->write_p(PSTR("now "));
            if(strcmp(token, "on") == 0) resendEnabled = true;
			else resendEnabled = false;
			save();
        }
        resendEnabled ? serial->write_p(PSTR("enabled.\n")) : serial->write_p(PSTR("disabled.\n")) ;
    }
    else 
    { 
        serial->write(token);
        serial->write_p(PSTR(" is not a valid subcommand: relay [add/delete/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)
        {
			uint8_t r = atoi(rToken);
			uint8_t g = atoi(gToken);
			uint8_t b = atoi(bToken);
            rgbled->setSolidColor(r,g,b);
			EEPROM_write_char(1, r);
			EEPROM_write_char(2, g);
			EEPROM_write_char(3, b);
            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, "set") == 0 )
    {
        token = strtok(NULL, " \n");
        if(token != NULL)
        {
			if(atoi(token) == 0) auxPwm->off();
			else auxPwm->on();
            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 set [value]\n"));
    }
}

void serialDispatch(Serial* serial, SVector<WirelessRelay, MAX_RELAYS>* relays, RgbLed* rgbled, Pwm16b* auxPwm, W433DataReciver* reciver)
{
    
    if(serial->dataIsWaiting())
    {
        char buffer[COMMAND_BUFFER_SIZE];
        unsigned int length = serial->getString(buffer, COMMAND_BUFFER_SIZE);
        if(length > 2)
        {
			setBit(&PCICR, PCIE1, false);
            char* token = strtok(buffer, " \n");
            if(strcmp(token, "relay") == 0)
            {
                relayDispatch(relays, auxPwm, strtok(NULL, " \n"), serial);
            }
            else if(strcmp(token, "rgb") == 0)
            {
                reciver->waitForReciveIdle();
                rgbDispatch(rgbled, strtok(NULL, " \n"), serial);
            }
            else if(strcmp(token, "aux") == 0)
            {
                auxDispatch(auxPwm, strtok(NULL, " \n"), serial);
            }
            else if(strcmp(token, "pause") == 0)
            {
                sensorsPaused = true;
				serial->write_p(PSTR("Sensors paused\n"));
            }
            else if(strcmp(token, "resume") == 0)
            {
                sensorsPaused = false;
				serial->write_p(PSTR("Sensors resumed\n"));
            }
            else if(strcmp(token, "state") == 0)
            {
					serial->write_p(PSTR("Relays:\n"));
					for(uint8_t i = 0; i < relays->count(); i++)
					{
						writeRelayState(serial, &relays->at(i), i);
						//serial->putChar('\n');
					}
					serial->write_p(PSTR("EOL\n"));
            }
            else if(strcmp(token, "erase") == 0)
            {
                for(uint16_t i = 0; i < 1024; 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 < 1024; i++) 
                {
                    if(i != 0) serial->putChar(',');
                    serial->write((uint16_t)EEPROM_read_char(i));
                }
                serial->putChar('\n');
            }
            else if(strcmp(token, "free") == 0)
            {
                serial->write_p(PSTR("Free Ram: "));
				serial->write(freeRAM());
				serial->write_p(PSTR(" Bytes.\n"));
            }
            else if(strcmp(token, "help") == 0)
            {
                printHelp(serial);
            }
            else
            {
                serial->write_p(PSTR("Not a valid command\n"));
            }
            setBit(&PCICR, PCIE1, true);
        }
    }
}

void reciverError(uint8_t code, void* userData)
{
	if(!sensorsPaused)
	{
		Serial* serial = reinterpret_cast<Serial*>(userData);
		serial->write_p(PSTR("ERROR CODE: "));
		serial->write(code);
		serial->putChar('\n');
	}
}

void sensorPacketRecived(uint32_t data, void* userData)
{
	if(!sensorsPaused)
	{
		Serial* serial = reinterpret_cast<Serial*>(userData);
		
		uint16_t field = data & 0x0000FFFF;
		
		serial->write_p(PSTR("SENSOR TYPE: "));
		serial->write(data >> 24);
		serial->write_p(PSTR(" ID: "));
		serial->write((data & 0x00FF0000) >> 16);
		
		     if(data >> 24 == 1) serial->write_p(PSTR(" TEMPERATURE: "));
		else if(data >> 24 == 2) serial->write_p(PSTR(" HUMIDITY: ")); 
		else serial->write_p(PSTR(" FIELD: ")); 
		
		serial->write(field);
		serial->putChar('\n');
	}
}


int main() 
{
	wdt_set(WDTO_8S);
	
    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);
    
    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 );
	loadRGB(&rgbled);
    
    Pwm16b pwmTc1 ( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001, true, false);
	
	setBit(&PCICR, PCIE1, true);
	setBit(&PCMSK1, PCINT8, true);
	W433DataReciver reciver(&PINC, PC0, &TCNT1, &TIFR1, &sensorPacketRecived, reinterpret_cast<void*>(&serial), &reciverError);
	
    serial.write_p(PSTR("RGBController v1.1 starting\n"));
    
    load();
    
    while(true)
    {
        serialDispatch(&serial, &relays, &rgbled, &pwmTc1, &reciver);
        rgbled.logic();
        
        if(doorOne != readPin(&PINB, PB3) && !sensorsPaused)
        {
            _delay_ms(10);
            if(doorOne != readPin(&PINB, PB3))
            {
				doorOne = readPin(&PINB, PB3);
				serial.write_p(PSTR("SENSOR TYPE: "));
				serial.putChar('0');
				serial.write_p(PSTR(" ID: "));
				serial.putChar('0');
				serial.write_p(PSTR(" STATE: "));
				serial.write(doorOne);
				serial.putChar('\n');
            }
        }
        
        
        if(doorTow != readPin(&PINB, PB4) && !sensorsPaused)
        {
            _delay_ms(10);
            if(doorTow != readPin(&PINB, PB4))
            {
				doorTow = readPin(&PINB, PB4);
				serial.write_p(PSTR("SENSOR TYPE: "));
				serial.putChar('0');
				serial.write_p(PSTR(" ID: "));
				serial.putChar('1');
				serial.write_p(PSTR(" STATE: "));
				serial.write(doorTow);
				serial.putChar('\n');
            }
        }
        
        if(resendNow)
		{
			for(uint16_t i = 0; i < relays.count(); i++) 
			{
                reciver.waitForReciveIdle();
				relays[i].resend();
				_delay_ms(100);
			}
			resendNow = false;
		}
        _delay_ms(2);
    }

    return 0;
}