#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include "serial.h"
#include "writepin.h"
#include "train.h"
#include "eeprom.h"
#include "bitrep.h"
#include "ringbuffer.h"
#include "staticvector.h"
#include "turnout.h"
#include "signal.h"
#include "shiftreg.h"
#include "softspim.h"
#include "nfcbord.h"
#include "defines.h"

char buffer[SNPRINTF_BUFFER_SIZE];

SVector<Train, 32> trains;
SVector<Turnout, 32> turnouts;
SVector<Signal, 32> signals;
NfcBoard nfcBoard;

bool autoff = true;
bool powerIsOn = true;

volatile uint8_t tick = 0;

volatile bool resendEvent = false;
uint8_t itemToResend = 0;

void setPower(bool on);
void save_state();

#include "traindispatch.h"
#include "turnoutdispatch.h"
#include "signaldispatch.h"

ISR(TIMER0_OVF_vect) 
{
	if(tick == 0)
		resendEvent = true;
	tick = !tick;
}

void timer0InterruptEnable(const bool enable)
{
	if(enable)
		TIMSK0 = 0b00000001;
	else
		TIMSK0 = 0b00000000;
}

void save_state()
{
	cli();
	EEPROM_write_char( 0, trains.count());
	EEPROM_write_char( 1, autoff);
	EEPROM_write_char( 2, trains.maxSize());
	EEPROM_write_char( 3, turnouts.count());
	EEPROM_write_char( 4, turnouts.maxSize());
	EEPROM_write_char( 5, signals.count());
	EEPROM_write_char( 6, signals.maxSize());
	
	const uint16_t trainOffset = EEPROM_RESERVE;
	for(uint8_t i = 0; i < trains.count(); i++)
	{
		EEPROM_write_char( i*BLOCK+trainOffset, trains[i].getAddress());
		EEPROM_write_char( i*BLOCK+trainOffset+1, trains[i].getFunctionMask());
		EEPROM_write_char( i*BLOCK+trainOffset+2, trains[i].getQuirks());
	}
	
	const uint16_t turnoutOffset = trains.maxSize()*BLOCK+EEPROM_RESERVE;
	for(uint8_t i = 0; i < turnouts.count(); i++)
	{
		EEPROM_write_char( i*BLOCK+turnoutOffset, turnouts[i].getAddress());
		EEPROM_write_char( i*BLOCK+turnoutOffset+1, turnouts[i].getSubaddress());
	}
	
	const uint16_t signalOffset = trains.maxSize()*BLOCK+turnouts.maxSize()*BLOCK+EEPROM_RESERVE;
	for(uint8_t i = 0; i < signals.count(); i++)
	{
		EEPROM_write_char( i*4+signalOffset, signals[i].getAddress());
		EEPROM_write_char( i*4+signalOffset+1, signals[i].getSubaddress());
		EEPROM_write_char( i*4+signalOffset+2, signals[i].getType());
	}
	sei();
}

void restore_state()
{
	uint8_t trainCount = EEPROM_read_char(0);
	uint8_t turnoutCount = EEPROM_read_char(3);
	uint8_t signalCount = EEPROM_read_char(5);
	autoff = EEPROM_read_char(1);
	
	trains.clear();
	turnouts.clear();
	signals.clear();
	if(trainCount > trains.maxSize() || trains.maxSize() != EEPROM_read_char(2) ||
	   turnoutCount > turnouts.maxSize() || turnouts.maxSize() != EEPROM_read_char(4) ||
	   signalCount > signals.maxSize() || signals.maxSize() != EEPROM_read_char(6)) 
	{
		for(uint16_t i = 0; i < EPPROM_SIZE; i++)
			EEPROM_write_char(i, 0);
	}
	else 
	{
		const uint16_t trainOffset = EEPROM_RESERVE;
		const uint16_t turnoutOffset = trains.maxSize()*BLOCK+EEPROM_RESERVE;
		const uint16_t signalOffset = trains.maxSize()*BLOCK+turnouts.maxSize()*BLOCK+EEPROM_RESERVE;
		
		for(uint8_t i = 0; i < trainCount; i++)
			trains.push_back(Train(EEPROM_read_char(trainOffset+i*4),
								   EEPROM_read_char(trainOffset+1+i*4),
								   EEPROM_read_char(trainOffset+2+i*4)));
		for(uint8_t i = 0; i < turnoutCount; i++)
			turnouts.push_back(Turnout(EEPROM_read_char(i*4+turnoutOffset),
									   EEPROM_read_char(1+i*4+turnoutOffset)));
		for(uint8_t i = 0; i < signalCount; i++)
			signals.push_back(Signal(EEPROM_read_char(i*4+signalOffset),
									 EEPROM_read_char(1+i*4+signalOffset),
									 EEPROM_read_char(2+i*4+signalOffset)));
	}
}

