#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
	float y = cos(richtung); // richtung ist winkel
	float x = sin(richtung);
	
	// Abweichung der Ausrichtung ermitteln(als winkel)
	int w = sensor.GetAusrichtung() - ausrichtung;

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

	// Ausgerechnete Stärke an die Motoren übergeben
	board.motor(0,(int)v0*geschwindigkeit +w);
	board.motor(1,(int)v1*geschwindigkeit +w);
	board.motor(2,(int)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
}