#include "navigation.h"

// Initialisiert die Klasse
Navigation::Navigation() {
	// Am Anfang stehen wir an der Stelle
	richtung = 0;
	ausrichtung = 0;
	geschwindigkeit = 0;
	Aktualisieren(); // und änderungen anwenden
}
Navigation::~Navigation() {
}

// Setze eine neue Richtung
void Navigation::SetzeRichtung(int nRichtung) {
	richtung = nRichtung;
}
// Setze eine neue Ausrichtung
void Navigation::SetzeAusrichtung(int nAusrichtung) {
	ausrichtung = nAusrichtung;
}
// Setze eine neue Geschwindigkeit
void Navigation::SetzeGeschwindigkeit(int nGeschwindigkeit) {
	geschwindigkeit = nGeschwindigkeit;
}


// Aktualieren ohne Parameter
void Navigation::Aktualisieren() {
	// Richtung in x und y-Kompontente zerlegen
	double y = cos(richtung); // richtung ist winkel
	double x = sin(richtung);
	
	// Abweichung der Ausrichtung ermitteln(als winkel)
	int w = sensor.GetAusrichtung() - ausrichtung;

	// Stärke der einzelnen Motoren berechnen	
	double v0 = (-x-sqrt(3)*y)/2;
	double v1 = x;
	double v2 = (-x+sqrt(3)*y)/2;

	// Ausgerechnete Stärke an die Motoren übergeben
	board.motor(0,(int)((double)v0*geschwindigkeit +w));
	board.motor(1,(int)((double)v1*geschwindigkeit +w));
	board.motor(2,(int)((double)v2*geschwindigkeit +w));
}

// Aktualieren mit allen Parametern
void Navigation::Aktualisieren(int nRichtung,int nAusrichtung,int nGeschwindigkeit) {
	SetzeRichtung(nRichtung); // Übergebene Werte schreiben
	SetzeAusrichtung(nAusrichtung);
	SetzeGeschwindigkeit(nGeschwindigkeit);
	Aktualisieren(); // Und anwenden
}