#include <iostream>
#include <Ogre.h>
#include "GameData.h"
#include "WaterModule.h"
#include "TerrainModule.h"
#include "Components.h"
#include "PhysicsModule.h"
#include "physics.h"
#include "CharacterAnimationModule.h"
#include "CharacterManagerModule.h"
#include "CharacterModule.h"
#include <tracy/Tracy.hpp>
namespace ECS
{
CharacterModule::CharacterModule(flecs::world &ecs)
{
    ZoneScoped;
	struct TriggerPhysicsChange {};
    static std::vector<Ogre::String> part_names;
    const std::vector<Ogre::String> &groups =
        Ogre::ResourceGroupManager::getSingleton().getResourceGroups();
    if (part_names.size() == 0) {
        int i;
        for (i = 0; i < groups.size(); i++) {
            std::vector<Ogre::String> names =
                *Ogre::ResourceGroupManager::getSingleton()
                     .findResourceNames(groups[i],
                                "body_part_*.json");
            part_names.insert(part_names.end(), names.begin(),
                      names.end());
        }
    }
    body_parts = nlohmann::json::object();
    for (auto &g : part_names) {
        Ogre::String group = Ogre::ResourceGroupManager::getSingleton()
                         .findGroupContainingResource(g);
        Ogre::DataStreamPtr stream =
            Ogre::ResourceGroupManager::getSingleton().openResource(
                g, group);
        Ogre::String json = stream->getAsString();
        nlohmann::json jdata = nlohmann::json::parse(json);
        if (jdata.find("age") == jdata.end())
            continue;
        if (jdata.find("sex") == jdata.end())
            continue;
        if (jdata.find("slot") == jdata.end())
            continue;
        if (jdata.find("mesh") == jdata.end())
            continue;
        Ogre::String age = jdata["age"].get<Ogre::String>();
        Ogre::String sex = jdata["sex"].get<Ogre::String>();
        Ogre::String slot = jdata["slot"].get<Ogre::String>();
        Ogre::String mesh = jdata["mesh"].get<Ogre::String>();
        if (body_parts.find(age) == body_parts.end())
            body_parts[age] = nlohmann::json::object();
        if (body_parts[age].find(sex) == body_parts[age].end())
            body_parts[age][sex] = nlohmann::json::object();
        if (body_parts[age][sex].find(slot) ==
            body_parts[age][sex].end())
            body_parts[age][sex][slot] = nlohmann::json::array();
        body_parts[age][sex][slot].push_back(mesh);
        mesh_names.insert(mesh);
        Ogre::MeshManager::getSingleton().load(mesh, "Characters");
    }
    std::cout << body_parts.dump(4) << std::endl;
    ecs.module<CharacterModule>();
	ecs.component<Character>();
	ecs.component<Player>();
    ecs.component<CharacterBase>()
        .on_remove([this](flecs::entity e, CharacterBase &ch) {
            ZoneScoped;
            if (characterEntitiesFace.find(e) !=
                    characterEntitiesFace.end() ||
                characterNodes.find(e) != characterNodes.end()) {
                // FIXME: clean up data
                characterEntitiesFace.erase(e);
                characterEntitiesTop.erase(e);
                characterEntitiesBottom.erase(e);
                characterEntitiesFeet.erase(e);
                characterNodes.erase(e);
                ECS::modified<CharacterModule>();
            }
        })
        .on_add([this](flecs::entity e, CharacterBase &ch) {
            if (characterNodes.find(e) == characterNodes.end()) {
                ZoneScoped;
                OgreAssert(characterModelsFace.find(e) !=
                           characterModelsFace.end(),
                       "no model set");
                const EngineData &eng = ECS::get<EngineData>();
                Ogre::SceneNode *bodyNode =
                    eng.mScnMgr->getRootSceneNode()
                        ->createChildSceneNode();
                Ogre::Entity *faceEnt =
                    eng.mScnMgr->createEntity(
                        characterModelsFace[e]);
                bodyNode->attachObject(faceEnt);
                characterNodes[e] = bodyNode;
                characterEntitiesFace[e] = faceEnt;
                Ogre::Entity *hairEnt =
                    eng.mScnMgr->createEntity(
                        characterModelsHair[e]);
                hairEnt->shareSkeletonInstanceWith(faceEnt);
                bodyNode->attachObject(hairEnt);
                characterEntitiesHair[e] = hairEnt;
                Ogre::Entity *topEnt =
                    eng.mScnMgr->createEntity(
                        characterModelsTop[e]);
                topEnt->shareSkeletonInstanceWith(faceEnt);
                bodyNode->attachObject(topEnt);
                characterEntitiesTop[e] = topEnt;
                Ogre::Entity *bottomEnt =
                    eng.mScnMgr->createEntity(
                        characterModelsBottom[e]);
                bottomEnt->shareSkeletonInstanceWith(faceEnt);
                bodyNode->attachObject(bottomEnt);
                characterEntitiesBottom[e] = bottomEnt;
                Ogre::Entity *feetEnt =
                    eng.mScnMgr->createEntity(
                        characterModelsFeet[e]);
                feetEnt->shareSkeletonInstanceWith(faceEnt);
                bodyNode->attachObject(feetEnt);
                characterEntitiesFeet[e] = feetEnt;
#if 0
                if (characterModelsTop.find(e) !=
                    characterModelsTop.end()) {
                    Ogre::String skeletonName =
                        bodyEnt->getMesh()
                            ->getSkeletonName();
                    Ogre::MeshPtr mesh =
                        Ogre::MeshManager::getSingleton()
                            .load(characterModelsTop
                                      [e],
                                  "General");
                    Ogre::String mname = mesh->getName();
                    mesh = mesh->clone(mname + "_clone");
                    OgreAssert(
                        mesh,
                        "No mesh " +
                            characterModelsTop[e]);
                    Ogre::String clothSkeleton =
                        mesh->getSkeletonName();
                    if (clothSkeleton != skeletonName) {
                        mesh->setSkeletonName(
                            skeletonName);
                        mesh->load();
                        if (Ogre::SkeletonManager::getSingleton()
                                .resourceExists(
                                    clothSkeleton))
                            Ogre::SkeletonManager::
                                getSingleton()
                                    .remove(clothSkeleton);
                    }
                    Ogre::Entity *characterTop =
                        eng.mScnMgr->createEntity(mesh);
                    characterTop->shareSkeletonInstanceWith(
                        bodyEnt);
                    bodyNode->attachObject(characterTop);
                }
#endif
                ECS::modified<CharacterModule>();
            }
            OgreAssert(characterOrientations.find(e) !=
                       characterOrientations.end(),
                   "Bad orientation/position");
            Ogre::SceneNode *bodyNode = characterNodes[e];
            bodyNode->setOrientation(characterOrientations[e]);
            bodyNode->setPosition(characterPositions[e]);
            OgreAssert(
                characterEntitiesFace[e]->getSkeleton()->hasBone(
                    "Root"),
                "No root bone");
            ch.mBoneMotion = Ogre::Vector3::ZERO;
            ch.mBonePrevMotion = Ogre::Vector3::ZERO;
        });
	ecs.component<CharacterGravity>();
    ecs.component<CharacterLocation>().on_set(
        [this](flecs::entity e, CharacterLocation &loc) {
            ZoneScoped;
            characterOrientations[e] = loc.orientation;
            characterPositions[e] = loc.position;
            ECS::modified<CharacterModule>();
        });
	ecs.component<CharacterBuoyancy>();
	ecs.component<CharacterDisablePhysics>();
	ecs.component<CharacterUpdatePhysicsState>();
	ecs.component<CharacterInActuator>();
	ecs.component<Male>();
	ecs.component<Female>();
    ecs.component<CharacterControlDisable>();
    ecs.import <CharacterAnimationModule>();
	ecs.import <TerrainModule>();
	ecs.import <WaterModule>();

	ecs.system<EngineData, CharacterBase>("UpdateTimer")
		.kind(flecs::OnUpdate)
		.each([this](EngineData &eng, CharacterBase &ch) {
            ZoneScopedN("UpdateTimer");
            ch.mTimer += eng.delta;
		});
	ecs.system<Input, Camera>("HandleInput")
		.kind(flecs::OnUpdate)
		.each([this](Input &input, Camera &camera) {
            ZoneScopedN("HandleInput");
            flecs::entity player =
                ECS::get<CharacterManagerModule>().getPlayer();
            if (!player.is_valid())
                return;
			/* handle input */
			//			if (input.control == input.control_prev)
			//				return;
			uint32_t pressed = input.control & ~input.control_prev;
			uint32_t released = input.control_prev & ~input.control;
			uint32_t active = input.control;
			float zaxis = input.motion.z;
			zaxis *= 0.9f;
			if (!camera.mCameraPivot || !camera.mCameraGoal)
				return;
			if (pressed & 1)
				std::cout << "W pressed\n";
			if (released & 1)
				std::cout << "W released\n";
			if (active & 1)
				zaxis -= 1.0f;
			if (active & 4)
				zaxis += 1.0f;
			if (zaxis > -1.0f && zaxis < 1.0f)
				zaxis = 0.0f;
			else
				zaxis = Ogre::Math::Sign(zaxis);
			if (active & 32)
				input.act = true;
			else
				input.act = false;
			if (pressed & 32)
				input.act_pressed = true;
			else
				input.act_pressed = false;
			if (active & 64)
				input.act2 = true;
			else
				input.act2 = false;
			if (pressed & 64)
				input.act2_pressed = true;
			else
				input.act2_pressed = false;
			input.motion.z = zaxis;
			float xaxis = input.motion.x;
			xaxis *= 0.9f;
			if (active & 2)
				xaxis = -1.0f;
			if (active & 8)
				xaxis += 1.0f;
			if (xaxis > -1.0f && xaxis < 1.0f)
				xaxis = 0.0f;
			else
				xaxis = Ogre::Math::Sign(xaxis);
			input.motion.x = xaxis;
			if (active & 16)
				input.fast = true;
			else
				input.fast = false;
			input.control_prev = input.control;
            float a = 0, b = 0, c = 0;
			if (input.mouse_moved) {
                a += -0.18f * input.mouse.x;
                b += -0.12f * input.mouse.y;
				input.mouse_moved = false;
				input.mouse.x = 0;
				input.mouse.y = 0;
                updateCameraGoal(camera, a, b, c);
            }
			if (input.wheel_moved) {
                c += -0.15f * input.wheel_y;
				input.wheel_moved = false;
				input.wheel_y = 0;
                updateCameraGoal(camera, a, b, c);
            }

			ECS::get().modified<ECS::Input>();
		});
#define TURN_SPEED 500.0f // character turning in degrees per second
	ecs.system<const Input, const Camera, CharacterBase>("UpdateBody")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.without<CharacterInActuator>()
        .without<CharacterControlDisable>()
        .each([](flecs::entity e, const Input &input,
			 const Camera &camera, CharacterBase &ch) {
            ZoneScopedN("UpdateBody");
            ch.mGoalDirection = Ogre::Vector3::ZERO;
			float delta = e.world().delta_time();
			if (!input.motion.zeroLength()) {
				// calculate actually goal direction in world based on player's key directions
				ch.mGoalDirection +=
					input.motion.z *
					camera.mCameraNode->getOrientation()
						.zAxis();
				ch.mGoalDirection +=
					input.motion.x *
					camera.mCameraNode->getOrientation()
						.xAxis();
				ch.mGoalDirection.y = 0;
				ch.mGoalDirection.normalise();

				Ogre::Quaternion toGoal =
                    ECS::get<CharacterModule>()
                        .characterNodes.at(e)
                        ->getOrientation()
						.zAxis()
						.getRotationTo(
							ch.mGoalDirection);
				// calculate how much the character has to turn to face goal direction
				Ogre::Real yawToGoal =
					toGoal.getYaw().valueDegrees();
				// this is how much the character CAN turn this frame
				Ogre::Real yawAtSpeed =
					yawToGoal / Ogre::Math::Abs(yawToGoal) *
					delta * TURN_SPEED;
				// reduce "turnability" if we're in midair
				// if (mBaseAnimID == ANIM_JUMP_LOOP) yawAtSpeed *= 0.2f;
				if (yawToGoal < 0)
					yawToGoal = std::min<Ogre::Real>(
						0,
						std::max<Ogre::Real>(
							yawToGoal,
							yawAtSpeed)); //yawToGoal = Math::Clamp<Real>(yawToGoal, yawAtSpeed, 0);
				else if (yawToGoal > 0)
					yawToGoal = std::max<Ogre::Real>(
						0,
						std::min<Ogre::Real>(
							yawToGoal,
							yawAtSpeed)); //yawToGoal = Math::Clamp<Real>(yawToGoal, 0, yawAtSpeed);
                ECS::get<CharacterModule>()
                    .characterNodes.at(e)
                    ->yaw(Ogre::Degree(yawToGoal));
			}
		});
#if 0
	ecs.system<const EngineData, CharacterLocation, CharacterBase,
		   CharacterBody>("UpdateCharacterBase")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<CharacterBody>()
		.with<CharacterBase>()
		.each([](const EngineData &eng, CharacterLocation &loc,
			 CharacterBase &ch, CharacterBody &body) {
			if (!ch.mBodyNode) {
			} else {
				loc.orientation =
					ch.mBodyNode->_getDerivedOrientation();
				loc.position =
					ch.mBodyNode->_getDerivedPosition();
			}
		});
#endif
    static int characterCount = 0;
    ecs.observer<AnimationControl>("SetupAnimationControlObs")
        .event(flecs::OnAdd)
        .each([](flecs::entity e, AnimationControl &anim) {
            anim.configured = false;
        });

#define CAM_HEIGHT 1.6f // height of camera above character's center of mass
	ecs.system<const EngineData, Camera, const CharacterBase>(
		   "UpdateCamera")
		.kind(flecs::OnUpdate)
		.with<Player>()
		.with<GroundCheckReady>()
        .each([](flecs::entity e, const EngineData &eng, Camera &camera,
			 const CharacterBase &ch) {
            ZoneScopedN("UpdateCamera");
            float delta = eng.delta;
			if (!camera.configured) {
				// create a pivot at roughly the character's shoulder
				camera.mCameraPivot =
					eng.mScnMgr->getRootSceneNode()
						->createChildSceneNode();
				camera.mCameraGoal =
					camera.mCameraPivot
						->createChildSceneNode(
							Ogre::Vector3(0, 2, 3));
				camera.mCameraNode->setPosition(
					camera.mCameraPivot->getPosition() +
					camera.mCameraGoal->getPosition());
				camera.mCameraPivot->setFixedYawAxis(true);
				camera.mCameraGoal->setFixedYawAxis(true);
				camera.mCameraNode->setFixedYawAxis(true);
				// our model is quite small, so reduce the clipping planes
				camera.mCamera->setNearClipDistance(0.1f);
				camera.mCamera->setFarClipDistance(700);

				camera.mPivotPitch = 0;
				camera.configured = true;
			} else {
				// place the camera pivot roughly at the character's shoulder
				camera.mCameraPivot->setPosition(
                    ECS::get<CharacterModule>()
                        .characterNodes.at(e)
                        ->getPosition() +
					Ogre::Vector3::UNIT_Y * CAM_HEIGHT);
				// move the camera smoothly to the goal
				Ogre::Vector3 goalOffset =
					camera.mCameraGoal
						->_getDerivedPosition() -
					camera.mCameraNode->getPosition();
				camera.mCameraNode->translate(goalOffset *
							      delta * 9.0f);
				// always look at the pivot
				camera.mCameraNode->lookAt(
					camera.mCameraPivot
						->_getDerivedPosition(),
					Ogre::Node::TS_PARENT);
			}
		});
#if 0
	class ClosestNotMeRayResultCallback
		: public btCollisionWorld::ClosestRayResultCallback {
		btCollisionObject *mMe;

	public:
		ClosestNotMeRayResultCallback(btCollisionObject *me,
					      const btVector3 &from,
					      const btVector3 &to)
			: btCollisionWorld::ClosestRayResultCallback(from, to)
		{
			mMe = me;
		}
		virtual btScalar
		addSingleResult(btCollisionWorld::LocalRayResult &rayResult,
				bool normalInWorldSpace)
		{
			if (rayResult.m_collisionObject == mMe)
				return 1.0f;
			return ClosestRayResultCallback::addSingleResult(
				rayResult, normalInWorldSpace);
		}
	};
#endif
	ecs.system<const EngineData, CharacterBase>("CheckGround")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.without<GroundCheckReady>()
		.each([](const EngineData &eng, CharacterBase &ch) {
            ZoneScopedN("CheckGround");
#if 0
			if (body.mGhostObject) {
				btVector3 from =
					body.mGhostObject->getWorldTransform()
						.getOrigin() +
					btVector3(0, 0.2f, 0);
				btVector3 to = from + btVector3(0, -3000.0f, 0);
				ClosestNotMeRayResultCallback resultCallback(
					body.mGhostObject, from, to);
				body.mGhostObject->rayTest(from, to,
							   resultCallback);
				body.checkGroundResult =
					resultCallback.hasHit();
				if (resultCallback.hasHit())
					ECS::get().add<GroundCheckReady>();
			}
#endif
			ECS::get().add<GroundCheckReady>();
		});
#if 0
	ecs.system<const WaterBody, const CharacterBase, CharacterBody>(
		   "CharacterWater1")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.without<InWater>()
		.each([](flecs::entity e, const WaterBody &waterb,
			 const CharacterBase &ch, CharacterBody &body) {
#if 0
			if (waterb.isInWater(body.mGhostObject) &&
			    ch.mBodyNode->_getDerivedPosition().y < -0.05f) {
				e.add<InWater>();
				std::cout << "Big Splash\n";
			}
#endif
#if 0
			if (waterb.mInWater.find(body.mGhostObject) ==
			    waterb.mInWater.end())
				e.add<InWater>();
			std::cout << waterb.mInWater.size() << " InWater\n";
#endif
		});
#endif
#if 0
	ecs.system<const WaterBody, const CharacterBase, CharacterBody>(
		   "CharacterWater2")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<InWater>()
		.each([](flecs::entity e, const WaterBody &waterb,
			 const CharacterBase &ch, CharacterBody &body) {
			float h = ch.mBodyNode->_getDerivedPosition().y;
#if 0
			if (waterb.isInWater(body.mGhostObject) && h > 0.05f)
				e.remove<InWater>();
			else if (!waterb.isInWater(body.mGhostObject) &&
				 h > 0.05f)
				e.remove<InWater>();
#endif
		});
#endif
#if 0
	ecs.system<const EngineData, CharacterBase, CharacterBody>(
		   "DisplayPlayerPos")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.each([](const EngineData &eng, CharacterBase &ch,
			 CharacterBody &body) {
			std::cout << "player: " << ch.mBodyNode->getPosition()
				  << "\n";
		});
#endif
#if 0
#endif
}

void CharacterModule::updateCameraGoal(Camera &camera, Ogre::Real deltaYaw,
				       Ogre::Real deltaPitch,
				       Ogre::Real deltaZoom)
{
    ZoneScoped;
    static float canonDist = 0;
	camera.mCameraPivot->yaw(Ogre::Degree(deltaYaw), Ogre::Node::TS_PARENT);
	if (!(camera.mPivotPitch + deltaPitch > 25 && deltaPitch > 0) &&
	    !(camera.mPivotPitch + deltaPitch < -60 && deltaPitch < 0)) {
		camera.mCameraPivot->pitch(Ogre::Degree(deltaPitch),
					   Ogre::Node::TS_LOCAL);
		camera.mPivotPitch += deltaPitch;
	}
	Ogre::Real dist = camera.mCameraGoal->_getDerivedPosition().distance(
		camera.mCameraPivot->_getDerivedPosition());
	Ogre::Real distChange = deltaZoom * dist;

	// bound the zoom
    if (!(dist + distChange < 1.5f && distChange < 0) &&
        !(dist + distChange > 10 && distChange > 0)) {
		camera.mCameraGoal->translate(0, 0, distChange,
					      Ogre::Node::TS_LOCAL);
        canonDist += distChange;
    }
    JPH::BodyID id;
    Ogre::Vector3 position;
    Ogre::Vector3 d = (camera.mCameraPivot->_getDerivedPosition() -
               camera.mCameraGoal->_getDerivedPosition())
                  .normalisedCopy();

    if (JoltPhysicsWrapper::getSingleton().raycastQuery(
            camera.mCameraPivot->_getDerivedPosition(),
            camera.mCameraGoal->_getDerivedPosition() - d * 0.6,
            position, id)) {
        float l = camera.mCameraPivot->_getDerivedPosition()
                  .squaredDistance(
                      camera.mCameraGoal
                          ->_getDerivedPosition());
        float m = camera.mCameraPivot->_getDerivedPosition()
                  .squaredDistance(position);
        if (m < l)
            camera.mCameraGoal->_setDerivedPosition(position +
                                d * 0.6f);
    } else {
        Ogre::Real dist2 =
            camera.mCameraGoal->_getDerivedPosition().distance(
                camera.mCameraPivot->_getDerivedPosition());
        if (deltaZoom < 0.0f || deltaZoom > 0.0f)
            canonDist = dist2;
        else {
            if (canonDist < dist2)
                canonDist = dist2;
            if (dist2 < canonDist)
                camera.mCameraGoal->translate(
                    0, 0, 0.08f, Ogre::Node::TS_LOCAL);
        }
    }
}

void CharacterModule::createCharacter(
    flecs::entity e, const Ogre::Vector3 &position,
    const Ogre::Quaternion &rotation, const Ogre::String &faceModel,
    const Ogre::String &hairModel, const Ogre::String &topModel,
    const Ogre::String &bottomModel, const Ogre::String &feetModel)
{
    ZoneScoped;
    if (e.has<CharacterBase>() || e.has<AnimationControl>())
        return;
    if (characterNodes.find(e) != characterNodes.end())
        return;
    e.set<CharacterLocation>({ rotation, position });
    characterOrientations[e] = rotation;
    characterPositions[e] = position;
    characterModelsFace[e] = faceModel;
    characterModelsHair[e] = hairModel;
    characterModelsTop[e] = topModel;
    characterModelsBottom[e] = bottomModel;
    characterModelsFeet[e] = feetModel;
    e.set<CharacterVelocity>(
        { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } });
    e.add<CharacterGravity>();
    e.add<CharacterBuoyancy>();
    e.add<Character>();
    e.add<CharacterBase>();
    e.add<AnimationControl>();
}

