#include "SmartObjectSystem.hpp"
#include "../components/SmartObject.hpp"
#include "../components/Transform.hpp"
#include "../components/Character.hpp"
#include "../components/CharacterSlots.hpp"
#include "../components/CharacterIdentity.hpp"
#include "../components/GoapBlackboard.hpp"
#include "CharacterRegistry.hpp"
#include "../components/ActionDatabase.hpp"
#include "../components/ActionDebug.hpp"
#include "../components/PathFollowing.hpp"
#include "../components/NavMesh.hpp"
#include "../components/AnimationTree.hpp"
#include "../components/PlayerController.hpp"
#include "../components/EntityName.hpp"
#include "../EditorApp.hpp"
#include "NavMeshSystem.hpp"
#include "BehaviorTreeSystem.hpp"
#include "AnimationTreeSystem.hpp"
#include <OgreLogManager.h>
#include <iostream>
#include <cmath>
#include <unordered_set>

/**
 * Check if a character entity has root motion enabled in its
 * AnimationTreeComponent. When root motion is active, the
 * AnimationTreeSystem computes linearVelocity from animation
 * root bone displacement, so SmartObjectSystem should NOT
 * overwrite it with direct movement velocity.
 */
static bool hasRootMotion(flecs::entity e)
{
	if (!e.has<AnimationTreeComponent>())
		return false;
	return e.get<AnimationTreeComponent>().useRootMotion;
}

static bool isPlayerControlled(flecs::entity e)
{
	return e.has<PlayerControlledComponent>();
}

SmartObjectSystem *SmartObjectSystem::s_instance = nullptr;

SmartObjectSystem::SmartObjectSystem(flecs::world &world,
				     Ogre::SceneManager *sceneMgr,
				     NavMeshSystem *navSystem,
				     BehaviorTreeSystem *btSystem)
	: m_world(world)
	, m_sceneMgr(sceneMgr)
	, m_navSystem(navSystem)
	, m_btSystem(btSystem)
	, m_animTreeSystem(nullptr)
{
	s_instance = this;
}

SmartObjectSystem::~SmartObjectSystem() = default;

Ogre::Vector3 SmartObjectSystem::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;
}

Ogre::Vector3 SmartObjectSystem::getFrontAxis(flecs::entity e) const
{
	if (e.has<CharacterIdentityComponent>()) {
		auto &id = e.get<CharacterIdentityComponent>();
		const CharacterRegistry::CharacterRecord *rec =
			CharacterRegistry::getSingleton().findCharacter(
				id.registryId);
		if (rec)
			return rec->frontAxis;
	}
	return Ogre::Vector3::NEGATIVE_UNIT_Z;
}

bool SmartObjectSystem::isInRange(const Ogre::Vector3 &charPos,
				  const Ogre::Vector3 &objPos, float radius,
				  float height)
{
	// Check XZ distance
	Ogre::Vector3 diff = charPos - objPos;
	float xzDist = std::sqrt(diff.x * diff.x + diff.z * diff.z);
	if (xzDist > radius)
		return false;

	// Check Y difference
	float yDiff = std::abs(diff.y);
	if (yDiff > height)
		return false;

	return true;
}

void SmartObjectSystem::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 CharacterSlots or default -Z)
	Ogre::Vector3 frontAxis = getFrontAxis(e);

	// 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));
}

void SmartObjectSystem::setLocomotionState(flecs::entity e,
					   const Ogre::String &animName)
{
	if (!m_animTreeSystem)
		return;
	if (!e.has<AnimationTreeComponent>())
		return;

	// Look up the path following state in PathFollowingComponent
	if (e.has<PathFollowingComponent>()) {
		auto &pf = e.get<PathFollowingComponent>();
		const PathFollowingState *pfState = pf.findState(animName);
		if (pfState) {
			// Apply ALL state machine name -> state name pairs
			for (const auto &pair : pfState->stateMachineStates) {
				m_animTreeSystem->setState(e, pair.first,
							   pair.second, false);
			}
			return;
		}
	}

	// Fallback: if no config found, try to set the state directly
	// using the animName as both state machine and state name
	m_animTreeSystem->setState(e, "main", animName, false);
}

