// Defender.cpp: implementation of the CDefender class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Defender.h"
#include <math.h>
#include "Computation.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CDefender::CDefender(Environment *envPointer) : CBasicAction(envPointer)
{
}

CDefender::~CDefender()
{

}



void CDefender::lexDefenderNormal(Environment *env,int whichRobot1, int whichRobot2)
{
	guardPosition.x = GUARD_POS_X;

	Vector3D oppoGoal;
	
	if(env->currentBall.pos.x > guardPosition.x && env->currentBall.pos.y<68 && env->currentBall.pos.y>15)
	{
		blockInFrontOfGoal(whichRobot1,whichRobot2);
	}

	else if(env->currentBall.pos.y>68)
	{
		guardPosition.y = 73 + 1.1;
		sineTurn(guardPosition,whichRobot1);
		guardPosition.y = 73 - 1.1;
		sineTurn(guardPosition,whichRobot2);
	
	}

	else if(env->currentBall.pos.y<15)
	{
		guardPosition.y = 9 + 1.1;
		sineTurn(guardPosition,whichRobot1);
		guardPosition.y = 9 - 1.1;
		sineTurn(guardPosition,whichRobot2);
	
	}
	else if(env->currentBall.pos.x <= guardPosition.x && env->currentBall.pos.y > CENTERY)
	{
		if(isInBetweenPos(env, HOME_GOAL_CENTER_POSITION, env->currentBall.pos, env->home[2].pos, 3.3) ==1)
		{
			spin(&env->home[whichRobot2], 125, 1);
			sineTurn(env->currentBall.pos,/*0,0,0,0,*/whichRobot1);
		}
		else
		{
			blockWhenBallNearGoal(whichRobot2,ANTI_CLOCKWISE);
			sineTurn(env->currentBall.pos,/*0,0,0,0,*/whichRobot1);

		}
	}

	else if(env->currentBall.pos.x <= guardPosition.x && env->currentBall.pos.y < CENTERY)
	{
		if(isInBetweenPos(env, HOME_GOAL_CENTER_POSITION, env->currentBall.pos, env->home[whichRobot2].pos, 3.3) ==1)
		{
			spin(&env->home[whichRobot2], 125, 0);
			sineTurn(env->currentBall.pos,/*0,0,0,0,*/whichRobot1);
		}
		else
		{
			blockWhenBallNearGoal(whichRobot2,ANTI_CLOCKWISE);
			sineTurn(env->currentBall.pos,/*0,0,0,0,*/whichRobot1);
		}
	}

}

void CDefender::blockInFrontOfGoal(int whichRobot1, int whichRobot2)//lex: two robot block in front of goal from ball
{
	double gradient, centrePositionY;
	guardPosition.x = GUARD_POS_X;

	gradient = (env->currentBall.pos.y - HOME_GOAL_CENTER_POSITION.y) / (env->currentBall.pos.x - HOME_GOAL_CENTER_POSITION.x);
	centrePositionY = (gradient * (guardPosition.x - HOME_GOAL_CENTER_POSITION.x)) + HOME_GOAL_CENTER_POSITION.y;

	guardPosition.y = centrePositionY + 1.1;
	sineTurn(guardPosition,/*20,1,-90,1,*/whichRobot1);
	guardPosition.y = centrePositionY - 1.1;
	sineTurn(guardPosition,/*20,1,90,1,*/whichRobot2);
	return;
}

void CDefender::blockWhenBallNearGoal(int whichRobot, int spinDirection)
{
	double gradient, centrePositionY;
	guardPosition.x = GUARD_POS_X;
		double ORBIT_DISTANCE = 3;//DECIDE ROBOT SHOUD ORBIT WHAT DISTANCE FROM BALL WHEN ROBOT IS IN FRONT OF THE BALL

	guardPosition.x = (env->currentBall.pos.x + HOME_GOAL_CENTER_POSITION.x) / 3 * 2;
	gradient = (env->currentBall.pos.y - HOME_GOAL_CENTER_POSITION.y) / (env->currentBall.pos.x - HOME_GOAL_CENTER_POSITION.x);
	guardPosition.y = (gradient * (guardPosition.x - HOME_GOAL_CENTER_POSITION.x)) + HOME_GOAL_CENTER_POSITION.y;
	if(findDistance(env->home[whichRobot].pos, guardPosition) < 3)
	{
		spin(&env->home[whichRobot], 125, spinDirection);
	}

	else if(isInBetweenPos(env,guardPosition,env->home[whichRobot].pos,env->currentBall.pos,5)==1)
	{
		if(env->home[whichRobot].pos.x > env->currentBall.pos.x && env->home[whichRobot].pos.y > env->currentBall.pos.y)
			{
				orbit(env->currentBall.pos,&(env->home[whichRobot]),whichRobot,ORBIT_DISTANCE,ANTI_CLOCKWISE);
			}
		else //if(env->home[whichRobot].pos.x > env->currentBall.pos.x && env->home[whichRobot].pos.y < env->currentBall.pos.y)
			{
				orbit(env->currentBall.pos,&(env->home[whichRobot]),whichRobot,ORBIT_DISTANCE,CLOCKWISE);
			}
	}
	else
	{
		sineTurn(guardPosition,/* 20, 1, 90, 1,*/ whichRobot);
	}
		
	return;
}

void CDefender::freeKickDefender(Environment *env,int whichRobot1, int whichRobot2)
{
	double gradient, centrePositionY;
	guardPosition.x = 15;

	gradient = (env->currentBall.pos.y - HOME_GOAL_CENTER_POSITION.y) / (env->currentBall.pos.x - HOME_GOAL_CENTER_POSITION.x);
	centrePositionY = (gradient * (guardPosition.x - HOME_GOAL_CENTER_POSITION.x)) + HOME_GOAL_CENTER_POSITION.y;

	guardPosition.y = centrePositionY + 1.1;
	sineTurn(guardPosition,/*20,1,-90,1,*/whichRobot1);
	guardPosition.y = centrePositionY - 1.1;
	sineTurn(guardPosition,/*20,1,90,1,*/whichRobot2);
	return;
}