/**
 * \file PlayerTactics.cpp
 *
 * Implementation of the PlayerTactics class
 */

#include "Player/PlayerTactics.h"
#include "Player/Formation.h"
#include "Common/Support/Debug.h"
#include "Common/Tasks.h"
#include <string.h>

#include <stdio.h>
#include <math.h>

#define max(x,y) ( ((x) > (y)) ? (x) : (y) )
#define min(x,y) ( ((x) > (y)) ? (y) : (x) )


PlayerTactics::PlayerTactics()
	: PlayerSkills("PlayerTactics", "PlayerKernel(PlayerTactics) module")
{
	scanBeforeKickOffAngle = 0;
	letGoalieRest = false;

// GP
//    m_cFormation.InitRawUnits( SIDE_LEFT, PlayerConfig.GetFormation(), PlayerConfig.GetDefUnit(), 
//		PlayerConfig.GetMidUnit(), PlayerConfig.GetForUnit() );

	m_cFormation.InitRawUnits( SIDE_LEFT, FORMATION_343, UNIT_3_L, UNIT_4_OV, UNIT_3_OV  );

	Main::GetInstance()->RegisterModule(this, TACTICS_TASK);
}

// GP
void PlayerTactics::OnInit(RealSide realSide, unsigned uniform, PlayMode iMode)	
{
	PlayerSkills::OnInit(realSide, uniform, iMode);

	// inicializacia formacii
	//m_cFormation.InitRawUnits( SIDE_LEFT, PlayerConfig.GetFormation(), PlayerConfig.GetDefUnit(), 
//	PlayerConfig.GetMidUnit(), PlayerConfig.GetForUnit() );
	m_cFormation.InitRawUnits( SIDE_LEFT, FORMATION_343, UNIT_3_L, UNIT_4_OV, UNIT_3_OV );
}

Interface* PlayerTactics::GetInterface(int task) const
{
	if(task == TACTICS_TASK) return (TacticsInterface*) this;
	else return PlayerSkills::GetInterface(task);
}

Player* PlayerTactics::GetEnemyGoalie() const
{
	static Player *enemyGoalie;
	static int i;
	
	enemyGoalie = NULL;
	for(i = 0; i < world->enemies.GetSize(); i++)
	{
		if(world->enemies[i].IsGoalie()) 
		{
			enemyGoalie = &(world->enemies[i]);
			break;
		}
	}
	return enemyGoalie;
}

bool PlayerTactics::ShouldKickOnGoal()
{
	if(DistanceToGoal(world->ball->pos) > 28) return false;

	Player *enemyGoalie = GetEnemyGoalie();
	static int i;
	
	if(enemyGoalie == NULL || enemyGoalie->GetAge() < 5)
	{
//		if(world->me->IsLookingAt(world->params->flags[GRT]) && 
//			world->me->IsLookingAt(world->params->flags[GRB])) return true;
		return false;
	}

	float posX = world->params->flags[GOAL_R].GetX();
	Point tGoal(posX, -(world->params->server->goal_width / 2) * .9f);
	Point bGoal(posX, (world->params->server->goal_width / 2) * .9f);
	Vector tVect = tGoal - world->ball->pos;
	Vector bVect = bGoal - world->ball->pos;

	int maxCycleB, maxCycleT, cycle, cycleB, cycleT;
	float edp, ekp;
	Vector v, a;
	float catchableArea;

	const int MAX_CYCLE = 30;

	Point res, T1, T2;
	float deg;
	int cycleb;
	Point futBposT[MAX_CYCLE];
	Point futBposB[MAX_CYCLE];
	Point futureHePos[MAX_CYCLE];
	float runDist[MAX_CYCLE];
	int maxCycle;
	bool found;

	Player p(*enemyGoalie);
	Ball tmpBall(*world->ball);

	catchableArea = max(world->params->server->catchable_area_l, world->params->server->catchable_area_w);
	catchableArea *= 0.95f;

	// manual page 44
	edp = enemyGoalie->type->effort_max * enemyGoalie->type->dash_power_rate * 100;
	if(edp > world->params->server->player_accel_max) edp = world->params->server->player_accel_max;

	// manual page 48
	ekp = world->params->server->maxpower * world->params->server->kick_power_rate;
	float dir_diff = ((world->ball->pos - world->me->pos) - world->me->GetAbsDir()).GetAngle().GetAbsDeg();
	float dist_ball = (world->ball->pos - world->me->pos).GetLength() - world->params->server->ball_size - world->params->server->player_size;
	ekp *= 1.f - 0.25f * dir_diff / 180.f - 0.25f * dist_ball / world->me->type->kickable_margin;

	// lopta smerujuca k hornej a dolnej tycke
	Ball ballT(*world->ball);
	Ball ballB(*world->ball);
	ballT.vel.SetLength(ekp);
	ballT.vel.SetAngle(tVect.GetAngle());
	ballB.vel.SetLength(ballT.vel.GetLength());
	ballB.vel.SetAngle(bVect.GetAngle());

	deg = fabsf(p.GetAbsDir().GetDeg() - world->ball->vel.GetAngle().GetDeg());
	if(deg > 180) deg = 360 - deg;
	if(deg > 90) deg = 180; // he'll go backwards
	else deg = 0;

	futureHePos[0] = p.pos;
	runDist[0] = 0;
	v.SetLength(0);
	maxCycleB = maxCycleT = -1;
	for(cycleb = 1; cycleb < MAX_CYCLE; cycleb++)
	{
		// without turning
		a = PolarVector(edp, p.GetAbsDir() + DegAngle(deg));
		p.vel += a;
		if(p.vel.GetLength() > p.type->player_speed_max) p.vel.SetLength(p.type->player_speed_max);
		futureHePos[cycleb] = futureHePos[cycleb - 1] + p.vel;
		p.vel *= p.type->player_decay;

		// with turning
		if(cycleb > 1) 
		{
			v.SetLength(v.GetLength() + edp);
			if(v.GetLength() > p.type->player_speed_max) v.SetLength(p.type->player_speed_max);
		}
		runDist[cycleb] = runDist[cycleb - 1] + v.GetLength();
		if(cycleb > 1) v.SetLength(v.GetLength() * p.type->player_decay);

		// ball
		ballT.Step();
		ballB.Step();
		if(fabsf(ballT.pos.GetX()) > 52.5 && maxCycleT == -1) maxCycleT = cycleb;
		if(fabsf(ballB.pos.GetX()) > 52.5 && maxCycleB == -1) maxCycleB = cycleb;
		if(maxCycleB != -1 && maxCycleT != -1) break;
		futBposT[cycleb] = ballT.pos;
		futBposB[cycleb] = ballB.pos;
	}
	maxCycle = cycleb;
	if(maxCycleT == -1) maxCycleT = MAX_CYCLE;
	if(maxCycleB == -1) maxCycleB = MAX_CYCLE;

	found = true;

	p = *enemyGoalie;
	// he'll move by inertia while turning
	p.pos = p.pos + p.vel;

	for(cycleB = 0; cycleB < maxCycleB; cycleB++)
	{
		// first try without turning
		for(cycle = cycleB; cycle >= 0; cycle--)
		{
			if((futBposB[cycleB + 1] - futureHePos[cycle]).GetLength() <= catchableArea)
			{
				if(fabsf(futureHePos[cycle].GetX()) <= 52.5)
				{
					found = false;
					break;
				}
			}
		}
		if(!found) break;

		// now try with turning
		for(cycle = cycleB; cycle >= 0; cycle--)
		{
			if((futBposB[cycleB + 1] - p.pos).GetLength() <= runDist[cycle] + catchableArea)
			{
				found = false;
				break;
			}
		}
	}

	if(found) 
	{ 
		TXTN("%d: Quite surely should be goal1", world->state->GetSimTime());
		TXTN(" (%.2f, %.2f, %.1f), (%.2f, %.2f) - %.2f", 
			enemyGoalie->pos.GetX(),
			enemyGoalie->pos.GetY(),
			enemyGoalie->GetAbsDir().GetDeg(),
			world->ball->pos.GetX(),
			world->ball->pos.GetY(),
			ekp);
		return found;
	}

	p = *enemyGoalie;
	// he'll move by inertia while turning
	p.pos = p.pos + p.vel;

	for(cycleT = 0; cycleT < maxCycleT; cycleT++)
	{
		// first try without turning
		for(cycle = cycleT; cycle >= 0; cycle--)
		{
			if((futBposT[cycleT + 1] - futureHePos[cycle]).GetLength() <= catchableArea)
			{
				if(fabsf(futureHePos[cycle].GetX()) <= 52.5)
				{
					found = false;
					break;
				}
			}
		}
		if(!found) break;

		// now try with turning
		for(cycle = cycleT; cycle >= 0; cycle--)
		{
			if((futBposT[cycleT + 1] - p.pos).GetLength() <= runDist[cycle] + catchableArea)
			{
				found = false;
				break;
			}
		}
	}

	if(found)
	{
		TXTN("%d: Quite surely should be goal2", world->state->GetSimTime());
		TXTN(" (%.2f, %.2f, %.1f), (%.2f, %.2f) - %.2f", 
			enemyGoalie->pos.GetX(),
			enemyGoalie->pos.GetY(),
			enemyGoalie->GetAbsDir().GetDeg(),
			world->ball->pos.GetX(),
			world->ball->pos.GetY(),
			ekp);
	}
	return found;
}

