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

#include "Player/GoalieTactics.h"
#include "Player/World/Ball.h"
#include "Player/World/Player.h"
#include "Player/World/Myself.h"
#include "Player/World/State.h"
#include "Player/World/Params.h"
#include "Common/World/Point.h"
#include "Common/World/Vector.h"
#include <stdio.h>
#include <math.h>
#include <time.h>

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

#define STD_POS_X	45.0f
#define STD_POS_Y	0.0f

/// Returns number of tangent points on circle S with radius r 
/// and tangent passes through point A
/// T1 and T2 are those points (if any)
int GetTangentPointsOnCircle(Point S, float r, Point A, Point& T1, Point& T2)
{
#define EPS 1e-6
	static float p1, p2, p3, p4;
	static float Sx, Sy, Ax, Ay;
	Sx = S.GetX(); Sy = S.GetY();
	Ax = A.GetX(); Ay = A.GetY();
	if(fabs(Sx - Ax) < EPS && fabs(Sy - Ay) < EPS) return 0;
	p1 = sqr(Ax) - sqr(r) - 2*Ax*Sx + sqr(Sx) + sqr(Ay - Sy);
	if(p1 < 0) return 0;

	p1 = r * sqrtf(p1);
	p2 = sqr(Ax - Sx) + sqr(Ay - Sy);
	p3 = sqr(Ay)*Sy + Ay*(sqr(r)-2*sqr(Sy)) + Sy*(-sqr(r)+sqr(Ax-Sx)+sqr(Sy));
	p4 = sqr(Ax)*Sx + Ax*(sqr(r)-2*sqr(Sx)) + Sx*(-sqr(r)+sqr(Ay-Sy)+sqr(Sx));
	T1.SetY(-p1 * (Ax - Sx)); T1.SetX(p1 * (Ay - Sy));
	T2.SetY(-T1.GetY()); T2.SetX(-T1.GetX());
	T1.SetY((T1.GetY() + p3) / p2);
	T2.SetY((T2.GetY() + p3) / p2);
	T1.SetX((p4 + T1.GetX()) / p2);
	T2.SetX((p4 + T2.GetX()) / p2);

	return 2;
}

/// Returns number of intersection points of two circles (S1, r1) and (S2, r2)
/// T1 and T2 are those points (if any)
int CirclesIntersection(const Point S1, const float r1, const Point S2, const float r2, Point& T1, Point& T2)
{
	static float S1x, S1y, S2x, S2y;
	static float a, b, c, d;

	S1x = S1.GetX(); S1y = S1.GetY();
	S2x = S2.GetX(); S2y = S2.GetY();

	if(fabsf(S1x-S2x) < EPS)
	{
		if(fabsf(S1y-S2y) < EPS) return 0;
		T1.SetY((sqr(r1) - sqr(r2) - sqr(S1y) + sqr(S2y)) / (2*(S2y-S1y)));
		T2.SetY(T1.GetY());
		a = -sqr(S1y-S2y) * (r1-r2+S1y-S2y) * (r1+r2+S1y-S2y) * (r1-r2-S1y+S2y) * (r1+r2-S1y+S2y);
		if(a < 0) return 0;
		a = sqrtf(a) / (2*sqr(S1y-S2y));
		T1.SetX(S2x - a);
		T2.SetX(S2x + a);
	} 
	else
	{
		a = sqr(S1x - S2x) + sqr(S1y - S2y);
		b = -sqr(S1x-S2x) * (-sqr(r1-r2)+a) * (-sqr(r1+r2)+a);
		if(b < 0) return 0;
		b = sqrtf(b);
		c = (S1x-S2x) * (sqr(r2)*(S1x-S2x) + sqr(r1)*(S2x-S1x) + (S1x+S2x)*a);
		T1.SetX((c + b*(S1y-S2y)) / (2*(S1x-S2x)*a));
		T2.SetX((c + b*(S2y-S1y)) / (2*(S1x-S2x)*a));
		d = sqr(r2)*(S1y-S2y) + sqr(r1)*(S2y-S1y) + a*(S1y+S2y);
		T1.SetY((-b + d) / (2*a));
		T2.SetY((b + d) / (2*a));
	}
	return 2;

#undef EPS
}
///////////////////////////////////////////////////////////////////////////////
// GoalieTactics interface

///////////////////////////////////////////////////////////////////////////////
// GoalieTactics construction

GoalieTactics::GoalieTactics()
	: PlayerSkills("GoalieTactics", "PlayerKernel(GoalieTactics) module")
{
	// Home position - vector from (0, 0)
	homePos = Point(-45, 0);

	caughtBall = false;
	inPosition = false;
	scaningSituation = false;
	goalieKicked = 0;
	enemyHadBall = false;
	m_PlayerRole = PR_Goalie;
}

