#include "AIOpponentObject.h"

AIOpponentObject::AIOpponentObject(string filePath) : GameObject() {
	grid = new NavigationGrid(filePath);
	stateMachine = new StateMachine();

	initStateMachine();
}
void AIOpponentObject::initStateMachine() {
	State* raceToFinish = new State([&](float dt)->void {
		this->StartMovingAIOpponentObject(dt);
		}
	);
	State* teleportBehindPlayer = new State([&](float dt)->void {
		this->TeleportAIOpponentObject(dt);
		}
	);

	stateMachine->AddState(raceToFinish);
	stateMachine->AddState(teleportBehindPlayer);

	stateMachine->AddTransition(new StateTransition(teleportBehindPlayer, raceToFinish, [&]()->bool {
		return  stateSwitch == false;
		}
	));

	stateMachine->AddTransition(new StateTransition(raceToFinish, teleportBehindPlayer, [&]()->bool {
		return stateSwitch == true;
		}
	));
}

void AIOpponentObject::StartMovingAIOpponentObject(float dt) {
	NavigationPath path;
	aiOpponent->nodes.clear();
	aiOpponent->targetPosition = Vector3(0, 0, -105);

	if (aiOpponent->grid->FindPath(aiOpponent->GetTransform().GetPosition(), aiOpponent->targetPosition, path)) {
		Vector3 pos;
		while (path.PopWaypoint(pos)) {
			aiOpponent->nodes.push_back(pos);
		}

	//	if (aiOpponent->grid->FindPath(aiOpponent->GetTransform().GetPosition(), aiOpponent->targetPosition, path))	{
			//aiOpponent->optimiseNodes();
			aiOpponent->drawPath();

		if (aiOpponent->nodes.size() >= 2) //Sanity check
		{
			Vector3 direction = aiOpponent->nodes[1] - aiOpponent->nodes[0];
			direction.Normalise();
			aiOpponent->GetPhysicsObject()->AddForce(direction * aiOpponent->speed * aiOpponent->currentDT);
		}
		}
	//}

	teleportationCounter += dt;
	if (teleportationCounter >= 10.0) {
		teleportationCounter = 0.0f;
		
		if (((this->GetTransform().GetPosition().x - enemyPreviousPos.x) >= 5) &&
			 ((this->GetTransform().GetPosition().z - enemyPreviousPos.z) >= 5)) {				
				stateSwitch = true;
			}
		enemyPreviousPos = this->GetTransform().GetPosition();
		}
}

void AIOpponentObject::TeleportAIOpponentObject(float dt) {
	teleportationCounter = 0.0f;
	this->transform.SetPosition(player->GetTransform().GetPosition() + Vector3(0,0,15));
	stateSwitch = false;

	std::cout << "Player should be teleported";
}

void AIOpponentObject::UpdatePosition(float dt)
{
	currentDT = dt;

	/// if ai opponent not moving - teleport behind player

	stateMachine->Update(dt);
} 


int AIOpponentObject::PickUpItem(GameObject* object, int bonusValue) {
	AddPointsCollected(bonusValue);
	return pickedUpItems++;
}

void AIOpponentObject::optimiseNodes()
{
	RayCollision collision[3];
	vector<Vector3> optimisedNodes;
	Vector3 currentPosition = this->GetTransform().GetPosition();
	currentPosition.y = 0;
	optimisedNodes.push_back(currentPosition);
	for (int i = 0; i < nodes.size() - 1;)	{
		for (int j = i + 1; j < nodes.size(); j++) {
			Vector3 direction = nodes[j] - nodes[i];
			direction.Normalise();

			//Approximate rays to prevent getting stuck in an object
			Ray ray1(nodes[i], direction);
			Ray ray2(nodes[i] + Vector3::Cross(direction, Vector3(0, 1, 0)), direction);
			Ray ray3(nodes[i] - Vector3::Cross(direction, Vector3(0, 1, 0)), direction);

			//Find if there are obstacles on the path
			if (raycast(ray3, collision[2], true) &&
				raycast(ray2, collision[1], true) &&
				raycast(ray1, collision[0], true))
			{
				float distance = (nodes[j] - nodes[i]).Length();
				if ((collision[2].rayDistance < distance ||
					collision[1].rayDistance < distance ||
					collision[0].rayDistance < distance) && i != j - 1) //Obstacle detected and sanity check
				{
					//Add previous node to the optimised path
					optimisedNodes.push_back(nodes[j - 1]);
					i = j - 1;
					break;
				}
			}
			//Reached the end of path without encountering obstacles
			if (j == nodes.size() - 1) {
				direction = targetPosition - nodes[i];
				direction.Normalise();
				ray1 = Ray::Ray(nodes[i], direction);
				if (raycast(ray1, collision[0], true)) {
					float distance = (targetPosition - nodes[i]).Length();
					if (collision[0].rayDistance >= distance) {  //No obstacles on the way directly to target		
						optimisedNodes.push_back(targetPosition);
						nodes = optimisedNodes;
						return;
					}
				}
				//There was an obstacle so we need to go around
				optimisedNodes.push_back(nodes[j]);
				optimisedNodes.push_back(targetPosition);
				nodes = optimisedNodes;
				return;
			}
		}
	}
}

bool AIOpponentObject::raycast(Ray& r, RayCollision& closestCollision, bool object) const {
	RayCollision collision;
	for (auto& i : obstacles) {
		if (!i->GetBoundingVolume()) {
			continue;
		}
		RayCollision thisCollision;
		if (CollisionDetection::RayIntersection(r, *i, thisCollision)) {
			if (!object) {
				closestCollision = collision;
				closestCollision.node = i;
				return true;
			}
			else {
				if (thisCollision.rayDistance < collision.rayDistance) {
					thisCollision.node = i;
					collision = thisCollision;
				}
			}
		}
	}
	if (collision.node) {
		closestCollision = collision;
		closestCollision.node = collision.node;
		return true;
	}
	return false;
}

void AIOpponentObject::drawPath() {
	for (int i = 0; i < nodes.size() - 1; i++) {
		Debug::DrawLine(nodes[i] + Vector3(0, 1, 0), nodes[i + 1] + Vector3(0, 1, 0), Vector4(0, 1, 0, 1));
	}
}