void PlayerTactics::BehaveWithBall()
{
	// We may pass the ball to this teammate (or not, see below)
	int teammate;

	if(SomeoneShouldKickIn())
	{
		// This is one of the kick in modes, someone from the team should
		// kick in. It it's not this player, do not attempt to kick ball
		// in any other way (no kick on goal, dribble, ...).
		// Update: letGoalieRest: goalie did a catch, now he has a free kick.
		// Let him handle the ball, do not try to kick it.

		if(ShouldKickIn() && !letGoalieRest)
		{
			if(ShouldKickOnGoal() && world->state->GetPlayMode() != PM_IndFreeKick_Our)
			{
				KickToGoal3();
				return;
			}

			// I will kick in. Anyone else should stay away from the ball.

		    // \author Matus Horvath
		    //DBGN("kick off, searching for world->friends");
    
		    // We have to pass somebody, if we just dribble, we get a free_kick_fault.
		    // We have enough time, so let's turn around first to see all players.
		    // Update> we do not have enough time on kick_in, when we do not need this either
		    // Update (Trebaticky)> we do have enough time and we need it :)
		    if(scanBeforeKickOffAngle <= 360)
		    {
			    //DBGN("scan");
			    scanBeforeKickOffAngle += Scan();
			    return;
		    }
    
		    // We have made a whole circle, find someone to pass to
		    // \todo Sem by sa hodila mozno neuronka
		    // \todo Ak ju sem nedame, treba dat aspon GetMostFreeFriend namiesto GetNearestFriend
		    teammate = GetPassAbleFriend();
		    //DBGN("passable friend: %i", teammate == -1 ? -1 : world->friends[teammate].uniform);
		    if(teammate == -1) teammate = GetNearestFriend(world->ball->pos);
			
		    if(!world->me->IsLookingAt(world->friends[teammate].pos))
			    TurnTo(world->friends[teammate].pos);
		    else PassBallToPlayer(world->friends[teammate], 1.f);
			
		    return;
		    //DBGN("nearest friend: %i", teammate == -1 ? -1 : world->friends[teammate].uniform);
	    }
	}
	else
	{
		// Not in "kick in" mode, that means we can kick the ball in other ways then kick in.

	    // If we can kick on goal, do so
	    if(ShouldKickOnGoal())
	    {
		    KickToGoal3();
		    return;
	    }
    
	    Player* enemyGoalie = GetEnemyGoalie();
	    // We are in game, try someone better for passing
	    teammate = GetPassAbleFriend();
	    // ak je brankar nebezpecne blizko a strielat nie je vhodne (!ShouldKickOnGoal)
	    if(enemyGoalie != NULL && (enemyGoalie->pos - world->ball->pos).GetLength() < 4.f)
	    {
		    if(teammate == -1)
		    {
			    TXT("%d: Passing blindly ", world->state->GetSimTime());
			    Vector meGoalie = (enemyGoalie->pos - world->me->pos);
			    if(world->me->pos.GetY() > 0)
				    meGoalie.SetAngle(meGoalie.GetAngle() - DegAngle(70));
			    else
				    meGoalie.SetAngle(meGoalie.GetAngle() + DegAngle(70));
			    TXTN(" from (%.2f,%.2f) to (%.2f,%.2f)",
				    world->me->pos.GetX(), world->me->pos.GetY(),
				    (world->me->pos + meGoalie).GetX(), (world->me->pos + meGoalie).GetY());
			    PassBallTo(world->me->pos + meGoalie);
			    return;
		    }
	    }
    
	    if(teammate == -1 && DistanceToGoal(world->me->pos) <= world->variables->GetDistanceToGoal())
	    {
		    Vector meGoal = (world->me->pos - Point(52.5f, 0.f));
		    if(meGoal.GetAngle().GetAbsDeg() > 115)
		    {
			    KickToGoal3();
			    return;
		    }
	    }
    
		    // Now either pass to found teammate, or dribble
	    if(teammate == -1)
	    {
		    
		    // Run to old position
		    /*float posX = 52.5f;
		    float posY = 0;/**/
		    
		    //Run to new position
		    float posX = 44.25f;
		    float posY = world->params->server->goal_width / 2.0f;
		    
		    if (world->me->pos.GetY() < 0) posY = -posY;/**/
    
		    /*
		    if(NoEnemiesAround())
		    {
			    //DribleTo(Point(posX,0));
			    DribleTo2(Point(posX,0),0);
		    }
		    else
		    {
			    //DribleTo(Point(posX,0));
			    if(!SmartDribleTo(Point(posX,0)))
			    {
				    //KickToGoal2();
				    DumbKick(Point(posX,0));
			    }
			    //DribleTo2(Point(posX,0),0);
		    }
		    */
    
		    if(!ShortKick(Point(posX, posY)))
		    {
			    if(NoEnemiesAround())
			    {
				    if(RecommDashPower(true) != world->params->server->maxpower)
					    DribbleTo2(Point(posX, posY), 0);
				    else
				    {
					    Vector where(Point(posX, posY) - world->me->pos);
					    PolarVector lookAt(10, DegAngle(60));
					    if(world->me->pos.GetY() > 0) lookAt.SetAngle(DegAngle(-lookAt.GetAngle().GetDeg()));
    
					    where.SetLength(2);
    //					TurnNeckTo(world->me->pos, world->me->pos + lookAt);
					    PassBallTo(world->me->pos + where);
				    }
			    }
			    else
			    {
				    if(!SmartDribleTo(Point(posX, posY)))
				    {
					    if(DistanceToGoal(world->me->pos) > 25)
					    {
						    Point aux(world->me->pos + PolarVector(8.f, world->me->GetAbsDir()));
						    if(aux.GetX() < 1.f) aux.SetX(-aux.GetX() + 3);
						    if(world->me->pos.GetX() < -15)
						    {
							    aux.SetY(world->me->pos.GetY() - 1);
							    aux.SetX(world->me->pos.GetX() + .5f * (world->me->pos.GetY() > 0 ? 1 : -1));
							    PassBallTo(aux, 100.f);
						    }
						    else PassBallTo(aux);
    //						DBGN("%d: Predkopavam si loptu (%.2f, %2f)", world->state->GetSimTime(), aux.GetX(), aux.GetY());
					    }
					    else
					    {
						    KickToGoal3();
    //						DBGN("%d: Strielam na branu", world->state->GetSimTime());
					    }
				    }
			    }
		    }
	    }
	    else
	    {
		    if(!PassIntoRun())
		    {
			    // Prihravka
  			    PassBallToPlayer(world->friends[teammate], world->me->pos.GetX() < -20 ? 3.f : .7f);
		    }
		    else
		    {
			    // P. do behu
		    }
	    }
	}

	//TurnBall();
}

