#include "stdafx.h"

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

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

	//Run
	while(true)
	{
		localRobot->Update();
	}

	//Cleanup
	delete localRobot;
	localRobot = NULL;
}