#include "Player/PlayerSkills.h"
#include "Player/PlayerTactics.h"
#include "Common/Main.h"
#include "Common/Tasks.h"
#include "Common/Support/Debug.h"
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <string.h>
#include <limits.h>

#define BALL_POS_TOL 5.0f
#define DRIBBLE_STOP_DIST 10.0f

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

static Angle CalcDir(const Vector& vector, Angle rotation)
{
	Vector tmp = vector - rotation;
	return tmp.GetAngle();
}




/// How many cycles after catching the ball do we continue simulation?
/// \author Matus Horvath
#define AFTER_CATCH_CYCLES 5
#define AFTER_CATCH_DRIBBLE_CYCLES 2

/// Compensate for the error in percepted ball position
#define POS_ERROR_DELTA 3

/// Simulate a player for one cycle. Input player speed and distance
/// run until now and dash power in next cycle. Output new speed
/// and cumulated distance. Used for catching ball.
/// \author Matus Horvath
void PlayerSkills::CatchSimRunningPlayer(float dashPower, float* speed, float* distance)
{
	float acceleration = dashPower * world->params->server->dash_power_rate;
	if(acceleration > world->params->server->player_accel_max)
		acceleration = world->params->server->player_accel_max;

	*speed += acceleration;
	if(*speed > world->params->server->player_speed_max)
		*speed = world->params->server->player_speed_max;

	*distance += *speed;
	*speed *= world->params->server->player_decay;
}



