#include "board.h"

static int beepFreq = 0;

Board::Board() {
	// Pin 1-6 sind ausgänge, 0 und 7 eingänge
	DDRA = (1 << PA1) | (1 << PA2) | (1 << PA3) | (1 << PA4) | (1 << PA5) | (1 << PA6);
	PORTA = 0; // Alle Low, kein Pollup

	// Alle Ausgänge
	DDRB = (1 << PB0) | (1 << PB1) | (1 << PB2) | (1 << PB3) | (1 << PB4) | (1 << PB5) | (1 << PB6)  | (1 << PB7);
	PORTB = (1 << PB1); // Alle Low bis PB1 , kein Pollup

	// Alle Ausgänge
	DDRC = (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3) | (1 << PC4) | (1 << PC5) | (1 << PC6)  | (1 << PC7);
	PORTC = 0; // Alle Low, kein Pollup

	// Alle Ausgänge bis auf PD0+1(I2C) + 2+3(RS232)
	DDRD = (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5) | (1 << PD6)  | (1 << PD7);
	PORTD = (1 << PD0) | (1 << PD1); // Pollup-Widerstand an PD0+1 aktivieren

	// PE5 ist eingang
	DDRE = (1 << PE0) | (1 << PE1) | (1 << PE2) | (1 << PE3) | (1 << PE4) | (1 << PE6)  | (1 << PE7);
	PORTE = 0; // Alle Low, kein Pollup

	// Alle Eingänge mit Pollup
	DDRF = 0;
	PORTF = (1 << PF0) | (1 << PF1) | (1 << PF2) | (1 << PF3) | (1 << PF4) | (1 << PF5) | (1 << PF6)  | (1 << PF7);

	// Alle Ausgänge, PG0 und PG1 high
	DDRG = (1 << PG0) | (1 << PG1) | (1 << PG2) | (1 << PG3) | (1 << PG4); 
	PORTG = (1 << PG0) | (1 << PG1);	

	// aktiviere Kanal A+C auf PWM1 mit 8Bit
	//TCCR1A = (1<< COM1A1) | (1<< COM1C1) | (1<< WGM10);
	//TCCR1B = (1<<ICNC1) | (1<<CS12) | (1<<CS10); // set clock/prescaler 1/1024 -> enable counter

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

	// Schalte Motoren auf 0
	motor(0,0);
	motor(1,0);
	motor(2,0);
	motor(3,0);

	// Uart-Interface einschalten
	uart1_init( 10); // 9600 BAUD bei 16MHz Atmel

	// aktiviere interrupt
	sei();
}

Board::~Board() {

}

// Gibt einen Analogen Wert zurück
int Board::GetADC(uint8_t channel) {
	uint8_t i;
	uint16_t result = 0;
	
	// Den ADC aktivieren und Teilungsfaktor auf 64 stellen
	ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1);

	// Kanal des Multiplexers waehlen
	ADMUX = channel;
	// Interne Referenzspannung verwenden (also 2,56 V)
	ADMUX |= (1<<REFS1) | (1<<REFS0);
	
	// Den ADC initialisieren und einen sog. Dummyreadout machen
	ADCSRA |= (1<<ADSC);
	while(ADCSRA & (1<<ADSC));
	
	// Jetzt 3x die analoge Spannung and Kanal channel auslesen
	// und dann Durchschnittswert ausrechnen.
	for(i=0; i<3; i++) {
		// Eine Wandlung
		ADCSRA |= (1<<ADSC);
		// Auf Ergebnis warten...
		while(ADCSRA & (1<<ADSC));
		
		result += ADCW;
	}
	
	// ADC wieder deaktivieren
	ADCSRA &= ~(1<<ADEN);
	
	result /= 3;
	
	return result;
}

void Board::beep(int freq) {
	beepFreq = freq;
}

void Board::ledOff() {
	PORTB |= (1 << PB1); // set bit
}

void Board::ledOn() {
	PORTB &= ~(1 << PB1); // clear bit
}

void Board::led(bool status) {
	if(status) ledOn();
	else ledOff();
}


void Board::motor(int i, int speed)
{
	if((i < 0) || (i > 3)) return;
	const int OFFSET = 40; // Motor does not work with very low ratio
	const int PWM_MAX = 255;

	int pwm = abs(speed)+OFFSET;
	if(pwm > PWM_MAX) pwm = PWM_MAX;

	if(i == 0)
	{
		MOTOR0_PWM = pwm;
		if(speed > 0)
		{
			PORTD |= (1 << 5);//In 1 ein
			PORTD &= ~(1 << 4);//In 2 aus
		}
		else if(speed < 0)
		{
			PORTD |= (1 << 4);//In 2 ein
			PORTD &= ~(1 << 5);//In 1 aus
		}
		else
		{
			PORTD |= (1 << 4);//In 2 ein
			PORTD |= (1 << 5);//In 1 ein
		}
	}
	else if(i == 1)
	{
		MOTOR1_PWM = pwm;
		if(speed > 0)
		{
			PORTD |= (1 << 6);//In 1 ein
			PORTD &= ~(1 << 7);//In 2 aus
		}
		else if(speed < 0)
		{
			PORTD |= (1 << 7);//In 2 ein
			PORTD &= ~(1 << 6);//In 1 aus
		}
		else
		{
			PORTD |= (1 << 6);//In 2 ein
			PORTD |= (1 << 7);//In 1 ein
		}
	}
	else if(i == 2)
	{
		MOTOR2_PWM = pwm;
		if(speed > 0)
		{
			PORTB |= (1 << 0);//In 1 ein
			PORTB &= ~(1 << 1);//In 2 aus
		}
		else if(speed < 0)
		{
			PORTB |= (1 << 1);//In 2 ein
			PORTB &= ~(1 << 0);//In 1 aus
		}
		else
		{
			PORTB |= (1 << 1);//In 2 ein
			PORTB |= (1 << 0);//In 1 ein
		}
	}
	else if(i == 3)
	{
		DRIBBLER_PWM = pwm;
		if(speed > 0)
		{
			PORTB |= (1 << 2);//In 1 ein
			PORTB &= ~(1 << 3);//In 2 aus
		}
		else if(speed < 0)
		{
			PORTB |= (1 << 3);//In 2 ein
			PORTB &= ~(1 << 2);//In 1 aus
		}
		else
		{
			PORTB |= (1 << 2);//In 2 ein
			PORTB |= (1 << 3);//In 1 ein
		}
	}
}

//PWM routine für den Beeper
ISR (TIMER0_OVF_vect)
{
	static int counter = 255;

	if(counter > beepFreq/2) PORTG |= (1<<BEEPER_PIN);
	else PORTG &= ~(1<<BEEPER_PIN);

	if (counter==0) counter = 255;
	else counter--;
}