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

#include "Player/PlayerKernel.h"
#include "Common/Interfaces/MsgProcess.h"
#include "Common/Interfaces/Commands.h"
#include "Player/PlayerKernel.h"
#include "Common/Tasks.h"
#include "Common/Support/Debug.h"
#include <stdio.h>
#include <math.h>
#include <string.h>

#include "Player/PlayerSkills.h"

#ifdef WIN32
#include <windows.h>
#endif

PlayerKernel::PlayerKernel(
	const char* name /* = "PlayerKernel"*/,
	const char* desc /* = "PlayerKernel module"*/)
	: Module(name, desc, "v0.2.000")
{
	// Create new world
	world = new World();
	
	// Initialize the world
	world->me->pos.SetX(-37);
	world->state->SetPlayMode(PM_BeforeKickOff);

	// Initialize pointers to last sense body and last visual info
	lastSb = NULL;
	lastVi = NULL;
	lastViTime = 0;
	lastSbTime = 0;
	
	// Initialize synchronization
	synchMode = Synch_Normal;
	nextSynchMode = Synch_Normal;
	decisionWasMade = true;

	// Initialize the feedback data to zeros
	feedback = new FEEDBACK_DATA;
	memset((void*) feedback, 0, sizeof(FEEDBACK_DATA));

	// Initialize command counts to zeros
	counts = new COMMAND_COUNTS;
	memset((void*) counts, 0, sizeof(COMMAND_COUNTS));

	Main::GetInstance()->RegisterModule(this, MEMORY_TASK);
	Main::GetInstance()->RegisterModule(this, SYNCHRONIZATION_TASK);
	World::BindInterfaces();

	// FARMARI_NEW
	this->m_neckAngle = DegAngle(0);
	wasLookRealized = false;
	//
}

Interface* PlayerKernel::GetInterface(int task) const
{
	if(task == SYNCHRONIZATION_TASK) return (SynchronizationInterface*) this;
	else if(task == MEMORY_TASK) return (MemoryInterface*) this;
	else return NULL;
}

void PlayerKernel::OnInit(RealSide realSide, int uniform, PlayMode mode)
{
	myside = realSide;
	// Overloaded in PlayerSkills
	TXTN("Connected, %s side, uniform %i", realSide == LEFT ? "left" : "right", uniform);


#ifdef WIN32
	char str[20];
	sprintf(str,"#%d", uniform);
	SetConsoleTitle(str);
#endif

	// Set up side swapping
	CommandsInterface *commandGen = 
		(CommandsInterface *) Main::GetInstance()->GetInterface(COMMANDS_TASK);
	if(commandGen != NULL) commandGen->SetRealSide(realSide);
	
	MsgProcessInterface *parser = 
		(MsgProcessInterface *) Main::GetInstance()->GetInterface(MESSAGE_PROCESS_TASK);
	if(parser != NULL) parser->SetRealSide(realSide);
	
	MsgProcessInterface *clParser = 
		(MsgProcessInterface *) Main::GetInstance()->GetInterface(CLANG_MESSAGE_PROCESS_TASK);
	if(clParser != NULL) clParser->SetRealSide(realSide);

	// This is the position set by server to a newly connected player
	world->me->pos = Point(- uniform * 3.f, -37);
	world->me->SetUniform(uniform);
	world->state->SetPlayMode(mode);
}

void PlayerKernel::OnChangeMode(int time, PlayMode mode)
{
	// Overloaded in PlayerTactics
	PlayMode currMode = world->state->GetPlayMode();

	if((currMode == PM_BeforeKickOff && mode == PM_KickOff_Our) ||
		(currMode == PM_BeforeKickOff && mode == PM_KickOff_Their) ||
		(currMode == PM_AfterGoal_Our && mode == PM_KickOff_Their) ||
		(currMode == PM_AfterGoal_Their && mode == PM_KickOff_Our))
	{
		// We now surely know ball position, it is in the middle
		world->ball->pos = Point(0, 0);
		world->ball->vel = RectVector(0, 0);
		world->ball->SetAge(0);
		world->ball->SetValid();
	}

	world->state->SetPlayMode(mode);
}

