/**
 * \file World.h
 * \date 14.04.2003
 *
 * World model
 */

#include "Player/World/World.h"
#include "Player/PlayerKernel.h"
#include "Common/Main.h"
#include "Common/Tasks.h"
#include "Common/Support/Debug.h"
#include "Common/World/Circle.h"
#include <string.h>


#define MAX_PLAYER_AGE 15
	

SynchronizationInterface* World::synchro = NULL;


World::World()
{
	memset((void*) friendTypes, 0, 12 * sizeof(PLAYER_TYPE*));

	ball = new Ball(this);
	state = new State(this);
	variables = new Variables(this);
	params = new Params(this);
	me = new Myself(this);
	log = new Log("world");
}

World::World(const World& world)
{
	ball = new Ball(*world.ball); ball->SetWorld(this);
	me = new Myself(*world.me); me->SetWorld(this);
	state = new State(*world.state); state->SetWorld(this);
	params = new Params(*world.params); params->SetWorld(this);
	variables = new Variables(*world.variables); variables->SetWorld(this);


	int i;
	for(i = 0; i < world.friends.GetSize(); i++)
	{
		friends.Add(world.friends[i]);
		friends[i].SetWorld(this);
	}
	for(i = 0; i < world.enemies.GetSize(); i++)
	{
		enemies.Add(world.enemies[i]);
		enemies[i].SetWorld(this);
	}

	memcpy((void*) friendTypes, (void*) world.friendTypes, 12 * sizeof(PLAYER_TYPE*));
}

void World::BindInterfaces()
{
	synchro = (SynchronizationInterface*) Main::GetInstance()->GetInterface(SYNCHRONIZATION_TASK);
}

int World::FindFriendIndex(int uniform) const
{
/*	if(uniform == 0) return -1;

	for(int i = 0; i < friends.GetSize(); i++)
	{
		if(friends[i].GetUniform() == uniform) return i;
	}

	return -1;
*/
	return friends.FindByUniformIndex(uniform);
}

int World::FindEnemyIndex(int uniform) const
{
/*	if(uniform == 0) return -1;

	for(int i = 0; i < enemies.GetSize(); i++)
	{
		if(enemies[i].GetUniform() == uniform) return i;
	}

	return -1;
	*/
	return enemies.FindByUniformIndex(uniform);
}

//DEL Player* World::FindFriend(int uniform) const
//DEL {
//DEL 	int index = FindFriendIndex(uniform);
//DEL 	
//DEL 	if(index == -1) return NULL;
//DEL 	else return &friends[index];
//DEL }

//DEL Player* World::FindEnemy(int uniform) const
//DEL {
//DEL 	int index = FindEnemyIndex(uniform);
//DEL 	
//DEL 	if(index == -1) return NULL;
//DEL 	else return &enemies[index];
//DEL }