/// \author Matus Horvath
void PlayerSkills::CatchSimBallPath(List<CatchSimData>* output, Ball* lopta, int playerNum, bool dribble, int skip)
{
	int i;
	Ball tmpBall(*lopta);

	bool ourKickIn = false;
	if(world->state->IsOurKickIn()) ourKickIn = true;

	// Supposed player speeds
	//FIX
	//const float myDashPower = (dribble ? DIDdashpower : RecommDashPower(true)) * (playerNum==-1 ? world->me->GetEffort() : 1);
	const float myDashPower = 100.0f;

	const float friendDashPower = 
		world->variables->GetFriendDashAdjust() * (world->params->server->stamina_inc_max + world->params->server->maxpower) / 2;
	const float enemyDashPower = 
		world->variables->GetEnemyDashAdjust() * world->params->server->maxpower;

	
	Player* player = (playerNum == -1 ? world->me : &world->friends[playerNum]);


	// Number of known players
	int friendCnt = world->friends.GetSize();
	int enemyCnt = world->enemies.GetSize();

	// Current distance run by all players.
	float myDist = 0;
	
	float* friendDists = (float*) malloc(sizeof(float) * friendCnt);
	for(i = 0; i < friendCnt; i++) friendDists[i] = 0;
	
	float* enemyDists = (float*) malloc(sizeof(float) * enemyCnt);
	for(i = 0; i < enemyCnt; i++) enemyDists[i] = 0;
	
	// Current speeds of all players
	float mySpd;

	mySpd = player->vel.GetLength() * player->type->player_decay;
	
	
	float* friendSpds = (float*) malloc(sizeof(float) * friendCnt);
	for(i = 0; i < friendCnt; i++) friendSpds[i] = world->friends[i].vel.GetLength();
	
	float* enemySpds = (float*) malloc(sizeof(float) * enemyCnt);
	for(i = 0; i < enemyCnt; i++) 
	{
		if(((world->enemies[i].pos - player->pos).GetLength() < world->params->server->visible_distance)
			|| (world->enemies[i].GetAge() != 0))
			enemySpds[i] = 0.5f * world->params->server->player_speed_max;
		else
			enemySpds[i] = world->enemies[i].vel.GetLength();
	}

	// For each cycle and each player, store his distance from ball.
	// For each cycle, store ball's position.
	// We do not know number of cycles ahead.
	
	// Simulate ball path. Finish after we catch the ball or when it leaves
	// the playground. When we catch ball, simulate some more cycles to see
	// if someone else catches it at the same time.
	
	int caught = -1;
	int cycle = 0;

	float hack_firstCycleDistNoTurn = 0.f;
	float hack_firstCycleDistWithTurn = 0.f;

	CATCH_DBGN("CATCHSIMBALLPATH ----------------------------------------------");
	CATCH_DBGN("\tPociatocna pozicia lopty je %f, %f", tmpBall.pos.GetX(), tmpBall.pos.GetY());

	while(true)
	{
		cycle++;
		tmpBall.Step();

		CATCH_DBGN("\tPozicia lopty v cykl %d je %f, %f", cycle, tmpBall.pos.GetX(), tmpBall.pos.GetY());
		// Save ball position in this cycle
		CatchSimData* data = new CatchSimData(friendCnt, enemyCnt);
		data->ballPos = tmpBall.pos;


		// For each player, compute the distance he could run in this cycle.
		// Compensate turning in first cycle, if appropriate.
		// Then compute player's distance from ball.

		// Me
		if(cycle == 1)
		{
			// Compute first cycle with and without turning
			CatchSimRunningPlayer(myDashPower, &mySpd, &hack_firstCycleDistNoTurn);
			CatchSimRunningPlayer(0, &mySpd, &hack_firstCycleDistWithTurn);
		}
		else
		{
			CatchSimRunningPlayer(myDashPower, &mySpd, &myDist);
		}

		// Where will I be, if I do not turn in the first cycle?
		Point noTurnPos = player->pos + PolarVector(hack_firstCycleDistNoTurn + myDist, player->GetAbsDir());

		float ballDiffNoTurn = (tmpBall.pos - noTurnPos).GetLength();
		//if(DoLinesIntersect(player->pos, noTurnPos - player->pos, tmpBall.pos, tmpBall.vel)) ballDiffNoTurn = -ballDiffNoTurn;
		
		float ballDiffWithTurn = (tmpBall.pos - player->pos).GetLength() - myDist - hack_firstCycleDistWithTurn;
		if(ballDiffWithTurn<0) ballDiffWithTurn = -ballDiffWithTurn;
	
		CATCH_DBGN("\tMoje vzdialenosti od lopty noturn %f     withturn %f", ballDiffNoTurn, ballDiffWithTurn); 

#define SOME_DELTA world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin
		
		bool dbgWhichOne;
		
		if(ballDiffNoTurn < SOME_DELTA)
		{
			// We do not have to turn
			dbgWhichOne = true;
			data->ballPos = noTurnPos;
			data->SetMyDif(ballDiffNoTurn);
		}
		else
		{
			// We have to turn
			dbgWhichOne = false;
			data->SetMyDif(ballDiffWithTurn);
		}

		// Check if we have caught the ball
		if(data->GetMyDif() <= SOME_DELTA && caught == -1 && cycle>skip)
		{
			if(playerNum == -1)
				CATCH_DBGN("  SIMULATION: I caught the ball (%3.2f, %3.2f) after %i", data->ballPos.GetX(), data->ballPos.GetY(), cycle);
			else
				CATCH_DBGN("  SIMULATION: Player %i caught the ball (%3.2f, %3.2f) after %i", playerNum, data->ballPos.GetX(), 
							data->ballPos.GetY(), cycle);
			
			CATCH_DBGN("  without turn: I am at (%3.2f, %3.2f), diff is %3.2f", noTurnPos.GetX(), noTurnPos.GetY(), ballDiffNoTurn);
			CATCH_DBGN("  with turn: diff is %3.2f", ballDiffWithTurn);
			CATCH_DBGN("  using: %s, cond %s: %3.2f < %3.2f", dbgWhichOne ? "no turn" : "with turn", dbgWhichOne ? "TRUE" : "FALSE", ballDiffNoTurn, SOME_DELTA);
			caught = 0;
		}


		// Teammates
		//if(friendCnt > 10) DBGN("friends %i", friendCnt);
		for(i = 0; i < friendCnt; i++)
		{
			if(playerNum == i)
			{
				data->SetFriendDif(i, 1000);
				continue;
			}

			CatchSimRunningPlayer(friendDashPower, &friendSpds[i], &friendDists[i]);
			data->SetFriendDif(i, (tmpBall.pos - world->friends[i].pos).GetLength() - friendDists[i]);
		}


		// Enemies
		//if(count > 11) DBGN("enemies %i", count);
		for(i = 0; i < enemyCnt; i++)
		{
			CatchSimRunningPlayer(enemyDashPower, &enemySpds[i], &enemyDists[i]);
			data->SetEnemyDif(
				i, (tmpBall.pos - world->enemies[i].pos).GetLength() - enemyDists[i]);
		}

		output->Add(data);

	
		// If the ball is out, ignore it, unless it's one of the OurKickIn modes.
		if(!ourKickIn &&
			((tmpBall.pos.GetX() < world->params->flags[LT].GetX() - POS_ERROR_DELTA) ||
			(tmpBall.pos.GetY() < world->params->flags[LT].GetY() - POS_ERROR_DELTA) ||
			(tmpBall.pos.GetX() > world->params->flags[RB].GetX() + POS_ERROR_DELTA) ||
			(tmpBall.pos.GetY() > world->params->flags[RB].GetY() + POS_ERROR_DELTA)))
		{
//			CATCH_DBGN("  ignoring ball - out");
			break;
		}
		
		// Did we already catch the ball and simulate AFTER_CATCH_CYCLES cycles more?
		if(caught >= AFTER_CATCH_CYCLES) break;
		// If we already have the ball, increase the counter. If we do not have
		// the ball, caught is -1. When we catch it, we set caught to 0.
		if(caught >= 0) caught++;
	}

	free(friendDists); free(enemyDists);
	free(friendSpds); free(enemySpds);
}


