#include "CharacterSystem.hpp"
#include "CharacterSlotSystem.hpp"
#include "../components/CharacterSlots.hpp"
#include <Jolt/Physics/Character/Character.h>
#include <Jolt/Physics/Collision/GroupFilterTable.h>
#include <Jolt/Physics/Body/BodyInterface.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <OgreLogManager.h>
#include <OgreEntity.h>
#include <OgreSkeletonInstance.h>
#include <iostream>

static Ogre::Entity *getMasterEntity(flecs::entity e)
{
	if (!e.is_alive())
		return nullptr;
	CharacterSlotSystem *slotSystem = CharacterSlotSystem::getSingletonPtr();
	if (!slotSystem)
		return nullptr;
	return slotSystem->getMasterEntity(e);
}

static bool getBoneWorldMatrix(Ogre::Entity *masterEnt,
			       const Ogre::String &boneName,
			       Ogre::Matrix4 &outMat)
{
	if (!masterEnt || boneName.empty() || !masterEnt->hasSkeleton())
		return false;

	Ogre::SkeletonInstance *skel = masterEnt->getSkeleton();
	if (!skel)
		return false;

	Ogre::Bone *bone = nullptr;
	try {
		bone = skel->getBone(boneName);
	} catch (...) {
		return false;
	}
	if (!bone)
		return false;

	outMat = bone->_getFullTransform();
	Ogre::SceneNode *node = masterEnt->getParentSceneNode();
	if (node)
		outMat = node->_getFullTransform() * outMat;
	return true;
}

CharacterSystem::CharacterSystem(flecs::world &world,
				 Ogre::SceneManager *sceneMgr)
	: m_world(world)
	, m_sceneMgr(sceneMgr)
{
	m_world.observer<CharacterComponent>("CharacterCleanup")
		.event(flecs::OnRemove)
		.each([this](flecs::entity e, CharacterComponent &) {
			teardownEntity(e);
		});
}

CharacterSystem::~CharacterSystem()
{
	std::vector<flecs::entity_t> toRemove;
	for (auto &pair : m_states)
		toRemove.push_back(pair.first);
	for (auto id : toRemove)
		teardownEntity(m_world.entity(id));
}

void CharacterSystem::initialize()
{
	m_physics = JoltPhysicsWrapper::getSingletonPtr();
	m_initialized = true;
}

JPH::ShapeRefC
CharacterSystem::createColliderShape(PhysicsColliderComponent &collider)
{
	if (!collider.shapeDirty && collider.shape)
		return collider.shape;

	JPH::ShapeRefC result;
	switch (collider.shapeType) {
	case PhysicsColliderComponent::ShapeType::Box:
		result = m_physics->createBoxShape(collider.parameters);
		break;
	case PhysicsColliderComponent::ShapeType::Sphere:
		result = m_physics->createSphereShape(collider.radius);
		break;
	case PhysicsColliderComponent::ShapeType::Capsule:
		result = m_physics->createCapsuleShape(collider.halfHeight,
						       collider.radius);
		break;
	case PhysicsColliderComponent::ShapeType::Cylinder:
		result = m_physics->createCylinderShape(collider.halfHeight,
							collider.radius);
		break;
	case PhysicsColliderComponent::ShapeType::Mesh: {
		if (!collider.meshName.empty()) {
			try {
				result = m_physics->createMeshShape(
					collider.meshName);
			} catch (...) {
				Ogre::LogManager::getSingleton().logMessage(
					"Failed to create mesh shape: " +
					collider.meshName);
			}
		}
		break;
	}
	case PhysicsColliderComponent::ShapeType::ConvexHull: {
		if (!collider.meshName.empty()) {
			try {
				result = m_physics->createConvexHullShape(
					collider.meshName);
			} catch (...) {
				Ogre::LogManager::getSingleton().logMessage(
					"Failed to create convex hull shape: " +
					collider.meshName);
			}
		}
		break;
	}
	}

	if (!result)
		result = m_physics->createBoxShape(
			Ogre::Vector3(0.1f, 0.1f, 0.1f));

	if (collider.offset != Ogre::Vector3::ZERO ||
	    collider.rotationOffset != Ogre::Quaternion::IDENTITY)
		result = m_physics->createRotatedTranslatedShape(
			collider.offset, collider.rotationOffset, result);

	collider.shape = result;
	collider.shapeDirty = false;
	return result;
}

