#include "position_tracker.h"

//-----------------------------------------------------------------------------
void Position_Tracker::Update()
{
	// We want to use the mouse-sensors
	Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
	Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

	// Generate a vector for the left mouse
	int8 leftX = mouseLeft->GetXMovement();
	int8 leftY = mouseLeft->GetYMovement();
	// Generate a vector for the right mouse
	int8 rightX = mouseRight->GetXMovement();
	int8 rightY = mouseRight->GetYMovement();

	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
	float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));

	float movementLeftX = cos(angleLeft) * distanceLeft;
	float movementLeftY = sin(angleLeft) * distanceLeft;

	// AVR calculates a little bit strange ;)
	if(!leftX && !leftY)
	{
		movementLeftX = 0;
		movementLeftY = 0;
	}

	float distanceRight = sqrt(rightX * rightX + rightY * rightY);
	float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));

	float movementRightX = cos(angleRight) * distanceRight;
	float movementRightY = sin(angleRight) * distanceRight;

	// AVR calculates a little bit strange ;)
	if(!rightX && !rightY)
	{
		movementRightX = 0;
		movementRightY = 0;
	}

	// Generate vector from P:left to P:right
	float movementDifferenceX = movementRightX - movementLeftX;
	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());

	// Calculate the difference of orientation
	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);

	float robotMovementX = movementDifferenceX / 2.0f;
	float robotMovementY = movementDifferenceY / 2.0f;
	robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));
	robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));

	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX + sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;
	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY - cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;

	if(!robotMovementX && !robotMovementY)
	{
		absoluteDiffX = 0;
		absoluteDiffY = 0;
	}

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);

	this->positionX += absoluteDiffX;
	this->positionY += absoluteDiffY;

	this->orientation += orientationDiff;

	this->orientation = easyAngle(this->orientation);

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);
	/*(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Clear();
	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1);
	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2);
	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionY, 1, 3);*/
}