#pragma once
#include <avr/pgmspace.h>

void printTurnoutState(uint8_t id, Serial* serial)
{
	snprintf_P(buffer, SNPRINTF_BUFFER_SIZE,
		                    PSTR("TURNOUT NUMBER: %u ADDRESS: %u SUBADDRESS: %u CURRENT PACKET: %x DIRECTION: %u\n"), 
		                     id, turnouts[id].getAddress(), turnouts[id].getSubaddress(),
		                     turnouts[id].getPacket(), turnouts[id].getDirection());
	serial->write(buffer, SNPRINTF_BUFFER_SIZE);
}

int turnoutDispatch(char* inBuffer, Serial* serial)
{
	if( strcmp(inBuffer, "add") == 0 )
	{
		char* token = strtok(NULL, " ");
		uint8_t address = 0;
		if(token != NULL)
			address = strtol(token, nullptr, 10);
		if(address != 0 && turnouts.remainingCapacity() > 0) 
		{
			uint8_t subaddress = 0;
			
			token = strtok(NULL, " ");
			if(token != NULL) 
				subaddress = strtol(token, nullptr, 10);

			turnouts.push_back(Turnout(address, subaddress));
			serial->write_p(PSTR("TUNROUT saved! "));
			printTurnoutState(turnouts.count()-1, serial);

			save_state();
			return 0;
		}
		serial->write_p(PSTR("Usage: turnout add [address] [subaddress]"));
	}
	else if( strcmp(inBuffer, "list") == 0 )
	{
		serial->write_p(PSTR("Turnouts:\n"));
		for(uint8_t i = 0; i < turnouts.count(); i++)
			printTurnoutState(i, serial);
		serial->putChar('\n');
		return 0;
	}
	else
	{
		uint8_t id = strtol(inBuffer, nullptr, 10);
		if(id < turnouts.count() )
		{
			setPower(true);
			char* token = strtok(NULL, " ");
			if(token != NULL && strcmp(token, "set") == 0 ) 
			{
				char* boolToken = strtok(NULL, " ");
				if(token != NULL && boolToken !=  NULL)
				{
					bool direction = (strcmp(boolToken, "right") == 0);
					turnouts[id].setDirection(direction);
					serial->write_p(PSTR("Set turnout direction "));
					serial->write(direction ? "right\n" : "left\n");
					printTurnoutState(id, serial);
					return 0;
				}
			}
			else if(token != NULL && strcmp(token, "raw") == 0 ) 
			{
				token = strtok(NULL, " ");
				serial->flush();
				if(token != NULL)
				{
					cli();
					uint16_t i = strtol(token, nullptr, 16 );
					snprintf_P(buffer, SNPRINTF_BUFFER_SIZE, PSTR("SENDING: %x to %x\n"), i, turnouts[id].getAddress());
					serial->write(buffer, strlen(buffer));
					while(!serial->dataIsWaiting())
					{
						turnouts[id].sendRaw(i);
						sei();
						_delay_ms(20);
						cli();
					}
					serial->write_p(PSTR("Finished\n"));
					serial->flush();
					sei();
					return 0;
				}
			}
			else if(token != NULL && strcmp(token, "delete") == 0)
			{
				turnouts.erase(id);
				serial->write_p(PSTR("Turnout: "));
				serial->write(id);
				serial->write_p(PSTR(" deleted\n"));
				save_state();
				return 0;
			}
			else 
			{
				serial->write_p(PSTR("Not a valid command\n"));
				return -1;
			}
		}
		else
		{
			serial->write_p(PSTR("Id out of range.\n"));
			return -2;
		}
	}
	return -3;
}