#include "raylib.h"
#include <cmath>
#include <algorithm>
#include <queue>
#include <vector>
#include <limits>

extern Vector3 playerPosition;
extern bool IsSolidAt(Vector3 position, float radius);

const int NPC_COUNT = 20;

Vector3 enemyPosition[NPC_COUNT] =
{
	{ -72.0f, 1.0f, -72.0f },
	{  72.0f, 1.0f, -72.0f },
	{ -72.0f, 1.0f,  72.0f },
	{  72.0f, 1.0f,  72.0f },

	{ -48.0f, 1.0f, -36.0f },
	{  48.0f, 1.0f, -36.0f },
	{ -48.0f, 1.0f,  36.0f },
	{  48.0f, 1.0f,  36.0f },

	{ -28.0f, 1.0f, -68.0f },
	{  28.0f, 1.0f, -68.0f },
	{ -28.0f, 1.0f,  68.0f },
	{  28.0f, 1.0f,  68.0f },

	{ -60.0f, 1.0f, -52.0f },
	{  60.0f, 1.0f, -52.0f },
	{ -60.0f, 1.0f,  52.0f },
	{  60.0f, 1.0f,  52.0f },

	{ -16.0f, 1.0f, -48.0f },
	{  16.0f, 1.0f, -48.0f },
	{ -16.0f, 1.0f,  48.0f },
	{  16.0f, 1.0f,  48.0f }
};

float enemyWalkTime[NPC_COUNT] = {};
float enemyStunTime[NPC_COUNT] = {};
float enemySpin[NPC_COUNT] = {};
bool enemyMoving[NPC_COUNT] = {};

const float enemySpeed = 5.5f;
const float enemyStunDuration = 2.5f;

static bool showEnemyPaths = false;

static const int GRID_SIZE = 41;
static const float CELL_SIZE = 4.0f;
static const float GRID_OFFSET = 20.0f;

struct GridNode
{
	int x;
	int z;

	bool operator==(const GridNode& other) const
	{
		return x == other.x && z == other.z;
	}
};

static std::vector<GridNode> enemyPath[NPC_COUNT];

static int WorldToGridX(float x)
{
	return (int)std::round(x / CELL_SIZE + GRID_OFFSET);
}

static int WorldToGridZ(float z)
{
	return (int)std::round(z / CELL_SIZE + GRID_OFFSET);
}

static Vector3 GridToWorld(int x, int z)
{
	return
	{
		(x - GRID_OFFSET) * CELL_SIZE,
		1.0f,
		(z - GRID_OFFSET) * CELL_SIZE
	};
}

static bool IsInsideGrid(int x, int z)
{
	return
	x >= 0 &&
	x < GRID_SIZE &&
	z >= 0 &&
	z < GRID_SIZE;
}

static bool IsWalkableCell(int x, int z)
{
	if (!IsInsideGrid(x, z))
		return false;

	Vector3 position = GridToWorld(x, z);

	return !IsSolidAt(position, 0.42f);
}

static float GridDistance(
	int x1,
	int z1,
	int x2,
	int z2
)
{
	float dx = (float)(x1 - x2);
	float dz = (float)(z1 - z2);

	return std::sqrt(dx * dx + dz * dz);
}