void applyWeightBasedScale(Ogre::Entity *ent,
               const Ogre::String &targetBoneName,
               const Ogre::Vector3 &scale)
{
    ZoneScoped;
    Ogre::MeshPtr mesh = ent->getMesh();
    Ogre::SkeletonInstance *skel = ent->getSkeleton();
    Ogre::Bone *targetBone = skel->getBone(targetBoneName);

    // Create a non-uniform scale matrix relative to the bone's local space
    Ogre::Matrix4 scaleMatrix = Ogre::Matrix4::IDENTITY;
    scaleMatrix.setScale(scale);

    for (unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i) {
        Ogre::SubMesh *submesh = mesh->getSubMesh(i);
        Ogre::VertexData *vertexData = submesh->useSharedVertices ?
                               mesh->sharedVertexData :
                               submesh->vertexData;

        // 1. Get Position Element
        const Ogre::VertexElement *posElem =
            vertexData->vertexDeclaration->findElementBySemantic(
                Ogre::VES_POSITION);
        Ogre::HardwareVertexBufferSharedPtr vbuf =
            vertexData->vertexBufferBinding->getBuffer(
                posElem->getSource());

        // 2. Access Bone Assignments
        // This map tells us which bones influence which vertex and by how much
        const Ogre::Mesh::VertexBoneAssignmentList &vbal =
            submesh->getBoneAssignments();

        // Lock buffer for reading and writing
        float *pVertices = static_cast<float *>(
            vbuf->lock(Ogre::HardwareBuffer::HBL_NORMAL));

        for (size_t vIdx = 0; vIdx < vertexData->vertexCount; ++vIdx) {
            float totalWeight = 0.0f;

            // Find assignments for this specific vertex
            for (auto const &[index, assignment] : vbal) {
                if (assignment.vertexIndex == vIdx &&
                    assignment.boneIndex ==
                        targetBone->getHandle()) {
                    totalWeight = assignment.weight;
                    break;
                }
            }

            if (totalWeight > 0.0f) {
                float *pPos;
                posElem->baseVertexPointerToElement(
                    reinterpret_cast<unsigned char *>(
                        pVertices) +
                        vIdx * vbuf->getVertexSize(),
                    &pPos);

                Ogre::Vector3 vertexPos(pPos[0], pPos[1],
                            pPos[2]);

                // Transform vertex to bone local space, scale it, then move back
                // This ensures scaling happens along the bone's axis, not the world axis
                Ogre::Vector3 localPos =
                    targetBone->_getDerivedOrientation()
                        .Inverse() *
                    (vertexPos -
                     targetBone->_getDerivedPosition());
                Ogre::Vector3 scaledLocalPos =
                    scaleMatrix * localPos;
                Ogre::Vector3 worldPos =
                    (targetBone->_getDerivedOrientation() *
                     scaledLocalPos) +
                    targetBone->_getDerivedPosition();

                // Interpolate based on weight (Lerp) to handle vertices shared with other bones
                Ogre::Vector3 finalPos = Ogre::Math::lerp(
                    vertexPos, worldPos, totalWeight);

                pPos[0] = finalPos.x;
                pPos[1] = finalPos.y;
                pPos[2] = finalPos.z;
            }
        }
        vbuf->unlock();
    }
}