bool SmartObjectSystem::testSmartObjectAction(flecs::entity character,
					      flecs::entity smartObject,
					      const Ogre::String &actionName)
{
	if (!character.is_alive() || !smartObject.is_alive())
		return false;

	// Find the action in the singleton database
	ActionDatabase *db = ActionDatabase::getSingletonPtr();
	if (!db)
		return false;

	const GoapAction *action = db->findAction(actionName);
	if (!action)
		return false;

	// Set up the character state to navigate to the smart object
	auto &state = m_states[character.id()];
	state.state = State::Pathfinding;
	state.target.smartObject = smartObject;
	state.target.actionName = actionName;
	state.target.path.clear();
	state.target.pathIndex = 0;
	state.target.pathRecalcTimer = 0.0f;
	state.target.isExecuting = false;
	state.target.executionTimer = 0.0f;

	std::cout << "[SmartObjectSystem] Character " << character.id()
		  << " navigating to smart object " << smartObject.id()
		  << " for action: " << actionName << std::endl;

	return true;
}

bool SmartObjectSystem::isIdle(flecs::entity character) const
{
	if (!character.is_alive())
		return true;
	auto it = m_states.find(character.id());
	if (it == m_states.end())
		return true;
	return it->second.state == State::Idle;
}

void SmartObjectSystem::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;
		});

	// Get the action database singleton
	ActionDatabase *db = ActionDatabase::getSingletonPtr();

	// Determine if we're in game mode. In game mode, player-controlled
	// characters are managed by PlayerControllerSystem and should NOT
	// be processed by SmartObjectSystem to avoid animation conflicts.
	// In editor mode, player characters can still be tested via ActionDebug.
	bool isGameMode = false;
	if (m_editorApp &&
	    m_editorApp->getGameMode() == EditorApp::GameMode::Game &&
	    m_editorApp->getGamePlayState() ==
		    EditorApp::GamePlayState::Playing) {
		isGameMode = true;
	}

	// Build a set of entity IDs that are player-controlled characters.
	std::unordered_set<flecs::entity_t> playerCharacterIds;
	if (isGameMode) {
		m_world.query<PlayerControllerComponent>().each(
			[&](flecs::entity, PlayerControllerComponent &pc) {
				if (pc.targetCharacterName.empty())
					return;
				m_world.query<EntityNameComponent>().each(
					[&](flecs::entity e,
					    EntityNameComponent &en) {
						if (en.name ==
						    pc.targetCharacterName)
							playerCharacterIds
								.insert(e.id());
					});
			});
	}

	// Process each character with a blackboard (AI-driven characters)
	m_world.query<CharacterComponent, TransformComponent, GoapBlackboard>()
		.each([&](flecs::entity e, CharacterComponent &cc,
			  TransformComponent &trans, GoapBlackboard &bb) {
			(void)cc;
			(void)trans;
			(void)bb;

			// Skip player-controlled characters - they are managed
			// by PlayerControllerSystem, not by SmartObjectSystem AI.
			if (playerCharacterIds.find(e.id()) !=
				    playerCharacterIds.end() ||
			    isPlayerControlled(e))
				return;

			auto &state = m_states[e.id()];
			state.scanTimer += deltaTime;

			// --- State: Idle - scan for smart objects ---
			if (state.state == State::Idle) {
				// Set idle animation
				setLocomotionState(e, "idle");

				// Only scan periodically
				if (state.scanTimer < 1.0f)
					return;
				state.scanTimer = 0.0f;

				Ogre::Vector3 charPos = getEntityPosition(e);

				// Find the nearest smart object with a
				// relevant action
				flecs::entity bestObject =
					flecs::entity::null();
				Ogre::String bestActionName;
				float bestDist =
					std::numeric_limits<float>::max();

				m_world.query<SmartObjectComponent,
					      TransformComponent>()
					.each([&](flecs::entity so,
						  SmartObjectComponent &soComp,
						  TransformComponent &soTrans) {
						(void)soTrans;

						for (const auto &actionName :
						     soComp.actionNames) {
							if (!db)
								continue;

							const GoapAction *action =
								db->findAction(
									actionName);
							if (!action)
								continue;

							if (!action->canRun(bb))
								continue;

							Ogre::Vector3 objPos =
								getEntityPosition(
									so);
							float dist =
								charPos.distance(
									objPos);

							if (dist < bestDist) {
								bestDist = dist;
								bestObject = so;
								bestActionName =
									actionName;
							}
						}
					});

				if (bestObject.is_alive()) {
					state.state = State::Pathfinding;
					state.target.smartObject = bestObject;
					state.target.actionName =
						bestActionName;
					state.target.path.clear();
					state.target.pathIndex = 0;
					state.target.pathRecalcTimer = 0.0f;
					state.target.isExecuting = false;
					state.target.executionTimer = 0.0f;

					std::cout
						<< "[SmartObjectSystem] Character "
						<< e.id()
						<< " found smart object "
						<< bestObject.id()
						<< " for action: "
						<< bestActionName << std::endl;
				}
				return;
			}

			// --- State: Pathfinding - find path to smart object ---
			if (state.state == State::Pathfinding) {
				if (!state.target.smartObject.is_alive()) {
					state.state = State::Idle;
					return;
				}

				Ogre::Vector3 charPos = getEntityPosition(e);
				Ogre::Vector3 objPos = getEntityPosition(
					state.target.smartObject);

				auto &soComp =
					state.target.smartObject
						.get<SmartObjectComponent>();
				if (isInRange(charPos, objPos, soComp.radius,
					      soComp.height)) {
					state.state = State::Executing;
					state.target.isExecuting = true;
					state.target.executionTimer = 0.0f;
					cc.linearVelocity = Ogre::Vector3::ZERO;
					return;
				}

				if (navmeshEntity.is_alive() && m_navSystem) {
					state.target.path.clear();
					state.target.pathIndex = 0;
					bool found = m_navSystem->findPath(
						navmeshEntity, charPos, objPos,
						state.target.path);
					if (found &&
					    !state.target.path.empty()) {
						state.state = State::Moving;
						std::cout
							<< "[SmartObjectSystem] Path found with "
							<< state.target.path
								   .size()
							<< " waypoints"
							<< std::endl;
					} else {
						state.state = State::Idle;
						cc.linearVelocity =
							Ogre::Vector3::ZERO;
					}
				} else {
					state.target.path.clear();
					state.target.path.push_back(objPos);
					state.target.pathIndex = 0;
					state.state = State::Moving;
				}
				return;
			}

			// --- State: Moving - follow path ---
			if (state.state == State::Moving) {
				if (!state.target.smartObject.is_alive() ||
				    state.target.path.empty()) {
					state.state = State::Idle;
					cc.linearVelocity = Ogre::Vector3::ZERO;
					return;
				}

				Ogre::Vector3 charPos = getEntityPosition(e);
				Ogre::Vector3 objPos = getEntityPosition(
					state.target.smartObject);

				auto &soComp =
					state.target.smartObject
						.get<SmartObjectComponent>();

				// Always check range first - if we're close enough,
				// execute the action regardless of path state
				if (isInRange(charPos, objPos, soComp.radius,
					      soComp.height)) {
					state.state = State::Executing;
					state.target.isExecuting = true;
					state.target.executionTimer = 0.0f;
					cc.linearVelocity = Ogre::Vector3::ZERO;
					return;
				}

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

				// Check if path is exhausted
				if (state.target.pathIndex >=
				    (int)state.target.path.size()) {
					float distToObj =
						charPos.distance(objPos);
					// If we're close enough to the object
					// (within radius*2), teleport to range
					// and execute
					if (distToObj < soComp.radius * 2.0f) {
						// Teleport character to be
						// within range
						Ogre::Vector3 dirToObj =
							objPos - charPos;
						dirToObj.y = 0;
						float xzDist =
							dirToObj.length();
						if (xzDist > 0.01f) {
							dirToObj /= xzDist;
							Ogre::Vector3 newPos =
								objPos -
								dirToObj *
									(soComp.radius *
									 0.5f);
							newPos.y = charPos.y;
							if (trans.node) {
								trans.node->setPosition(
									newPos);
							}
							trans.position = newPos;
						}
						state.state = State::Executing;
						state.target.isExecuting = true;
						state.target.executionTimer =
							0.0f;
						cc.linearVelocity =
							Ogre::Vector3::ZERO;
						return;
					}
					// Too far - recalculate path
					state.target.path.clear();
					state.target.path.push_back(objPos);
					state.target.pathIndex = 0;
				}

				Ogre::Vector3 targetPos =
					state.target
						.path[state.target.pathIndex];
				Ogre::Vector3 toTarget = targetPos - charPos;

				// Waypoint arrival threshold - use generous
				// value since waypoints are just guide points
				const float WAYPOINT_THRESHOLD = 1.0f;
				if (toTarget.length() < WAYPOINT_THRESHOLD) {
					state.target.pathIndex++;
					if (state.target.pathIndex >=
					    (int)state.target.path.size()) {
						// Path exhausted - teleport to
						// range and execute if close
						// enough
						float distToObj =
							charPos.distance(
								objPos);
						if (distToObj <
						    soComp.radius * 2.0f) {
							Ogre::Vector3 dirToObj =
								objPos -
								charPos;
							dirToObj.y = 0;
							float xzDist =
								dirToObj.length();
							if (xzDist > 0.01f) {
								dirToObj /=
									xzDist;
								Ogre::Vector3 newPos =
									objPos -
									dirToObj *
										(soComp.radius *
										 0.5f);
								newPos.y =
									charPos.y;
								if (trans.node) {
									trans.node
										->setPosition(
											newPos);
								}
								trans.position =
									newPos;
							}
							state.state =
								State::Executing;
							state.target
								.isExecuting =
								true;
							state.target
								.executionTimer =
								0.0f;
							cc.linearVelocity =
								Ogre::Vector3::
									ZERO;
							return;
						}
						// Too far - recalculate path
						state.target.path.clear();
						state.target.path.push_back(
							objPos);
						state.target.pathIndex = 0;
						cc.linearVelocity =
							Ogre::Vector3::ZERO;
						return;
					}
					targetPos =
						state.target.path
							[state.target.pathIndex];
					toTarget = targetPos - charPos;
				}

				// Only set velocity direction - AnimationTreeSystem
				// applies root motion if useRootMotion is enabled.
				// The character is rotated to face the movement
				// direction, no other transforms are applied.
				toTarget.y = 0;
				if (toTarget.squaredLength() > 0.0001f) {
					toTarget.normalise();

					// Determine speed based on distance
					// to target
					float distToTarget =
						(charPos - objPos).length();
					float speed = 2.5f; // walk speed

					// Use run speed if far away
					if (e.has<PathFollowingComponent>()) {
						auto &pf = e.get<
							PathFollowingComponent>();
						if (distToTarget >
						    pf.walkSpeed * 3.0f) {
							speed = pf.runSpeed;
							setLocomotionState(
								e, "run");
						} else {
							speed = pf.walkSpeed;
							setLocomotionState(
								e, "walk");
						}
					} else {
						setLocomotionState(e, "walk");
					}

					// When root motion is enabled, the
					// AnimationTreeSystem already computes
					// linearVelocity from animation root
					// bone displacement. Do NOT overwrite
					// it with direct movement velocity.
					if (!hasRootMotion(e))
						cc.linearVelocity =
							toTarget * speed;

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

			// --- State: Executing - run the action ---
			if (state.state == State::Executing) {
				if (!state.target.smartObject.is_alive()) {
					state.state = State::Idle;
					return;
				}

				// Keep character stopped
				cc.linearVelocity = Ogre::Vector3::ZERO;

				// Set idle animation while executing
				setLocomotionState(e, "idle");

				if (state.target.isExecuting && db) {
					const GoapAction *action =
						db->findAction(
							state.target.actionName);
					if (action && m_btSystem) {
						state.target.executionTimer +=
							deltaTime;

						if (state.target.executionTimer >
						    3.0f) {
							bb.apply(
								action->effects);
							state.state =
								State::Idle;
							state.target
								.isExecuting =
								false;
							std::cout
								<< "[SmartObjectSystem] Action '"
								<< state.target
									   .actionName
								<< "' completed"
								<< std::endl;
						}
					} else {
						if (action) {
							bb.apply(
								action->effects);
						}
						state.state = State::Idle;
						state.target.isExecuting =
							false;
					}
				} else {
					state.state = State::Idle;
				}
			}
		});

	// Also process characters with ActionDebug (for editor testing)
	m_world.query<CharacterComponent, TransformComponent, ActionDebug>().each([&](flecs::entity
											      e,
										      CharacterComponent
											      &cc,
										      TransformComponent
											      &trans,
										      ActionDebug
											      &debug) {
		(void)trans;
		(void)debug;

		// Skip player-controlled characters - they are managed
		// by PlayerControllerSystem, not by SmartObjectSystem AI.
		if (playerCharacterIds.find(e.id()) != playerCharacterIds.end() ||
		    isPlayerControlled(e))
			return;

		auto &state = m_states[e.id()];

		// When the behavior tree is running (via ActionDebug),
		// do NOT override animation states - the behavior tree's
		// setAnimationState nodes should be the sole controller.
		if (debug.isRunning && !debug.currentActionName.empty()) {
			// Behavior tree controls animations, skip
			// locomotion state override
		} else if (state.state == State::Idle) {
			setLocomotionState(e, "idle");
			return;
		}

		GoapBlackboard &bb = debug.blackboard;

		// --- State: Pathfinding ---
		if (state.state == State::Pathfinding) {
			if (!state.target.smartObject.is_alive()) {
				std::cout
					<< "[SO] Pathfinding: target dead, going idle"
					<< std::endl;
				state.state = State::Idle;
				return;
			}

			Ogre::Vector3 charPos = getEntityPosition(e);
			Ogre::Vector3 objPos =
				getEntityPosition(state.target.smartObject);

			auto &soComp = state.target.smartObject
					       .get<SmartObjectComponent>();
			if (isInRange(charPos, objPos, soComp.radius,
				      soComp.height)) {
				std::cout
					<< "[SO] Pathfinding: already in range, executing"
					<< std::endl;
				state.state = State::Executing;
				state.target.isExecuting = true;
				state.target.executionTimer = 0.0f;
				cc.linearVelocity = Ogre::Vector3::ZERO;
				return;
			}

			if (navmeshEntity.is_alive() && m_navSystem) {
				state.target.path.clear();
				state.target.pathIndex = 0;
				bool found = m_navSystem->findPath(
					navmeshEntity, charPos, objPos,
					state.target.path);
				if (found && !state.target.path.empty()) {
					std::cout
						<< "[SO] Pathfinding: path found with "
						<< state.target.path.size()
						<< " waypoints" << std::endl;
					state.state = State::Moving;
				} else {
					std::cout
						<< "[SO] Pathfinding: no path found, going idle"
						<< std::endl;
					state.state = State::Idle;
					cc.linearVelocity = Ogre::Vector3::ZERO;
				}
			} else {
				std::cout
					<< "[SO] Pathfinding: no navmesh, going direct"
					<< std::endl;
				state.target.path.clear();
				state.target.path.push_back(objPos);
				state.target.pathIndex = 0;
				state.state = State::Moving;
			}
			return;
		}

		// --- State: Moving ---
		if (state.state == State::Moving) {
			if (!state.target.smartObject.is_alive() ||
			    state.target.path.empty()) {
				std::cout
					<< "[SO] Moving: target dead or path empty, idle"
					<< std::endl;
				state.state = State::Idle;
				cc.linearVelocity = Ogre::Vector3::ZERO;
				return;
			}

			Ogre::Vector3 charPos = getEntityPosition(e);
			Ogre::Vector3 objPos =
				getEntityPosition(state.target.smartObject);

			auto &soComp = state.target.smartObject
					       .get<SmartObjectComponent>();

			// Always check range first
			if (isInRange(charPos, objPos, soComp.radius,
				      soComp.height)) {
				std::cout << "[SO] Moving: in range, executing"
					  << std::endl;
				state.state = State::Executing;
				state.target.isExecuting = true;
				state.target.executionTimer = 0.0f;
				cc.linearVelocity = Ogre::Vector3::ZERO;
				return;
			}

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

			// Check if path is exhausted
			if (state.target.pathIndex >=
			    (int)state.target.path.size()) {
				float distToObj = charPos.distance(objPos);
				std::cout
					<< "[SO] Moving: path exhausted, dist="
					<< distToObj
					<< " radius*2=" << soComp.radius * 2.0f
					<< std::endl;
				// If close enough, teleport to range
				// and execute
				if (distToObj < soComp.radius * 2.0f) {
					std::cout
						<< "[SO] Moving: close enough, teleporting"
						<< std::endl;
					Ogre::Vector3 dirToObj =
						objPos - charPos;
					dirToObj.y = 0;
					float xzDist = dirToObj.length();
					if (xzDist > 0.01f) {
						dirToObj /= xzDist;
						Ogre::Vector3 newPos =
							objPos -
							dirToObj *
								(soComp.radius *
								 0.5f);
						newPos.y = charPos.y;
						if (trans.node) {
							trans.node->setPosition(
								newPos);
						}
						trans.position = newPos;
					}
					state.state = State::Executing;
					state.target.isExecuting = true;
					state.target.executionTimer = 0.0f;
					cc.linearVelocity = Ogre::Vector3::ZERO;
					return;
				}
				// Too far - recalculate path
				std::cout << "[SO] Moving: too far, recalc path"
					  << std::endl;
				state.target.path.clear();
				state.target.path.push_back(objPos);
				state.target.pathIndex = 0;
			}

			Ogre::Vector3 targetPos =
				state.target.path[state.target.pathIndex];
			Ogre::Vector3 toTarget = targetPos - charPos;

			// Waypoint arrival threshold - generous since
			// waypoints are just guide points
			const float WAYPOINT_THRESHOLD = 1.0f;
			if (toTarget.length() < WAYPOINT_THRESHOLD) {
				state.target.pathIndex++;
				if (state.target.pathIndex >=
				    (int)state.target.path.size()) {
					// Path exhausted - teleport to
					// range and execute if close
					// enough
					float distToObj =
						charPos.distance(objPos);
					std::cout
						<< "[SO] Moving: last waypoint reached, dist="
						<< distToObj << " radius*2="
						<< soComp.radius * 2.0f
						<< std::endl;
					if (distToObj < soComp.radius * 2.0f) {
						std::cout
							<< "[SO] Moving: close enough at waypoint, teleporting"
							<< std::endl;
						Ogre::Vector3 dirToObj =
							objPos - charPos;
						dirToObj.y = 0;
						float xzDist =
							dirToObj.length();
						if (xzDist > 0.01f) {
							dirToObj /= xzDist;
							Ogre::Vector3 newPos =
								objPos -
								dirToObj *
									(soComp.radius *
									 0.5f);
							newPos.y = charPos.y;
							if (trans.node) {
								trans.node->setPosition(
									newPos);
							}
							trans.position = newPos;
						}
						state.state = State::Executing;
						state.target.isExecuting = true;
						state.target.executionTimer =
							0.0f;
						cc.linearVelocity =
							Ogre::Vector3::ZERO;
						return;
					}
					// Too far - recalculate path
					std::cout
						<< "[SO] Moving: too far at waypoint, recalc"
						<< std::endl;
					state.target.path.clear();
					state.target.path.push_back(objPos);
					state.target.pathIndex = 0;
					cc.linearVelocity = Ogre::Vector3::ZERO;
					return;
				}
				targetPos =
					state.target
						.path[state.target.pathIndex];
				toTarget = targetPos - charPos;
			}

			toTarget.y = 0;
			if (toTarget.squaredLength() > 0.0001f) {
				toTarget.normalise();

				float distToTarget =
					(charPos - objPos).length();
				float speed = 2.5f;
				if (e.has<PathFollowingComponent>()) {
					auto &pf =
						e.get<PathFollowingComponent>();
					speed = pf.walkSpeed;
					if (distToTarget >
					    pf.walkSpeed * 3.0f) {
						speed = pf.runSpeed;
						setLocomotionState(e, "run");
					} else {
						speed = pf.walkSpeed;
						setLocomotionState(e, "walk");
					}
				} else {
					setLocomotionState(e, "walk");
				}

				// When root motion is enabled, the
				// AnimationTreeSystem already computes
				// linearVelocity from animation root
				// bone displacement. Do NOT overwrite
				// it with direct movement velocity.
				if (!hasRootMotion(e))
					cc.linearVelocity = toTarget * speed;

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

		// --- State: Executing ---
		if (state.state == State::Executing) {
			cc.linearVelocity = Ogre::Vector3::ZERO;

			// When the behavior tree is running (via ActionDebug),
			// do NOT override animation states - the behavior tree's
			// setAnimationState nodes should be the sole controller.
			if (!debug.isRunning ||
			    debug.currentActionName.empty()) {
				setLocomotionState(e, "idle");
			}

			if (state.target.isExecuting && db) {
				const GoapAction *action =
					db->findAction(state.target.actionName);
				if (action) {
					// Kick off the action's
					// behavior tree via
					// BehaviorTreeSystem's
					// ActionDebug path.
					// Only reset runTimer on the
					// first frame of execution
					// (when isRunning is false) so
					// BehaviorTreeSystem re-initializes
					// the runner state for a fresh
					// execution. Do NOT reset every
					// frame or isNewlyActive will
					// fire repeatedly.
					if (!debug.isRunning) {
						debug.runTimer = 0.0f;
					}
					debug.isRunning = true;
					debug.currentActionName =
						state.target.actionName;

					// Check if the behavior tree
					// has finished (sequence
					// completed or failed).
					// Only check completion when
					// runTimer > 0 (at least one
					// frame has passed since
					// starting) to avoid
					// immediately detecting
					// completion on the same frame
					// the tree was started.
					auto &btState =
						m_btSystem->getActionDebugState(
							e.id());
					if (debug.runTimer > 0.0f &&
					    btState.treeResult !=
						    BehaviorTreeSystem::Status::
							    running) {
						// Behavior tree
						// completed - stop
						// evaluation
						debug.isRunning = false;
						debug.currentActionName.clear();

						bb.apply(action->effects);
						state.state = State::Idle;
						state.target.isExecuting =
							false;
						debug.lastResult =
							"Smart object action '" +
							state.target.actionName +
							"' completed";
					}
				} else {
					state.state = State::Idle;
					state.target.isExecuting = false;
				}
			} else {
				state.state = State::Idle;
			}
		}
	});
}
