#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"
namespace ECS
{
CharacterModule::CharacterModule(flecs::world &ecs)
{
	struct TriggerPhysicsChange {};
	ecs.module<CharacterModule>();
	ecs.component<Character>();
	ecs.component<Player>();
	ecs.component<CharacterBase>();
	ecs.component<CharacterGravity>();
	ecs.component<CharacterLocation>();
	ecs.component<CharacterBuoyancy>();
	ecs.component<CharacterConf>();
	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) {
			ch.mTimer += eng.delta;
		});
	ecs.system<Input, Camera>("HandleInput")
		.kind(flecs::OnUpdate)
		.each([this](Input &input, Camera &camera) {
            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;
			if (input.mouse_moved) {
				updateCameraGoal(camera, -0.18f * input.mouse.x,
						 -0.12f * input.mouse.y, 0);
				input.mouse_moved = false;
				input.mouse.x = 0;
				input.mouse.y = 0;
			}
			if (input.wheel_moved) {
				updateCameraGoal(camera, 0, 0,
						 -0.15f * input.wheel_y);
				input.wheel_moved = false;
				input.wheel_y = 0;
			}
			ECS::get().modified<ECS::Input>();
		});
	ecs.system<CharacterBase>()
		.kind(flecs::OnUpdate)
		.with<TerrainReady>()
		.with<WaterReady>()
		.with<InWater>()
		.each([this](flecs::entity e, CharacterBase &ch) {
			float full_subm = 2.0f;
			Ogre::Vector3 pos = ch.mBodyNode->getPosition();
			float current_subm = -Ogre::Math::Clamp(
				pos.y + Ogre::Math::Sin(ch.mTimer * 0.13f +
							130.0f) *
						0.07f,
				-full_subm, 0.0f);
			if (current_subm > 0.9f)
				ch.is_submerged = true;
			else if (current_subm < 0.8f)
				ch.is_submerged = false;
		});
#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) {
			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 =
					ch.mBodyNode->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);
				ch.mBodyNode->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<const EngineData, const CharacterLocation,
             const CharacterConf, CharacterBase>(
           "SetupCharacterBaseObs")
        .event(flecs::OnAdd)
        .each([&](flecs::entity e, const EngineData &eng,
              const CharacterLocation &loc,
              const CharacterConf &conf, CharacterBase &ch) {
            ch.mBodyEnt = eng.mScnMgr->createEntity(conf.type);
            ch.mBodyNode = eng.mScnMgr->getRootSceneNode()
                           ->createChildSceneNode();
            ch.mBodyNode->setOrientation(loc.orientation);
            ch.mBodyNode->setPosition(loc.position);
            ch.mBodyNode->attachObject(ch.mBodyEnt);
            OgreAssert(ch.mBodyEnt->getSkeleton()->hasBone("Root"),
                   "No root bone");
            ch.mBoneMotion = Ogre::Vector3::ZERO;
            ch.mBonePrevMotion = Ogre::Vector3::ZERO;
        });
    ecs.observer<AnimationControl>("SetupAnimationControlObs")
        .event(flecs::OnAdd)
        .each([](flecs::entity e, AnimationControl &anim) {
            anim.configured = false;
        });