void World::ProcessInfo(const SENSE_BODY* sb, const VISUAL_INFO* vi)
{
	WORLD_RECALC_DBG("---> enter ProcessInfo at %i", state->GetSimTime());

	if(me->Recalculate(sb, vi))
	{
		WORLD_RECALC_DBG("  me: recalc ok, pos: (%3.2f, %3.2f), vel (%3.2f, %3.2f)", 
                me->pos.GetX(), me->pos.GetY(), me->vel.GetX(), me->vel.GetY());
		WORLD_RECALC_DBG("  me: viewDir %3.2f absDir %3.2f neckDir %3.2f", 
                me->GetAbsDir().GetDeg() + me->GetNeckDir().GetDeg(), 
                me->GetAbsDir().GetDeg(), me->GetNeckDir().GetDeg());


		// I know my position. Recalculate the ball, if we can.
		if(vi->ballCnt != 0 && vi->ball.distValid)
		{
			WORLD_RECALC_DBG("  ball: I see ball");
	
			// I see the ball
			if(ball == NULL) ball = new Ball(this);
			ball->Recalculate(sb, vi);
		
			// We see the ball: change synchronization mode from WaitForVI to Normal.
			if(synchro->GetNextSynchMode() == Synch_WaitForVI)
				synchro->SetNextSynchMode(Synch_Normal);
		}
		else
		{
			WORLD_RECALC_DBG("  ball: I do NOT see the ball");

			// I do not see the ball, simulate it
			if(ball->IsValid() && me->IsLookingAt(ball->pos))
			{
				// I should see the ball, but do not
				ball->SetValid(false);
				ball->vel = RectVector(0, 0);

				WORLD_RECALC_DBG("  ball: simulate: I should SEE ball");
			}

			if(ball->IsValid())
			{
				// I should feel the ball (it should be near me) but do not
				Vector diff = ball->pos - me->pos;
				if(diff.GetLength() <= params->server->visible_distance)
				{
					ball->SetValid(false);
					ball->vel = RectVector(0, 0);

					WORLD_RECALC_DBG("  ball: simulate: I should FEEL ball");
				}
			}

			WORLD_RECALC_DBG("  ball: SIMULATE: pos: (%f, %f), vel (%f, %f)", ball->pos.GetX(), ball->pos.GetY(), ball->vel.GetX(), ball->vel.GetY());
		}
		
		// Recalculate the players
		RecalculateFriends(sb, vi);
		RecalculateEnemies(sb, vi);
		
		// Save the time and position of my last VI
		me->SaveLastSeenData();
	}
	else
	{
		WORLD_RECALC_DBG("  me: recalc BAD, pos: (%f, %f), vel (%f, %f)", me->pos.GetX(), me->pos.GetY(), me->vel.GetX(), me->vel.GetY());

		// I do not know my position
		ball->SetValid(false);
		ball->vel = RectVector(0, 0);

		WORLD_RECALC_DBG("  ball: setting to invalid: pos: (%f, %f), vel (%f, %f)", ball->pos.GetX(), ball->pos.GetY(), ball->vel.GetX(), ball->vel.GetY());
	}
	int i;
	for (i=0; i<passableFriends.GetSize(); i++)
	{
		passableFriends[i].age++; 
	}
	
	for (i=0; i<passableFriends.GetSize(); i++)
	{
		if (passableFriends[i].age>variables->GetMaximumPassableFriendAge())
		{
			passableFriends.RemoveAt(i);
			i=0;
		}
	}
	

	WORLD_RECALC_DBG("<--- exit ProcessInfo");
}
	
void World::RecalculateFriends(const SENSE_BODY* sb, const VISUAL_INFO* vi)
{
	int i;

	// I am looking this way
	Angle viewDir = me->GetAbsDir() + me->GetNeckDir();

	// Remove old friends and friends who I should see
	for(i = friends.GetSize() - 1; i >= 0; i--)
	{
		if(friends[i].GetAge() >= MAX_PLAYER_AGE || me->IsLookingAt(friends[i].pos))
		{
			friends.RemoveAt(i);
			UpdatePassAbleIndexAfterDelete(i);
		}
	}
	
	// Add friends that I see
	for(i = 0; i < vi->oursCnt; i++)
	{
		if(!vi->ours[i].valid) DBGN("vi->ours[i].valid is false");

		SimPlayer player(this);

		// Compute player absolute position
		Angle angle = viewDir + DegAngle(vi->ours[i].dir);
		player.pos = me->pos + PolarVector(vi->ours[i].dist, angle);
		
		// Compute player speed
		if(vi->ours[i].changesValid)
		{
			// I see his speed
			// From TsinghuAeolus2002
			Vector rpos = PolarVector(vi->ours[i].dist, DegAngle(vi->ours[i].dir));
			rpos /= rpos.GetLength();

			Vector rvel = RectVector(
				vi->ours[i].distChange * rpos.GetX() - vi->ours[i].dirChange * M_PI / 180.f * vi->ours[i].dist * rpos.GetY(),
				vi->ours[i].distChange * rpos.GetY() + vi->ours[i].dirChange * M_PI / 180.f * vi->ours[i].dist * rpos.GetX());
				
			// Rotate rvel
			rvel += viewDir;
			player.vel = me->vel + rvel;

//			float dirCh = vi->ours[i].dirChange * M_PI / 180.f;
//			player.vel = me->vel - PolarVector(vi->ours[i].distChange, angle) - 
//				PolarVector(vi->ours[i].dist * dirCh, angle).OrthoLeft();
		}
		else
		{
			// Unknown speed, set to zero
			player.vel = RectVector(0, 0);
		}

		// Head and body direction
		if(vi->ours[i].facingValid)
		{
			player.SetAbsDir(viewDir + DegAngle(vi->ours[i].bodyFacingDir));
			player.SetNeckDir(viewDir + DegAngle(vi->ours[i].headFacingDir));
			player.SetDirValid();
		}
		
		// Arm direction
		if(vi->ours[i].armValid)
		{
			player.SetArmDir(viewDir + DegAngle(vi->ours[i].armDir));
			player.SetArmValid();
		}

		player.SetSide(OUR);
		player.SetUniform(vi->ours[i].uniformNumber);
		player.SetGoalie(vi->ours[i].goalie);
		player.SetAge(0);

		PLAYER_TYPE* type = friendTypes[player.GetUniform()];
		if(type != NULL) player.SetType(type->id);

		// If we already have such a player, replace him, otherwise add new		
		int index = FindFriendIndex(player.GetUniform());
		if(index == -1) friends.Add(player);
		else friends[index] = player;
	}
}