///////////////////////////////////////////////////////////////////////////////
// GoalieTactics operations
void GoalieTactics::BehaveWithBallGoalie()
{
	PlayMode playMode = world->state->GetPlayMode();

	if(playMode == PM_FreeKick_Our || 
		playMode == PM_FreeKick_Their ||
		playMode == PM_GoalKick_Our ||
		playMode == PM_GoalKick_Their)
	{
		if(inPosition)
		{
			int spoluhrac = GetPassAbleFriend();
			if(spoluhrac == -1) KickToGoal3();
			else PassBallTo(world->friends[spoluhrac].pos, 100);
			goalieKicked = 1;
		}
		else if(scaningSituation)
		{
			if(playMode != PM_GoalKick_Our)
			{
				Point kickOffPos = GetKickOffPos();
				MoveTo(kickOffPos);
			}
			inPosition = true;
		}
		else if(caughtBall)
		{
			// zisti situaciu
			TurnTo(Point(0, 0));
			AlignNeck();
			SetView(WIDE_VIEW);
			SetNextSynchMode(Synch_WaitForVI);
			// pokracuj v tom, kym si neoddychnes
			if(world->me->GetStamina() > world->params->server->stamina_max * 0.9f)
				scaningSituation = true;
		}
		else if(playMode == PM_GoalKick_Our)
		{
			if((world->ball->pos - world->me->pos).GetLength() < world->params->server->kickable_margin * .6f)
			{
				caughtBall = true;
			}
			else
			{
				RunToPoint(world->ball->pos, false, false, 0.f);
			}

		}
		else
		{
			MoveTo(Point(-37.f, 0));
			caughtBall = true;
		}
	}
	else if(!caughtBall)
	{
		if(CanCatchBall())
		{
			CatchBall();
		}
		else if(IsBallKickAble())
		{
			int spoluhrac = GetPassAbleFriend();
			// \todo Treba kopnut radsej do autu, nie len tak
			// po kopnuti treba utekat domov - na to sluzi goalieKicked
			if(spoluhrac == -1 || fabsf(world->me->pos.GetY()) > 30)
			{
//				if(m_iGoalieCatchBan)
				{
					DumbKick(Point(world->me->pos.GetX(), (world->me->pos.GetY() > 0 ? 1 : -1) * 40.f));
				}
//				else KickToGoal3();
			}
			else PassBallToPlayer(world->friends[spoluhrac], 100);
			goalieKicked = 1;
		}
		else if(BallHeadingOnOurGoal(.5f))
		{
			TXTN("Ball is surely heading on our goal and I can't catch it. Desparately trying to tackle...");
			Tackle(100.f);
		}
		else BehaveWithoutBallGoalie();
	}
}

bool GoalieTactics::InPenaltyArea(const Point p, bool our)
{
	if(our)
	{
		if(p.GetX() < world->params->flags[PLC].GetX() && fabsf(p.GetX()) < 52.5 &&
			fabsf(p.GetY()) < fabsf(world->params->flags[PLT].GetY())) return true;
	}
	else
	{
		if(p.GetX() < world->params->flags[PLC].GetX() && fabsf(p.GetX()) < 52.5 &&
			fabsf(p.GetY()) < fabsf(world->params->flags[PLT].GetY())) return true;
	}

	return false;
}

