#include "PhysicsSystem.hpp"
#include "../components/Transform.hpp"
#include <OgreLogManager.h>

EditorPhysicsSystem::EditorPhysicsSystem(flecs::world &world,
					 Ogre::SceneManager *sceneMgr)
	: m_world(world)
	, m_sceneMgr(sceneMgr)
	, m_rigidBodyQuery(
		  world.query<RigidBodyComponent, TransformComponent>())
{
}

EditorPhysicsSystem::~EditorPhysicsSystem() = default;

void EditorPhysicsSystem::initialize()
{
	if (m_initialized)
		return;

	// Create physics wrapper
	Ogre::SceneNode *cameraNode =
		m_sceneMgr->getRootSceneNode()->createChildSceneNode(
			"PhysicsCameraNode");
	m_physics =
		std::make_unique<JoltPhysicsWrapper>(m_sceneMgr, cameraNode);

	m_initialized = true;
	Ogre::LogManager::getSingleton().logMessage(
		"Physics system initialized");
}

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

	// Sync bodies before simulation
	syncBodies();

	// Step physics (skip if physics stepping is disabled)
	if (m_physicsEnabled) {
		m_physics->update(deltaTime);
	}

	// Update SceneNodes from physics for dynamic bodies
	updateDynamicBodies();
}

void EditorPhysicsSystem::syncBodies()
{
	if (!m_initialized || !m_physics)
		return;

	// Process all rigid bodies
	m_rigidBodyQuery.each([&](flecs::entity entity,
				  RigidBodyComponent &rigidBody,
				  TransformComponent &transform) {
		// Handle enabled/disabled state
		if (!rigidBody.enabled) {
			// Body is disabled - remove from physics if it exists
			if (rigidBody.bodyCreated) {
				removeRigidBody(rigidBody);
			}
			return; // Skip rest of processing for disabled bodies
		}

		// Body is enabled
		if (rigidBody.bodyDirty || !rigidBody.bodyCreated) {
			// Create or recreate the body
			// When transitioning from disabled to enabled, this will sync SceneNode -> physics
			updateRigidBody(entity, rigidBody, transform);
		} else if (rigidBody.bodyCreated && transform.node) {
			// Body exists and is enabled - handle per-body-type updates
			switch (rigidBody.bodyType) {
			case RigidBodyComponent::BodyType::Dynamic:
				// Dynamic: Physics controls position, SceneNode is updated after physics step
				// Nothing to do here - updateDynamicBodies handles SceneNode update
				break;

			case RigidBodyComponent::BodyType::Static:
				// Static: SceneNode controls position
				// Sync SceneNode -> physics when transform changes
				m_physics->setPositionAndRotation(
					rigidBody.bodyID,
					transform.node->_getDerivedPosition(),
					transform.node->_getDerivedOrientation(),
					false);
				break;

			case RigidBodyComponent::BodyType::Kinematic:
				// Kinematic: SceneNode controls position (moved by code, not forces)
				// Sync SceneNode -> physics every frame
				m_physics->setPositionAndRotation(
					rigidBody.bodyID,
					transform.node->_getDerivedPosition(),
					transform.node->_getDerivedOrientation(),
					false);
				break;
			}
		}
	});
}

void EditorPhysicsSystem::updateDynamicBodies()
{
	if (!m_initialized || !m_physics)
		return;

	// Update SceneNode positions from physics for dynamic bodies
	m_rigidBodyQuery.each([&](flecs::entity entity,
				  RigidBodyComponent &rigidBody,
				  TransformComponent &transform) {
		if (!rigidBody.enabled || !rigidBody.bodyCreated ||
		    !transform.node)
			return;

		if (rigidBody.bodyType ==
		    RigidBodyComponent::BodyType::Dynamic) {
			// Get position and rotation from physics body
			Ogre::Vector3 position =
				m_physics->getPosition(rigidBody.bodyID);
			Ogre::Quaternion rotation =
				m_physics->getRotation(rigidBody.bodyID);

			// Update SceneNode
			transform.node->_setDerivedPosition(position);
			transform.node->_setDerivedOrientation(rotation);

			// Also update the Transform component to match
			transform.position = position;
			transform.rotation = rotation;
		}
	});
}