/// \author Matus Horvath
bool PlayerSkills::CatchEvaluateData(Point* point, List<CatchSimData>* input, bool dribble, int skip)
{
	// TODO: different kickable areas for different heteroplayers
	const float karea = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;

	// Should our side kick the ball to game?
	// If yes, ignore enemies - they cannot kick the ball
	bool ourKickIn = false;
	if(world->state->IsOurKickIn()) ourKickIn = true;
//	CATCH_DBGN("  ourKickIn = %s", ourKickIn ? "true" : "false");
	
	int friendCaught = 0, enemyCaught = 0, meCaught = 0;
	float friendBalance = 0.f, enemyBalance = 0.f, meBalance = 0.f;

	int catchTime = INT_MAX;
	int currTime = 1;


	int i;

	// Which players are already near the ball
	bool* friendGotBall = (bool*) malloc(input->GetFirst()->data->GetFriendCnt() * sizeof(bool));
	for(i = 0; i < input->GetFirst()->data->GetFriendCnt(); i++) friendGotBall[i] = false;
	bool* enemyGotBall = (bool*) malloc(input->GetFirst()->data->GetEnemyCnt() * sizeof(bool));
	for(i = 0; i < input->GetFirst()->data->GetEnemyCnt(); i++) enemyGotBall[i] = false;

	// Process simulation data
	LIST_FOR_EACH(CatchSimData, cycle, *input)
	{
		for(i = 0; i < cycle->data->GetFriendCnt(); i++)
		{
			if(!friendGotBall[i] && cycle->data->GetFriendDif(i) <= karea)
			{
				friendCaught++; friendBalance++;
				friendGotBall[i] = true;
				CATCH_DBGN("  friend %i caught at %i: caught %i balance %3.2f", world->friends[i].GetUniform(), currTime, friendCaught, friendBalance);
				
				if(catchTime == INT_MAX)
				{
					catchTime = currTime;
					CATCH_DBGN("  catchTime = %i", catchTime);
				}
			}
		}

		if(!ourKickIn) for(i = 0; i < cycle->data->GetEnemyCnt(); i++)
		{
			if(!enemyGotBall[i] && cycle->data->GetEnemyDif(i) <= karea)
			{
				enemyCaught++; enemyBalance++;
				enemyGotBall[i] = true;
				CATCH_DBGN("  enemy %i caught at %i: caught %i balance %3.2f", world->enemies[i].GetUniform(), currTime, enemyCaught, enemyBalance);

				if(catchTime == INT_MAX)
				{
					catchTime = currTime;
					CATCH_DBGN("  catchTime = %i", catchTime);
				}
			}
		}

		if((cycle->data->GetMyDif() <= karea)&&(currTime > skip))
		{
			*point = cycle->data->ballPos;
			meCaught++; meBalance++;
			CATCH_DBGN("  me caught at %i, BREAK", currTime);
			
			if(catchTime == INT_MAX)
			{
				catchTime = currTime;
				CATCH_DBGN("  catchTime = %i", catchTime);
			}

			break;
		}

		// Too much time passed since first player caught the ball
		if(currTime - catchTime >= (dribble ? AFTER_CATCH_DRIBBLE_CYCLES : AFTER_CATCH_CYCLES))
		{
			//if(meCaught == 0) *point = cycle->data->ballPos;
			CATCH_DBGN("  currTime = %i, catchTime = %i, BREAK", currTime, catchTime);
			break;
		}
		// Too many players have caught the ball
//		if(friendCaught > GetCoachAdjParams()->GetFriendMaxCatchers()) break;
//		if(enemyCaught > GetCoachAdjParams()->GetEnemyMaxCatchers()) break;

		friendBalance *= world->variables->GetFriendBallCtrlBonus();
		enemyBalance *= world->variables->GetEnemyBallCtrlBonus();
		meBalance *= world->variables->GetFriendBallCtrlBonus();
		
		currTime++;
	}

	TXTN("  SUMMARY meCaught: %i, friendCaught: %i, enemyCaught: %i", meCaught, friendCaught, enemyCaught);

	CATCH_DBGN("  SUMMARY:");
	CATCH_DBGN("  meCaught: %i, friendCaught: %i, enemyCaught: %i", meCaught, friendCaught, enemyCaught);
	CATCH_DBGN("  meBalance: %3.2f, friendBalance: %3.2f, enemyBalance: %3.2f", meBalance, friendBalance, enemyBalance);
	CATCH_DBGN("");

	/*int dbgt = 0;
	LIST_FOR_EACH(CatchSimData, cycle2, *input)
	{
		dbgt++;
		if(cycle2->data->GetMyDif() <= karea)
		{
			CATCH_DBGN("  just DBG: I would catch at %i", currTime);
			break;
		}
	}*/

	free(friendGotBall); free(enemyGotBall);

	if(meCaught == 0) return false;

	if(dribble==true)
	{
		if((friendBalance==0)&&(enemyBalance==0))
			return true;
		else
			return false;
	}



	// I can quite surely catch the ball, should I?
	// Who would control the ball if I ignore it?
	float balance = friendBalance - enemyBalance;
	CATCH_DBGN("  balance: %3.2f", balance);

	if(!ourKickIn)
	{
		if(balance + meBalance < -world->variables->GetTooMuchEnemyCnt())
		{
			// Too many enemies. 
			CATCH_DBGN("  RESULT: Too many enemies");
			CATCH_DBGN("  IGNORE");
			return false;
		}
		else if(balance < world->variables->GetTooMuchFriendCnt())
		{
			// It's almost fifty-fifty.
			CATCH_DBGN("  RESULT: Fifty-fifty");
		}
		else
		{
			// Too many friends. No need to add more.
			CATCH_DBGN("  RESULT: Too many friends");
			CATCH_DBGN("  IGNORE");
			return false;
		}
	}

	// If we get there, we are catching the ball.
	CATCH_DBGN("  CATCHING at (%3.2f, %3.2f)", point->GetX(), point->GetY());
	return true;
}