/// Returns point nearest to actual position of goalie where he can catch ball
/// or at least can kick the ball; it returns the most probable intercept point
/// If it is impossible to catch/kick ball, the returned point has x == -1000
Point GoalieTactics::GetNearestBallInterceptPos()
{
	const int MAX_CYCLE = 30;

	Point res, T1, T2;
	float catchableArea, kickableArea, actArea;
	float edp, deg;
	Vector a, v;
	int cycle, cycleb;
	int minc;
	struct FBS {Point p; bool in;} futureBallPos[MAX_CYCLE];
	Point futureMePos[MAX_CYCLE];
	float runDist[MAX_CYCLE];
	int maxCycle;

	Myself p(*world->me);
	Ball tmpBall(*world->ball);

	catchableArea = max(world->params->server->catchable_area_l, world->params->server->catchable_area_w);
	kickableArea = world->me->type->player_size + world->params->server->ball_size + world->me->type->kickable_margin;
	edp = p.GetEffort() * world->me->type->dash_power_rate * 100;
	if(edp > world->params->server->player_accel_max) edp = world->params->server->player_accel_max;

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

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

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

		// ball
		tmpBall.Step();
		futureBallPos[cycleb].p = tmpBall.pos;
		futureBallPos[cycleb].in = InPenaltyArea(tmpBall.pos);
		if(m_iGoalieCatchBan > 0 && cycleb <= m_iGoalieCatchBan) 
		{
			futureBallPos[cycleb].in = false;
		}
		else if(fabsf(tmpBall.pos.GetX()) > 52.5) break;
	}
	maxCycle = cycleb;

	minc = -1;
	res.SetX(-1000);
	p = *world->me;
	// we'll move by inertia while turning
	p.pos = p.pos + p.vel;

	// we are decreasing the probability
	for(float per = .3f; per <= 1.1; per += .1f) {
		kickableArea *= per;
		catchableArea *= per;

		for(cycleb = 0; cycleb < maxCycle; cycleb++)
		{
			if(minc != -1 && cycleb > minc) break;

			actArea = futureBallPos[cycleb].in ? catchableArea : kickableArea;

			// first try without turning
			for(cycle = cycleb; cycle >= 0; cycle--)
			{
				if((futureBallPos[cycleb].p - futureMePos[cycle]).GetLength() <= actArea)
				{
					if((cycleb <= minc && per < .8) || minc == -1) {
						res = futureMePos[cycle];
						if(fabsf(res.GetX()) > 52.5) res.SetX(-1000);
						else
						{
							minc = cycleb;
	//						DBGN("%.1f ", per);
						}
					}
				}
			}

			// if nothing was found, try with turning
			for(cycle = cycleb; cycle >= 0 && minc == -1; cycle--)
			{
				if((futureBallPos[cycleb].p - p.pos).GetLength() <= runDist[cycle] + actArea)
				{
					if((cycleb < minc && per < .8) || minc == -1) {
						res = futureBallPos[cycleb].p;
						if(fabsf(res.GetX()) > 52.5) res.SetX(-1000);
						else
						{
							minc = cycleb;
//							DBGN("%.1f ", per);
						}
					}
					break;
				}
			}
		}

		kickableArea /= per;
		catchableArea /= per;
	}

	if(minc == 0 && (world->ball->GetAge() == 0 || world->state->GetPlayMode() == PM_GoalKick_Our)) res = futureMePos[2];

	return res;
}

void GoalieTactics::BehaveWithoutBallKnowledgeGoalie()
{
	Point home = GetHomePos();
	if((world->me->pos - home).GetLength() > 1)
	{
		if((home - world->me->pos).GetAngle().GetAbsDeg() > 110)
			RunToPointBackwards(home, true, true);
		else
			RunToPoint(home, true, true);
	}
	else Scan();
}