void PlayerKernel::OnHear(int time, float direction, bool ourTeam, int uniform, const char* msg)
{
	// Overloaded in PlayerSkills
}

void PlayerKernel::OnScore(int time, int our, int their)
{
	world->state->SetScore(our, their);
}

void PlayerKernel::OnSenseBody(const SENSE_BODY* sb)
{
	static int sbCount = 0;
	sbCount++;

	PlayMode pm = world->state->GetPlayMode();
	if(sbCount % 3 != 0 &&
	   (pm == PM_Offside_Our || pm == PM_Offside_Their ||
		pm == PM_CatchFault_Our || pm == PM_CatchFault_Their ||
		pm == PM_BackPass_Our || pm == PM_BackPass_Their ||
		pm == PM_FreeKickFault_Our || pm == PM_FreeKickFault_Their
	   )
	  )
	{
		world->state->SetSimTime(world->state->GetSimTime() + 1);
		nextSynchMode = Synch_Normal;
	}

	if(world->state->IsTimeStoped() || sb->time != world->state->GetSimTime())
	{
		// Sense body after simulation time change

		// Save my previous position
		Point prevPos = world->me->pos;

		// Apply all commands I have performed since last sense body
		world->ApplyFeedback(feedback);
		
		// Simulate all object movement
		world->Step();
 
		// Save this sense body data pointer and time
		lastSb = sb;
		lastSbTime = lastSb->time;

		// Synchronization mode
		synchMode = nextSynchMode;

		// Change my state according to sense body data
		world->me->SetStamina(lastSb->stamina);
		world->me->SetEffort(lastSb->effort);
		world->me->SetNeckDir(DegAngle(lastSb->headAngle));
		world->me->SetViewWidth(lastSb->viewWidth);
		
		// Store command counts
		memcpy((void*) counts, (void*) &lastSb->counts, sizeof(COMMAND_COUNTS));

		// Do we have the visual information too?
		if(lastVi != NULL)
		{
			if(lastVi->time > world->state->GetSimTime())
			{
				// We got the VI sooner then SB - possibly an old server
				world->state->SetSimTime(lastSb->time);
				world->ProcessInfo(lastSb, lastVi);
				lastVi = NULL;

				// Decision making
				Act();
				decisionWasMade = true;
			}
		}
		else
		{
			// No VI yet
			world->state->SetSimTime(lastSb->time);
			
			// Set my speed
			Angle angle = world->me->GetAbsDir() + world->me->GetNeckDir() + DegAngle(lastSb->speedDirection);
			world->me->vel = PolarVector(lastSb->speedSize, angle);
		
			// Set my position
			world->me->pos = prevPos + world->me->vel / world->me->type->player_decay;
		
			// In Normal mode start making decision
			if(synchMode == Synch_Normal)
			{
				Act();
				decisionWasMade = true;
			}
			else
			{
				decisionWasMade = false;
			}
		}
	}
	else
	{
		// Sense body data, but no change in simulation time
		lastSb = sb;

		// Change world according to SB
		world->me->SetStamina(lastSb->stamina);
		world->me->SetEffort(lastSb->effort);
		world->me->SetNeckDir(DegAngle(lastSb->headAngle));
		world->me->SetViewWidth(lastSb->viewWidth);
		
		// Store command counts
		memcpy((void*) counts, (void*) &lastSb->counts, sizeof(COMMAND_COUNTS));

		Angle angle = world->me->GetAbsDir() + world->me->GetNeckDir() + DegAngle(lastSb->speedDirection);
		world->me->vel = PolarVector(lastSb->speedSize, angle);
	}
}

/*static void VIDUMP(const VISUAL_INFO* vi)
{
	int i;
		
	DBGN("--> VI DUMP start");

	DBGN("  LANDMARKS:");
	for(i = 0; i < vi->landmarkCnt; i++)
	{
		DBGN("    type %i dist %3.2f dir %3.2f distch %3.2f dirch %3.2f", vi->landmarks[i].type, vi->landmarks[i].dist, vi->landmarks[i].dir, vi->landmarks[i].distChange, vi->landmarks[i].dirChange);
	}

	DBGN("  LINES:");
	for(i = 0; i < vi->lineCnt; i++)
	{
		DBGN("    type %i dist %3.2f dir %3.2f", vi->lines[i].type, vi->lines[i].dist, vi->lines[i].dir);
	}

	DBGN("<-- VI DUMP end");
}*/

