#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/PC5 (mousesensor SDA)
	DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 6)  | (1 << 7);
	PORTC = 0;

	//All output except PD0+1(I2C) + 2+3(RS232)
	DDRD = (1 << 2) | (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 <<CS12) | (1 <<CS10); // set clock/prescaler 1/1024 -> enable counter

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

	//Activate interrupt
	sei();

	//Interface
	for(uint8 i = 0; 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()
{
	//insert code here
}

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

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

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