#include "command_handler.h"

//-----------------------------------------------------------------------------
void Command_Handler::Update()
{
	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
	Keyboard* ourKeyboard = parent->GetModule<Keyboard>(IO_KEYBOARD_MAIN);

	uint8 curInput = ourKeyboard->GetInput();


	while(curInput != 0xEE && curInput != 0xFF)
	{
		if(curInput == 10)
		{
			ExecuteCommand();

			for(uint8 i = 0; i < this->currentCommandLength; i++)
			{
				this->buffer[i] = NULL;
				ourDisplay->Print(" ", i + 1, 1);
			}

			this->currentCommandLength = 0;
		}
		else if(curInput == 11)
		{
			if(this->currentCommandLength > 0)
			{
				ourDisplay->Print(" ", this->currentCommandLength, 1);
				this->currentCommandLength--;
				this->buffer[currentCommandLength] = NULL;
			}
		}
		else if(curInput >= 0 && curInput <= 9)
		{
			if(this->currentCommandLength < COMMAND_BUFFER_SIZE)
			{
				this->buffer[this->currentCommandLength] = curInput;
				this->currentCommandLength++;
				ourDisplay->Print(curInput, this->currentCommandLength, 1);
			}
		}

		curInput = ourKeyboard->GetInput();
	}
}

//-----------------------------------------------------------------------------
void Command_Handler::ExecuteCommand()
{
	if(this->buffer[0] == 1)
	{
		if(this->buffer[1] == 1)
		{
			this->displayDistanceSensors = true;
			this->displayPositionTracker = false;
			this->displayBallTracker = false;
			this->displayMouseSensors = false;
		}
		else if(this->buffer[1] == 2)
		{
			this->displayDistanceSensors = false;
			this->displayPositionTracker = true;
			this->displayBallTracker = false;
			this->displayMouseSensors = false;
		}
		else if(this->buffer[1] == 3)
		{
			this->displayDistanceSensors = false;
			this->displayPositionTracker = false;
			this->displayBallTracker = true;
			this->displayMouseSensors = false;
		}
		else if(this->buffer[1] == 4)
		{
			this->displayDistanceSensors = false;
			this->displayPositionTracker = false;
			this->displayBallTracker = false;
			this->displayMouseSensors = true;
		}
		else if(this->buffer[1] == 0)
		{
			this->displayDistanceSensors = false;
			this->displayPositionTracker = false;
			this->displayBallTracker = false;
			this->displayMouseSensors = false;
		}
	}
	else if(this->buffer[0] == 2)
	{
		Logic* ourLogic = parent->GetModule<Logic>(IO_LOGIC_MAIN);	
		if(this->buffer[1] == 1)
		{
			ourLogic->SetKeeper(true);
		}
		else if(this->buffer[1] == 2)
		{
			ourLogic->SetKeeper(false);
		}
	}
	else if(this->buffer[0] == 3)
	{
		if(this->buffer[1] == 1)
		{
			Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
			ourNavigator->RotateTo(180, 200);
		}
		else if(this->buffer[1] == 2)
		{
			Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
			ourAktuator->Kick();
		}
		else if(this->buffer[1] == 3)
		{
			Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
			ourNavigator->RotateTo(float(buffer[2]) * 100.0f + float(buffer[3]) * 10.0f + float(buffer[4]), 200);
		}
		else if(this->buffer[1] == 4)
		{
			Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
			Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

			this->ticksPerCmOffset += 2.5f;
			ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
			ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
			ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
			ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
		}
		else if(this->buffer[1] == 5)
		{
			Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
			Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

			this->ticksPerCmOffset -= 2.5f;
			ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
			ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
			ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
			ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
		}
	}
	else if(this->buffer[0] == 4)
	{
		Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
		int16 speed = 200;
		switch(this->buffer[1])
		{
			case 1:
				ourNavigator->Drive(225, 0, speed, 0);
				break;
			case 2:
				ourNavigator->Drive(180, 0, speed, 0);
				break;
			case 3:
				ourNavigator->Drive(135, 0, speed, 0);
				break;
			case 4:
				ourNavigator->Drive(270, 0, speed, 0);
				break;
			case 5:
				ourNavigator->Rotate(DEFAULT_ROTATION_SPEED);
				break;
			case 6:
				ourNavigator->Drive(90, 0, speed, 0);
				break;
			case 7:
				ourNavigator->Drive(315, 0, speed, 0);
				break;
			case 8:
				ourNavigator->Drive(0, 0, speed, 0);
				break;
			case 9:
				ourNavigator->Drive(45, 0, speed, 0);
				break;
			case 0:
				ourNavigator->Stop();
				break;
		}
	}
	else if(this->buffer[0] == 5)
	{
		Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
		if(this->buffer[1] == 1)
		{
			ourPositionTracker->SetPosition(float(), ourPositionTracker->GetPositionY(), ourPositionTracker->GetOrientation());
		}
		else if(this->buffer[1] == 0)
		{
			ourPositionTracker->SetPosition(0, 0, 0);
		}
	}
	else if(this->buffer[0] == 6)
	{
		if(this->buffer[1] == 1)
		{
			Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);
			ourWireless->Send("Heyho!");
		}
	}
}

//-----------------------------------------------------------------------------
void Command_Handler::PrintCommand()
{
	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
	
	for(uint8 i = 0; i < currentCommandLength; i++)
	{
		ourDisplay->Print(this->buffer[i], i + 1, 1);
	}
}