void GoalieTactics::BehaveWithoutBallGoalie()
{
	if(caughtBall) caughtBall = false;
	if(inPosition) inPosition = false;
	if(scaningSituation) scaningSituation = false;
	if(goalieKicked >= CYCLES_TO_RUN_HOME_AFTER_KICK) goalieKicked = 0;
	if(goalieKicked > 0) goalieKicked++;
	static Point home;
	static int degTol;
	degTol = 10;

	if((world->ball->pos - world->me->pos).GetLength() < 10)
	{
		if(enemyHadBall && world->ball->GetAge() != 0)
		{
			Point home = GetHomePos();
			if((home - world->me->pos).GetAngle().GetAbsDeg() > 110)
				RunToPointBackwards(home, true, true);
			else
				RunToPoint(home, true, true);
			return;
		}

		int ne = GetNearestEnemyTo(world->ball->pos, NULL);
		if(ne != -1)
		{
			if((world->enemies[ne].pos - world->ball->pos).GetLength() < 
				world->params->server->ball_size + world->enemies[ne].type->player_size + world->enemies[ne].type->kickable_margin)
			{
				enemyHadBall = true;
			}
			else enemyHadBall = false;
		}
		else enemyHadBall = false;
	}
	else enemyHadBall = false;

	if(BallHeadingOnOurGoal()) { // urgent situation
		DBG("%d: Ball is heading on our goal!", world->state->GetSimTime());
		home = GetNearestBallInterceptPos();

		if(fabsf(home.GetX()) > 52.45) // it seems that it's impossible to catch it
		{
			// if ball is near, try to catch it anyway
			if((world->me->pos - world->ball->pos).GetLength() < 2.1f)
			{
				DBGN("Trying to catch it anyway... %.2f", (world->me->pos - world->ball->pos).GetLength());
				CatchBall();
			}
			else
			{
				// try to catch it at least on goal line :(
				static Point ourGoal;
				ourGoal = world->params->flags[GOAL_L];
				home.SetY(world->ball->pos.GetY() + world->ball->vel.GetY() * (ourGoal.GetX() - world->ball->pos.GetX()) / world->ball->vel.GetX());
				home.SetX(ourGoal.GetX());
			}
		}
		if((home - world->me->pos).GetLength() > 0)
		{
//			Vector meHome = (home - world->me->pos);
//			float deg = fabsf(meHome.GetAngle() - world->me->GetAbsDir());
			Ball tmpBall(*world->ball); tmpBall.Step();

			Vector meHome = (home - world->me->pos);
			Vector meBall = (tmpBall.pos - world->me->pos);
			float deg; // = fabsf(meHome.GetAngle().GetDeg() - meBall.GetAngle().GetDeg());
			deg = fabsf(meHome.GetAngle().GetDeg() - world->me->GetAbsDir().GetDeg());
			if(deg > 180) deg = 360 - deg;
			if(deg < 110) RunToPoint(home, true, true, 0, degTol);
			else RunToPointBackwards(home, true, true, 0, degTol);
			DBGN(" - Running for life from (%.2f, %.2f) to (%.2f, %.2f) p(%.2f, %.2f) v(%.2f, %.2f) %.2f",
				world->me->pos.GetX(), world->me->pos.GetY(),
				home.GetX(), home.GetY(),
				world->ball->pos.GetX(), world->ball->pos.GetY(),
				world->ball->vel.GetX(), world->ball->vel.GetY(),
				(world->me->pos - world->ball->pos).GetLength());
		}
		return;
	}

	if(IsPassNearGoal())
	{
		DBGN("%d: It's a pass!", world->state->GetSimTime());
		home = SolvePassNearGoal();
		if(home.GetX() != -1000)
		{
			Ball tmpBall(*world->ball); tmpBall.Step();

			Vector meHome = (home - world->me->pos);
			Vector meBall = (tmpBall.pos - world->me->pos);
			float deg; // = fabsf(meHome.GetAngle().GetDeg() - meBall.GetAngle().GetDeg());
			deg = fabsf(meHome.GetAngle().GetDeg() - world->me->GetAbsDir().GetDeg());
			if(deg > 180) deg = 360 - deg;
			if(deg < 110 || (world->me->pos - home).GetLength() > 15) RunToPoint(home, true, true, 0, 40);
			else RunToPointBackwards(home, true, true, 0, 40);
			DBGN("Running to (%.2f, %.2f)", home.GetX(), home.GetY());
			return;
		}
	}

	home = GetHomePos();
	if(goalieKicked != 0 && (world->me->pos - home).GetLength() > 3) 
	{
		// utekam rychlo naspat po odkopnuti lopty
		if((home - world->me->pos).GetLength() > 1){
			RunToPoint(home, true);
		}
	}
	else if (DistanceToGoal(world->ball->pos, true) > 40 ||
		world->state->GetPlayMode() == PM_CornerKick_Our || 
		world->state->GetPlayMode() == PM_CornerKick_Their)
	{
		goalieKicked = 0;
		if((home - world->me->pos).GetLength() > 1)
		{
			Ball tmpBall(*world->ball); tmpBall.Step();

			Vector meHome = (home - world->me->pos);
			Vector meBall = (tmpBall.pos - world->me->pos);
			float deg = fabsf(meHome.GetAngle().GetDeg() - meBall.GetAngle().GetDeg());
//			float deg = fabsf(meHome.GetAngle() - world->me->GetAbsDir());
			if(deg > 180) deg = 360 - deg;
			if(deg < 110 || world->state->GetPlayMode() != PM_PlayOn) RunToPoint(home, true);
			else RunToPointBackwards(home, true);
		}
        else 
		{
			TurnTo(world->ball->pos);
			AlignNeck();
		}
	}
	else
	{
		if((world->me->pos - world->ball->pos).GetLength() < 4 && 
			world->ball->vel.GetLength() < 1.5f)
		{
			static int cyc;
			static int enemy;
			enemy = GetNearestEnemyTo(world->ball->pos, &cyc);
			if(enemy > -1 && cyc <= 1)
			{
				RunToPoint(world->enemies[enemy].pos, true, true);
				return;
			}
		}

		Point target;
		target.SetX(-1000);
		if(CanInterceptBall(&target))
			target = GetNearestBallInterceptPos();
		if(target.GetX() == -1000 || fabs(target.GetY()) >= 22)
		{
			Point goal(-52.5f, 0);
			Vector goal2ball = world->ball->pos - goal;

			target = goal + goal2ball * .44f;
			target = GetDefensePos();
		}

		if ((target - world->me->pos).GetLength() > 0)
		{
			Ball tmpBall(*world->ball); tmpBall.Step();

			Vector meTarget = (target - world->me->pos);
			Vector meBall = (tmpBall.pos - world->me->pos);
			float deg = fabsf(meTarget.GetAngle().GetDeg() - meBall.GetAngle().GetDeg());
//			float deg = fabsf(meTarget.GetAngle().GetDeg() - world->me->GetAbsDir().GetDeg());
			if(deg > 180) deg = 360 - deg;
			if(deg < 110 || world->state->GetPlayMode() != PM_PlayOn || (world->me->pos - home).GetLength() > 10) 
				RunToPoint(
					target,
					true,
					world->state->GetPlayMode() == PM_PlayOn,
					POS_TOL,
					(target - world->me->pos).GetLength() > 3 ? 40 : 15
				);
			else 
				RunToPointBackwards(
					target,
					true,
					fabsf(world->ball->pos.GetX()) > 15,
					POS_TOL,
					(target - world->me->pos).GetLength() > 3 ? 40 : 15
				);
		}
		else
		{
			Ball tmpBall(*world->ball); tmpBall.Step();
			// natocime sa kolmo na loptu, aby sme sa pri jej chytani uz tocit nemuseli
			// \todo s tymto ale funkcia GetDefencePos zatial nerata
/*
			Vector meBall;
			meBall = world->me->pos - tmpBall.pos;
			Point where;
			where.SetX(-meBall.GetY() + world->me->pos.GetX());
			where.SetY(meBall.GetX() + world->me->pos.GetY());

			TurnTo(where);
			TurnNeckTo(tmpBall.pos, world->me->pos, 0);
*/

//			TurnTo(tmpBall.pos);
//			AlignNeck();

			TurnNeckTo(tmpBall.pos, world->me->pos, 0);
		}
	}
}