inline static void printHelp(Serial* serial)
{
	serial->write_p(PSTR("Available Commands: \n\
				help                                        :  Show this prompt.\n\
				train add [address] [functionmask] [quriks] :  Add train.\n\
				train [nn] delete                           :  Delete last train.\n\
				train list                                  :  Print list of saved trains.\n\
				train [nn] s(top)                           :  Stop nth train.\n\
				train [nn] s(peed) [sp]                     :  Set nth train speed.\n\
				train [nn] function [x]                     :  Toggle x'ed fuction on train n.\n\
				train [nn] r(everse)                        :  Reverse train n.\n\
				train [nn] edit [functionmask] [quriks]     :  Edit train n.\n\
				turnout add [address] [subaddress]          :  Add a turnout\n\
				turnout list                                :  List turnouts\n\
				turnout [nn] set [left/right]               :  Set turnout direction\n\
				turnout [nn] delete                         :  Delete Turnout\n\
				signal add [address] [subaddress] [type]    :  Add a signal\n\
				signal list                                 :  List signal\n\
				signal [nn] set [status]                    :  Set signal direction\n\
				signal [nn] delete                          :  Delete signal\n\
				stop                                        :  stop all trains\n\
				power off                                   :  power off the rail\n\
				power on                                    :  power on the rail\n\
				power auto                                  :  power off the rail when no trains are moveing\n\
				dump                                        :  prints epprom contence\n\
				erase                                       :  Erase epprom.\n"));
}

void setPower(bool on)
{
	if(powerIsOn != on)
	{
		powerIsOn = on;
		
		if(on)
		{
			Train::setOutput(Train::LOW);
			_delay_ms(100);
			timer0InterruptEnable(true);
		}
		else
		{
			timer0InterruptEnable(false);
			Train::setOutput(Train::OFF);
		}
	}
}

int powerDispatch(char* token, Serial* serial)
{
	if(strcmp(token, "off") == 0)
	{
		setPower(false);
		return 0;
	}
	else if( strcmp(token, "on") == 0)
	{
		for(uint16_t i = 0; i < trains.count(); i++)
		{
			trains[i].setSpeed(0);
		}
		setPower(true);
		return 0;
	}
	else if(strcmp(token, "auto") == 0)
	{
		token = strtok(NULL, " ");
		if(token != NULL && strcmp(token, "on") == 0)
		{
			autoff = true;
			serial->write_p(PSTR("auto power off turned on.\n"));
			save_state();
		}
		else if(token != NULL && strcmp(token, "off") == 0)
		{
			autoff = false;
			serial->write_p(PSTR("auto power off turned off.\n"));
			save_state();
		}
		else 
		{
			serial->write_p(PSTR("argument must be \"on\" or \"off\". This feature is currently "));
			autoff ? serial->write_p(PSTR("on.\n")) : serial->write_p(PSTR("off.\n"));
		}
		return 0;
	}
	return -1;
}