JPH::ShapeRefC CharacterSystem::buildCharacterShape(flecs::entity e,
						    CharacterComponent &cc)
{
	std::vector<JPH::ShapeRefC> shapes;
	std::vector<Ogre::Vector3> positions;
	std::vector<Ogre::Quaternion> rotations;

	/* Main capsule */
	float halfHeight = cc.getHalfHeight();
	JPH::ShapeRefC capsule =
		m_physics->createCapsuleShape(halfHeight, cc.radius);
	/* Shift capsule so its bottom sits at y = 0 */
	Ogre::Vector3 capsulePos(0.0f, halfHeight + cc.radius, 0.0f);
	capsule = m_physics->createRotatedTranslatedShape(
		capsulePos + cc.offset, Ogre::Quaternion::IDENTITY, capsule);
	shapes.push_back(capsule);
	positions.push_back(Ogre::Vector3::ZERO);
	rotations.push_back(Ogre::Quaternion::IDENTITY);

	/* Child colliders */
	e.children([&](flecs::entity child) {
		if (!child.has<PhysicsColliderComponent>() ||
		    !child.has<TransformComponent>())
			return;
		auto &collider = child.get_mut<PhysicsColliderComponent>();
		auto &transform = child.get<TransformComponent>();

		JPH::ShapeRefC shape = createColliderShape(collider);
		if (shape) {
			shapes.push_back(shape);
			positions.push_back(transform.position);
			rotations.push_back(transform.rotation);
		}
	});

	if (shapes.size() == 1)
		return shapes[0];

	return m_physics->createStaticCompoundShape(shapes, positions,
						    rotations);
}

void CharacterSystem::createHeadCollider(flecs::entity e,
					 CharacterComponent &cc,
					 const TransformComponent &transform)
{
	if (!m_physics)
		return;

	if (cc.headColliderBody.IsInvalid() == false) {
		m_physics->removeBody(cc.headColliderBody);
		m_physics->destroyBody(cc.headColliderBody);
		cc.headColliderBody = JPH::BodyID();
	}

	if (cc.collisionGroupId == 0)
		return;

	float headRadius = cc.headRadius > 0.0f ? cc.headRadius : 0.13f;

	Ogre::Matrix4 headWorld;
	bool hasBone = getBoneWorldMatrix(getMasterEntity(e), cc.headBoneName,
					  headWorld);
	if (hasBone) {
		headWorld.setTrans(headWorld.getTrans() +
				   Ogre::Vector3(0.0f, cc.headOffsetY, 0.0f));
	} else {
		float headOffsetY = cc.headOffsetY > 0.0f ?
					    cc.headOffsetY :
					    cc.getTotalHeight() - headRadius;
		Ogre::Vector3 headOffset(0.0f, headOffsetY, 0.0f);
		headOffset += cc.offset;
		Ogre::Vector3 headPos =
			transform.node->_getDerivedPosition() +
			transform.node->_getDerivedOrientation() *
				Ogre::Vector3(cc.offset.x, 0.0f, cc.offset.z) +
			Ogre::Vector3(0.0f, headOffsetY + cc.offset.y, 0.0f);
		Ogre::Matrix4 local = Ogre::Matrix4::IDENTITY;
		local.makeTransform(headPos, Ogre::Vector3::UNIT_SCALE,
				    Ogre::Quaternion::IDENTITY);
		headWorld = local;
	}

	JPH::ShapeRefC sphere = m_physics->createSphereShape(headRadius);
	cc.headColliderBody = m_physics->createBody(
		sphere, 0.0f, headWorld.getTrans(),
		Ogre::Quaternion::IDENTITY,
		JPH::EMotionType::Kinematic, Layers::HEAD);

	JPH::PhysicsSystem *physSystem = m_physics->getPhysicsSystem();
	if (physSystem) {
		JPH::BodyInterface &bi = physSystem->GetBodyInterface();
		JPH::GroupFilterTable *filter =
			m_physics->getOrCreateGroupFilter(
				cc.collisionGroupId);
		bi.SetCollisionGroup(cc.headColliderBody,
		     JPH::CollisionGroup(filter, cc.collisionGroupId,
						 1));
	}

	m_physics->addBody(cc.headColliderBody,
			   JPH::EActivation::Activate);
}