void PlayerTactics::BehaveWithoutBall()
{
//	if(DependentForBall()) {
//		m_sKickInfo.Invalidate();
//		RunToBall3();
	//}

	// ked som natoceny na branku a moze padnut gol smykackou
	float posX = world->params->flags[GOAL_R].GetX();
	Point tGoal(posX, -(world->params->server->goal_width / 2) * .9f);
	Point bGoal(posX, (world->params->server->goal_width / 2) * .9f);
	Vector tVect = tGoal - world->me->pos;
	Vector bVect = bGoal - world->me->pos;

	Player* enemyGoalie = GetEnemyGoalie();
	if(CanTackleBall() && 
	   world->me->GetAbsDir().GetDeg() >= tVect.GetAngle().GetDeg() && 
	   world->me->GetAbsDir().GetDeg() <= bVect.GetAngle().GetDeg() && 
	   (ShouldKickOnGoal() || 
	   	(enemyGoalie != NULL && (enemyGoalie->pos - world->ball->pos).GetLength() < 4.f)))
	{
		TXTN("%d: I am tackling", world->state->GetSimTime());
		Tackle(100.0f);
		return;
	}

	bool catchIt = false;
	
	// Do not try to intercept ball while GoalKick lasts
	bool goalKickIgnore = world->state->GetPlayMode() == PM_GoalKick_Our;
		
	// Update: GoalKick and FreeKick last until ball leaves the penalty area. 
	// Do not ignore ball even in Goal/FreeKick if ball is not near goalie.
	Player *goalie = world->FindFriend(1);
	bool doNotIgnore = !world->ball->IsValid() || goalie == NULL;
	if(!doNotIgnore) doNotIgnore = doNotIgnore || 
		(world->ball->pos - goalie->pos).GetLength() > 0.105;
	
	// Try to intercept the ball
	if(doNotIgnore || (!goalKickIgnore && !letGoalieRest)) catchIt = InterceptBall();

	if(!catchIt)
	{						//nie je zodpovedny za loptu
		const float off_l = GetOffsideLine();

		if(InOffsidePos(off_l))
		{
			// je v ofsajde
			ExitOffside();
		}
		else
		{
			//nie je v ofsajde

			// otestuj ci nahodou nemas bezat za prihravkou
            // zatial StjupitDox nekricia na seba ...
            /*
			if ( m_sKickInfo.IsValid() && m_sKickInfo.teammate == -1 && PlayerConfig.FollowCommands() ) {
				// sprava urcena mne
				//Log("\n %d run for ball from kick info", GetSimTime());
				CfPoint dist = m_sKickInfo.pos - world->me->pos;

				if ( dist.GetDistance() < 2.0f ) {
					// uz si dobehol za loptu
					m_sKickInfo.Invalidate();
				}
				else {
					RunToPos( m_sKickInfo.pos );
					return;
				}
			}
            */

			m_cFormation.SetSide( SIDE_LEFT );
			Point pos = m_cFormation.GetPlayerPosition( world->me->GetUniform(), world->ball->pos );
			Vector dist = pos - world->me->pos;

			//if ( GetNearestEnemyDistance( world->me->pos ) < 5.0f && BlockEnemy() ) {
            // GP, blokovanie vyhodene
//			if ( BlockEnemy( pos ) ) {
				// blokuj supera od lopty
//			}
//			else {
			if(dist.GetLength() > 5.0)
			{
				// test offside
				if(pos.GetX() > off_l - 0.1f) pos.SetX(off_l - 0.1f);
				// pobehni na svoju poziciu
				RunToPos(pos);
			}
			else
			{
				// if I am blocked by an enemy, move a little towards the ball
				Vector shift;
				if(UnblockMyself(&shift))
				{
					Point tmp = world->me->pos + shift;
					UBLK_DBGN("%i: Unblocking by (%3.2f, %3.2f) - from (%3.2f, %3.2f) to (%3.2f, %3.2f)", world->state->GetSimTime(), shift.GetX(), shift.GetY(), world->me->pos.GetX(), world->me->pos.GetY(), tmp.GetX(), tmp.GetY());

					// test offside
					if(tmp.GetX() > off_l - 0.1f) tmp.SetX(off_l - 0.1f);
					// pobehni na svoju poziciu
					RunToPoint(tmp, true, false, .2f);
				}
				else
				{

					int t = world->me->GetSeeCount() % 3;

					// pozeraj sa na vsetky strany
					if(t != 0)
					{
						TurnTo(world->ball->pos);
						AlignNeck();
					}
					else
					{
						Point tmp = world->ball->pos;
						tmp.SetY(-world->ball->pos.GetY());

						TurnNeckTo(tmp, world->me->pos);
					}
				}
			}
//			}
		}
	}

/*
	//bool catchIt = DependentForBall();
	//if(catchIt) 
	bool catchIt = InterceptBall();

	if(!catchIt)
	{
		// Could not intercept ball
		const float off_l = GetOffsideLine();

		if(InOffsidePos(off_l))
		{	
			// Is offside
			ExitOffside();
		}
		else
		{
			// Not offside
			Patrol();
		}
*/
		/*
			CTNode tn = tnodes[world->me->GetUniform()];

			float f = world->ball->pos.GetX() / 52.5f;
			if(f > 1) f = 1;
			else if(f < -1) f = 1;

			float tX = tn.GetTacticalXCoord(f);

			bool OffsideWarning = false; //ci idem do ofsajdu
			if(tX >= off_l && world->me->pos.GetX() > off_l - 1.0f) OffsideWarning = true;

			if(fabsf(tX - world->me->pos.GetX()) > 5 && !OffsideWarning)
			{						//takticky presun
				if(world->me->pos.GetX() < tX)
				{
					if(f < 0) RunToPoint(Point(0, 0) + tn.GetMiddlePos(), false);
					else RunToPoint(Point(0, 0) + tn.GetOffencePos(), false);
				}
				else
				{
					if(f < 0) RunToPoint(Point(0, 0) + tn.GetDefencePos(), false);
					else RunToPoint(Point(0, 0) + tn.GetMiddlePos(), false);
				}
			}
			else
			{						//nepresuvam sa, sledujem loptu
				TurnTo(world->ball->pos);
				AlignNeck();
			}
		}*/
//	}
}