void serialDispatch(Serial* serial)
{
	static uint8_t lastItemToResend = 0;
	if(serial->dataIsWaiting())
	{
		char buffer[COMMAND_BUFFER_SIZE];
		unsigned int length = serial->getString(buffer, COMMAND_BUFFER_SIZE);
		if(length > 0)
		{
			int ret = -1;
			char* token = strtok(buffer, " ");

			if(lastItemToResend == itemToResend)
				Item::directSendBlock = true;

			if(strcmp(token, "train") == 0 || strcmp(token, "t") == 0 )
			{
				token = strtok(NULL, " ");
				if(token != NULL)
					ret = trainDispatch(token, serial);
			}
			else if(strcmp(token, "turnout") == 0)
			{
				token = strtok(NULL, " ");
				if(token != NULL)
					ret = turnoutDispatch(token, serial);
			}
			else if(strcmp(token, "signal") == 0)
			{
				token = strtok(NULL, " ");
				if(token != NULL)
					ret = signalDispatch(token, serial);
			}
			else if(strcmp(token, "nfc") == 0)
			{
				token = strtok(NULL, " ");
				if(token != NULL)
					ret = nfcBoard.dispatch(token, serial);
			}
			else if(strncmp(token, "erase", 4) == 0)
			{
				for(uint16_t i = 0; i < EPPROM_SIZE; i++) EEPROM_write_char(i, 0);
				serial->write_p(PSTR("EEPROM erased\n"));
				trains.clear();
				ret = 0;
			}
			else if(strcmp(token, "dump") == 0)
			{
				for(uint16_t i = 0; i < EPPROM_SIZE; i++) 
				{
					if(i != 0) serial->putChar(',');
					serial->write((uint16_t)EEPROM_read_char(i));
				}
				serial->putChar('\n');
				ret = 0;
			}
			else if((strcmp(token, "stop") == 0 || strcmp(token, "s") == 0 ))
			{
				for(uint16_t i = 0; i < trains.count(); i++)
				{
					cli();
					trains[i].stop();
					printTrainState(i, serial);
					sei();
				}
				ret = 0;
			}
			else if(strcmp(token, "power") == 0)
			{
				token = strtok(NULL, " ");
				if(token != NULL)
					ret = powerDispatch(token, serial);
			}
			else if(strcmp(token, "help") == 0)
			{
				printHelp(serial);
				ret = 0;
			}
			else
			{
				serial->putChar('\"');
				serial->write(buffer, length);
				serial->putChar('\"');
				serial->write_p(PSTR(" is not a valid command\n"));
				ret = -1;
			}
			
			if(ret < 0)
			{
				serial->write_p(PSTR("Command Failed\n"));
			}
			else
			{
				lastItemToResend = itemToResend;
				/*for(uint8_t i = 0; i < length; ++i)
				{
					if(buffer[i] == '\0' || buffer[i] == '\n')
						buffer[i] = ' ';
				}
				serial->write_p(PSTR("Sucess \""));
				serial->write(buffer, length);
				serial->write("\"\n");*/
			}

			Item::directSendBlock = false;
		}
	}
}

int main() 
{
	TCNT0 = 0;
	TCCR0B = (1<<CS02); // run timer0 with /256 scaler

	DDRD = (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);

	restore_state();

	if(autoff) 
	{
		timer0InterruptEnable(false);
		Train::setOutput(Train::OFF);
	}
	else
	{
		timer0InterruptEnable(true);
		Train::setOutput(Train::LOW);
	}

	sei();

	Serial serial;

	serial.write_p(PSTR("TrainController v0.5 starting\n"));

	while(true)
	{        
		if(resendEvent && (trains.count() || turnouts.count() || signals.count())) 
		{
			timer0InterruptEnable(false);
			if(itemToResend < trains.count())
				trains[itemToResend].sendData();
			else if(itemToResend < trains.count() + turnouts.count())
				turnouts[itemToResend-trains.count()].sendData();
			else if(itemToResend < trains.count() + turnouts.count()+signals.count())
				signals[itemToResend-trains.count()-turnouts.count()].sendData();
			itemToResend++;
			if(trains.count()+turnouts.count()+signals.count() <= itemToResend)
				itemToResend = 0;
			resendEvent = false;
			timer0InterruptEnable(true);
		}
		
		if(autoff) 
		{
			bool trainsRunning = false;
			for(uint16_t i = 0; i < trains.count(); i++)
				trainsRunning = trainsRunning || trains[i].isActive();
			if(!trainsRunning)
			{
				powerIsOn = false;
				timer0InterruptEnable(false);
				Train::setOutput(Train::OFF);
			}
		}
		serialDispatch(&serial);
	}
	return 0;
}