void World::RecalculateEnemies(const SENSE_BODY *sb, const VISUAL_INFO *vi)
{
	int i;

	// I am looking this way
	Angle viewDir = me->GetAbsDir() + me->GetNeckDir();

	// Remove old enemies and enemies who I should see
	for(i = enemies.GetSize() - 1; i >= 0; i--)
	{
		if(enemies[i].GetAge() >= MAX_PLAYER_AGE || me->IsLookingAt(enemies[i].pos))
			enemies.RemoveAt(i);
	}
	
	// Add enemies that I see
	for(i = 0; i < vi->theirCnt; i++)
	{
		if(!vi->their[i].valid) DBGN("vi->their[i].valid is false");

		SimPlayer player(this);

		// Compute player absolute position
		Angle angle = viewDir + DegAngle(vi->their[i].dir);
		player.pos = me->pos + PolarVector(vi->their[i].dist, angle);
		
		// Compute player speed
		if(vi->their[i].changesValid) //vidime rychlost
		{
			// I see his speed
			// From TsinghuAeolus2002
			Vector rpos = PolarVector(vi->their[i].dist, DegAngle(vi->their[i].dir));
			rpos /= rpos.GetLength();

			Vector rvel = RectVector(
				vi->their[i].distChange * rpos.GetX() - vi->their[i].dirChange * M_PI / 180.f * vi->their[i].dist * rpos.GetY(),
				vi->their[i].distChange * rpos.GetY() + vi->their[i].dirChange * M_PI / 180.f * vi->their[i].dist * rpos.GetX());
				
			// Rotate rvel
			rvel += viewDir;
			player.vel = me->vel + rvel;
			
//			float dirCh = vi->their[i].dirChange * M_PI / 180.f;
//			player.vel = me->vel - PolarVector(vi->their[i].distChange, angle) - 
//				PolarVector(vi->their[i].dist * dirCh, angle).OrthoLeft();
		}
		else
		{
			// Unknown speed, set to zero
			player.vel = RectVector(0,0);
		}

		// Head and body direction
		if(vi->their[i].facingValid)
		{
			player.SetAbsDir(viewDir + DegAngle(vi->their[i].bodyFacingDir));
			player.SetNeckDir(viewDir + DegAngle(vi->their[i].headFacingDir));
			player.SetDirValid();
		}

		// Arm direction
		if(vi->their[i].armValid)
		{
			player.SetArmDir(viewDir + DegAngle(vi->their[i].armDir));
			player.SetArmValid();
		}

		player.SetSide(THEIR);
		player.SetUniform(vi->their[i].uniformNumber);
		player.SetGoalie(vi->their[i].goalie);
		player.SetAge(0);
		
		// If we already have such a player, replace him, otherwise add new		
		int index = FindEnemyIndex(player.GetUniform());
		if(index == -1) enemies.Add(player);
		else enemies[index] = player;
	}

	// Add unknow players near me - the are considered enemies
	for(i = 0; i < vi->nearbyCnt; i++)
	{
		SimPlayer player(this);

		// Compute player absolute position
		Angle angle = viewDir + DegAngle(vi->nearby[i].dir);
		player.pos = me->pos + PolarVector(vi->nearby[i].dist, angle);

		// We do not hnow his speed - set it to zero
		player.vel = RectVector(0,0);

		player.SetSide(UNKNOWN);
		player.SetUniform(0);
		player.SetGoalie(false);
		
		// We leave these players in memory only until next VI
		player.SetAge(MAX_PLAYER_AGE);

		enemies.Add(player);
	}
}

