#pragma once

void printTrainState(int id, Serial* serial)
{
	snprintf(buffer, SNPRINTF_BUFFER_SIZE,
		     "TRAIN NUMBER: %u ADDRESS: %u CURRENT PACKET: %x SPEED: %i FUNCTIONS: %s FUNCTIONMASK: %s QUIRKS: %s\n", 
		      id, trains[id].getAddress(),
		      trains[id].getLastPacket(), trains[id].getSpeed(),
		      bit_rep[trains[id].getFunctions() & 0x0F], bit_rep[trains[id].getFunctionMask() & 0x0F],
		      bit_rep[trains[id].getQuirks() & 0x0F]);
	serial->write(buffer, SNPRINTF_BUFFER_SIZE);
}

int trainDispatch(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 && trains.remainingCapacity() > 0) 
		{
			uint8_t functionMask = 0;
			uint8_t quirks = 0;
			
			token = strtok(NULL, " ");
			if(token != NULL) 
			{
				functionMask = strtol(token, nullptr, 2);
				token = strtok(NULL, " ");
				if(token != NULL)
				{
					quirks = strtol(token, nullptr, 2);
				}
			}

			trains.push_back(Train(address, functionMask, quirks));
			
			serial->write_p(PSTR("TRAIN saved! "));
			printTrainState(trains.count()-1, serial);

			save_state();
			return 0;
		}
		serial->write_p(PSTR("Usage: train add [address] [functionmask] [quirks]"));
	}
	else if( strcmp(inBuffer, "list") == 0 )
	{
		serial->write_p(PSTR("Trains:\n"));
		for(uint8_t i = 0; i < trains.count(); i++)
			printTrainState(i, serial);
		serial->putChar('\n');
		return 0;
	}
	else if( strcmp(inBuffer, "probe") == 0 )
	{
		for(uint16_t j = 0; j < 255; j++)
		{
			snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRYING ADDR: %x\n", j);
			serial->write(buffer, strlen(buffer));
			cli();
			for(uint8_t k = 0; k < 10; k++)
			{
				Train::sendRawAddr(j, 60);
				_delay_ms(20);
			}
			for(uint8_t k = 0; k < 3; k++)
			{
				Train::sendRawAddr(j, 0);
				_delay_ms(20);
			}
			sei();
		}
		return 0;
	}
	else
	{
		uint8_t id = strtol(inBuffer, nullptr, 10);
		if(id < trains.count() )
		{
			setPower(true);
			char* token = strtok(NULL, " ");
			if( token != NULL &&  (strcmp(token, "speed") == 0 || strcmp(token, "s") == 0) )
			{
				token = strtok(NULL, " ");
				trains[id].setSpeed(atoi(token));
				serial->write_p(PSTR("Set Train speed\n"));
				printTrainState(id, serial);
				return 0;
			}
			else if(token != NULL && strcmp(token, "function") == 0 ) 
			{
				token = strtok(NULL, " ");
				char* boolToken = strtok(NULL, " ");
				if(token != NULL && boolToken !=  NULL)
				{
					uint8_t functionId = atoi(token);
					bool on = (strcmp(boolToken, "on") == 0);
					trains[id].sendFunction(functionId, on );
					serial->write_p(PSTR("Set Train function "));
					serial->write(functionId);
					serial->write(on ? " on\n" : " off\n");
					printTrainState(id, serial);
					return 0;
				}
			}
			else if(token != NULL && strcmp(token, "probe") == 0 ) 
			{
				serial->flush();
				for(uint16_t j = 0; j < 1024 && !serial->dataIsWaiting(); j++)
				{
					trains[id].sendRaw(j);
					snprintf(buffer, SNPRINTF_BUFFER_SIZE, "TRYING: %x\n", j);
					serial->write(buffer, strlen(buffer));
					sei();
					_delay_ms(250);
					cli();
				}
				sei();
				serial->flush();
				return 0;
			}
			else if(token != NULL && strcmp(token, "raw") == 0 ) 
			{
				token = strtok(NULL, " ");
				if(token != NULL)
				{
					cli();
					uint16_t i = strtol(token, nullptr, 16 );
					snprintf(buffer, SNPRINTF_BUFFER_SIZE, "SENDING: %x to %x\n", i, trains[id].getAddress());
					serial->write(buffer, strlen(buffer));
					for(uint8_t j = 0; j < 100; j++)
					{
						trains[id].sendRaw(i);
						_delay_ms(20);
					}
					serial->write_p(PSTR("Finished\n"));
					sei();
					return 0;
				}
			}
			else if( token != NULL && (strcmp(token, "reverse") == 0 || strcmp(token, "r") == 0) )
			{
				trains[id].reverse();
				serial->write_p(PSTR("Reversed Train\n"));
				return 0;
			}
			else if( token != NULL && (strcmp(token, "stop") == 0 || strcmp(token, "p") == 0))
			{
				trains[id].stop();
				printTrainState(id, serial);
				return 0;
			}
			else if( token != NULL && strcmp(token, "edit") == 0)
			{
				uint8_t functionMask = 0;
				uint8_t quirks = 0;
				token = strtok(NULL, " ");
				if(token != NULL) 
				{
					functionMask = strtol(token, nullptr, 2);
					token = strtok(NULL, " ");
					if(token != NULL)
					{
						quirks = strtol(token, nullptr, 2);
					}
				}
				trains[id].stop();
				trains[id].setFunctionMask(functionMask);
				trains[id].setQuirks(quirks);
				save_state();
				printTrainState(id, serial);
				return 0;
			}
			else if( token != NULL && strcmp(token, "delete") == 0)
			{
				trains.erase(id);
				serial->write_p(PSTR("Train: "));
				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;
}