JPH::ShapeRefC
EditorPhysicsSystem::createShape(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);
				if (!result) {
					Ogre::LogManager::getSingleton().logMessage(
						"Mesh shape creation returned null for: " +
						collider.meshName);
				}
			} catch (const Ogre::Exception &e) {
				Ogre::LogManager::getSingleton().logMessage(
					"Exception creating mesh shape: " +
					collider.meshName + " - " +
					e.getDescription());
			} catch (...) {
				Ogre::LogManager::getSingleton().logMessage(
					"Unknown exception creating 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 no shape created, default to a small box
	if (!result) {
		result = m_physics->createBoxShape(
			Ogre::Vector3(0.1f, 0.1f, 0.1f));
	}

	// Apply offset if any
	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
EditorPhysicsSystem::buildCompoundShape(flecs::entity rigidBodyEntity)
{
	std::vector<JPH::ShapeRefC> shapes;
	std::vector<Ogre::Vector3> positions;
	std::vector<Ogre::Quaternion> rotations;

	// Check if the rigid body entity itself has a collider
	if (rigidBodyEntity.has<PhysicsColliderComponent>() &&
	    rigidBodyEntity.has<TransformComponent>()) {
		auto &collider =
			rigidBodyEntity.get_mut<PhysicsColliderComponent>();
		auto &transform = rigidBodyEntity.get<TransformComponent>();
		JPH::ShapeRefC shape = createShape(collider);
		if (shape) {
			shapes.push_back(shape);
			positions.push_back(transform.position);
			rotations.push_back(transform.rotation);
		}
	}

	// Collect all collider children
	rigidBodyEntity.children([&](flecs::entity child) {
		if (child.has<PhysicsColliderComponent>() &&
		    child.has<TransformComponent>()) {
			auto &collider =
				child.get_mut<PhysicsColliderComponent>();
			auto &transform = child.get<TransformComponent>();

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

	if (shapes.empty()) {
		// No colliders, use default box
		return m_physics->createBoxShape(
			Ogre::Vector3(0.5f, 0.5f, 0.5f));
	}

	if (shapes.size() == 1) {
		// Single shape - use rotated translated shape
		return m_physics->createRotatedTranslatedShape(
			positions[0], rotations[0], shapes[0]);
	}

	// Multiple shapes - create static compound
	return m_physics->createStaticCompoundShape(shapes, positions,
						    rotations);
}

void EditorPhysicsSystem::updateRigidBody(flecs::entity entity,
					  RigidBodyComponent &rigidBody,
					  TransformComponent &transform)
{
	// Remove existing body if any
	if (rigidBody.bodyCreated) {
		removeRigidBody(rigidBody);
	}

	// Check for mesh colliders - they only work with Static bodies
	bool hasMeshCollider = false;
	if (entity.has<PhysicsColliderComponent>()) {
		auto &collider = entity.get<PhysicsColliderComponent>();
		if (collider.shapeType ==
		    PhysicsColliderComponent::ShapeType::Mesh) {
			hasMeshCollider = true;
		}
	}
	if (!hasMeshCollider) {
		entity.children([&](flecs::entity child) {
			if (child.has<PhysicsColliderComponent>()) {
				auto &collider =
					child.get<PhysicsColliderComponent>();
				if (collider.shapeType ==
				    PhysicsColliderComponent::ShapeType::Mesh) {
					hasMeshCollider = true;
				}
			}
		});
	}

	if (hasMeshCollider &&
	    rigidBody.bodyType != RigidBodyComponent::BodyType::Static) {
		Ogre::LogManager::getSingleton().logMessage(
			"ERROR: Mesh colliders can only be used with Static bodies. "
			"Body type is " +
			std::string(rigidBody.bodyType ==
						    RigidBodyComponent::
							    BodyType::Dynamic ?
					    "Dynamic" :
					    "Kinematic") +
			". Please use Convex Hull collider instead, or set body type to Static. "
			"Physics body will be removed until configuration is valid.");

		// Remove existing body if any
		if (rigidBody.bodyCreated) {
			removeRigidBody(rigidBody);
		}

		// Mark as not created but not dirty (we don't want to retry every frame)
		rigidBody.bodyCreated = false;
		rigidBody.bodyDirty = false;
		return;
	}

	// Build collision shape
	JPH::ShapeRefC shape = buildCompoundShape(entity);
	if (!shape)
		return;

	// Determine motion type and layer
	JPH::EMotionType motionType;
	JPH::ObjectLayer layer;

	switch (rigidBody.bodyType) {
	case RigidBodyComponent::BodyType::Static:
		motionType = JPH::EMotionType::Static;
		layer = Layers::NON_MOVING;
		break;
	case RigidBodyComponent::BodyType::Kinematic:
		motionType = JPH::EMotionType::Kinematic;
		layer = Layers::MOVING;
		break;
	case RigidBodyComponent::BodyType::Dynamic:
	default:
		motionType = JPH::EMotionType::Dynamic;
		layer = Layers::MOVING;
		break;
	}

	// Create body at the current SceneNode position
	// This ensures that when enabling a body, it starts at the current visual position
	Ogre::Vector3 startPosition =
		transform.node ? transform.node->_getDerivedPosition() :
				 transform.position;
	Ogre::Quaternion startRotation =
		transform.node ? transform.node->_getDerivedOrientation() :
				 transform.rotation;

	// Create body or sensor
	if (rigidBody.isSensor) {
		rigidBody.bodyID = m_physics->createSensor(
			shape, startPosition, startRotation, motionType, layer);
	} else {
		rigidBody.bodyID = m_physics->createBody(shape, rigidBody.mass,
							 startPosition,
							 startRotation,
							 motionType, layer);
	}

	if (rigidBody.bodyID.IsInvalid()) {
		Ogre::LogManager::getSingleton().logMessage(
			"Failed to create rigid body");
		return;
	}

	// Set properties
	m_physics->setFriction(rigidBody.bodyID, rigidBody.friction);

	// Add to physics world
	m_physics->addBody(rigidBody.bodyID, JPH::EActivation::Activate);

	rigidBody.bodyCreated = true;
	rigidBody.bodyDirty = false;

	Ogre::LogManager::getSingleton().logMessage("Rigid body created");
}

void EditorPhysicsSystem::removeRigidBody(RigidBodyComponent &rigidBody)
{
	if (!rigidBody.bodyCreated || rigidBody.bodyID.IsInvalid())
		return;

	m_physics->removeBody(rigidBody.bodyID);
	m_physics->destroyBody(rigidBody.bodyID);

	rigidBody.bodyCreated = false;
	rigidBody.bodyID = JPH::BodyID();
}

void EditorPhysicsSystem::setDebugDraw(bool enable)
{
	m_debugDraw = enable;
	if (m_physics) {
		m_physics->setDebugDraw(enable);
	}
}