void World::ApplyFeedback(FEEDBACK_DATA* fd)
{
	// Use recent commands stored in fb to modify the world.
	
	// Apply recent dash command
	if(fd->lastPower != 0)
	{
		// Calculate real dash energy
		float energy = fd->lastPower * me->type->dash_power_rate * me->GetEffort();
		if(energy > params->server->player_accel_max)
		{
			energy = params->server->player_accel_max;
		}
		else if(energy < -params->server->player_accel_max)
		{
			energy = -params->server->player_accel_max;
		}

		me->vel += PolarVector(energy, me->GetAbsDir());

		if(me->vel.GetLength() > me->type->player_speed_max)
		{
			me->vel *= (me->type->player_speed_max / me->vel.GetLength());
		}

		fd->lastPower = 0;
	}

	// Apply recent turn command
	if(fd->lastTurn != 0)
	{
		Angle angle = me->GetAbsDir() + DegAngle(fd->lastTurn / (1 + me->vel.GetLength() * me->type->inertia_moment));
		me->SetAbsDir(angle);

		fd->lastTurn = 0;
	}

	// Apply recent turn neck command
	if(fd->lastNeck != 0)
	{
		me->SetNeckDir(me->GetNeckDir() + DegAngle((float) fd->lastNeck));
		if(me->GetNeckDir().GetDeg() > params->server->maxneckang)
		{
			me->SetNeckDir(DegAngle(params->server->maxneckang));
		}
		else if(me->GetNeckDir().GetDeg() < params->server->minneckang)
		{
			me->SetNeckDir(DegAngle(params->server->minneckang));
		}

		fd->lastNeck = 0;
	}

	// Apply recent kick command
	if(fd->lastKickPower != 0)
	{
		Vector diff = ball->pos - me->pos;
		float ballDist = diff.GetLength();

		// If the ball is near enough
		if(ballDist < (me->type->player_size + me->type->kickable_margin + params->server->ball_size))
		{
			if(ballDist < (me->type->player_size + params->server->ball_size))
			{
				ballDist = me->type->player_size + params->server->ball_size;
			}

			float dirDiff = (diff.GetAngle() - me->GetAbsDir()).GetAbsDeg();
			
			float accel = fd->lastKickPower * params->server->kick_power_rate *
				(1 - 0.25f * (dirDiff / 180) - 0.25f * (ballDist - me->type->player_size - params->server->ball_size) / me->type->kickable_margin);

			if(accel > params->server->ball_accel_max)
			{
				accel = params->server->ball_accel_max;
			}
			else if(accel < -params->server->ball_accel_max)
			{
				accel = -params->server->ball_accel_max;
			}

			Angle kickDir = me->GetAbsDir() + DegAngle(fd->lastKickDir);
			
			ball->vel += PolarVector(accel, kickDir);

			if(ball->vel.GetLength() > params->server->ball_speed_max)
			{
				ball->vel *= params->server->ball_speed_max / ball->vel.GetLength();
			}
		}

		fd->lastKickPower = 0;
	}
}