void CharacterModule::remapMeshToMasterSkeleton(Ogre::MeshPtr clothMesh,
                        Ogre::MeshPtr masterMesh)
{
    Ogre::SkeletonPtr masterSkel = masterMesh->getSkeleton();
    Ogre::SkeletonPtr clothSkel = clothMesh->getSkeleton();

    if (!masterSkel || !clothSkel)
        return;

    // 1. Create a Lookup Table: ClothIndex -> MasterIndex
    std::map<unsigned short, unsigned short> indexMap;
    for (unsigned short i = 0; i < clothSkel->getNumBones(); ++i) {
        Ogre::String boneName = clothSkel->getBone(i)->getName();
        if (masterSkel->hasBone(boneName)) {
            indexMap[i] =
                masterSkel->getBone(boneName)->getHandle();
        } else {
            indexMap[i] = 0; // Fallback to root
        }
    }

    // 2. Update the Hardware Buffers for each SubMesh
    for (unsigned short i = 0; i < clothMesh->getNumSubMeshes(); ++i) {
        Ogre::SubMesh *sub = clothMesh->getSubMesh(i);
        Ogre::VertexData *vdata = sub->useSharedVertices ?
                          clothMesh->sharedVertexData :
                          sub->vertexData;

        // Find the element containing bone indices (VES_BLEND_INDICES)
        const Ogre::VertexElement *idxElem =
            vdata->vertexDeclaration->findElementBySemantic(
                Ogre::VES_BLEND_INDICES);
        if (!idxElem)
            continue;

        Ogre::HardwareVertexBufferSharedPtr vbuf =
            vdata->vertexBufferBinding->getBuffer(
                idxElem->getSource());
        unsigned char *vertex = static_cast<unsigned char *>(
            vbuf->lock(Ogre::HardwareBuffer::HBL_NORMAL));

        for (size_t j = 0; j < vdata->vertexCount; ++j) {
            unsigned char *pIndices;
            idxElem->baseVertexPointerToElement(vertex, &pIndices);

            // Remap the 4 indices (Ogre hardware skinning usually uses 4 bytes)
            for (int k = 0; k < 4; ++k) {
                pIndices[k] = static_cast<unsigned char>(
                    indexMap[pIndices[k]]);
            }
            vertex += vbuf->getVertexSize();
        }
        vbuf->unlock();
    }

    // 3. Link to Master Skeleton and rebuild
    clothMesh->setSkeletonName(masterSkel->getName());
    clothMesh->_compileBoneAssignments();
}

void CharacterModule::getSlotMeshes(const Ogre::String &age,
                    const Ogre::String &sex,
                    const Ogre::String &slotName,
                    std::vector<Ogre::String> &meshes)
{
    OgreAssert(body_parts.find(age) != body_parts.end(), "bad age: " + age);
    OgreAssert(body_parts[age].find(sex) != body_parts[age].end(),
           "bad sex: " + sex);
    OgreAssert(body_parts[age][sex].find(slotName) !=
               body_parts[age][sex].end(),
           "bad slot: " + slotName);
    for (auto &slots : body_parts[age][sex][slotName])
        meshes.push_back(slots.get<Ogre::String>());
}

void CharacterModule::preloadMeshes()
{
    for (const auto &mesh : mesh_names) {
        Ogre::Entity *ent =
            ECS::get<EngineData>().mScnMgr->createEntity(mesh);
        ECS::get<EngineData>().mScnMgr->destroyEntity(ent);
    }
}
}