void PlayerKernel::OnSee(const VISUAL_INFO* vi)
{
//	VIDUMP(vi);

	lastVi = vi;
	lastViTime = lastVi->time;

	// If we do not have the SB, do nothing
	if(lastSb == 0) return;

	// If this is the first VI in a new cycle, wait for SB
	if(lastVi->time > lastSb->time) return;

	// Change world without making decisions (Normal mode)
	world->ProcessInfo(lastSb, lastVi);
	if(synchMode != Synch_Normal && !decisionWasMade)
	{
		Act();
		decisionWasMade = true;
	}

	lastVi = NULL;
	
	// FARMARI_NEW

	Angle relative_ball_angle = getRelativeBallAngle();

	// ak nevidim na loptu nastavim priznak a otocim hlavu do spravnej strany
	if(relative_ball_angle.GetAbsDeg() > 135)
	{
		canSeeBall = false;

		// nastav hlavu na spravnu stranu
		if(relative_ball_angle.GetDeg() < 0)
			m_neckAngle = DegAngle(-90);
		else // > 0
			m_neckAngle = DegAngle(90);

	}
	// ak mozem vidiet loptu normalnym pohladom
	else
	{
		canSeeBall = true;

		switch(m_viewingMode)
		{
		case ViewingMode::RoundRobin:
			changeNeckAngle_roundRobin(90);
			break;
		case ViewingMode::BallChecking:
		case ViewingMode::BallBehind:
			changeNeckAngle_ballChecking();
			break;
		case ViewingMode::BallDetailChecking:
			break;
		case ViewingMode::DetailRoundRobin:
			changeNeckAngle_detailRoundRobin();
			break;

		default:
			break;
		}
	}

		/*
		if(relative_ball_angle == 500 )
		{
			// ak sa uz pozeral za seba ale nevidel loptu pozri an druhu stranu
			if(m_viewingMode == ViewingMode::BallBehind)
			{
				lookBehind(-(world->me->GetNeckDir().GetDeg()));
			}
			// ak nebol nastaveny siroky pohlad nastav ho
			else
			{
				m_viewingMode = ViewingMode::BallBehind;

				Vector my_pos = world->me->pos - Point(0,0);
				my_pos.SetX(-(my_pos.GetX()));
				my_pos.SetY(-(my_pos.GetY()));
				float angle_diff = world->me->GetAbsDir().GetDeg() - my_pos.GetAngle().GetDeg();
				
				lookBehind(angle_diff/fabs(angle_diff)*90);
			}

		}
		/*
		else if(fabs(relative_ball_angle) >135)
		{
			lookBehind(relative_ball_angle/fabs(relative_ball_angle)*90);
		}
		else
		{*/
		//m_viewingMode = RoundRobin;
		//}
		
}


void PlayerKernel::OnServerParam(const SERVER_PARAMS* params)
{
	world->params->ProcessServerParams(params);
}

void PlayerKernel::OnPlayerParam(const PLAYER_PARAMS* params)
{
	world->params->ProcessPlayerParams(params);
}

void PlayerKernel::OnPlayerType(int id, const PLAYER_TYPE* type)
{
	world->params->ProcessPlayerType(id, type);
}

void PlayerKernel::OnExecKick(float power, float direction)
{
	if(power > world->params->server->maxpower)
	{
		power = world->params->server->maxpower;
	}
	else if(power < world->params->server->minpower)
	{
		power = world->params->server->minpower;
	}

	if(feedback->lastKickPower == 0)
	{
		feedback->lastKickPower = power;
		feedback->lastKickDir = direction;
	}
}

void PlayerKernel::OnExecTurn(float moment)
{
	if(moment > world->params->server->maxmoment)
	{
		moment = world->params->server->maxmoment;
	}
	else if(moment < world->params->server->minmoment)
	{
		moment = world->params->server->minmoment;
	}

	if(feedback->lastTurn == 0) feedback->lastTurn = moment;
}

