#include "navigator.h"

//-----------------------------------------------------------------------------
void Navigator::Stop()
{	
	this->direction = EMPTY_FLOAT;
	this->targetAngle = EMPTY_FLOAT;
	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->robotSpeed = 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 = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed);
	this->direction = EMPTY_FLOAT;
	this->targetAngle = EMPTY_FLOAT;
	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->robotSpeed = 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::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)
{
	this->rotationSpeed = min(rotationSpeed, 255.0f);
	this->direction = newDirection * PI / 180.0f;
	this->targetAngle = newAngle * PI / 180.0f;
	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->robotSpeed = newSpeed;

	if(targetAngle == EMPTY_FLOAT)
	{
		rotationSpeed = 0;
	}
	else
	{
		rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
	}

	CalculateEngines();
}

//-----------------------------------------------------------------------------
void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
{
	if(newX < 0 || newY < 0) return;

	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

	this->rotationSpeed = min(rotationSpeed, 255.0f);
	this->targetX = newX;
	this->targetY = newY;
	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
	this->targetAngle = newAngle * PI / 180.0f;
	this->robotSpeed = newSpeed;

	if(targetAngle - locationeer->GetOrientation() > PI)
	{
		if(rotationSpeed > 0)
		{
			rotationSpeed = -rotationSpeed;
		}
	}
	else
	{
		if(rotationSpeed < 0)
		{
			rotationSpeed = -rotationSpeed;
		}
	}

	CalculateDirection();
}

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

	bool targetReached = false;
	bool targetAngleReached = false;

	if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
	{
		targetX = EMPTY_FLOAT;
		targetY = EMPTY_FLOAT;
		direction = EMPTY_FLOAT;
		robotSpeed = 0;

		targetReached = true;
	}

	if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
	{
		targetAngle = EMPTY_FLOAT;
		rotationSpeed = 0;

		targetAngleReached = true;
	}

	if(targetReached && targetAngleReached)
	{
		Stop();
	}
	else if(targetReached || targetAngleReached)
	{
		CalculateDirection();
	}

	if(!(correctionCountdown--))
	{
		CalculateDirection();
	}
}

//-----------------------------------------------------------------------------
void Navigator::CalculateDirection()
{
	correctionCountdown = CYCLES_PER_CORRECTION;

	if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;

	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	
	if(HasTarget())
	{
		direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
	}

	if(targetAngle != EMPTY_FLOAT)
	{
		rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
	}

	CalculateEngines();
}

//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
	// Use the position_tracker
	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	// get the relative position of the robot
	float relativeDirection = this->direction - locationeer->GetOrientation();

	// calculate x and y-koordinates
	float xDrive = cos(relativeDirection + PI / 6.0f);
	float yDrive = sin(relativeDirection + PI / 6.0f);

	// calculate relative speed of engines
	float vLeft = xDrive;
	float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
	float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

	// Get the maximal value
	float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));

	// calculate the correct speeds of the engines
	vLeft = ((float)vLeft * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
	vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
	vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);



	// Transfer the values to the engines
	Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
	curEngine->SetSpeed(vLeft);
	curEngine->SetEnabled(true);
	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
	curEngine->SetSpeed(vBack);
	curEngine->SetEnabled(true);
	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
	curEngine->SetSpeed(vRight);
	curEngine->SetEnabled(true);
	curEngine = NULL;
}