Point GoalieTactics::GetHomePos()
{
	return homePos;
}

Point GoalieTactics::GetDefensePos()
{
// BEGIN - Roztoce
	float posX = -52.5f;
	Point homepos;

	Point defPos(-STD_POS_X, STD_POS_Y);
	Point ballPos(world->ball->pos.GetX(), world->ball->pos.GetY());
	Point tGoal(posX, -world->params->server->goal_width / 2);
	Point bGoal(posX, world->params->server->goal_width / 2);
	Vector tVect = tGoal - ballPos;
	Vector bVect = bGoal - ballPos;
	float fScale;
// END - Roztoce

	Point futBposT[100];
	Point futBposB[100];
	float runDist[100]; // kolko mozem zabehnut za i cyklov s otocenim
	Point futureMePos[100];
	Myself p(*world->me);
	int minCycle, maxCycleB, maxCycleT, cycle, cycleB, cycleT, minc;
	float edp;
	Vector v, a;
	float catchableArea;
	catchableArea = max(world->params->server->catchable_area_l, world->params->server->catchable_area_w);
	catchableArea *= 0.5f;
	edp = world->me->GetEffort() * world->me->type->dash_power_rate * 100;
	if(edp > world->params->server->player_accel_max) edp = world->params->server->player_accel_max;

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

	futBposT[0] = ballT.pos;
	futBposB[0] = ballB.pos;
	futureMePos[0] = world->me->pos;
	v.SetLength(0);
	runDist[0] = 0;
	maxCycleB = maxCycleT = -1;
	for(cycle = 1; cycle < 100; cycle++)
	{
		// without turning
		a = PolarVector(edp, p.GetAbsDir());
		p.vel += a;
		if(p.vel.GetLength() > world->me->type->player_speed_max) p.vel.SetLength(world->me->type->player_speed_max);
		futureMePos[cycle] = futureMePos[cycle - 1] + p.vel;
		p.vel *= world->me->type->player_decay;

		ballT.Step();
		ballB.Step();
		if(fabsf(ballT.pos.GetX()) > 52.5 && maxCycleT == -1) maxCycleT = cycle;
		if(fabsf(ballB.pos.GetX()) > 52.5 && maxCycleB == -1) maxCycleB = cycle;
		if(maxCycleB != -1 && maxCycleT != -1) break;
		futBposT[cycle] = ballT.pos;
		futBposB[cycle] = ballB.pos;
		// v prvom cykle este len zbadam smer a dam prikaz na otocenie
		// v druhom cykle som otoceny a davam prikaz na pohnutie
		// preto sa pohnem az v tretom cykle
		if(cycle > 2) v.SetLength(v.GetLength() + edp);
		if(v.GetLength() > world->me->type->player_speed_max) v.SetLength(world->me->type->player_speed_max);
		runDist[cycle] = runDist[cycle - 1] + v.GetLength();
		v.SetLength(v.GetLength() * world->me->type->player_decay);
	}
	minCycle = min(maxCycleB, maxCycleT);
	runDist[1] = runDist[2] = catchableArea/.5f * 0.8f - catchableArea;

	// uz len staci najst ten spravny bod, aby som kryl mozne strely
	// a aby ten bod bol co najblizsie k celkom dobrej bulharskej hodnote
	Point T1, T2;
	float minDist;
	Point goal(-52.5f, 0);
	Vector goal2ball = world->ball->pos - goal;
	Point target = goal + goal2ball * .44f;

	tVect.SetAngle(tVect.GetAngle() - bVect.GetAngle());
	bVect += tVect.GetAngle() / 2;	// Rotate
	fScale = (defPos - ballPos).GetX() / bVect.GetX();
	bVect *= fScale;				// ScaleXY
	ballPos += bVect;
	if (ballPos.GetY() < -world->params->server->goal_width / 2 - 2) ballPos.SetY(-world->params->server->goal_width / 2 - 2);
	else if (ballPos.GetY() > world->params->server->goal_width / 2 + 2) ballPos.SetY(world->params->server->goal_width / 2 + 2);
	target.SetX(defPos.GetX());
	target.SetY(ballPos.GetY());
	if(fabsf(world->ball->pos.GetX()) > 43)
	{
		target.SetX(target.GetX() - (target.GetX() - goal.GetX()) / 2);
	}

	//	target = world->me->pos;
	minDist = 1000;
	minc = -1;
	// vyskusame, ci pri behu rovno budeme mat vsetko vykryte 
	// tym vyskusame aj terajsiu poziciu
/*
	for(cycle = 0; cycle < minCycle; cycle++)
	{
		for(cycleB = 1; cycleB < maxCycleB; cycleB++)
		{
			if((futureMePos[cycle] - futBposB[cycleB]).GetLength() < runDist[cycleB] + catchableArea)
				for(cycleT = 1; cycleT < maxCycleT; cycleT++)
				{
					if((futureMePos[cycle] - futBposT[cycleB]).GetLength() < runDist[cycleT] + catchableArea)
						if(InPenaltyArea(futureMePos[cycle]))
						{
							homepos = futureMePos[cycle];
							minc = cycle;
							minDist = (futureMePos[cycle] - target).GetLength();
							break;
						}
				}
			if(minc > -1) break;
		}
		if(minc > -1) break;
	}
*/
//	DBGN("%d: uhol %.2f", GetSimTime(), fabsf(goal2ball.GetAngle()));
	// najdeme bod, kde bude vsetko vykryte
	for(cycleB = 1; cycleB < maxCycleB; cycleB++)
	{
		for(cycleT = 1; cycleT < maxCycleT; cycleT++)
		{
			if(CirclesIntersection(
				futBposB[cycleB], runDist[cycleB] + catchableArea,
				futBposT[cycleT], runDist[cycleT] + catchableArea,
				T1, T2) != 0
				)
			{
				if(InPenaltyArea(T1) && 
					(fabsf(T1.GetX()) < 47 || 
					 DistanceToGoal(world->ball->pos) < 13 ||
					 (goal2ball.GetAngle().GetAbsDeg() > 40 && goal2ball.GetAngle().GetAbsDeg() < 140)
					) &&
					minDist > (target - T1).GetLength() &&
					(goal - T1).GetLength() < 17) 
				{
					minDist = (target - T1).GetLength();
					homepos = T1;
				}
				if(InPenaltyArea(T2) &&
					(fabsf(T2.GetX()) < 47 || 
					 DistanceToGoal(world->ball->pos) < 13 ||
					 (goal2ball.GetAngle().GetAbsDeg() > 40 && goal2ball.GetAngle().GetAbsDeg() < 140)
					) &&
					minDist > (target - T2).GetLength() &&
					(goal - T2).GetLength() < 17) 
				{
					minDist = (target - T2).GetLength();
					homepos = T2;
				}
			}
		}
	}
	if(minDist == 1000 && fabsf(world->ball->pos.GetX()) > 42.5 )
	{
		homepos = GetNearestBallInterceptPos();
		if(homepos.GetX() == -1000) // What to do now???
		{
			homepos = world->ball->pos;
		}
	}
	if(!InPenaltyArea(homepos)) homepos = target;
/*
// BEGIN - Roztoce
	tVect.SetAngle(tVect.GetAngle() - bVect.GetAngle());
	bVect += tVect.GetAngle() / 2;	// Rotate
	fScale = (defPos - ballPos).GetX() / bVect.GetX();
	bVect *= fScale;				// ScaleXY
	ballPos += bVect;
	if (ballPos.GetY() < -world->params->server->goal_width / 2 - 2) ballPos.SetY(-world->params->server->goal_width / 2 - 2);
	else if (ballPos.GetY() > world->params->server->goal_width / 2 + 2) ballPos.SetY(world->params->server->goal_width / 2 + 2);
	homepos.SetX(defPos.GetX());
	homepos.SetY(ballPos.GetY());
// END - Roztoce
*/
/*
	DBGN("%d:", GetSimTime());
	DBG("d={%.2f", runDist[0]);
	for(cycle = 1; cycle < max(maxCycleB, maxCycleT); cycle++)
		DBG(",%.2lf", runDist[cycle]);
	DBGN("};");
	DBG("B={{%.2f,%.2f}", futBposB[0].GetX(), futBposB[0].GetY());
	for(cycle = 1; cycle < maxCycleB; cycle++)
		DBG(",{%.2f,%.2f}", futBposB[cycle].GetX(), futBposB[cycle].GetY());
	DBGN("};");
	DBG("T={{%.2f,%.2f}", futBposT[0].GetX(), futBposT[0].GetY());
	for(cycle = 1; cycle < maxCycleT; cycle++)
		DBG(",{%.2f,%.2f}", futBposT[cycle].GetX(), futBposT[cycle].GetY());
	DBGN("};");
*/
	if(world->state->GetPlayMode() != PM_PlayOn)
	{
		homepos.SetY(homepos.GetY() - world->ball->pos.GetY() / 8.f);
	}
//	TXTN("homepos (%.2f,%.2f)", homepos.GetX(), homepos.GetY());
	return homepos;
}

