#include "r015290hTank.h"
#include "../../TankManager.h"
#include "../../Commons.h"
#include "../../C2DMatrix.h"
#include "../../ObstacleManager.h"
#include "r015290hSteeringBehaviour.h"
#include "r015290hPathFinder_DRS.h"
#include "r015290hMaths.h"

r015290hTank::r015290hTank(SDL_Renderer* renderer, TankSetupDetails details) : BaseTank(renderer, details)
{
	mTankTurnDirection  = DIRECTION_UNKNOWN;
	mTankMoveDirection	= DIRECTION_NONE;
	mManTurnDirection   = DIRECTION_UNKNOWN;

	steeringBehaviour = new r015290hSterringBehaviour(this);

	pathFinder = new r015290hPathFinder_DRS(renderer, mCollisionMap);
	pathFound = false;
	waypointRadius = 50.0f;
	currentWaypointIndex = 0;

	targetPos = Vector2D(0, 0);
	mCurrentSpeed = 40.0f;

	mManRotation = 0;
}

r015290hTank::~r015290hTank()
{
}

void r015290hTank::ChangeState(BASE_TANK_STATE newState)
{
	BaseTank::ChangeState(newState);
}

void r015290hTank::Update(float deltaTime, SDL_Event e)
{
	if (e.type == SDL_MOUSEBUTTONDOWN)
	{
		canMove = true;
		SDL_GetMouseState(&x, &y);
		pathFound = pathFinder->FindPath(GetCentralPosition(), Vector2D(x, y));
		currentWaypointIndex = pathFinder->mPathData.size() - 1;
	}

	if (pathFound)
	{
		targetPos = pathFinder->mPathData[currentWaypointIndex];
		if (CheckNearbyWaypoints())
		{
			if (currentWaypointIndex == 0)
			{
				pathFound = false;
				canMove = false;
			}
			else
			{
				currentWaypointIndex--;
			}
		}
	}

	RotateTurret(deltaTime, targetPos);

	//Call parent update.
	BaseTank::Update(deltaTime, e);
}

void r015290hTank::Render()
{
	BaseTank::Render();

	DrawDebugCircle(Steering()->ObstaclePos, 4.0f, 0, 255, 0);

	DrawDebugCircle(Steering()->ahead, 4.0f, 255, 255, 255);
	DrawDebugCircle(Steering()->aheadLeftCorner, 4.0f, 255, 255, 255);
	DrawDebugCircle(Steering()->aheadRightCorner, 4.0f, 255, 255, 255);
	DrawDebugCircle(Steering()->aheadLeftCorner2, 4.0f, 255, 255, 255);
	DrawDebugCircle(Steering()->aheadRightCorner2, 4.0f, 255, 255, 255);

	//DrawDebugCircle(GetCentralPosition(), waypointRadius, 255, 0, 0);

	/*
	for (size_t i = 0; i < pathFinder->mPathData.size(); i++)
	{
		DrawDebugCircle(pathFinder->mPathData[i], 5.0f, 255, 255, 255);
		if (i > 0)
		{
			DrawDebugLine(pathFinder->mPathData[i - 1], pathFinder->mPathData[i], 0, 255, 0);
		}
	}
	*/
}

void r015290hTank::MoveInHeadingDirection(float deltaTime)
{
	RotateTankInMoveDirection(deltaTime);

	//Get the force that propels in current heading.
	Vector2D force = Vector2D();
	if (canMove)
	{
		//mRotationAngle = r015290hMaths::Lerp(mRotationAngle, RadsToDegs(atan2(GetVelocity().y, GetVelocity().x)) + 90, 0.2);
		Vector2D obstacleAvoidence = Steering()->ObstacleAvoidance();
		force += Steering()->Seek(targetPos) + obstacleAvoidence;
	}
	else
	{
		force -= mVelocity;
	}

	//Acceleration = Force/Mass
	Vector2D acceleration = force/GetMass();

	//Update velocity.
	mVelocity += acceleration * deltaTime;

	//Don't allow the tank does not go faster than max speed.
	mVelocity.Truncate(GetMaxSpeed()); //TODO: Add Penalty for going faster than MAX Speed.

	//Finally, update the position.
	Vector2D newPosition = GetPosition();
	newPosition.x += mVelocity.x * deltaTime;
	newPosition.y += (mVelocity.y/**-1.0f*/) * deltaTime;	//Y flipped as adding to Y moves down screen.
	SetPosition(newPosition);
}

void r015290hTank::RotateTurret(float deltaTime, Vector2D target)
{
	Vector2D toTarget = target - GetCentralPosition();
	toTarget.Normalize();

	double dot = toTarget.Dot(mManFireDirection);
	if (dot < 0.95f)
	{
		RotateManByRadian(mMaxTurnRate, -1, deltaTime);
	}
}

void r015290hTank::RotateTankInMoveDirection(float deltaTime)
{
	double difference = GetVelocity().Dot(GetHeading());

	std::cout << difference << std::endl;

	RotateHeadingByRadian(mMaxTurnRate, -1, deltaTime);
}

bool r015290hTank::CheckNearbyWaypoints()
{
	Vector2D vecToWaypoint = targetPos - GetCentralPosition();

	if (vecToWaypoint.Length() <= waypointRadius)
	{
		return true;
	}

	return false;
}

std::vector<GameObject*> r015290hTank::GetNearbyObstacles(double collisionRadius)
{
	std::vector<GameObject*> nearbyObstacles = std::vector<GameObject*>();
	std::vector<GameObject*> obstacles = ObstacleManager::Instance()->GetObstacles();

	for (int i = 0; i < obstacles.size(); i++)
	{
		Vector2D vecToObstacle = GetCentralPosition() - obstacles[i]->GetCentralPosition();
		if (vecToObstacle.Length() <= collisionRadius)
		{
			nearbyObstacles.push_back(obstacles[i]);
		}
	}

	return nearbyObstacles;
}