static std::vector<GridNode> FindPath(
	Vector3 startWorld,
	Vector3 targetWorld
)
{
	GridNode start =
	{
		WorldToGridX(startWorld.x),
		WorldToGridZ(startWorld.z)
	};

	GridNode target =
	{
		WorldToGridX(targetWorld.x),
		WorldToGridZ(targetWorld.z)
	};

	if (!IsInsideGrid(start.x, start.z))
		return {};

	if (!IsInsideGrid(target.x, target.z))
		return {};

	// Si el destino está ocupado por un mueble,
	// buscamos la celda caminable más cercana.
	if (!IsWalkableCell(target.x, target.z))
	{
		bool found = false;

		for (int radius = 1; radius <= 5 && !found; radius++)
		{
			for (int z = target.z - radius;
				 z <= target.z + radius && !found;
			z++)
				 {
					 for (int x = target.x - radius;
						  x <= target.x + radius;
					 x++)
						  {
							  if (IsWalkableCell(x, z))
							  {
								  target = {x, z};
								  found = true;
								  break;
							  }
						  }
				 }
		}

		if (!found)
			return {};
	}

	if (!IsWalkableCell(start.x, start.z))
		return {};

	struct Node
	{
		int x;
		int z;
		float g;
		float f;
	};

	std::vector<Node> open;
	std::vector<bool> closed(
		GRID_SIZE * GRID_SIZE,
		false
	);

	std::vector<GridNode> cameFrom(
		GRID_SIZE * GRID_SIZE,
		{-1, -1}
	);

	std::vector<float> gScore(
		GRID_SIZE * GRID_SIZE,
		std::numeric_limits<float>::infinity()
	);

	auto indexOf =
	[](int x, int z)
	{
		return z * GRID_SIZE + x;
	};

	gScore[indexOf(start.x, start.z)] = 0.0f;

	open.push_back(
		{
			start.x,
			start.z,
			0.0f,
			GridDistance(
				start.x,
				start.z,
				target.x,
				target.z
			)
		}
	);

	const int directions[4][2] =
	{
		{ 1, 0 },
		{-1, 0 },
		{ 0, 1 },
		{ 0,-1 }
	};

	while (!open.empty())
	{
		int best = 0;

		for (int i = 1; i < (int)open.size(); i++)
		{
			if (open[i].f < open[best].f)
				best = i;
		}

		Node current = open[best];
		open.erase(open.begin() + best);

		if (current.x == target.x &&
			current.z == target.z)
		{
			std::vector<GridNode> path;

			GridNode currentNode =
			{
				target.x,
				target.z
			};

			while (!(currentNode == start))
			{
				path.push_back(currentNode);

				GridNode previous =
				cameFrom[
					indexOf(
						currentNode.x,
			 currentNode.z
					)
				];

				if (previous.x < 0)
				{
					path.clear();
					return path;
				}

				currentNode = previous;
			}

			path.push_back(start);

			std::reverse(
				path.begin(),
						 path.end()
			);

			return path;
		}

		int currentIndex =
		indexOf(current.x, current.z);

		if (closed[currentIndex])
			continue;

		closed[currentIndex] = true;

		for (const auto& direction : directions)
		{
			int nx = current.x + direction[0];
			int nz = current.z + direction[1];

			if (!IsInsideGrid(nx, nz))
				continue;

			if (!IsWalkableCell(nx, nz))
				continue;

			int nextIndex = indexOf(nx, nz);

			if (closed[nextIndex])
				continue;

			float newG = current.g + 1.0f;

			if (newG < gScore[nextIndex])
			{
				gScore[nextIndex] = newG;

				cameFrom[nextIndex] =
				{
					current.x,
					current.z
				};

				float h =
				GridDistance(
					nx,
				 nz,
				 target.x,
				 target.z
				);

				open.push_back(
					{
						nx,
						nz,
						newG,
						newG + h
					}
				);
			}
		}
	}

	return {};
}

static bool PositionIsFreeForEnemy(
	Vector3 position,
	int ignoreIndex
)
{
	if (IsSolidAt(position, 0.45f))
		return false;

	for (int i = 0; i < NPC_COUNT; i++)
	{
		if (i == ignoreIndex)
			continue;

		float dx =
		enemyPosition[i].x -
		position.x;

		float dz =
		enemyPosition[i].z -
		position.z;

		if (dx * dx + dz * dz < 0.75f * 0.75f)
			return false;
	}

	return true;
}

void MoveEnemyWithCollision(
	int index,
	float moveX,
	float moveZ
)
{
	Vector3 nextX =
	enemyPosition[index];

	nextX.x += moveX;

	if (PositionIsFreeForEnemy(nextX, index))
		enemyPosition[index].x = nextX.x;

	Vector3 nextZ =
	enemyPosition[index];

	nextZ.z += moveZ;

	if (PositionIsFreeForEnemy(nextZ, index))
		enemyPosition[index].z = nextZ.z;
}

static void RecalculatePath(int index)
{
	enemyPath[index] =
	FindPath(
		enemyPosition[index],
		playerPosition
	);
}

static void MoveEnemyAlongPath(
	int index,
	float deltaTime
)
{
	if (enemyPath[index].size() < 2)
	{
		enemyMoving[index] = false;
		return;
	}

	Vector3 target =
	GridToWorld(
		enemyPath[index][1].x,
		enemyPath[index][1].z
	);

	float dx =
	target.x -
	enemyPosition[index].x;

	float dz =
	target.z -
	enemyPosition[index].z;

	float distance =
	std::sqrt(dx * dx + dz * dz);

	if (distance < 0.18f)
	{
		enemyPath[index].erase(
			enemyPath[index].begin()
		);

		return;
	}

	dx /= distance;
	dz /= distance;

	MoveEnemyWithCollision(
		index,
		dx * enemySpeed * deltaTime,
		dz * enemySpeed * deltaTime
	);

	enemyMoving[index] = true;
	enemyWalkTime[index] += deltaTime * 9.0f;
}

