#include "navigator.h"

//-----------------------------------------------------------------------------
void Navigator::Stop()
{	
	this->direction = -1.0f;
	this->targetAngle = -1.0f;
	this->targetX = -1.0f;
	this->targetY = -1.0f;
	this->speed = 0;
	this->rotationSpeed = 0;

	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
	{
		(parent->GetModule<Engine>(i))->SetSpeed(0);
		(parent->GetModule<Engine>(i))->SetEnabled(true);
	}
}

//-----------------------------------------------------------------------------
void Navigator::Rotate(float rotationSpeed)
{
	this->rotationSpeed = rotationSpeed;
	this->direction = -1.0f;
	this->targetAngle = -1.0f;
	this->targetX = -1.0f;
	this->targetY = -1.0f;
	this->speed = 0;

	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
	{
		(parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);
		(parent->GetModule<Engine>(i))->SetEnabled(true);
	}
}

//-----------------------------------------------------------------------------
void Navigator::Update()
{
	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

	bool hasDistanceToDrive = true;
	bool hasOrientationToAdjust = true;

	//Check for distance to drive
	if((targetX >= 0) != (targetY >= 0))
	{
		targetX = -1.0f;
		targetY = -1.0f;

		hasDistanceToDrive = false;
	}
	else if(targetX >= 0 && targetY >= 0)
	{
		if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
		{
			targetX = -1.0f;
			targetY = -1.0f;

			hasDistanceToDrive = false;
		}
		else
		{
			hasDistanceToDrive = true;
		}
	}
	else
	{
		if(direction >= 0)
		{
			hasDistanceToDrive = true;
		}
		else
		{
			hasDistanceToDrive = false;
		}
	}

	//Check for orientation to adjust
	if(targetAngle >= 0)
	{
		if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
		{
			hasOrientationToAdjust = false;
		}
		else
		{
			hasOrientationToAdjust = true;
		}
	}
	else
	{
		if(rotationSpeed != 0)
		{
			hasOrientationToAdjust = true;
		}
		else
		{
			hasOrientationToAdjust = false;
		}
	}

	//Calculate directional/rotational engine speed
	if(hasDistanceToDrive)
	{


		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;

		if(speed > maxRobotSpeed)
		{
			speed = maxRobotSpeed;
		}

		maxMotorSpeed = speed / (sqrt(2) / 2);



		relAngel = direction - orientation
		
		robotSpeed = sin(45) * maxMotorSpeed
		maxMotorSpeed = robotSpeed / sin(45)

		if(relAngel > 45)
		{
			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);

			back = speed / sin(relAngel);
		}
		else
		{
			
		}

		left = 
		back = sin(relAngel) * speed


		direction = 0:
		orientation = 0:
		left = speed
		right = -speed
		back = 0

		direction = 0:
		orientation = 90:
		left = speed
		right = speed
		back = (sinVcos)(45) * speed

		direction = 0:
		orientation = 45:
		left = speed
		right = 0
		back = -speed

		direction = 0:
		orientation = 180:
		left = -speed
		right = speed
		back = 0

	}
	else if(!hasOrientationToAdjust)
	{
		Stop();
	}
	else
	{
	}
}

// Aktualieren ohne Parameter
/*void Navigator::Update() {	
	// Richtung in x und y-Kompontente zerlegen
	double y = cos((double)direction*0.01745); // richtung ist winkel
	double x = sin((double)direction*0.01745);
	
	// 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*speed +w));
	board.motor(1,(int)((double)v1*speed +w));
	board.motor(2,(int)((double)v2*speed +w));
}

// Aktualieren mit allen Parametern
void Navigator::Drive(float newDirection, float newAngle, float newSpeed) {
	SetDirection(newDirection);
	SetAngle(newAngle);
	SetSpeed(newSpeed);
	Update(); // Und anwenden
}*/