void World::Step()
{
	int i;

	// Step myself
	me->Step();
	
	// Step the ball
	if(ball != NULL) ball->Step();

#ifdef DEBUG
	int dFa, dEa, dFd, dEd;
	int dFna, dEna, dFnd, dEnd;
	dFa = dEa = dFd = dEd = 0;
	dFna = dEna = dFnd = dEnd = 0;
#endif // DEBUG

	// Step friends
	for(i = 0; i < friends.GetSize(); i++)
	{
#ifdef DEBUG
		if(friends[i].IsArmValid()) dFa++; else dFna++;
		if(friends[i].IsDirValid()) dFd++; else dFnd++;
#endif // DEBUG

		friends[i].Step();
	}

	// Step enemies
	for(i = 0; i < enemies.GetSize(); i++)
	{
#ifdef DEBUG
		if(enemies[i].IsArmValid()) dEa++; else dEna++;
		if(enemies[i].IsDirValid()) dEd++; else dEnd++;
#endif // DEBUG

		enemies[i].Step();
	}

#ifdef DEBUG
	WORLD_STATS_DBG("--> Step at %i", state->GetSimTime());
	if(friends.GetSize() > 10) WORLD_STATS_DBG("too many friends: %i", friends.GetSize());
	if(enemies.GetSize() > 11) WORLD_STATS_DBG("too many enemies: %i", enemies.GetSize());

	WORLD_STATS_DBG("dFa: %i, dFna: %i", dFa, dFna);
	WORLD_STATS_DBG("dEa: %i, dEna: %i", dEa, dEna);
	WORLD_STATS_DBG("dFd: %i, dFnd: %i", dFd, dFnd);
	WORLD_STATS_DBG("dEd: %i, dEnd: %i", dEd, dEnd);
	WORLD_STATS_DBG("<-- Step");
#endif // DEBUG
}

void World::RelinkPlayerTypes()
{
	int i;

	// This is called when player type information arrives
	// from the server. We reset all `type' pointers of
	// all friend players to point to default player type.
	// Set all player types to default - 0.
	
	if(params->types[0] != NULL)
		for(i = 0; i < 12; i++) friendTypes[i] = params->types[0];
	
	for(i = 0; i < friends.GetSize(); i++) friends[i].SetType(0);
	for(i = 0; i < enemies.GetSize(); i++) enemies[i].SetType(0);

	me->SetType(0);

	// Set my current stamina and effort to maximum, too
	me->ResetStamina();
	me->ResetEffort();
}

World::~World()
{
	if(ball != NULL) delete ball;
	if(me != NULL) delete me;
	if(state != NULL) delete state;
	if(params != NULL) delete params;
	if(variables != NULL) delete variables;
	if(log!=NULL) delete log;
}

void World::SetPlayerType(Side side, int uniform, int type)
{
	if (uniform>1 && uniform <13){
		if(side == OUR){
			friendTypes[uniform] = params->types[type];	
		}
		else if (side==THEIR){
			enemieTypes[uniform] = params->types[type];	
		}
	}
}

/// vrati stranu, ktora ma loptu, 
/// OUR ak moze do nej kopnut nas hrac
/// THEIR ak moze kopnut do ne super
/// UNKNOWN ak do lopty nemoze kopnut nikto
/// player index obsahuje index hraca, ktory moze do nej kopnut z prislusneho pola
/// ak vrati OUR tak z friends
/// inak ak vrati THEIR tak z enemies,
/// inak player index = -1;
void World::WhoHasBall(Side &who, int &playerIndex)
{
	int i;
	Circle c;
	for (i=0; i<enemies.GetSize(); i++){
		c.Set(enemies[i].pos, enemies[i].GetKickableMargin());
		if (c.IsInside(ball->pos)){
			playerIndex = i;
			who = THEIR;
			return;
		}
	}
	for (i=0; i<friends.GetSize(); i++){
		c.Set(friends[i].pos, friends[i].GetKickableMargin());
		if (c.IsInside(ball->pos)){
			playerIndex = i;
			who = OUR;
			return;
		}
	}
	playerIndex = -1;

	who = UNKNOWN;
}