void CharacterSystem::createChestCollider(flecs::entity e,
					  CharacterComponent &cc)
{
	if (!m_physics)
		return;

	if (cc.chestColliderBody.IsInvalid() == false) {
		m_physics->removeBody(cc.chestColliderBody);
		m_physics->destroyBody(cc.chestColliderBody);
		cc.chestColliderBody = JPH::BodyID();
	}

	if (cc.collisionGroupId == 0 ||
	    cc.chestHalfExtents.x <= 0.0f ||
	    cc.chestHalfExtents.y <= 0.0f ||
	    cc.chestHalfExtents.z <= 0.0f)
		return;

	Ogre::Matrix4 chestWorld;
	if (!getBoneWorldMatrix(getMasterEntity(e), cc.chestBoneName,
				chestWorld))
		return;

	Ogre::Vector3 chestPos = chestWorld.getTrans() +
				 Ogre::Vector3(0.0f, cc.chestOffsetY, 0.0f);
	Ogre::Matrix3 chestRotMat = chestWorld.linear().orthonormalised();
	Ogre::Quaternion chestRot = Ogre::Quaternion(chestRotMat);

	JPH::ShapeRefC box = m_physics->createBoxShape(cc.chestHalfExtents);
	cc.chestColliderBody = m_physics->createBody(
		box, 0.0f, chestPos, chestRot,
		JPH::EMotionType::Kinematic, Layers::HEAD);

	JPH::PhysicsSystem *physSystem = m_physics->getPhysicsSystem();
	if (physSystem) {
		JPH::BodyInterface &bi = physSystem->GetBodyInterface();
		JPH::GroupFilterTable *filter =
			m_physics->getOrCreateGroupFilter(
				cc.collisionGroupId);
		bi.SetCollisionGroup(cc.chestColliderBody,
		     JPH::CollisionGroup(filter, cc.collisionGroupId,
						 2));
	}

	m_physics->addBody(cc.chestColliderBody,
			   JPH::EActivation::Activate);
}

void CharacterSystem::setupEntity(flecs::entity e, CharacterComponent &cc)
{
	if (!m_physics)
		return;

	if (!e.has<TransformComponent>())
		return;
	auto &transform = e.get<TransformComponent>();
	if (!transform.node)
		return;

	teardownEntity(e);

	JPH::ShapeRefC shape = buildCharacterShape(e, cc);
	if (!shape)
		return;

	/* Assign a non-zero collision group for per-character filtering
	 * (body = subgroup 0, head = subgroup 1, chest = subgroup 2,
	 *  hair joints = 3+). */
	if (cc.collisionGroupId == 0)
		cc.collisionGroupId = static_cast<uint32_t>(e.id()) |
				      0x80000000;

	JPH::CharacterBase *base =
		m_physics->createCharacter(transform.node, shape);
	if (!base)
		return;

	auto *ch = static_cast<JPH::Character *>(base);

	/* Set collision group on character body so hair filtering works. */
	JPH::PhysicsSystem *physSystem = m_physics->getPhysicsSystem();
	if (physSystem) {
		JPH::BodyInterface &bi = physSystem->GetBodyInterface();
		JPH::GroupFilterTable *filter =
			m_physics->getOrCreateGroupFilter(
				cc.collisionGroupId);
		bi.SetCollisionGroup(
			ch->GetBodyID(),
			JPH::CollisionGroup(filter, cc.collisionGroupId,
					    0));
	}

	ch->AddToPhysicsSystem();

	createHeadCollider(e, cc, transform);
	createChestCollider(e, cc);
	// Don't modify gravity factor - let buoyancy handle floating/sinking
	// m_physics->setGravityFactor(ch->GetBodyID(), 0.0f);
	cc.hasFloor = false;
	std::cout << "CharacterSystem::setupEntity: entity=" << e.id()
		  << " nodePos=" << transform.node->_getDerivedPosition()
		  << " parent="
		  << (transform.node->getParent() ?
			      transform.node->getParent()->getName() :
			      "<root>")
		  << " radius=" << cc.radius << " height=" << cc.height
		  << " dirty=" << cc.dirty << " hasFloor=" << cc.hasFloor
		  << std::endl;

	CharacterState state;
	state.character = ch;
	state.sceneNode = transform.node;
	m_states[e.id()] = state;

	cc.dirty = false;
}

