#include "stdafx.h"

int main()
{
	//Init our robot
	Robot* localRobot = new Robot();

	sleep(1); // Wait for LCD-Display

	//Init Displays
	for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
	{
		Display* newDisplay = new Display(i);
		localRobot->AddModule(newDisplay);
		newDisplay = NULL;
	}

	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
	// ourDisplay->Print("Starting..."); ourDisplay->NewLine(); // Debug output

	//Init Engines
	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
	{
		Engine* newEngine = new Engine(i);
		localRobot->AddModule(newEngine);
		newEngine = NULL;
	}
	// ourDisplay->Print("Engines Ready"); ourDisplay->NewLine(); // Debug output

	//Init Dribbler
	for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
	{
		Dribbler* newDribbler = new Dribbler(i);
		localRobot->AddModule(newDribbler);
		newDribbler = NULL;
	}
	// ourDisplay->Print("Dribbler Ready"); ourDisplay->NewLine(); // Debug output

	//Init Kicker
	for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
	{
		Kicker* newKicker = new Kicker(i);
		localRobot->AddModule(newKicker);
		newKicker = NULL;
	}
	// ourDisplay->Print("Kicker Ready"); ourDisplay->NewLine(); // Debug output

	//Init Sensors
	for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
	{
		switch(i)
		{
			//Create correct type of sensor
			case IO_SENSOR_IR_0_DEG:
			case IO_SENSOR_IR_30_DEG:
			case IO_SENSOR_IR_60_DEG:
			case IO_SENSOR_IR_100_DEG:
			case IO_SENSOR_IR_180_DEG:
			case IO_SENSOR_IR_260_DEG:
			case IO_SENSOR_IR_300_DEG:
			case IO_SENSOR_IR_330_DEG:
			{
				IR_Sensor* newSensor = new IR_Sensor(i);
				localRobot->AddModule(newSensor);
				newSensor = NULL;
				// ourDisplay->Print("SensorIR Ready"); ourDisplay->NewLine(); // Debug output
				break;
			}
			case IO_SENSOR_DISTANCE_0_DEG:
			case IO_SENSOR_DISTANCE_90_DEG:
			case IO_SENSOR_DISTANCE_180_DEG:
			case IO_SENSOR_DISTANCE_270_DEG:
			{
				Distance_Sensor* newSensor = new Distance_Sensor(i);
				localRobot->AddModule(newSensor);
				newSensor = NULL;
				// ourDisplay->Print("DistanceSensor Ready"); // Debug output
			
				break;
			}
			case IO_SENSOR_MOUSE_LEFT:
			case IO_SENSOR_MOUSE_RIGHT:
			{
				Mouse_Sensor* newSensor = new Mouse_Sensor(i);
				localRobot->AddModule(newSensor);
				newSensor = NULL;
				// ourDisplay->Print("MouseSensor Ready"); ourDisplay->NewLine(); // Debug output
			
				break;
			}
			//Other cases
			default:
			{
				Sensor* newSensor = new Sensor(i);
				localRobot->AddModule(newSensor);
				newSensor = NULL;
				break;
			}
		}
	}
	// ourDisplay->Print("Sensors Ready"); ourDisplay->NewLine(); // Debug output

	//Init Leds
	for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
	{
		Led* newLed = new Led(i);
		localRobot->AddModule(newLed);
		newLed = NULL;
	}
	// ourDisplay->Print("LEDs Ready"); ourDisplay->NewLine(); // Debug output

	//Init Keyboards
	for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
	{
		Keyboard* newKeyboard = new Keyboard(i);
		localRobot->AddModule(newKeyboard);
		newKeyboard = NULL;
	}
	// ourDisplay->Print("Keyboard Ready"); ourDisplay->NewLine(); // Debug output

	//Init Position Tracker
	for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
	{
		Position_Tracker* newPositionTracker = new Position_Tracker(i);
		localRobot->AddModule(newPositionTracker);
		newPositionTracker = NULL;
	}
	// ourDisplay->Print("PositionTracker Ready"); ourDisplay->NewLine(); // Debug output

	//Init Ball Tracker
	for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++)
	{
		Ball_Tracker* newBallTracker = new Ball_Tracker(i);
		localRobot->AddModule(newBallTracker);
		newBallTracker = NULL;
	}
	// ourDisplay->Print("Balltracker Ready"); ourDisplay->NewLine(); // Debug output

	//Init Navigators
	for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
	{
		Navigator* newNavigator = new Navigator(i);
		localRobot->AddModule(newNavigator);
		newNavigator = NULL;
	}
	// ourDisplay->Print("Navigator Ready"); ourDisplay->NewLine(); // Debug output
	// ourDisplay->Print("All Ready"); ourDisplay->NewLine(); // Debug output
	sleep(1);
	ourDisplay->Clear();
	

	IR_Sensor* ourSensor = NULL;
	Distance_Sensor* distanceSensor = NULL;
	uint16 value = 0;
	int8 value2 = 0;
	
	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
	uint32 i = 1;
	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
	float rotation = 0;

	Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
	Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);


	//Run
	while(true)
	{
		/*ourDisplay->Print(i++,1,1);
		ourDisplay->Print(mouse_left->GetXMovement(),1,2);
		ourDisplay->Print("        ");
		ourDisplay->Print(mouse_left->GetYMovement(),10,2);
		ourDisplay->Print("        ");
		ourDisplay->Print(mouse_right->GetXMovement(),1,3);
		ourDisplay->Print("        ");
		ourDisplay->Print(mouse_right->GetYMovement(),10,3);
		ourDisplay->Print("        ");
		ourDisplay->Print("                    ",1,4); // clear
		
		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
		value = distanceSensor->GetDistance();
		ourDisplay->Print(value, 1, 4);
		ourDisplay->Print(";");
		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
		value = distanceSensor->GetDistance();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
		value = distanceSensor->GetDistance();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
		value = distanceSensor->GetDistance();
		ourDisplay->Print(value);
		ourDisplay->Print(";");		


		msleep(500);*/

		uint8 someInput = ourKeyboard->GetInput();
		//ourDisplay->Clear();
		ourDisplay->Print(i++,1,1);
		//ourDisplay->Print("Ready to accept...", 1, 2);
		ourDisplay->Print(someInput, 1, 3);
		ourDisplay->PrintFloat(rotation, 1, 4);
		switch(someInput)
		{
			case 0:
				ourNavigator->Stop();
			case 1:
				ourNavigator->Drive(225, rotation, 200, rotation);
				break;
			case 2:
				ourNavigator->Drive(180, rotation, 200, rotation);
				break;
			case 3:
				ourNavigator->Drive(135, rotation, 200, rotation);
				break;
			case 4:
				ourNavigator->Drive(270, rotation, 200, rotation);
				break;
			case 5:
				ourNavigator->Drive(0, rotation, 0, rotation);
				break;
			case 6:
				ourNavigator->Drive(90, rotation, 200, rotation);
				break;
			case 7:
				ourNavigator->Drive(315, rotation, 200, rotation);
				break;
			case 8:
				ourNavigator->Drive(0, rotation, 200, rotation);
				break;
			case 9:
				ourNavigator->Drive(45, rotation, 200, rotation);
				break;
			case 10:
				rotation += 10;
				break;
			case 12:
				rotation -= 10;
				break;
		}
		
		msleep(50);
		
		/*
		//ourDisplay->Clear();
		ourDisplay->Print(i++,1,1);
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value, 1, 2);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value, 1, 3);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
		value = ourSensor->GetIRIntensity();
		ourDisplay->Print(value);
		ourDisplay->Print(";");
		
		msleep(500);
		*/
		localRobot->Update();

		//ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);
	}

	//Cleanup
	delete localRobot;
	localRobot = NULL;
}