#include "position.h"
#include "../global.h"

Position::Position() {
	// inititalisiere die Maussensoren
	maussensor0.init(MAUSSENSOR0);
	maussensor1.init(MAUSSENSOR1);

	// Wir starten in der Mitte, nach vorne gerichtet
	x = 0;
	y = 0;
	ausrichtung = 0;
}

Position::~Position() {
}

// Aktualisiert die internen Variablen x,y und ausrichtung
void Position::Aktualisieren() {
	// Lese die Maussensoren ein
	uint8 maus0_x = maussensor0.GetX();
	uint8 maus0_y = maussensor0.GetY();
	uint8 maus1_x = maussensor1.GetX();
	uint8 maus1_y = maussensor1.GetY();

	// Und berechne die neuen Werte 
	// ACHTUNG!!!!! Die Funktionen sind noch ungetestet und werden fehler enthalten
	x += (maus0_x*sin(ausrichtung+45)+maus1_x*cos(ausrichtung+45))/2;
	y += (maus0_y*sin(ausrichtung+45)+maus1_y*cos(ausrichtung+45))/2;
	ausrichtung += ((maus0_y + maus1_y)/2)*(360/MAUS_FULL_TURN);
}

// Gibt die X-Koordinate zurück
int Position::GetX() {
	return (int)x;
}

// Gibt die Y-Koordinate zurück
int Position::GetY() {
	return (int)y;
}

// Gibt die Ausrichtung zurück
int Position::GetAusrichtung() {
	return ausrichtung;
}