void CharacterSystem::teardownEntity(flecs::entity e)
{
	auto it = m_states.find(e.id());
	if (it == m_states.end())
		return;

	CharacterState &state = it->second;
	if (state.character) {
		state.character->RemoveFromPhysicsSystem();
		m_physics->destroyCharacter(
			std::shared_ptr<JPH::Character>(state.character));
	}

	if (e.has<CharacterComponent>()) {
		auto &cc = e.get_mut<CharacterComponent>();
		if (cc.headColliderBody.IsInvalid() == false) {
			m_physics->removeBody(cc.headColliderBody);
			m_physics->destroyBody(cc.headColliderBody);
			cc.headColliderBody = JPH::BodyID();
		}
		if (cc.chestColliderBody.IsInvalid() == false) {
			m_physics->removeBody(cc.chestColliderBody);
			m_physics->destroyBody(cc.chestColliderBody);
			cc.chestColliderBody = JPH::BodyID();
		}
	}

	m_states.erase(it);
}

void CharacterSystem::disablePhysics(flecs::entity e)
{
	if (!m_physics)
		return;

	auto it = m_states.find(e.id());
	if (it == m_states.end())
		return;

	CharacterState &state = it->second;
	if (!state.character)
		return;

	JPH::BodyID bodyID = state.character->GetBodyID();
	if (m_physics->isAdded(bodyID)) {
		state.character->RemoveFromPhysicsSystem();
	}

	if (e.has<CharacterComponent>()) {
		auto &cc = e.get_mut<CharacterComponent>();
		cc.physicsDisabled = true;
	}
}

void CharacterSystem::enablePhysics(flecs::entity e)
{
	if (!m_physics)
		return;

	auto it = m_states.find(e.id());
	if (it == m_states.end())
		return;

	CharacterState &state = it->second;
	if (!state.character)
		return;

	JPH::BodyID bodyID = state.character->GetBodyID();
	if (!m_physics->isAdded(bodyID)) {
		state.character->AddToPhysicsSystem();
	}

	if (e.has<CharacterComponent>()) {
		auto &cc = e.get_mut<CharacterComponent>();
		cc.physicsDisabled = false;
	}
}