bool PlayerTactics::UnblockMyself(Vector* shift)
{
//	UBLK_DBGN("%i: Trying Unblock", world->state->GetSimTime());

	// move a little oif blocked by enemy
	if(!world->ball->IsValid()) 
		{ UBLK_DBGN("%i: ball invalid", world->state->GetSimTime()); return false; }

	// is the ball far enough?
	Vector dBall = world->ball->pos - world->me->pos;
	if(dBall.GetLength() < 5.f) 
		{ UBLK_DBGN("%i: ball too near %3.2f", world->state->GetSimTime(), dBall.GetLength()); return false; }

	// am I facing the same direction as the ball?
	Angle delta = (world->me->GetAbsDir() - dBall.GetAngle()).Abs();
	if(delta > DegAngle(25)) 
		{ UBLK_DBGN("%i: bad absdir %3.2f", world->state->GetSimTime(), delta.GetDeg()); return false; }

	// where should I move
	*shift = RectVector(0, 0);
	
	int posCnt = 0;
	int negCnt = 0;

	int i;
	for(i = 0; i < world->enemies.GetSize(); i++)
	{
		Vector dEnemy = world->enemies[i].pos - world->me->pos;
		Angle alpha = (dEnemy.GetAngle() - dBall.GetAngle()).Abs();

		float eLen = (dBall - dEnemy).GetLength();

	    if(eLen < dBall.GetLength() && dEnemy.GetLength() < 2.f && alpha < DegAngle(60))
		{
/*			if(dEnemy.GetAngle() > DegAngle(0)) posCnt++;
			else if(dEnemy.GetAngle() < DegAngle(0)) negCnt++;
			else { posCnt++; negCnt++; }*/

			Vector tmp = PolarVector(5.f/*2.f * dEnemy.GetLength()*/, world->me->GetAbsDir());
			if(tmp.GetLength() > shift->GetLength()) *shift = tmp;
		
			UBLK_DBGN("%i: Blocked by %i angle %3.2f", world->state->GetSimTime(), world->enemies[i].GetUniform(), alpha.GetDeg());
		}
	}

	if(shift->GetLength() == 0) return false;

	if(shift->GetLength() > 5.f) shift->SetLength(5.f);
	return true;
}

