#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 bis auf PC4/PC5 (Maussensor SDA)
	DDRC = (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3) | (1 << PC6)  | (1 << PC7);
	PORTC = 0; // Alle Low, kein Pollup

	// Alle Ausgänge bis auf PD0+1(I2C) + 2+3(RS232)
	DDRD = (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+B auf PWM1 mit 8Bit
	TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
	TCCR1B = (1<<ICNC1) | (1<<CS11); // | (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<<CS31); // | (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);

	// Kicker-richtung einstellen
	PORTE &= ~(1 << PE6); // Pin2 low
	PORTA |= (1 << PA2); // Pin1 high

	// Dribbler-richtung einstellen
	DRIBBLER_PORT |= DRIBBLER_A; // Pin1 high
	DRIBBLER_PORT &= ~DRIBBLER_B; // Pin2 low

	

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

// Gibt den Wert vom Abstandsensor zurück
int Board::GetAbstand(int i) {
	int result = -1;
	if((i < 0) || (i > 3)) return result; // Ungültige Nummern rausfiltern

	// Sende zunächst einen Impuls aus
	ABSTAND_DDR |= (1 << i); // Konfiguriere Pin als Ausgang
	ABSTAND_PORT |= (1 << i); // Und setze ihn auf High
	usleep(10); // Warte jetzt 10us
	ABSTAND_PORT &= ~(1 << i); // Und setze den Pin wieder auf Low
	ABSTAND_DDR &= ~(1 << i); // Konfiguriere Pin als Eingang

	// Jetzt warten wir auf die Antwort vom Sensor
	while(!(ABSTAND_PIN & i)) {} // während er low ist nichts machen
	while(ABSTAND_PIN & i) { // Und während er high ist
		result++; //schleifendurchläufe zähenlen (unsauber, ich weiß)
		asm volatile("nop"); // ein ganz bisschen warten
	}  

	// Die Zahl der Schleifendurchläufe geben wir dann zurück
	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 = 0; // PWM-Speed

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

	if(i == 0) {
		MOTOR0_PWM = pwm;
		if(speed > 0)
		{
			MOTOR0_PORT |= MOTOR0_A;//In 1 ein
			MOTOR0_PORT &= ~MOTOR0_B;//In 2 aus
		}
		else if(speed < 0)
		{
			MOTOR0_PORT |= MOTOR0_B;//In 2 ein
			MOTOR0_PORT &= ~MOTOR0_A;//In 1 aus
		}
		else
		{
			MOTOR0_PORT |= MOTOR0_B;//In 2 ein
			MOTOR0_PORT |= MOTOR0_A;//In 1 ein
		}
	}
	else if(i == 1) {
		MOTOR1_PWM = pwm;
		if(speed > 0)
		{
			MOTOR1_PORT |= MOTOR1_A;//In 1 ein
			MOTOR1_PORT &= ~MOTOR1_B;//In 2 aus
		}
		else if(speed < 0)
		{
			MOTOR1_PORT |= MOTOR1_B;//In 2 ein
			MOTOR1_PORT &= ~MOTOR1_A;//In 1 aus
		}
		else
		{
			MOTOR1_PORT |= MOTOR1_B;//In 2 ein
			MOTOR1_PORT |= MOTOR1_A;//In 1 ein
		}
	}
	else if(i == 2)
	{
		MOTOR2_PWM = pwm;
		if(speed > 0)
		{
			MOTOR2_PORT |= MOTOR2_A;//In 1 ein
			MOTOR2_PORT &= ~MOTOR2_B;//In 2 aus
		}
		else if(speed < 0)
		{
			MOTOR2_PORT |= MOTOR2_B;//In 2 ein
			MOTOR2_PORT &= ~MOTOR2_A;//In 1 aus
		}
		else
		{
			MOTOR2_PORT |= MOTOR2_B;//In 2 ein
			MOTOR2_PORT |= MOTOR2_A;//In 1 ein
		}
	}
}

void Board::kicker() {
	KICKER_AN
	usleep(KICKER_USLEEP);
	KICKER_AUS
}

void Board::dribblerOn() {
	DRIBBLER_AN
}

void Board::dribblerOff() {
	DRIBBLER_AUS
}

void Board::dribbler(bool status) {
	if(status) dribblerOn();
	else dribblerOff();
}

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