Point GoalieTactics::GetKickOffPos() const
{
	Point kickOffPos(-37.f, STD_POS_Y);
	int iCycles;
	int iPlayer;
	int iOppDist = 0;
	float fY = 0.0;

	for (int i = -(int)(world->params->server->goal_width / 2);
		i <= (int)(world->params->server->goal_width / 2); i++)
	{
		kickOffPos.SetY((float) i);
		iPlayer = GetNearestEnemyTo(kickOffPos, &iCycles);
		if (iPlayer == -1) iCycles = 50;
		if (iCycles > iOppDist) {
			iOppDist = iCycles;
			fY = kickOffPos.GetY();
		}
	}
	kickOffPos.SetY(fY);

	return kickOffPos;
}

int GoalieTactics::NearFriend(int* pCycles) const
{
	float psteps;
	Ball tmpBall(*world->ball);
	Vector diff;

	const float mykarea = world->me->type->player_size + world->params->server->ball_size + world->me->type->kickable_margin;
	float kareas[12];
	float speeds[12];
	for(unsigned k=0;k < 12;k++)
	{
		kareas[k] = world->params->server->ball_size + world->friendTypes[k]->player_size + world->friendTypes[k]->kickable_margin;
		speeds[k] = world->friendTypes[k]->player_speed_max;
	}

	for(int ncycles = 1;;ncycles++)
	{
		tmpBall.Step();
		diff = tmpBall.pos - world->me->pos;

		psteps = (diff.GetLength() - mykarea) / world->me->type->player_speed_max;
		if(psteps < ncycles)
		{
			*pCycles = ncycles;
			return -1;
		}

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

			psteps = (diff.GetLength() - kareas[world->friends[i].GetUniform()])/ speeds[world->friends[i].GetUniform()];
			if(psteps < ncycles) {
				*pCycles = ncycles;
				return i;
			}
		}
	}
}

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

	float psteps;
	Ball tmpBall(*world->ball);
	Vector diff;
	const float karea = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;

	for(int ncycles = 1;;ncycles++)
	{
		tmpBall.Step();

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

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

int GoalieTactics::GetNearestEnemyTo(const Point& pos, int* pCycles) const
{
	if(world->enemies.GetSize() == 0) return -1;

	float psteps;
	Vector diff;
	const float karea = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;

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

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

int GoalieTactics::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 superovej 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
			if(distToFriend <= 30 && distToFriend > 5)
			{
				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 k superovej brane
				}
			}
		}
	}

	return best;
}