void PlayerTactics::BehaveWithBallGoalie()
{
	WARNN("I am not goalie, why BehaveWithBallGoalie???");
	BehaveWithBall();
}

void PlayerTactics::BehaveWithoutBallGoalie()
{
	WARNN("I am not goalie, why BehaveWithoutBallGoalie???");
	//bool catchIt = DependentForBall();
	//if(catchIt) 
	bool catchIt = InterceptBall();

	if(!catchIt)
	{
		TurnTo(world->ball->pos);
		AlignNeck();
	}
}

void PlayerTactics::OnChangeMode(int time, PlayMode mode)
{
	// Matus: Ignore ball while PM_FreeKick_Our lasts after a PM_GoalieCatch_Our.
	// Let the goalie rest if he wants to.
	letGoalieRest = false;
	if(mode == PM_FreeKick_Our)
	{
		PlayMode previousMode = world->state->GetPlayMode();
		if(previousMode == PM_GoalieCatch_Our) letGoalieRest = true;
	}
		
	// Call overloaded method from CPlayerKernel
	PlayerKernel::OnChangeMode(time, mode);

	// If this is some kick-off mode, start new scanning for players
	// see BehaveWithBall
	scanBeforeKickOffAngle = 0;
}

/// \author Peter Trebaticky
bool PlayerTactics::ShouldKickIn()
{
	bool should = PlayerSkills::ShouldKickIn();
	if(!should) return false;

#ifdef GOALIE
	// For a goalie, PlayerSkills::ShouldKickIn() works just fine.

#else // GOALIE
	if(IsBallKickAble()) return true;

	// Find player whose home position is nearest to the ball.
	float distance = 100000;	// a big number
	int uniform;

	m_cFormation.SetSide(SIDE_LEFT);
	for(int i = 1; i < 12; i++) if(m_cFormation.GetPlayerRole(SIDE_LEFT, i - 1) != PR_Goalie)
	{
		Point pos = m_cFormation.GetPlayerPosition(i, world->ball->pos);
		
		float d = (pos - world->ball->pos).GetLength();
		if(d < distance) { distance = d; uniform = i; }
	}

	KICKIN_DBGN("%i: Number %i will kick in the ball", world->state->GetSimTime(), uniform);

	Point pos = m_cFormation.GetPlayerPosition(world->me->GetUniform(), world->ball->pos);
	if(fabsf((pos - world->ball->pos).GetLength() - distance) < 3.f)
		KICKIN_DBGN("%i: I am going to kick in", world->state->GetSimTime());
	else
		should = false;

#endif // GOALIE

	return should;
}

Point PlayerTactics::GetHomePos()
{
    // GP
	float s = -27.0f;
	m_cFormation.SetSide(SIDE_LEFT);
	
	//CfPoint homepos = m_cFormation.GetPlayerPosition( world->me->Unum, CfPoint( s, 0) );
    Point homepos = m_cFormation.GetPlayerPosition( world->me->GetUniform(), Point( s, 0) );
	homepos.SetX((homepos.GetX() > -2.f) ? -2.f : homepos.GetX());

	return homepos;
}

/// Find best friend player for passing
int PlayerTactics::GetPassAbleFriend() const
{
	float myDist = DistanceToGoal(world->me->pos);
	float distFriendGoal;
	Vector diffFriend;
	float distToFriend;

	Vector diffEnemy;
	float distToEnemy;

	int best = -1;

	for(int who = 0;who < world->friends.GetSize(); who++) {
		distFriendGoal = DistanceToGoal(world->friends[who].pos);

		//spohuhrac je blizsie ku brane
		if(distFriendGoal < myDist) {
			diffFriend = world->friends[who].pos - world->me->pos;
			distToFriend = diffFriend.GetLength();
			//spoluhrac nie je dalej ako 30 a nie blizsie ako 3m (30 a 5 su default hodnoty \mroz)
			if(
				distToFriend <= world->variables->GetPassSurroundingsMaxWidth() && 
				distToFriend > world->variables->GetPassSurroundingsMinWidth())
			{
				bool blockingEnemy = false;
				for(int e=0; e < world->enemies.GetSize() && !blockingEnemy; e++)
				{
					diffEnemy = world->enemies[e].pos - world->me->pos;
					distToEnemy = diffEnemy.GetLength();
					//potencialne by mohol blokovat
					if(distToEnemy < distToFriend)
					{
						//do 17 stupnov blokuje
						if((diffFriend.GetAngle() - diffEnemy.GetAngle()).GetAbsDeg() < 17)
						{
							blockingEnemy = true;
						}
					}
				}

				if(!blockingEnemy)
				{
					best = who;
					myDist = distFriendGoal; //aby sa nasiel najblizsi
				}
			}
		}
	}

	return best;
}

bool PlayerTactics::InOffsidePos(float off_l) const
{
	if (world->me->pos.GetX() < off_l) return false;
	return true;
}

float PlayerTactics::GetOffsideLine() const
{
	float offside_line = 0;
	float fast = 0;
	float second = 0;

	float tmp;
	int i;

	for (i = 0; i < world->enemies.GetSize(); i++) {	//druhy obranca
		if (world->enemies[i].pos.GetX() > second) {
			second = world->enemies[i].pos.GetX();
			if (second > fast) {
				tmp = fast;
				fast = second;
				second = tmp;
			}
		}
	}

	if (second > world->ball->pos.GetX()) {
		offside_line = second;
	}
	else {
		offside_line = world->ball->pos.GetX();
	}

	return offside_line;
}

void PlayerTactics::ExitOffside()
{
	RunToPoint(Point(-1, world->me->pos.GetY()), true);
}