void World::LogWorld()
{
	log->printf("\n%d", state->GetSimTime());
	log->printf("\n\tBall pos: (%3.2f , %3.2f)", ball->pos.GetX(), ball->pos.GetY());
	log->printf("\n\tBall vel: (%3.2f) dir:(%3.2fo)", ball->vel.GetLength(), ball->vel.GetAngle().GetAbsDeg());
	log->printf("\n\tMe pos: (%3.2f , %3.2f)", me->pos.GetX(), me->pos.GetY());
	Vector meVel = me->vel;
	meVel = meVel / me->type->player_decay;
	log->printf("\n\tMe vel: (%3.2f , %3.2f)", me->vel.GetX(), me->vel.GetY());
	log->printf("\n\tMe vel: (|%3.2f| , %3.2fo)", me->vel.GetLength(), me->vel.GetAngle().GetAbsDeg());
	log->printf("\n\tRychlost:");
	log->printf("\n\t\tMe vel: (%3.2f , %3.2f)", meVel.GetX(), meVel.GetY());
	log->printf("\n\t\tMe vel: (|%3.2f| , %3.2fo)", meVel.GetLength(), meVel.GetAngle().GetAbsDeg());


	int i;
	for (i=0; i<enemies.GetSize(); i++)
	{
		log->printf("\n\tE%d: pos: (%3.2f , %3.2f)", enemies[i].GetUniform(),  enemies[i].pos.GetX(), enemies[i].pos.GetY());
	}
	for (i=0; i<friends.GetSize(); i++)
	{
		log->printf("\n\tF%d: pos: (%3.2f , %3.2f)", friends[i].GetUniform(),  friends[i].pos.GetX(), friends[i].pos.GetY());
	}
}

/// \brief Najde index nepriatela, ktory nie je obsadeny
/// \param formPos Pozicia ku ktorej sa ma hladat nepriatel
/// \param radius Sirka kruhovej oblasti, v ktorej sa ma super nachadzat
/// \return \li index nepriatela, ktory je volny
///			\li -1 ak som nenasiel
int World::FindFreeEnemy(Point formPos, float radius)
{
	int i, j;
	int *idEnemies = new int[enemies.GetSize()];
	if (idEnemies == NULL)
		return -1;
	for (i=0; i<enemies.GetSize(); i++)
	{
		Circle c(enemies[i].pos, 3.0);
		
		for (j=0; j<friends.GetSize(); j++)
		{
			if (c.IsInside(friends[j].pos))
			{
				idEnemies[i] = j;			// oznac tych, ktory su obsadeny
			}
			else
			{
				idEnemies[i] = -1;		// tyto su volny
			}
		}
	}
	
	for (i=0; i<enemies.GetSize(); i++)
	{
		Circle c(formPos, radius);
		if (idEnemies[i] == -1)
		{
			if (c.IsInside(enemies[i].pos))
			{
				delete[] idEnemies;
				return i;
			}
		}
	}
	delete[] idEnemies;
	return -1;
}

/// \brief Najde index nepriatela, ktory nie je obsadeny a je najblizsie k zadanej pozicii
/// \param formPos Pozicia ku ktorej sa ma hladat najblizissi nepriatel
/// \param radius Sirka kruhovej oblasti, v ktorej sa ma super nachadzat
/// \return \li index najblizsieho volneho nepriatela
///			\li -1 ak som nenasiel
int World::FindNearestFreeEnemy(Point formPos, float radius)
{
	int i, j;
	int *idEnemies = new int[enemies.GetSize()];
	if (idEnemies == NULL)
		return -1;

	// najde volnych superov
	for (i = 0; i < enemies.GetSize(); i++)
	{
		Circle c(enemies[i].pos, 3.0);
		
		for (j = 0; j < friends.GetSize(); j++)
		{
			if (c.IsInside(friends[j].pos))
			{
				idEnemies[i] = j;			// oznac tych, ktory su obsadeny
			}
			else
			{
				idEnemies[i] = -1;		// tyto su volny
			}
		}
	}

	// najde z volnych superov najblizsieho k zadanej pozicii
	int nearestEnemyIndex = -1;
	float actDistance;
	float minDistance = radius;
	for (i = 0; i < enemies.GetSize(); i++)
	{
		if (idEnemies[i] == -1)
		{
			actDistance = (enemies[i].pos - formPos).GetLength();
			if (actDistance < minDistance)
			{
				minDistance = actDistance;
				nearestEnemyIndex = i;
			}
		}
	}

	delete[] idEnemies;

	return nearestEnemyIndex;
}