bool GoalieTactics::InTheMiddle(const Point& tested, const Point& p1, const Point& p2) const
{
	const float distP1P2 = (p1 - p2).GetLength();

	if ((tested - p1).GetLength() > distP1P2) return false;
	if ((tested - p2).GetLength() > distP1P2) return false;

	return true;
}

#if 0
bool GoalieTactics::DependentForBall() const
{
	int ourc;
	int theirc;
	int closestFriend = NearFriend(&ourc);
	int enemy = NearEnemy(&theirc);

	if(closestFriend == -1) return true;
	else
	{
		//nas tam bude skor
		if(enemy == -1 || theirc > ourc) return false;
		else
		{
			//medzi nasim a loptou je cudzi
			if(InTheMiddle(world->enemies[enemy].pos, world->friends[closestFriend].pos, world->ball->pos))
			{
				//najblizsi cudzi nie je medzi mnou a loptou
				bool dependent = !InTheMiddle(world->enemies[enemy].pos, world->me->pos, world->ball->pos);
				//nikto od nas nie je medzi mnou a loptou
				for(int i = 0; i < world->friends.GetSize() && dependent; i++)
				{
					if(InTheMiddle(world->friends[i].pos, world->me->pos, world->ball->pos))
						dependent = false;
				}
				return dependent;
			}
			else return false;
		}
	}
}
#endif // 0

