#include "logic.h"

//-----------------------------------------------------------------------------
void Logic::OnBallOwned()
{
	bool wasKeeper = this->IsKeeper();

	this->SetKeeper(false);

	if(!WIRELESS_ACTIVE)
	{
		Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);

		ourAktuator->Kick();
		this->SetKeeper(wasKeeper);
	}
}

//-----------------------------------------------------------------------------
void Logic::OnBallLost()
{
	
}

//-----------------------------------------------------------------------------
void Logic::OnBecomeKeeper()
{
	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
	
	if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) > DISTANCE_TOLERANCE)
	{
		status = STATUS_KEEPER_RETURN_HOME;

		ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetOrientation(), DEFAULT_SPEED, 0);
	}
	else
	{
		UpdateKeeperMovement();
	}
}

//-----------------------------------------------------------------------------
void Logic::OnBecomeAttacker()
{
	 Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

	 ourNavigator->Stop();

	 UpdateAttackerMovement();
}

//-----------------------------------------------------------------------------
void Logic::Update()
{
	// We want to use a navigator
	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
	Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	Obstacle_Tracker* ourObstacleTracker = parent->GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN);

	if(!this->HasBall() && ourBallTracker->HasBall())
	{
		this->hasBall = true;
		this->OnBallOwned();
	}
	else if(this->HasBall() && !ourBallTracker->HasBall())
	{
		this->hasBall = false;
		this->OnBallLost();
	}

	switch(status)
	{
		case STATUS_ATTACKER_SEARCHING_AT_HOME:
		case STATUS_ATTACKER_SEARCHING_AT_ENEMY:
			if(ourBallTracker->KnowsBallDirection())
			{
				ourNavigator->Stop();
			}
			break;
		case STATUS_KEEPER_HUNT_BALL:
			if(ourBallTracker->KnowsBallDirection())
			{
				ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
			}
			else
			{
				ourNavigator->Stop();
			}
			break;
	}

	if(!ourNavigator->IsMoving())
	{
		if(this->IsKeeper())
		{
			UpdateKeeperMovement();
		}
		else
		{
			UpdateAttackerMovement();
		}
	}
	
	if(ourNavigator->IsMoving())
	{
		if(ourObstacleTracker->IsObstacleOnPath())
		{
			avoidsObstacle = true;
			for(uint8 i = 0; i < 5; i++)
			{
				if(ourObstacleTracker->IsObstacleOnPath(easyAngle(ourNavigator->GetDirection() - ((i - 2) * PI / 4.0f))))
				{
				}
				ourNavigator->GetDirection();
			}
		}
	}
}

//-----------------------------------------------------------------------------
void Logic::UpdateKeeperMovement()
{
	Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

	if(ourBallTracker->KnowsBallDirection())
	{
		if(ourBallTracker->GetBallDirection() > KEEPER_LEFT_ANGLE && 
			ourBallTracker->GetBallDirection() <= PI)
		{
			if(distance2d(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
			{
				float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
				if(angleDifference > PI)
				{
					angleDifference = (2 * PI) - angleDifference;
				}

				if(angleDifference > ORIENTATION_TOLERANCE)
				{
					status = STATUS_KEEPER_TURN_TO_BALL;
					ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
				}
				else
				{
					status = STATUS_KEEPER_HUNT_BALL;
					ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
				}
			}
			else
			{
				status = STATUS_KEEPER_DRIVE_TO_DEFEND;
				ourNavigator->DriveTo(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
			}
		}
		else if(ourBallTracker->GetBallDirection() < KEEPER_RIGHT_ANGLE && 
			ourBallTracker->GetBallDirection() >= PI)
		{
			if(distance2d(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
			{
				float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
				if(angleDifference > PI)
				{
					angleDifference = (2 * PI) - angleDifference;
				}

				if(angleDifference > ORIENTATION_TOLERANCE)
				{
					status = STATUS_KEEPER_TURN_TO_BALL;
					ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
				}
				else
				{
					status = STATUS_KEEPER_HUNT_BALL;
					ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
				}
			}
			else
			{
				status = STATUS_KEEPER_DRIVE_TO_DEFEND;
				ourNavigator->DriveTo(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
			}
		}
		else
		{
			if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
			{
				float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
				if(angleDifference > PI)
				{
					angleDifference = (2 * PI) - angleDifference;
				}

				if(angleDifference > ORIENTATION_TOLERANCE)
				{
					status = STATUS_KEEPER_TURN_TO_BALL;
					ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
				}
				else
				{
					status = STATUS_KEEPER_HUNT_BALL;
					ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
				}
			}
			else
			{
				status = STATUS_KEEPER_DRIVE_TO_DEFEND;
				ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
			}
		}
	}
	else
	{
		if(status == STATUS_KEEPER_TURN_LEFT)
		{
			status = STATUS_KEEPER_TURN_RIGHT;
			ourNavigator->RotateTo(315, DEFAULT_ROTATION_SPEED);
		}
		else
		{
			status = STATUS_KEEPER_TURN_LEFT;
			ourNavigator->RotateTo(45, DEFAULT_ROTATION_SPEED);
		}
	}
}

//-----------------------------------------------------------------------------
void Logic::UpdateAttackerMovement()
{
	Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

	if(ourBallTracker->KnowsBallDirection())
	{
		float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());

		if(angleDifference > PI)
		{
			angleDifference = (2 * PI) - angleDifference;
		}

		if(angleDifference > ORIENTATION_TOLERANCE)
		{
			status = STATUS_ATTACKER_TURN_TO_BALL;
			ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
		}
		else
		{
			status = STATUS_ATTACKER_DRIVE_TO_BALL;
			ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
		}
	}
	else
	{
		if(ourPositionTracker->GetPositionX() > 0)
		{
			status = STATUS_ATTACKER_SEARCHING_AT_HOME;
			ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
		}
		else
		{
			status = STATUS_ATTACKER_SEARCHING_AT_ENEMY;
			ourNavigator->DriveTo(ENEMY_LOC_X, ENEMY_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
		}
	}
}