#include "robot.h"

//-----------------------------------------------------------------------------
Robot::Robot()
{
	//Hardware

	//Set pin 1-6 output, 0 and 7 input
	DDRA = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6);
	PORTA = 0;

	//All output
	DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
	PORTB = (1 << 1);

	//All output except PC4/PC7 (mousesensor SDA)
	DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 5) | (1 << 6);
	PORTC = 0;

	//All output except PD0+1(I2C) + 2+3(RS232)
	DDRD = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
	PORTD = (1 << 0) | (1 << 1);//Activate pullup at PD0+1

	//PE5 for input
	DDRE = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 6)  | (1 << 7);
	PORTE = 0;

	//All input with pullup
	DDRF = 0;
	PORTF = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);

	//All input
	DDRG = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4); 
	PORTG = (1 << 0) | (1 << 1);

	// activate channel A+B on PWM1 at 8Bit
	TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
	TCCR1B = (1<<ICNC1) | (1<<CS11); // set clock/prescaler 1/8 -> enable counter

	// activate Kanal A+B on PWM3 at 8Bit
	TCCR3A = (1<< COM3A1) | (1<< COM3B1) | (1<< WGM10);
	TCCR3B =  (1<<ICNC3) | (1<<CS31); // set clock/prescaler 1/8 -> enable counter

	//Activate interrupt
	sei();

	// Initialiate TWI
	Init_TWI();

	//Interface
	for(uint8 i = IO_START; i < IO_END; i++)
	{
		modules[i] = NULL;
	}
}

//-----------------------------------------------------------------------------
Robot::~Robot()
{
	for(uint8 i = 0; i < IO_END; i++)
	{
		if(!modules[i]) continue;

		delete modules[i];
		modules[i] = NULL;
	}
}

//-----------------------------------------------------------------------------
bool Robot::AddModule(IO_Module* newModule)
{
	if(modules[newModule->GetId()])
		return false;

	newModule->SetParent(this);

	modules[newModule->GetId()] = newModule;

	return true;
}

//-----------------------------------------------------------------------------
bool Robot::RemoveModule(IO_Module* oldModule)
{
	return RemoveModule(oldModule->GetId());
}

//-----------------------------------------------------------------------------
void Robot::Update()
{
	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();

	GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();

	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();

	//insert code here
}

//-----------------------------------------------------------------------------
uint16 Robot::GetADCValue(uint8 channel)
{
	if(channel > 7) return 0;

	uint32 result = 0;
	
	//Activate ADC and set division factor to 128
	ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

	//Set multiplexer channel
	ADMUX = channel;
	//Use internal referencevoltage (2,56 V for atmega32)
	ADMUX |= (1 << REFS1) | (1 << REFS0);
	
	//Initialise ADC and start dummyreadout
	ADCSRA |= (1 << ADSC);
	while(ADCSRA & (1 << ADSC));
	
	//Get voltage three times and calculate average value
	for(uint8 i = 0; i < 3; i++)
	{
		// Eine Wandlung
		ADCSRA |= (1 << ADSC);
		// Auf Ergebnis warten...
		while(ADCSRA & (1 << ADSC));
		
		result += ADCW;
	}
	
	//Disable ADC
	ADCSRA &= ~(1 << ADEN);
	
	result /= 3;
	
	return (uint16)(result);
}

//--- EOF ---