static int FindClosestEnemy()
{
	int best = 0;
	float bestDistance =
	std::numeric_limits<float>::infinity();

	for (int i = 0; i < NPC_COUNT; i++)
	{
		float dx =
		enemyPosition[i].x -
		playerPosition.x;

		float dz =
		enemyPosition[i].z -
		playerPosition.z;

		float distance =
		dx * dx + dz * dz;

		if (distance < bestDistance)
		{
			bestDistance = distance;
			best = i;
		}
	}

	return best;
}

static bool HasNearbyTalan()
{
	for (int i = 0; i < NPC_COUNT; i++)
	{
		float dx =
		enemyPosition[i].x -
		playerPosition.x;

		float dz =
		enemyPosition[i].z -
		playerPosition.z;

		float distance =
		std::sqrt(dx * dx + dz * dz);

		if (distance < 28.0f)
			return true;
	}

	return false;
}

static void TeleportOneTalanToPlayerArea()
{
	int index = FindClosestEnemy();

	float bestDistance = 18.0f;

	for (int attempt = 0; attempt < 40; attempt++)
	{
		float angle =
		(float)attempt *
		(PI * 2.0f / 40.0f);

		float distance =
		bestDistance +
		(attempt % 5) * 1.5f;

		Vector3 position =
		{
			playerPosition.x +
			std::cos(angle) * distance,

			1.0f,

			playerPosition.z +
			std::sin(angle) * distance
		};

		if (!PositionIsFreeForEnemy(
			position,
			index))
		{
			continue;
		}

		std::vector<GridNode> testPath =
		FindPath(
			position,
		   playerPosition
		);

		if (!testPath.empty())
		{
			enemyPosition[index] =
			position;

			enemyWalkTime[index] = 0.0f;
			enemyStunTime[index] = 0.0f;
			enemySpin[index] = 0.0f;

			enemyPath[index] =
			testPath;

			return;
		}
	}
}

void UpdateEnemy(float deltaTime)
{
	// P muestra/oculta las rutas.
	if (IsKeyPressed(KEY_P))
	{
		showEnemyPaths =
		!showEnemyPaths;
	}

	// Siempre debe haber un talán relativamente
	// cerca del jugador.
	if (!HasNearbyTalan())
	{
		TeleportOneTalanToPlayerArea();
	}

	for (int i = 0; i < NPC_COUNT; i++)
	{
		float dx =
		playerPosition.x -
		enemyPosition[i].x;

		float dz =
		playerPosition.z -
		enemyPosition[i].z;

		float distance =
		std::sqrt(dx * dx + dz * dz);

		// =========================================
		// MAREADO
		// =========================================

		if (enemyStunTime[i] > 0.0f)
		{
			enemyStunTime[i] -= deltaTime;
			enemySpin[i] += deltaTime * 18.0f;

			enemyMoving[i] = false;

			if (enemyStunTime[i] < 0.0f)
				enemyStunTime[i] = 0.0f;

			continue;
		}

		// =========================================
		// EMPUJAR JUGADOR
		// =========================================

		if (distance > 0.001f &&
			distance < 1.5f)
		{
			float pushX =
			(dx / distance) *
			10.0f *
			deltaTime;

			float pushZ =
			(dz / distance) *
			10.0f *
			deltaTime;

			Vector3 next = playerPosition;

			next.x += pushX;
			next.z += pushZ;

			if (!IsSolidAt(next, 0.45f))
				playerPosition = next;
		}

		// =========================================
		// RUTA INTELIGENTE
		// =========================================

		// Recalcular frecuentemente para que los talanes
		// reaccionen si el jugador cambia de dirección.
		if (enemyPath[i].empty() ||
			((int)(GetTime() * 4.0) + i) % 5 == 0)
		{
			RecalculatePath(i);
		}

		if (distance > 1.8f)
		{
			MoveEnemyAlongPath(
				i,
				deltaTime
			);
		}
		else
		{
			enemyMoving[i] = false;
			enemyWalkTime[i] = 0.0f;
		}
	}

	// =========================================
	// CLICK DERECHO
	// =========================================

	if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT))
	{
		for (int i = 0; i < NPC_COUNT; i++)
		{
			float dx =
			enemyPosition[i].x -
			playerPosition.x;

			float dz =
			enemyPosition[i].z -
			playerPosition.z;

			float distance =
			std::sqrt(
				dx * dx +
				dz * dz
			);

			if (distance < 4.0f &&
				distance > 0.001f)
			{
				dx /= distance;
				dz /= distance;

				MoveEnemyWithCollision(
					i,
					dx * 3.5f,
					dz * 3.5f
				);

				enemyStunTime[i] =
				enemyStunDuration;

				enemySpin[i] = 0.0f;
				enemyWalkTime[i] = 0.0f;
			}
		}
	}
}

