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

void printSignalState(uint8_t id, Serial* serial)
{
	snprintf_P(buffer, SNPRINTF_BUFFER_SIZE,
		                    PSTR("SIGNAL NUMBER: %u ADDRESS: %u SUBADDRESS: %u TYPE: %u CURRENT PACKET: %x STATE: %u\n"), 
		                     id, signals[id].getAddress(), signals[id].getSubaddress(), signals[id].getType(),
		                     signals[id].getPacket(), signals[id].getState());
	serial->write(buffer, SNPRINTF_BUFFER_SIZE);
}

int signalDispatch(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 && signals.remainingCapacity() > 0) 
		{
			uint8_t subaddress = 0;
			uint8_t type = 0;
			
			token = strtok(NULL, " ");
			if(token != NULL) 
				subaddress = strtol(token, nullptr, 10);
			
			token = strtok(NULL, " ");
			if(token != NULL) 
				type = strtol(token, nullptr, 10);

			signals.push_back(Signal(address, subaddress, type));
			serial->write_p(PSTR("SIGNAL saved! "));
			printSignalState(signals.count()-1, serial);

			save_state();
			return 0;
		}
		serial->write_p(PSTR("Usage: signal add [address] [subaddress] [type]"));
	}
	else if( strcmp(inBuffer, "list") == 0 )
	{
		serial->write_p(PSTR("Signals:\n"));
		for(uint8_t i = 0; i < signals.count(); i++)
			printSignalState(i, serial);
		serial->putChar('\n');
		return 0;
	}
	else
	{
		uint8_t id = strtol(inBuffer, nullptr, 10);
		if(id < signals.count() )
		{
			setPower(true);
			char* token = strtok(NULL, " ");
			if(token != NULL && strcmp(token, "set") == 0 ) 
			{
				token = strtok(NULL, " ");
				if(token != NULL)
				{
					uint8_t state = strtol(token, nullptr, 10);;
					signals[id].setState(state);
					serial->write_p(PSTR("Set signal state"));
					serial->write((int)state);
					serial->putChar('\n');
					printSignalState(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, signals[id].getAddress());
					serial->write(buffer, strlen(buffer));
					while(!serial->dataIsWaiting())
					{
						signals[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)
			{
				signals.erase(id);
				serial->write_p(PSTR("Signal: "));
				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;
}