float PlayerTactics::DistanceToGoal(const Point& pos) const
{
	const float posX = 52.5f;

	if (pos.GetY() < -world->params->server->goal_width / 2) {
		const Point UpperGoal(posX, -world->params->server->goal_width / 2);
		return (UpperGoal - pos).GetLength();
	}

	if (pos.GetY() > world->params->server->goal_width / 2) {
		const Point LowerGoal(posX, world->params->server->goal_width / 2);
		return (LowerGoal - pos).GetLength();
	}

	return fabsf(posX - pos.GetX());
}

bool PlayerTactics::NoEnemiesAround() const
{
	if(DistanceToGoal(world->me->pos) < 18) return false;
	for (int i = 0; i < world->enemies.GetSize(); i++) {
		if ((world->enemies[i].pos - world->me->pos).GetLength() <= 5.0f &&
			(world->enemies[i].pos - world->me->pos).GetAngle().GetAbsDeg() < 90
			) 
		{
			DBGN("%d: %.2f", world->state->GetSimTime(), 
				(world->enemies[i].pos - world->me->pos).GetAngle().GetAbsDeg());
			return false;
		}
	}

	return true;
}

bool PlayerTactics::SmartDribleTo(const Point& pos)
{
	int who;
	const float karea = world->variables->GetDribleSurroundingsMinWidth();
	for (who = 0; who < world->enemies.GetSize(); who++)
	{
		//protihrac uz moze kopnut do lopty
		if ((world->ball->pos - world->enemies[who].pos).GetLength() <= karea * 1.1f) return false;
	}

	const float dir = (pos - world->me->pos).GetAngle().GetDeg();
	const float controll_dist = world->params->server->ball_size + world->me->type->player_size + 0.45f * world->me->type->kickable_margin;

	float bestdir = 0;
	float maxvaha = 0;
	float vaha;

	for (float reldir = -90; reldir <= 90; reldir += 5) {
		float uhol = (dir + reldir) * M_PI/180;

		Point driblepos = world->me->pos + RectVector(cosf(uhol)* controll_dist,sinf(uhol) * controll_dist);

		vaha = 1000; //velke cislo
		for(who = 0; who < world->enemies.GetSize(); who++)
		{
			if((world->enemies[who].pos - world->me->pos).GetLength() > world->variables->GetDribleSurroundingsMaxWidth()) continue;

			const float vzdial = (world->enemies[who].pos - driblepos).GetLength();
			if (vzdial <= karea) {
				vaha = 0;
				break;
			}
			if (!InTheMiddle(world->me->pos, driblepos, world->enemies[who].pos)) {
				if (vzdial <= (karea + world->params->server->player_speed_max)) {
					vaha = 0;
					break;
				}
				vaha = min(vaha, vzdial - (karea + world->params->server->player_speed_max));
			}
			else {
				vaha = min(vaha, vzdial - karea);
			}

		}

		if(vaha > maxvaha) {
			maxvaha = vaha;
			bestdir = reldir;
		}

	}

	if(maxvaha > 0) {
		DribbleTo2(pos,bestdir);
		//DBGN("%f",bestdir);
		return true;
	}
	return false;
}

bool PlayerTactics::ShortKick(const Point& pos)
{
	SKILLS_DBGN("%i: ->short kicking to position", world->state->GetSimTime());
	SKILLS_DBGN("%i: ->pos (%3.2f, %3.2f)", world->state->GetSimTime(), pos.GetX(), pos.GetY());

	const Vector diff = pos - world->ball->pos;
	const float dist = diff.GetLength();
	if(dist == 0) return false;

	if(!world->me->IsLookingAt(pos)) return false;

	for(float d = 15.0f;d >= 5.0f; d -= 1.0f)
	{
		const Point target = world->ball->pos + diff * (d/dist);
		const int my_steps = PredictSteps((target - world->me->pos).GetLength(),RecommDashPower(true),0);
		float enemy_steps = 6000; //velke cislo

		for(int i = 0; i < world->enemies.GetSize(); i++)
		{
			enemy_steps = min(enemy_steps, (target - world->enemies[i].pos).GetLength()/world->params->server->player_speed_max);
		}

		if(my_steps < enemy_steps)
		{
			PassBallTo(target,0);
			return true;
		}
	}

	return false;
}

/**
 * Move player to given place, or watch the ball
 * \author Daniel Novomesky
 */
void PlayerTactics::Patrol() 
{
	Point fpTacticalPosition = GetHomePos();

	bool OffsideWarning = false; //ci idem do ofsajdu

	const float off_l = GetOffsideLine();
	if(fpTacticalPosition.GetX() >= off_l && world->me->pos.GetX() > off_l - 1.0f) OffsideWarning = true;

	if(fabsf(fpTacticalPosition.GetX() - world->me->pos.GetX()) > 5 || fabsf(fpTacticalPosition.GetY() - world->me->pos.GetY()) > 5) //takticky presun
	{
		// ak sa nedostanem do offside-u
		if(!OffsideWarning)
		{
			RunToPoint(fpTacticalPosition, true);
		}
		else if (OffsideWarning && fabsf(fpTacticalPosition.GetY() - world->me->pos.GetY()) > 5)
		{
			Point position(world->me->pos.GetX(), fpTacticalPosition.GetY());
			RunToPoint(position, false);
		} 
		else 
		{
  			TurnTo(world->ball->pos);
	  		AlignNeck();
		}
	}
	else //nepresuvam sa, sledujem loptu
	{
		TurnTo(world->ball->pos);
		AlignNeck();
	}
}

/**
 * Find out if position is offside
 * \param target examined position
 * \return true if offside, false if not
 * \author Martin Zuzcak
 */
bool PlayerTactics::IsOffsidePosition(Point target)
{
	const float off_l = GetOffsideLine();

	if(target.GetX() >= off_l && world->me->pos.GetX() > off_l - 1.0f) return true;
	return false;
}

/**
 * Find out a better position for receiving a pass and move there
 * \return true if player should move, false if no better place found
 * \author Martin Zuzcak
 */