/// \brief Najde index spoluhraca, ktory neobsadzuje ziadneho hraca a je najblizsie k zadanej pozicii
/// \param formPos Pozicia ku ktorej sa ma hladat najblizissi spoluhraca
/// \param radius Sirka kruhovej oblasti, v ktorej sa ma spoluhraca nachadzat
/// \return \li index najblizsieho volneho spoluhraca
///			\li -1 ak som nenasiel
int World::FindNearestFreeFriend(Point formPos, float radius)
{
	int i, j, k;
	int *idFriends = new int[friends.GetSize()];
	if (idFriends == NULL)
		return -1;

	// najde volnych spoluhracov
	for (i = 0; i < friends.GetSize(); i++)
	{
		Circle c(friends[i].pos, 3.0);
		
		for (j = 0; j < enemies.GetSize(); j++)
		{
			if (c.IsInside(enemies[j].pos))
			{
				idFriends[i] = j;			// oznac tych, ktory su obsadeny

				// zisti ci sa nenachadza v okoli supera este niektry moj spoluhrac
				Circle enemyCircle(enemies[j].pos, 3.0);
				for (k = 0; k < i; k++)
					if (enemyCircle.IsInside(friends[k].pos))
						idFriends[i] = -1;	// v okoli daneho supera je este daky spoluhrac, takze aktualny spoluhrac je volny
			}
			else
			{
				idFriends[i] = -1;		// tyto su volny
			}
		}
	}

	// najde z volnych spoluhracov najblizsieho k zadanej pozicii
	int nearestFriendIndex = -1;
	float actDistance;
	float minDistance = radius;
	for (i=0; i<friends.GetSize(); i++)
	{
		if (idFriends[i] == -1)
		{
			actDistance = (friends[i].pos - formPos).GetLength();
			if (actDistance < minDistance)
			{
				minDistance = actDistance;
				nearestFriendIndex = i;
			}
		}
	}

	delete[] idFriends;

	return nearestFriendIndex;
}

void World::Debug()
{
	DBGN("%d", state->GetSimTime());
	DBGN("\tBall pos: (%3.2f , %3.2f)", ball->pos.GetX(), ball->pos.GetY());
	DBGN("\tMe pos: (%3.2f , %3.2f)", me->pos.GetX(), me->pos.GetY());
	int i;
	for (i=0; i<enemies.GetSize(); i++)
	{
		DBGN("\tE%d: pos: (%3.2f , %3.2f)", enemies[i].GetUniform(),  enemies[i].pos.GetX(), enemies[i].pos.GetY());
	}
	for (i=0; i<friends.GetSize(); i++)
	{
		DBGN("\tF%d: pos: (%3.2f , %3.2f)", friends[i].GetUniform(),  friends[i].pos.GetX(), friends[i].pos.GetY());
	}
}

int World::NumberOfEnemiesToRadius(float radius)
{
	int count=0;
	Circle c(me->pos, radius);
	for (int i=0; i<enemies.GetSize(); i++){
		if (c.IsInside(enemies[i].pos))
			count++;
	}
	return count;
}

int World::GetNumberOfEnemisInQuadrant(Circle c, QUADRANT q)
{
	int count=0;
	for (int i=0; i<enemies.GetSize(); i++){
		if (c.IsInQuadrant(enemies[i].pos, q))
			count++;
	}
	return count;
}

int World::GetNumberOfEnemisInQuadrant(Circle c, QUADRANT q, vector<int> &idx)
{
	int count=0;
	for (int i=0; i<enemies.GetSize(); i++){
		if (c.IsInQuadrant(enemies[i].pos, q))
		{
			idx.push_back(i);
			count++;
		}
	}
	return count;
}

int World::GetNumberOfEnemisInQuadrant(Circle c, QUADRANT q, vector<Point> &idx)
{
	int count=0;
	for (int i=0; i<enemies.GetSize(); i++){
		if (c.IsInQuadrant(enemies[i].pos, q))
		{
			idx.push_back(enemies[i].pos);
			count++;
		}
	}
	return count;
}

void World::UpdatePassAbleIndexAfterDelete(int nFrom)
{
	for (int i=0; i<passableFriends.GetSize(); i++)
	{
		if (passableFriends[i].index >=nFrom)
			passableFriends[i].index--;
	}
}

