#include "stdafx.h"

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

	sleep(1);//Wait for Hardware

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

	//Init Engines
	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
	{
		Engine* newEngine = new Engine(i);
		localRobot->AddModule(newEngine);
		newEngine = NULL;
	}

	//Init Dribbler
	for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
	{
		Dribbler* newDribbler = new Dribbler(i);
		localRobot->AddModule(newDribbler);
		newDribbler = NULL;
	}

	//Init Kicker
	for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
	{
		Kicker* newKicker = new Kicker(i);
		localRobot->AddModule(newKicker);
		newKicker = NULL;
	}

	//Init Aktuator
	for(uint8 i = IO_AKTUATOR_START; i < IO_AKTUATOR_END; i++)
	{
		Aktuator* newAktuator = new Aktuator(i);
		localRobot->AddModule(newAktuator);
		newAktuator = NULL;
	}

	//Init Wireless
	for(uint8 i = IO_WIRELESS_START; i < IO_WIRELESS_END; i++)
	{
		Wireless* newWireless = new Wireless(i);
		localRobot->AddModule(newWireless);
		newWireless = NULL;
	}

	//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;
				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;
			
				break;
			}
			case IO_SENSOR_MOUSE_LEFT:
			case IO_SENSOR_MOUSE_RIGHT:
			{
				Mouse_Sensor* newSensor = new Mouse_Sensor(i);
				localRobot->AddModule(newSensor);
				newSensor = NULL;
			
				break;
			}
			//Other cases
			default:
			{
				Sensor* newSensor = new Sensor(i);
				localRobot->AddModule(newSensor);
				newSensor = NULL;
				break;
			}
		}
	}

	//Init Leds
	for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
	{
		Led* newLed = new Led(i);
		localRobot->AddModule(newLed);
		newLed = NULL;
	}

	//Init Keyboards
	for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
	{
		Keyboard* newKeyboard = new Keyboard(i);
		localRobot->AddModule(newKeyboard);
		newKeyboard = NULL;
	}

	//Init Command Handler
	for(uint8 i = IO_COMMAND_HANDLER_START; i < IO_COMMAND_HANDLER_END; i++)
	{
		Command_Handler* newCommandHandler = new Command_Handler(i);
		localRobot->AddModule(newCommandHandler);
		newCommandHandler = NULL;
	}

	//Init Obstacle Tracker
	for(uint8 i = IO_OBSTACLE_TRACKER_START; i < IO_OBSTACLE_TRACKER_END; i++)
	{
		Obstacle_Tracker* newObstacleTracker = new Obstacle_Tracker(i);
		localRobot->AddModule(newObstacleTracker);
		newObstacleTracker = NULL;
	}

	//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;
	}

	//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;
	}

	//Init Navigators
	for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
	{
		Navigator* newNavigator = new Navigator(i);
		localRobot->AddModule(newNavigator);
		newNavigator = NULL;
	}

	//Init Logics
	for(uint8 i = IO_LOGIC_START; i < IO_LOGIC_END; i++)
	{
		Logic* newLogic = new Logic(i);
		localRobot->AddModule(newLogic);
		newLogic = NULL;
	}

	sleep(1);
	
	//Debug code
	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
	ourDisplay->Clear();
	ourDisplay->Print("Roboter fertig gestartet",1,1);
	sleep(1);
	

	IR_Sensor* ourSensor = NULL;
	Distance_Sensor* distanceSensor = NULL;
	uint16 value = 0;
	int8 value2 = 0;
	
	Command_Handler* ourCommandHandler = localRobot->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
	uint32 i = 1;
	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
	Position_Tracker* ourPositionTracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
	Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN);
	//ourDribbler->SetSpeed(1);
	//ourDribbler->SetEnabled(true);
	Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
	Logic* ourLogic = localRobot->GetModule<Logic>(IO_LOGIC_MAIN);	
	Wireless* ourWireless = localRobot->GetModule<Wireless>(IO_WIRELESS_MAIN);

	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200);

	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);

	//sleep(2);

	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200);
	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);

	//sleep(2);

	

	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);

	/*(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(200);
	(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetEnabled(true);*/

	/*(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80);
	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/

	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(255);
	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetEnabled(true);

	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255);
	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);

	sleep(2);

	//Run
	while(true)
	{
		i++;
		
		/*if(!(i % 20))
		{
			ourDisplay->Clear();
			msleep(10);
			ourCommandHandler->PrintCommand();
		}
		
		localRobot->Update();

		if(ourCommandHandler->displayDistanceSensors && !(i % 20))
		{
			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(";");
		}

		if(ourCommandHandler->ticksPerCmOffset && !(i % 20))
		{
			ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1);
		}

		if(ourCommandHandler->displayPositionTracker && !(i % 20))
		{
			ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
			ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
			ourDisplay->Print(ourPositionTracker->GetOrientation(), 1, 4);
		}

		if(ourCommandHandler->displayPositionTracker && !(i % 20))
		{
			ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
			ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
			ourDisplay->Print(ourPositionTracker->GetOrientation() * 180.0f / PI, 1, 4);
		}

		if(ourCommandHandler->displayBallTracker && !(i % 20))
		{
			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, 6, 2);
			ourDisplay->Print(";");
			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
			value = ourSensor->GetIRIntensity();
			ourDisplay->Print(value, 11, 2);
			ourDisplay->Print(";");
			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
			value = ourSensor->GetIRIntensity();
			ourDisplay->Print(value, 16, 2);
			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, 6, 3);
			ourDisplay->Print(";");
			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
			value = ourSensor->GetIRIntensity();
			ourDisplay->Print(value, 11, 3);
			ourDisplay->Print(";");
			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
			value = ourSensor->GetIRIntensity();
			ourDisplay->Print(value, 16, 3);
			ourDisplay->Print(";");
			if(ourBallTracker->KnowsBallDirection())
			{
				ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
			}
			else
			{
				ourDisplay->Print("Ball not found", 1, 4);
			}

			if(ourBallTracker->HasBall()) {
				ourDisplay->Print("Has Ball", 5, 4);
			}
		}

		//if(!(i % 20))
		//{
		//	ourDisplay->Print(ourWireless->Recieve(), 10, 1);
		//}

		if(ourBallTracker->KnowsBallDirection())
		{
			ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
		}
		else
		{
			ourNavigator->Stop();
		}*/
	}

	//Cleanup
	delete localRobot;
	localRobot = NULL;
}