void PlayerKernel::OnExecDash(float power)
{
	if(power > world->params->server->maxpower)
	{
		power = world->params->server->maxpower;
	}
	else if(power < world->params->server->minpower)
	{
		power = world->params->server->minpower;
	}

	if(power >= 0)
	{
		if(power > world->me->GetStamina() + world->me->type->extra_stamina)
		{
			power = world->me->GetStamina() + world->me->type->extra_stamina;
		}
	}
	else
	{
		// Power is spended twice that much
		if(fabsf(power * 2) > world->me->GetStamina() + world->me->type->extra_stamina)
		{
			power = (world->me->GetStamina() + world->me->type->extra_stamina) / -2.0f;
		}
	}

	if(feedback->lastPower == 0) feedback->lastPower = power;
}

void PlayerKernel::OnExecNeck(int angle)
{
	if(angle > world->params->server->maxneckmoment)
	{
		angle = int(world->params->server->maxneckmoment);
	}
	else if(angle < world->params->server->minneckmoment)
	{
		angle = int(world->params->server->minneckmoment);
	}

	if(feedback->lastNeck == 0) feedback->lastNeck = angle;
}

void PlayerKernel::OnChangePlayerType(int uniform, int type)
{
	world->SetPlayerType(OUR, uniform, type);
	if (uniform==world->me->GetUniform())
	{
		world->me->SetSide(OUR);
		world->me->SetType(type);
	}
}

// GP
void PlayerKernel::OnFreeform(const char* msg)
{
   // TODO
   // take apropriate action based on recieved freeform message
   ;
}

PlayerKernel::~PlayerKernel()
{
	if(counts != NULL) delete counts;
	if(feedback != NULL) delete feedback;
	if(world != NULL) delete world;
}

//FARMARI_NEW
void PlayerKernel::incrementViewingPart_roundRobin()
{
	m_viewingPart_roundRobin = (m_viewingPart_roundRobin + 1) % 4;
}

void PlayerKernel::incrementViewingPart_detailRoundRobin()
{
	m_viewingPart_detailRoundRobin = (m_viewingPart_detailRoundRobin + 1) % 4;
}

void PlayerKernel::incrementViewingPart_ballChecking()
{
	m_viewingPart_ballChecking = (m_viewingPart_ballChecking + 1) % 2;
}

// vrati uhol medzi smerom hraca a loptou
Angle PlayerKernel::getRelativeBallAngle()
{
	Vector ball_vector = world->ball->pos - world->me->pos;
	Angle ball_angle = ball_vector.GetAngle();
	return ball_angle - world->me->GetAbsDir();
}

// vrati relativny uhol ihriska
Angle PlayerKernel::getRelativeCenterYardAngle()
{
	Point centerYard(0,0);
	Vector centerYard_vector = centerYard - world->me->pos;
	Angle centerYard_angle = centerYard_vector.GetAngle();

	
	return centerYard_angle - world->me->GetAbsDir();
}


void PlayerKernel::changeNeckAngle_roundRobin(float viewWide)
{
	Angle final_dir;
	float angle_with_ball;
	float angle_no_ball_A;
	float angle_no_ball_B;
	
	Angle relative_ball_angle = getRelativeBallAngle();

	if(wasLookRealized)
	{
		if(relative_ball_angle.GetAbsDeg() < viewWide/2)
		{
			angle_no_ball_A = viewWide;
			angle_with_ball = 0.0f;
			angle_no_ball_B = -viewWide;
		}
		else if(relative_ball_angle.GetDeg() > viewWide/2)
		{
			angle_no_ball_A = 0.0f;
			angle_with_ball = viewWide;
			angle_no_ball_B = 0.0f;
		}
		else if(relative_ball_angle.GetDeg() < -viewWide/2)
		{
			angle_no_ball_A = 0.0f;
			angle_with_ball = -viewWide;
			angle_no_ball_B = 0.0f;
		}

		switch(m_viewingPart_roundRobin)
		{
			case 0:
				final_dir = DegAngle(angle_no_ball_A);
				break;
			case 1:
			case 3:
				final_dir = DegAngle(angle_with_ball);
				break;
			case 2:
				final_dir = DegAngle(angle_no_ball_B);
				break;
		}
		m_neckAngle = final_dir;
		wasLookRealized = false;
		incrementViewingPart_roundRobin();
	}
}