void CharacterSystem::update(float deltaTime)
{
	if (!m_initialized || !m_physics)
		return;

	m_world.query<CharacterComponent, TransformComponent>().each(
		[this, deltaTime](flecs::entity e, CharacterComponent &cc,
				  TransformComponent &transform) {
			if (!cc.enabled) {
				teardownEntity(e);
				return;
			}

			if (cc.dirty) {
				setupEntity(e, cc);
				cc.dirty = false;
			}

			auto it = m_states.find(e.id());
			if (it == m_states.end())
				return;

			CharacterState &state = it->second;
			if (!state.character || !state.sceneNode)
				return;

			/* Skip physics processing when physics is disabled */
			if (cc.physicsDisabled)
				return;

			/* Read current physics position */
			Ogre::Vector3 charPos = JoltPhysics::convert(
				state.character->GetPosition());
			Ogre::Vector3 nodePos =
				state.sceneNode->_getDerivedPosition();

			/*
		 * Root motion drives physics velocity (AnimationTreeSystem
		 * computes it from the animation displacement). The physics
		 * character moves naturally via velocity, and physics writes
		 * the position back to the scene node. No teleportation or
		 * position sync needed - this avoids jitter.
		 *
		 * If the scene node was moved externally (editor gizmo),
		 * teleport the physics character to match.
		 */
			Ogre::Vector3 diff = nodePos - charPos;
			if (diff.squaredLength() > 0.001f) {
				state.character->SetPosition(
					JoltPhysics::convert(nodePos));
			}
			/* Root motion velocity is applied via linear velocity
			 * above. No special physics handling needed - physics
			 * writes position back to scene node normally. */

			/* Apply velocity via Jolt linear velocity.
			 * For a dynamic character body, preserving the
			 * physics-driven Y velocity when on the ground
			 * causes gravity to continuously pull the character
			 * into the floor. The collision solver pushes back,
			 * but the body never settles, producing visible
			 * micro-jitter. Zero out Y when supported and no
			 * explicit vertical input is requested. */
			JPH::Vec3 currentVel =
				state.character->GetLinearVelocity();
			JPH::Vec3 desiredVel = JoltPhysics::convert<JPH::Vec3>(
				cc.linearVelocity);
			JPH::Vec3 finalVel;
			finalVel.SetX(desiredVel.GetX());
			finalVel.SetZ(desiredVel.GetZ());
			if (desiredVel.GetY() != 0.0f) {
				finalVel.SetY(desiredVel.GetY());
			} else if (state.character->IsSupported()) {
				finalVel.SetY(0.0f);
			} else {
				finalVel.SetY(currentVel.GetY());
			}
			state.character->SetLinearVelocity(finalVel);


			/* Floor detection: raycast downward to find ground */
			if (cc.useGravity && !cc.hasFloor) {
				Ogre::Vector3 rayStart = charPos;
				Ogre::Vector3 rayEnd =
					rayStart +
					Ogre::Vector3(0.0f,
						      -cc.floorCheckDistance,
						      0.0f);
				Ogre::Vector3 hitPos;
				JPH::BodyID hitBody;
				if (m_physics->raycastQuery(rayStart, rayEnd,
							    hitPos, hitBody)) {
					cc.hasFloor = true;
					// Don't modify gravity factor - let buoyancy handle floating/sinking
					// m_physics->setGravityFactor(
					//	state.character->GetBodyID(),
					//	1.0f);
					std::cout
						<< "CharacterSystem::floor detected: entity="
						<< e.id() << " dist="
						<< (charPos - hitPos).length()
						<< std::endl;
				}
			}

			/* Sync rotation from scene node */
			state.character->SetRotation(JoltPhysics::convert(
				state.sceneNode->_getDerivedOrientation()));

			/* Sync head and chest colliders to animated bones. */
			Ogre::Entity *masterEnt = getMasterEntity(e);
			Ogre::Matrix4 boneWorld;

			if (cc.headColliderBody.IsInvalid() == false &&
			    transform.node) {
				float headRadius = cc.headRadius > 0.0f ?
							   cc.headRadius :
							   0.13f;
				float headOffsetY = cc.headOffsetY > 0.0f ?
							    cc.headOffsetY :
							    cc.getTotalHeight() - headRadius;

				Ogre::Vector3 headPos;
				if (getBoneWorldMatrix(masterEnt, cc.headBoneName,
						       boneWorld)) {
					headPos = boneWorld.getTrans() +
						  Ogre::Vector3(0.0f,
								cc.headOffsetY,
								0.0f);
				} else {
					headPos =
						transform.node->_getDerivedPosition() +
						transform.node->_getDerivedOrientation() *
							Ogre::Vector3(cc.offset.x,
								      0.0f,
								      cc.offset.z) +
						Ogre::Vector3(0.0f,
							      headOffsetY +
								      cc.offset.y,
							      0.0f);
				}
				m_physics->setPositionAndRotation(
					cc.headColliderBody, headPos,
					Ogre::Quaternion::IDENTITY, true);
			}

			if (cc.chestColliderBody.IsInvalid() == false &&
			    getBoneWorldMatrix(masterEnt, cc.chestBoneName,
					       boneWorld)) {
				Ogre::Vector3 chestPos =
					boneWorld.getTrans() +
					Ogre::Vector3(0.0f, cc.chestOffsetY,
						      0.0f);
				Ogre::Matrix3 chestRotMat =
					boneWorld.linear().orthonormalised();
				Ogre::Quaternion chestRot =
					Ogre::Quaternion(chestRotMat);
				m_physics->setPositionAndRotation(
					cc.chestColliderBody, chestPos,
					chestRot, true);
			}
		});
}