void TeleportEnemyNearPlayer(
	int index,
	float yaw
)
{
	if (index < 0 ||
		index >= NPC_COUNT)
		return;

	float dx =
	enemyPosition[index].x -
	playerPosition.x;

	float dz =
	enemyPosition[index].z -
	playerPosition.z;

	float oldDistance =
	std::sqrt(
		dx * dx +
		dz * dz
	);

	if (oldDistance < 3.0f)
		oldDistance = 3.0f;

	float baseAngle =
	std::atan2(
		dz,
		dx
	);

	for (int attempt = 0; attempt < 16; attempt++)
	{
		float angle =
		baseAngle +
		PI +
		attempt * 0.4f;

		Vector3 newPosition =
		{
			playerPosition.x +
			std::cos(angle) * oldDistance,

			1.0f,

			playerPosition.z +
			std::sin(angle) * oldDistance
		};

		if (!PositionIsFreeForEnemy(
			newPosition,
			index))
		{
			continue;
		}

		enemyPosition[index] =
		newPosition;

		enemyStunTime[index] = 0.0f;
		enemySpin[index] = 0.0f;
		enemyWalkTime[index] = 0.0f;

		RecalculatePath(index);

		return;
	}
}

int CheckEnemyHit(Vector3 bulletPosition)
{
	for (int i = 0; i < NPC_COUNT; i++)
	{
		float hitX =
		bulletPosition.x -
		enemyPosition[i].x;

		float hitY =
		bulletPosition.y -
		1.2f;

		float hitZ =
		bulletPosition.z -
		enemyPosition[i].z;

		float distance =
		std::sqrt(
			hitX * hitX +
			hitY * hitY +
			hitZ * hitZ
		);

		if (distance < 0.8f)
			return i;
	}

	return -1;
}

// =============================================
// RUTAS VISIBLES CON P
// =============================================

void DrawEnemyPaths()
{
	if (!showEnemyPaths)
		return;

	for (int i = 0; i < NPC_COUNT; i++)
	{
		if (enemyPath[i].size() < 2)
			continue;

		Vector3 previous =
		enemyPosition[i];

		previous.y = 0.12f;

		for (size_t p = 1;
			 p < enemyPath[i].size();
		p++)
			 {
				 Vector3 current =
				 GridToWorld(
					 enemyPath[i][p].x,
				 enemyPath[i][p].z
				 );

				 current.y = 0.12f;

				 DrawLine3D(
					 previous,
				current,
				LIME
				 );

				 previous = current;
			 }
	}
}

// =============================================
// TALÁN ORIGINAL
// NO TOCAR
// =============================================

void DrawEnemy()
{
	for (int i = 0; i < NPC_COUNT; i++)
	{
		float legMove = 0.0f;
		float armMove = 0.0f;

		if (enemyMoving[i])
		{
			legMove =
			std::sin(enemyWalkTime[i]) *
			0.18f;

			armMove =
			std::sin(enemyWalkTime[i]) *
			0.25f;
		}

		float spinX = 0.0f;
		float spinZ = 0.0f;

		if (enemyStunTime[i] > 0.0f)
		{
			spinX =
			std::sin(enemySpin[i]) *
			0.25f;

			spinZ =
			std::cos(enemySpin[i]) *
			0.25f;
		}

		DrawCube(
			{
				enemyPosition[i].x -
				0.22f +
				spinX,

				0.45f,

				enemyPosition[i].z +
				legMove +
				spinZ
			},
		   0.25f,
		   0.9f,
		   0.25f,
		   DARKGRAY
		);

		DrawCube(
			{
				enemyPosition[i].x +
				0.22f -
				spinX,

				0.45f,

				enemyPosition[i].z -
				legMove -
				spinZ
			},
		   0.25f,
		   0.9f,
		   0.25f,
		   DARKGRAY
		);

		DrawCube(
			{
				enemyPosition[i].x,
				1.25f,
				enemyPosition[i].z
			},
		   0.8f,
		   1.0f,
		   0.45f,
		   BLUE
		);

		DrawSphere(
			{
				enemyPosition[i].x + spinX,
				2.05f,
				enemyPosition[i].z + spinZ
			},
			 0.38f,
			 BEIGE
		);

		DrawCube(
			{
				enemyPosition[i].x -
				0.55f +
				spinX,

				1.25f + armMove,

				enemyPosition[i].z +
				spinZ
			},
		   0.18f,
		   0.75f,
		   0.18f,
		   BLUE
		);

		DrawCube(
			{
				enemyPosition[i].x +
				0.55f -
				spinX,

				1.25f - armMove,

				enemyPosition[i].z -
				spinZ
			},
		   0.18f,
		   0.75f,
		   0.18f,
		   BLUE
		);
	}
}