bool PlayerTactics::GotoPositionToPass()
{
	Vector positions[10];
	positions[0].SetX(5.0f);
	positions[0].SetY(0.0f);
	positions[1].SetX(3.5f);
	positions[1].SetY(3.5f);
	positions[2].SetX(3.5f);
	positions[2].SetY(-3.5f);
	positions[3].SetX(0.0f);
	positions[3].SetY(5.0f);
	positions[4].SetX(0.0f);
	positions[4].SetY(-5.0f);

	int i;
	for (i = 0; i < 5; i++)
	{
		positions[i + 5].SetX(positions[i].GetX() * 2);
		positions[i + 5].SetY(positions[i].GetY() * 2);
	}

	Point fpDefaultPosition = GetHomePos();
	float fMyEnemyDistance = GetNearestEnemyDistance(world->me->pos);
	// pre kazdu z positions zistim, ci tam hrac moze ist
	for (i = 0; i < 10; i++) {
		Point fpDestination = world->me->pos + positions[i];
		if (IsOffsidePosition(fpDestination) == true)
			continue;
		float fTargetDistance = (fpDestination - fpDefaultPosition).GetLength();
		float fMyDistance = (world->me->pos - fpDefaultPosition).GetLength();
		if (fTargetDistance < 20.0f || fTargetDistance < fMyDistance) {
			// teoreticky tam mozem ist, este zistit, ci sa to oplati...
			float fTargetEnemyDistance = GetNearestEnemyDistance(fpDestination);
			if (fTargetEnemyDistance > (fMyEnemyDistance + 2.0f) && GetNearestFriendDistance(fpDestination) > 5.0f) {
				RunToPoint(fpDestination, true);
				return true;
			}
		}
	}
	
	return false;
}

#if 0
bool PlayerTactics::IsOurBall() 
{
	if(!world->ball->IsValid()) return false;

	int ourc;
	int theirc;
	/*int closestFriend = */(void) NearFriend(&ourc);	/// \todo Why do we call NearFriend?
	int enemy = NearEnemy(&theirc);

	if(enemy == -1) return true;
	if (ourc < theirc) return true;
    return false;
}
#endif // 0

/**
 * Find out speed needed to kick ball to given place
 * \param ciel place where ball should be kicked
 * \param steps how many steps should it take
 * \return kick power
 * \author Martin Zuzcak
 */
float PlayerTactics::GetKickPower(const Point& ciel, int steps)
{
  int nStepTolerance = 5;
	Vector path = ciel - world->ball->pos;
	float vzdial = path.GetLength();
	float req_spd = vzdial * (1.0f - world->params->server->ball_decay) / (1.0f - float(pow(world->params->server->ball_decay, steps - 1)));
  if (req_spd > world->params->server->ball_speed_max) {
	  req_spd = vzdial * (1.0f - world->params->server->ball_decay) / (1.0f - float(pow(world->params->server->ball_decay, steps + nStepTolerance)));
    return req_spd;
    }
  return req_spd;
}

/*
int PlayerTactics::NearEnemy(int* pCycles) const
{
	if(world->enemies.GetSize() == 0) return -1;

	float psteps;
	CBall tmpBall(ball);
	Point diff;
	const float karea = world->params->server->psize + world->params->server->bsize + world->params->server->kmargin;

	for(int ncycles = 1;;ncycles++)
	{
		tmpBall = stepBall(tmpBall);

		for(int i=0;i < world->enemies.GetSize();i++)
		{
			diff = tmpBall.pos - world->enemies[i].pos;

			psteps = (diff.GetLength() - karea) / world->params->server->pspeed_max;
			if(psteps < ncycles)
			{
				*pCycles = ncycles;
				return i;
			}
		}
	}
}
*/

/**
 * Pass to a running player, if possible
 * \return true if passed, false of not
 * \author Martin Zuzcak
 */
bool PlayerTactics::PassIntoRun()
{
	return false;

	if(world->me->pos.GetX() < world->variables->GetMinXPosition()) return false;
  
	float fOurPlayerAdvantage = 0.2f;
	int n;

	for(int who = 0; who < world->friends.GetSize(); who++)
	{
		if(world->friends[who].pos.GetX() <= (world->me->pos.GetX() - 10.0f)) continue;
		
		for(n = 40; n > 10;  n = n - 2) 
		{
			float pos = world->friends[who].pos.GetX() + n;
			Point destination(pos, world->friends[who].pos.GetY());
			float diff = (destination - world->friends[who].pos).GetLength();
			float fNearestEnemy = GetNearestEnemyDistance(destination);
			
			if(diff > (fNearestEnemy - fOurPlayerAdvantage)) continue;
			if(IsBlockingEnemy(destination)) continue;

			int stepCount = int(diff / float(world->params->server->player_speed_max));
			float power = GetKickPower(destination, stepCount);
			//Point path = destination - world->ball->pos;
			//float vzdial = path.GetLength();
			
			if(power <= world->params->server->ball_speed_max) 
			{
				SKILLS_DBGN("%i: ->passing to run (really 1)", world->state->GetSimTime());
				SKILLS_DBGN("%i: ->%s player %i", world->state->GetSimTime(), world->friends[who].GetSide() == OUR ? "our" : (world->friends[who].GetSide() == THEIR ? ">>their<<" : ">>unknown<<"), world->friends[who].GetUniform());
				SKILLS_DBGN("%i: ->player pos (%3.2f, %3.2f) dest (%3.2f, %3.2f)", world->state->GetSimTime(), world->friends[who].pos.GetX(), world->friends[who].pos.GetY(), destination.GetX(), destination.GetY());

      			PassBallTo(destination, 0.2f);
				return true;
			}
		}

		for(n = 40; n > 10;  n = n - 2)
		{
			float xpos = world->friends[who].pos.GetX() + n;
			float ypos = (world->me->pos.GetY() > world->friends[who].pos.GetY()) ? world->friends[who].pos.GetY() + n : world->friends[who].pos.GetY() - n;
			Point destination(xpos, ypos);
			float diff = (destination - world->friends[who].pos).GetLength();
			float fNearestEnemy = GetNearestEnemyDistance(destination);

			if(diff > (fNearestEnemy - fOurPlayerAdvantage)) continue;
			if(IsBlockingEnemy(destination)) continue;

			int stepCount = int(diff / float(world->params->server->player_speed_max));
			float power = GetKickPower(destination, stepCount);
			//Point path = destination - world->ball->pos;
			//float vzdial = path.GetLength();
			
			if(power <= world->params->server->ball_speed_max) 
			{
				SKILLS_DBGN("%i: ->passing to run (really 2)", world->state->GetSimTime());
				SKILLS_DBGN("%i: ->%s player %i", world->state->GetSimTime(), world->friends[who].GetSide() == OUR ? "our" : (world->friends[who].GetSide() == THEIR ? ">>their<<" : ">>unknown<<"), world->friends[who].GetUniform());
				SKILLS_DBGN("%i: ->player pos (%3.2f, %3.2f) dest (%3.2f, %3.2f)", world->state->GetSimTime(), world->friends[who].pos.GetX(), world->friends[who].pos.GetY(), destination.GetX(), destination.GetY());

      			PassBallTo(destination, 0.2f);
				return true;
			}
		}
	}

	return false;
}