#if 0
	ecs.system<const EngineData, const CharacterLocation,
		   const CharacterConf, Body2Entity>("SetupCharacter")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.without<CharacterBase>()
		.without<CharacterBody>()
		.each([](flecs::entity e, const EngineData &eng,
			 const CharacterLocation &loc,
			 const CharacterConf &conf, Body2Entity &b2e) {
			CharacterBase &ch = e.ensure<CharacterBase>();
			CharacterBody &body = e.ensure<CharacterBody>();
			AnimationControl &anim = e.ensure<AnimationControl>();
			ch.mBodyEnt = eng.mScnMgr->createEntity(conf.type);
			ch.mBodyNode = eng.mScnMgr->getRootSceneNode()
					       ->createChildSceneNode();
			ch.mBodyNode->setOrientation(loc.orientation);
			ch.mBodyNode->setPosition(loc.position);
			ch.mBodyNode->attachObject(ch.mBodyEnt);
			ch.mSkeleton = ch.mBodyEnt->getSkeleton();
			OgreAssert(ch.mBodyEnt->getSkeleton()->hasBone("Root"),
				   "No root bone");
			OgreAssert(ch.mSkeleton->hasBone("Root"),
				   "No root bone");
			ch.mRootBone = ch.mSkeleton->getBone("Root");
			OgreAssert(ch.mRootBone, "No root bone");
			// body.mController = nullptr;
			ch.mBoneMotion = Ogre::Vector3::ZERO;
			ch.mBonePrevMotion = Ogre::Vector3::ZERO;
			e.set<CharacterVelocity>(
				{ { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } });
			body.checkGround = false;
			body.checkGroundResult = false;
#if 0
			body.mCollisionShape = nullptr;
			body.mGhostObject = nullptr;
			body.mController = nullptr;
			body.mGhostObject = new btPairCachingGhostObject();
			b2e.entities[body.mGhostObject] = e;
			body.mCollisionShape = new btCompoundShape(false);
			body.mGhostObject->setCollisionShape(
				body.mCollisionShape);
			{
				btVector3 inertia(0, 0, 0);
				//        mCollisionShape = new btCompoundShape();
				btScalar height = 1.0f;
				btScalar radius = 0.3f;
				btCapsuleShape *shape = new btCapsuleShape(
					radius, 2 * height - 2 * radius);
				btTransform transform;
				transform.setIdentity();
				transform.setOrigin(btVector3(0, 1, 0));
				static_cast<btCompoundShape *>(
					body.mCollisionShape)
					->addChildShape(transform, shape);
				btScalar masses[1] = { 0 };
				btTransform principal;
				static_cast<btCompoundShape *>(
					body.mCollisionShape)
					->calculatePrincipalAxisTransform(
						masses, principal, inertia);
			}
			body.mGhostObject->setCollisionFlags(body.mGhostObject->getCollisionFlags() | btCollisionObject::CF_CHARACTER_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT
					/*btCollisionObject::CF_KINEMATIC_OBJECT |
					btCollisionObject::CF_NO_CONTACT_RESPONSE */);
			body.mGhostObject->setActivationState(
				DISABLE_DEACTIVATION);
			eng.mWorld->attachCollisionObject(
				body.mGhostObject, ch.mBodyEnt, 1, 0x7FFFFFFF);
			OgreAssert(body.mGhostObject, "Need GhostObject");
			OgreAssert(body.mCollisionShape, "No collision shape");
#endif
			e.add<CharacterGravity>();
			e.add<CharacterBuoyancy>();
			anim.configured = false;
			// OgreAssert(body.mGhostObject->hasContactResponse(),
			//	   "need contact response");
		});
#endif
#if 0
	ecs.system<const EngineData, CharacterBase, CharacterBody>(
		   "UpdateCharacterPhysics")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<TerrainReady>()
		.with<WaterReady>()
		.each([](const EngineData &eng, CharacterBase &ch,
			 CharacterBody &body) {
#if 0
			if (ch.mBodyNode && !body.mController &&
			    eng.startupDelay < 0.0f) {
				body.mController =
					new Ogre::Bullet::KinematicMotionSimple(
						body.mGhostObject,
						ch.mBodyNode);
				body.mController->enableManualNarrowPhase(true);
				eng.mWorld->getBtWorld()->addAction(
					body.mController);
				OgreAssert(body.mController, "Need controller");
			}
#endif
		});
#endif
#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([](const EngineData &eng, Camera &camera,
			 const CharacterBase &ch) {
			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(
					ch.mBodyNode->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) {
#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)
{
    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 applyWeightBasedScale(Ogre::Entity *ent,
               const Ogre::String &targetBoneName,
               const Ogre::Vector3 &scale)
{
    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::createCharacter(flecs::entity e,
                      const Ogre::Vector3 &position,
                      const Ogre::Quaternion &rotation,
                      const Ogre::String model)
{
    if (e.has<CharacterBase>() || e.has<AnimationControl>())
        return;
    e.set<CharacterLocation>({ rotation, position });
    e.set<CharacterConf>({ model });
    e.add<CharacterBase>();
    e.add<AnimationControl>();
    e.set<CharacterVelocity>(
        { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } });
    e.add<CharacterGravity>();
    e.add<CharacterBuoyancy>();
    e.add<Character>();
}
}