#include "navigator.h"

//-----------------------------------------------------------------------------
void Navigator::Stop()
{	
	this->direction = -1.0f;
	this->targetAngle = -1.0f;
	this->targetX = -1.0f;
	this->targetY = -1.0f;
	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(rotationSpeed, 255.0f);;
	this->direction = -1.0f;
	this->targetAngle = -1.0f;
	this->targetX = -1.0f;
	this->targetY = -1.0f;
	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;
	this->targetAngle = newAngle;
	this->targetX = -1.0f;
	this->targetY = -1.0f;
	this->robotSpeed = newSpeed;

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

	Update();
}

//-----------------------------------------------------------------------------
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;
	this->robotSpeed = newSpeed;

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

	Update();
}

//-----------------------------------------------------------------------------
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 relativeDirection = this->direction - locationeer->GetOrientation();

		if(targetX >= 0 && targetY >= 0)
		{
			float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
			relativeDirection = directionToTarget - locationeer->GetOrientation();
		}

		float xDrive = cos(relativeDirection);
		float yDrive = sin(relativeDirection);

		float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f;
		float vBack = yDrive;
		float vRight = (-yDrive - sqrt(3) * xDrive)/2;

		float speedCorrection = 1.0f;

		float maxEngineSpeed = 255.0f;
		float minEngineSpeed = -255.0f;

		float maxSingleSpeed = max(max(vLeft, vBack), vRight);

		float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 
											vBack * (vBack + vLeft + vRight));

		if(calcSpeed != 1.0f)
		{
			speedCorrection = 1.0f / calcSpeed;
		}

		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
		if(maxOverallSpeed > maxEngineSpeed)
		{
			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
		}
		

		vLeft = vLeft * this->robotSpeed * speedCorrection;
		vBack = vBack * this->robotSpeed * speedCorrection;
		vRight = vRight * this->robotSpeed * speedCorrection;

		maxSingleSpeed = max(max(vLeft, vBack), vRight);
		float minSingleSpeed = min(min(vLeft, vBack), vRight);

		if(hasOrientationToAdjust)
		{
			if(this->rotationSpeed > 0)
			{
				if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)
				{
					vLeft += maxEngineSpeed - maxSingleSpeed;
					vBack += maxEngineSpeed - maxSingleSpeed;
					vRight += maxEngineSpeed - maxSingleSpeed;
				}
				else
				{
					vLeft += this->rotationSpeed;
					vBack += this->rotationSpeed;
					vRight += this->rotationSpeed;
				}
			}
			else
			{
				if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)
				{
					vLeft -= minEngineSpeed - minSingleSpeed;
					vBack -= minEngineSpeed - minSingleSpeed;
					vRight -= minEngineSpeed - minSingleSpeed;
				}
				else
				{
					vLeft -= this->rotationSpeed;
					vBack -= this->rotationSpeed;
					vRight -= this->rotationSpeed;
				}
			}
		}

		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;
	}
	else if(hasOrientationToAdjust)
	{
		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
		curEngine->SetSpeed(this->rotationSpeed);
		curEngine->SetEnabled(true);
		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
		curEngine->SetSpeed(this->rotationSpeed);
		curEngine->SetEnabled(true);
		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
		curEngine->SetSpeed(this->rotationSpeed);
		curEngine->SetEnabled(true);
		curEngine = NULL;
	}	
	else
	{
		Stop();
	}
}