#include "PathFollowingSystem.hpp"
#include "AnimationTreeSystem.hpp"
#include "NavMeshSystem.hpp"
#include "CharacterRegistry.hpp"
#include "../components/PathFollowing.hpp"
#include "../components/Character.hpp"
#include "../components/CharacterIdentity.hpp"
#include "../components/CharacterSlots.hpp"
#include "../components/Transform.hpp"
#include "../components/NavMesh.hpp"
#include <OgreLogManager.h>
#include <cmath>

PathFollowingSystem::PathFollowingSystem(flecs::world &world,
					 Ogre::SceneManager *sceneMgr,
					 NavMeshSystem *navSystem)
	: m_world(world)
	, m_sceneMgr(sceneMgr)
	, m_navSystem(navSystem)
{
}

PathFollowingSystem::~PathFollowingSystem() = default;

Ogre::Vector3 PathFollowingSystem::getEntityPosition(flecs::entity e)
{
	if (e.has<TransformComponent>()) {
		auto &trans = e.get<TransformComponent>();
		if (trans.node)
			return trans.node->_getDerivedPosition();
		return trans.position;
	}
	return Ogre::Vector3::ZERO;
}

void PathFollowingSystem::rotateTowards(flecs::entity e,
					const Ogre::Vector3 &direction,
					float deltaTime)
{
	if (!e.has<TransformComponent>())
		return;
	auto &trans = e.get_mut<TransformComponent>();
	if (!trans.node)
		return;

	Ogre::Vector3 flatDir = direction;
	flatDir.y = 0;
	if (flatDir.squaredLength() < 0.0001f)
		return;
	flatDir.normalise();

	// Get the character's front-facing axis from the registry
	Ogre::Vector3 frontAxis = Ogre::Vector3::NEGATIVE_UNIT_Z;
	if (e.has<CharacterIdentityComponent>()) {
		auto &id = e.get<CharacterIdentityComponent>();
		const CharacterRegistry::CharacterRecord *rec =
			CharacterRegistry::getSingleton().findCharacter(
				id.registryId);
		if (rec)
			frontAxis = rec->frontAxis;
	}

	// Use yaw rotation only (Y plane)
	Ogre::Quaternion currentRot = trans.node->getOrientation();
	Ogre::Quaternion targetRot = frontAxis.getRotationTo(flatDir);

	// Slerp for smooth rotation
	Ogre::Quaternion newRot = Ogre::Quaternion::Slerp(
		deltaTime * 10.0f, currentRot, targetRot, true);

	// Extract only the Y-axis rotation (yaw) to keep character upright
	Ogre::Radian yaw = newRot.getYaw();
	trans.node->setOrientation(
		Ogre::Quaternion(yaw, Ogre::Vector3::UNIT_Y));
	trans.rotation = Ogre::Quaternion(yaw, Ogre::Vector3::UNIT_Y);
}

void PathFollowingSystem::applyLocomotionState(flecs::entity e)
{
	if (!e.has<PathFollowingComponent>() || !m_animTreeSystem)
		return;

	auto &path = e.get<PathFollowingComponent>();
	const PathFollowingState *state =
		path.findState(path.currentLocomotionState);
	if (!state)
		return;

	for (const auto &pair : state->stateMachineStates) {
		m_animTreeSystem->setState(e, pair.first, pair.second, false);
	}
}

void PathFollowingSystem::update(float deltaTime)
{
	// Find the navmesh entity
	flecs::entity navmeshEntity = flecs::entity::null();
	m_world.query<NavMeshComponent>().each(
		[&](flecs::entity e, NavMeshComponent &) {
			if (!navmeshEntity.is_alive())
				navmeshEntity = e;
		});

	m_world.query<PathFollowingComponent, CharacterComponent,
		      TransformComponent>()
		.each([&](flecs::entity e, PathFollowingComponent &pf,
			  CharacterComponent &cc, TransformComponent &trans) {
			(void)trans;

			if (!pf.hasTarget)
				return;

			Ogre::Vector3 charPos = getEntityPosition(e);
			Ogre::Vector3 targetPos = pf.targetPosition;

			// Check if we've reached the destination
			Ogre::Vector3 toFinal = targetPos - charPos;
			toFinal.y = 0;
			float distToFinal = toFinal.length();
			if (distToFinal < 0.5f) {
				if (!cc.useRootMotion)
					cc.linearVelocity = Ogre::Vector3::ZERO;
				pf.hasTarget = false;
				pf.currentLocomotionState = "idle";
				pf.path.clear();
				pf.pathIndex = 0;
				applyLocomotionState(e);
				return;
			}

			// Recalculate path periodically
			pf.pathRecalcTimer += deltaTime;
			if (pf.pathRecalcTimer > 2.0f) {
				pf.pathRecalcTimer = 0.0f;
				if (navmeshEntity.is_alive() && m_navSystem) {
					std::vector<Ogre::Vector3> newPath;
					bool found = m_navSystem->findPath(
						navmeshEntity, charPos,
						targetPos, newPath);
					if (found && !newPath.empty()) {
						pf.path = std::move(newPath);
						pf.pathIndex = 0;
					}
				}
			}

			// Fallback: if no path, go direct
			if (pf.path.empty()) {
				pf.path.clear();
				pf.path.push_back(targetPos);
				pf.pathIndex = 0;
			}

			// Advance waypoints
			if (pf.pathIndex >= (int)pf.path.size()) {
				pf.path.clear();
				pf.path.push_back(targetPos);
				pf.pathIndex = 0;
			}

			Ogre::Vector3 waypointPos = pf.path[pf.pathIndex];
			Ogre::Vector3 toTarget = waypointPos - charPos;

			const float WAYPOINT_THRESHOLD = 0.5f;
			if (toTarget.length() < WAYPOINT_THRESHOLD) {
				pf.pathIndex++;
				if (pf.pathIndex >= (int)pf.path.size()) {
					// Last waypoint - check if close to final target
					if (distToFinal < 0.5f) {
						if (!cc.useRootMotion)
							cc.linearVelocity =
								Ogre::Vector3::
									ZERO;
						pf.hasTarget = false;
						pf.currentLocomotionState =
							"idle";
						pf.path.clear();
						pf.pathIndex = 0;
						applyLocomotionState(e);
						return;
					}
					// Extend with direct target
					pf.path.clear();
					pf.path.push_back(targetPos);
					pf.pathIndex = 0;
				}
				waypointPos = pf.path[pf.pathIndex];
				toTarget = waypointPos - charPos;
			}

			// Move toward waypoint
			toTarget.y = 0;
			if (toTarget.squaredLength() > 0.0001f) {
				toTarget.normalise();

				// Determine speed based on distance to final target
				float speed = pf.walkSpeed;
				if (distToFinal > pf.walkSpeed * 3.0f) {
					speed = pf.runSpeed;
					pf.currentLocomotionState = "run";
				} else {
					speed = pf.walkSpeed;
					pf.currentLocomotionState = "walk";
				}

				/* Only set velocity if root motion is not
				 * active. When root motion is active,
				 * AnimationTreeSystem already computed the
				 * velocity from the animation displacement. */
				if (!cc.useRootMotion)
					cc.linearVelocity = toTarget * speed;

				// Rotate character to face movement direction
				rotateTowards(e, toTarget, deltaTime);
			} else {
				if (!cc.useRootMotion)
					cc.linearVelocity = Ogre::Vector3::ZERO;
			}

			applyLocomotionState(e);
		});
}