/**
 * Compute distance to nearest opponent
 * \param target reference point
 * \return distance
 * \author Martin Zuzcak
 */
float PlayerTactics::GetNearestEnemyDistance(const Point& target)
{
  float fMinDistance = 1000.0;
	for(int i = 0; i < world->enemies.GetSize(); i++)
	{
	  float fDiffEnemy = (world->enemies[i].pos - target).GetLength();
    if (fDiffEnemy < fMinDistance)
      fMinDistance = fDiffEnemy;
    }
  return fMinDistance;
  }

/**
 * Find nearest friend player
 * \param target Reference point
 * \return Nearest player index or -1 of none found
 * \author Matus Horvath
 */
int PlayerTactics::GetNearestFriend(const Point& target)
{
	float min = 1000.0;
	int player = -1;

	for(int i = 0; i < world->friends.GetSize(); i++)
	{
		float diff = (world->friends[i].pos - target).GetLength();
		if(diff < min)
		{
			min = diff;
			player = i;
		}
	}

	return player;
}

/**
 * Compute distance to nearest friend
 * \param target Reference point
 * \return Distance
 * \author Matus Horvath
 */
float PlayerTactics::GetNearestFriendDistance(const Point& target)
{
	int player = GetNearestFriend(target);
	return player == -1 ? 1000.0f : (world->friends[player].pos - target).GetLength();
}

bool PlayerTactics::IsBlockingEnemy(const Point& target)
{
	Vector diffTarget = target - world->me->pos;
	float distToTarget = diffTarget.GetLength();

	for(int e = 0; e < world->enemies.GetSize(); e++)
	{
		Vector diffEnemy = world->enemies[e].pos - world->me->pos;
		float fDistToEnemy = diffEnemy.GetLength();

	    if(fDistToEnemy < distToTarget && fDistToEnemy > 2)
		{
  			if((diffTarget.GetAngle() - diffEnemy.GetAngle()).GetAbsDeg() < 17)
			{
				return true;
			}
		}
	}
	
	return false;
}

// GP
int PlayerTactics::GetFormation(const char* formation)
{
    if (strncmp(formation, "Formation622", 12) == 0)
       return FORMATION_622;
    else if (strncmp(formation, "Formation532", 12) == 0)
       return FORMATION_532;
    else if (strncmp(formation, "Formation523", 12) == 0)
       return FORMATION_523;
    else if (strncmp(formation, "Formation433", 12) == 0)
       return FORMATION_433;
    else if (strncmp(formation, "Formation343", 12) == 0)
       return FORMATION_343;
    else if (strncmp(formation, "Formation334", 12) == 0)
       return FORMATION_334;

   return -1;

}

int PlayerTactics::GetUnit(int PlayerCount, char type)
{
    if (PlayerCount == 2) 
        return UNIT_2_L;

    else if ((PlayerCount == 3) && (type == 'A'))
        return UNIT_3_L;
    else if ((PlayerCount == 3) && (type == 'B'))
        return UNIT_3_V;
    else if ((PlayerCount == 3) && (type == 'C'))
        return UNIT_3_OV;

    else if ((PlayerCount == 4) && (type == 'A'))
        return UNIT_4_L;
    else if ((PlayerCount == 4) && (type == 'B'))
        return UNIT_4_V;
    else if ((PlayerCount == 4) && (type == 'C'))
        return UNIT_4_OV;
    else if ((PlayerCount == 4) && (type == 'D'))
        return UNIT_4_S;

    else if ((PlayerCount == 5) && (type == 'A'))
        return UNIT_5_L;
    else if ((PlayerCount == 5) && (type == 'E'))
        return UNIT_5_23;
    else if ((PlayerCount == 5) && (type == 'F'))
        return UNIT_5_32;
    else if ((PlayerCount == 5) && (type == 'B'))
        return UNIT_5_V;
    else if ((PlayerCount == 5) && (type == 'C'))
        return UNIT_5_OV;

    else if ((PlayerCount == 6) && (type == 'A'))
        return UNIT_6_2L;
    else if ((PlayerCount == 6) && (type == 'B'))
        return UNIT_6_V;
    else if ((PlayerCount == 6) && (type == 'C'))
        return UNIT_6_OV;

    return -1;
}

void PlayerTactics::ChangeFormation(const char* formation)
{
    int unitcount1, unitcount2, unitcount3;

    unitcount1 = (int)(formation[9] - 48);
    unitcount2 = (int)(formation[10] - 48);
    unitcount3 = (int)(formation[11] - 48);

    m_cFormation.InitRawUnits(SIDE_LEFT, GetFormation(formation), GetUnit(unitcount1, (char)formation[12]), GetUnit(unitcount2, (char)formation[13]), GetUnit(unitcount3, (char)formation[14]));
	TXTN("We are changing formation to %s", formation);
}