void PlayerKernel::changeNeckAngle_detailRoundRobin()
{
	Angle final_dir;
	float angle_with_ball, angle_no_ball_A, angle_no_ball_B, angle_no_ball_C;

	float viewWide = 45.0f;
	
	Angle relative_ball_angle = getRelativeBallAngle();
	Angle relativeCenterYardAngle = getRelativeCenterYardAngle();



	if(wasLookRealized)
	{
		// lopta je vo viditelnom rozmedzi pred hracom
		if(relative_ball_angle.GetAbsDeg() < viewWide/2)
		{
			angle_no_ball_A = viewWide;
			angle_with_ball = 0.0f;
			angle_no_ball_B = -viewWide;

			if(relativeCenterYardAngle.GetDeg() > 0) angle_no_ball_C = 2*viewWide;
			else angle_no_ball_C = -2*viewWide;

		}
		else if(relative_ball_angle.GetDeg() > viewWide/2)
		{
			angle_no_ball_A = 0.0f;
			angle_with_ball = viewWide;
			angle_no_ball_B = 2*viewWide;
			angle_no_ball_C = -viewWide;
		}
		else if(relative_ball_angle.GetDeg() < -viewWide/2)
		{
			angle_no_ball_A = 0.0f;
			angle_with_ball = -viewWide;
			angle_no_ball_B = -2*viewWide;
			angle_no_ball_C = viewWide;
		}

		switch(m_viewingPart_detailRoundRobin)
		{
			case 0:
				final_dir = DegAngle(angle_with_ball);
				break;
			case 1:
				final_dir = DegAngle(angle_no_ball_A);
				break;
			case 2:
				final_dir = DegAngle(angle_no_ball_B);
				break;
			case 3:
				final_dir = DegAngle(angle_no_ball_C);
				break;
		}
		m_neckAngle = final_dir;
		wasLookRealized = false;
		incrementViewingPart_detailRoundRobin();
	}
}

void PlayerKernel::changeNeckAngle_ballChecking()
{
	Angle final_dir;
	float angle_with_ball;
	float angle_no_ball_A;
	float angle_no_ball_B;
	
	Angle relative_ball_angle = getRelativeBallAngle();

	if(wasLookRealized)
	{
		// vzdialenost lopty od hraca
		Vector vBallDistance = world->ball->pos - world->me->pos;
		float ballDistance = vBallDistance.GetLength();

		// vypocitaj uhol otocenia podla vzdialenosti polty od hraca
		float turnAngle = ((BALL_CHECKING_SHIFT_MAX_ANGLE - BALL_CHECKING_SHIFT_MIN_ANGLE) / (FAR_AWAY_DISTANCE - CONTACT_DISTANCE))
							* ( ballDistance - CONTACT_DISTANCE) + BALL_CHECKING_SHIFT_MIN_ANGLE;

		switch(m_viewingPart_ballChecking)
		{
			case 0:
				final_dir = DegAngle(relative_ball_angle.GetDeg() - turnAngle);
				break;
			case 1:
				final_dir = DegAngle(relative_ball_angle.GetDeg() + turnAngle);
				break;
			default:
				break;
		}
		// nastavenie maximalnych otoceni
		if(final_dir.GetDeg() < -90) final_dir = DegAngle(-90);
		if(final_dir.GetDeg() > 90)  final_dir =  DegAngle(90);

		m_neckAngle = final_dir;
		wasLookRealized = false;
		incrementViewingPart_ballChecking();
	}
}

void PlayerKernel::lookBehind(float angle_dir)
{
	m_neckAngle = DegAngle(angle_dir);
	wasLookRealized = false;
}

Angle PlayerKernel::getNeckAngle()
{
	wasLookRealized = true;
	return m_neckAngle;
}
//