float GoalieTactics::DistanceToGoal(const Point& pos, bool bOur) const
{
	const float posX = bOur ? -52.5f : 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 GoalieTactics::NoEnemiesAround() const
{
	for (int i = 0; i < world->enemies.GetSize(); i++)
	{
		if ((world->enemies[i].pos - world->me->pos).GetLength() < 5.0f) return false;
	}

	return true;
}

bool GoalieTactics::SmartDribleTo(const Point& pos)
{
	int who;
	const float karea = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;
	for (who = 0; who < world->enemies.GetSize(); who++)
	{
		//protihrac uz moze kopnut do lopty
		if ((world->ball->pos - world->enemies[who].pos).GetLength() <= karea) 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() > 5) 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 GoalieTactics::BallHeadingOnOurGoal(float tolerance)
{
	static float t;
	static float len, spdl;
	static float intersectGoalY;
	static Point ourGoal;

	ourGoal = world->params->flags[GOAL_L];

	t = (ourGoal.GetX() - world->ball->pos.GetX()) / world->ball->vel.GetX();
	intersectGoalY = world->ball->pos.GetY() + world->ball->vel.GetY() * t;
	if(t > 0 && intersectGoalY > -tolerance && intersectGoalY < tolerance)
	{
		len = (world->ball->pos - Point(ourGoal.GetX(), intersectGoalY)).GetLength();
		spdl = world->ball->vel.GetLength() * world->params->server->ball_decay;
		while(len > 0 && spdl > SPEED_ON_GOAL_LINE_MIN)
		{
			len -= spdl;
			spdl *= world->params->server->ball_decay;
		}

		if(len <= 0) return true;
	}

	return false;
}

/// Simulate ball future positions for 30 cycles
/// If ball ever reaches our penalty area and still has speed -> it's a pass
bool GoalieTactics::IsPassNearGoal()
{
	int cycle;
	Ball tmpBall(*world->ball);

	for(cycle = 0; cycle < 30; cycle++)
	{
		if(tmpBall.vel.GetLength() < world->params->server->ball_speed_max * 0.3) return false;
		if(InPenaltyArea(tmpBall.pos)) return true;
		tmpBall.Step();
	}

	return false;
}

Point GoalieTactics::SolvePassNearGoal()
{
	static Point res;
	if(CanInterceptBall(&res)) res = GetNearestBallInterceptPos();
	else res.SetX(-1000);
	if(res.GetX() == -1000)
	{
/*
		res.SetX(world->me->pos.GetX());
		res.SetY(world->me->pos.GetY() + world->ball->vel.GetY() * 2);
*/
		List<CatchSimData> data;
		CatchSimBallPath(&data);
		float karea = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;
		int i, c, enemyWillCatchIn;

		c = 0;
		enemyWillCatchIn = -1;
		LIST_FOR_EACH(CatchSimData, cycle, data)
		{
			for(i = 0; i < cycle->data->GetEnemyCnt(); i++)
			{
				if(cycle->data->GetEnemyDif(i) <= karea)
				{
					enemyWillCatchIn = c;
					break;
				}
			}
			if(enemyWillCatchIn > -1) break;
			c++;
		}

		Ball tmpBall(*world->ball);

		if(enemyWillCatchIn > -1)
		{
			DBGN("Enemy will catch it in %d. cycle", enemyWillCatchIn);
			for(i = 0; i < enemyWillCatchIn; i++)
				world->ball->Step();
			DBGN("  At position (%.2f, %.2f)", world->ball->pos.GetX(), world->ball->pos.GetY());
		}
		else
		{
			for(i = 0; i < 8; i++)
				world->ball->Step();
		}

		res = GetDefensePos();
		*world->ball = tmpBall;
	}
	return res;
}