/// Find out if we should intercept the ball, if yes,
/// find out where should we run
/// \author Matus Horvath
bool PlayerSkills::CanInterceptBall(Point* point)
{
	CATCH_DBGN("---> enter CanInterceptBall at %i", world->state->GetSimTime());

	if(world->state->GetPlayMode() != PM_PlayOn)
	{
		if(ShouldKickIn()) return true;
		return false;
	}

	Ball tmpBall(*world->ball);

	// Simulate ball path
	List<CatchSimData> data;
	CatchSimBallPath(&data, &tmpBall);

	// Evaluate the results of simulation
	bool retval = CatchEvaluateData(point, &data, false);

	CATCH_DBGN("<--- exit CanInterceptBall");
	
	return retval;
}


bool PlayerSkills::CanInterceptBall(Point* point, Ball* lopta, int playerNum, bool dribble, int skip)
{
	//CATCH_DBGN("---> enter CanInterceptBall at %i", world->state->GetSimTime());

	if(world->state->GetPlayMode() != PM_PlayOn)
	{
		if(ShouldKickIn()) return true;
		return false;
	}

	// Simulate ball path
	List<CatchSimData> data;
	CatchSimBallPath(&data, lopta, playerNum, dribble, skip);

	// Evaluate the results of simulation
	bool retval = CatchEvaluateData(point, &data, dribble, skip);

	//CATCH_DBGN("<--- exit CanInterceptBall");
	
	return retval;
}


// vrati cislo hraca ktory prave kontroluje loptu, ak je ich viacej tak -1 (ci uz spoluhraci alebo protihraci), seba nepocitam
int PlayerSkills::FriendPlayerWithBall()
{
	int playerNum = -1;
	
	const float kareae = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;
	const float kareaf = world->params->server->player_size + world->params->server->ball_size;

	int friendCnt = world->friends.GetSize();
	int enemyCnt = world->enemies.GetSize();
	Point ball_pos = world->ball->pos;

	for(int i=0;i<friendCnt;i++)
	{
		if((ball_pos - world->friends[i].pos).GetLength() < (kareaf + world->friends[i].type->kickable_margin))
		{
			if(playerNum == -1) playerNum = i; else return -1;
		}
	}

	if(playerNum == -1) return -1;

	for(int j=0;j<enemyCnt;j++)
	{
		if((ball_pos - world->enemies[j].pos).GetLength() < kareae)
		{
			return -1;
		}
	}

	return playerNum;
}


int PlayerSkills::CyclesDistance(Point distTo)
{
	float karea = world->params->server->player_size + world->params->server->ball_size + world->params->server->kickable_margin;

	int enemyCnt = world->enemies.GetSize(); 
	float* enemyDists = (float*) malloc(sizeof(float) * enemyCnt);

	for(int i = 0; i < enemyCnt; i++) enemyDists[i] = -((distTo - world->enemies[i].pos).GetLength() - karea);
	
	for(int cycle=0;cycle<8;cycle++)
	{
		for(i = 0; i < enemyCnt; i++)
		{
			//TXTN("CyclesDistance : protihrac %i, cyklus %d, vzdialenost %f", world->enemies[i].GetUniform(), cycle, enemyDists[i]);
			if(enemyDists[i]>0) return cycle; 
		
			enemyDists[i] += world->params->server->player_speed_max;
		}
	}
	
	return 8;
}

