#include "BuoyancySystem.hpp"
#include "../components/Character.hpp"
#include <OgreLogManager.h>
#include <set>

BuoyancySystem::BuoyancySystem(flecs::world &world, JoltPhysicsWrapper *physics)
	: m_world(world)
	, m_physics(physics)
	, m_buoyancyInfoQuery(world.query<BuoyancyInfo>())
{
}

void BuoyancySystem::initialize()
{
	Ogre::LogManager::getSingleton().logMessage(
		"Buoyancy system initialized");
}

void BuoyancySystem::update(float deltaTime)
{
	// Check if there's a WaterPhysics component in the world
	WaterPhysics *waterPhysics = nullptr;
	m_world.query<WaterPhysics>().each(
		[&](flecs::entity entity, WaterPhysics &wp) {
			waterPhysics = &wp;
		});

	if (!waterPhysics) {
		Ogre::LogManager::getSingleton().logMessage(
			"BuoyancySystem: No WaterPhysics component found");
		return;
	}

	if (!waterPhysics->enabled) {
		return;
	}

	if (!m_physics || deltaTime <= 0.0f) {
		return;
	}

	// Rebuild scene node -> entity cache each frame
	// This ensures we have up-to-date mappings even if entities are added/removed
	m_nodeToEntity.clear();
	m_world.query<TransformComponent>().each(
		[&](flecs::entity entity, TransformComponent &transform) {
			if (transform.node) {
				m_nodeToEntity[transform.node] = entity.id();
			}
		});

	// Clear previous frame's water bodies
	m_bodiesInWater.clear();

	// Remove InWater tag from entities that were in water last frame
	for (flecs::entity_t id : m_entitiesInWater) {
		flecs::entity e = m_world.entity(id);
		if (e.is_alive() && e.has<InWater>()) {
			e.remove<InWater>();
		}
	}
	m_entitiesInWater.clear();

	// Perform broadphase query to find bodies in water
	// Use water surface position from WaterPhysics component, camera XZ position
	Ogre::Vector3 waterSurfacePos(m_cameraPosition.x,
				      waterPhysics->waterSurfaceY,
				      m_cameraPosition.z);
	m_physics->broadphaseQuery(deltaTime, waterSurfacePos, m_bodiesInWater);

	// Debug logging if enabled
	if (m_debugEnabled) {
		debugLogWaterDetection(waterSurfacePos);
	}

	// Build a set of entity IDs that have BuoyancyInfo for quick lookup
	std::set<flecs::entity_t> entitiesWithBuoyancyInfo;
	m_buoyancyInfoQuery.each(
		[&](flecs::entity entity, BuoyancyInfo &buoyancyInfo) {
			if (buoyancyInfo.enabled) {
				entitiesWithBuoyancyInfo.insert(entity.id());
			}
		});

	// Apply buoyancy to all bodies found in water
	for (JPH::BodyID bodyID : m_bodiesInWater) {
		// Try to find the entity associated with this body
		Ogre::SceneNode *node =
			m_physics->getSceneNodeFromBodyID(bodyID);
		if (!node) {
			// No scene node found for this body - apply default buoyancy
			applyBuoyancyToBody(bodyID, deltaTime, nullptr,
					    waterPhysics);
			continue;
		}

		// Find entity from scene node
		auto it = m_nodeToEntity.find(node);
		if (it == m_nodeToEntity.end()) {
			// No entity found for this scene node - apply default buoyancy
			applyBuoyancyToBody(bodyID, deltaTime, nullptr,
					    waterPhysics);
			continue;
		}

		flecs::entity entity = m_world.entity(it->second);
		if (!entity.is_valid()) {
			applyBuoyancyToBody(bodyID, deltaTime, nullptr,
					    waterPhysics);
			continue;
		}

		// Only mark as InWater if body center is below surface
		// (for swim animation logic; buoyancy still applied to
		// bodies partially submerged via broadphase)
		Ogre::Vector3 bodyPos = m_physics->getPosition(bodyID);
		if (bodyPos.y < waterPhysics->waterSurfaceY) {
			if (!entity.has<InWater>()) {
				entity.add<InWater>();
			}
			m_entitiesInWater.insert(entity.id());
		}

		// Debug logging for buoyancy application
		if (m_debugEnabled) {
			Ogre::Vector3 bodyPos = m_physics->getPosition(bodyID);
			Ogre::LogManager::getSingleton().logMessage(
				"BuoyancySystem: Applying buoyancy to body " +
				Ogre::StringConverter::toString(
					bodyID.GetIndex()) +
				" at position " +
				Ogre::StringConverter::toString(bodyPos) +
				" (character: " +
				Ogre::StringConverter::toString(
					entity.has<CharacterComponent>()) +
				")");
		}

		// Check if entity has BuoyancyInfo component
		if (entitiesWithBuoyancyInfo.find(entity.id()) !=
		    entitiesWithBuoyancyInfo.end()) {
			const BuoyancyInfo *buoyancyInfo =
				entity.try_get<BuoyancyInfo>();
			if (buoyancyInfo && buoyancyInfo->enabled) {
				applyBuoyancyToBody(bodyID, deltaTime,
						    buoyancyInfo, waterPhysics);
				continue;
			}
		}

		// Entity found but no BuoyancyInfo - apply default buoyancy
		applyBuoyancyToBody(bodyID, deltaTime, nullptr, waterPhysics);
	}
}

void BuoyancySystem::applyBuoyancyToBody(JPH::BodyID bodyID, float deltaTime,
					 const BuoyancyInfo *buoyancyInfo,
					 const WaterPhysics *waterPhysics)
{
	if (!m_physics || !m_physics->isActive(bodyID) || !waterPhysics) {
		return;
	}

	// Get water surface parameters from WaterPhysics component
	float waterSurfaceY = waterPhysics->waterSurfaceY;
	float buoyancy = waterPhysics->defaultBuoyancy;
	float linearDrag = waterPhysics->defaultLinearDrag;
	float angularDrag = waterPhysics->defaultAngularDrag;

	// Override with per-entity settings if available
	if (buoyancyInfo) {
		if (buoyancyInfo->useCustomWaterLevel) {
			waterSurfaceY = buoyancyInfo->waterSurfaceY;
		}
		buoyancy = buoyancyInfo->buoyancy;
		linearDrag = buoyancyInfo->linearDrag;
		angularDrag = buoyancyInfo->angularDrag;
	}

	// Get body position
	Ogre::Vector3 bodyPos = m_physics->getPosition(bodyID);

	// Let Jolt handle submersion detection internally via ApplyBuoyancyImpulse.
	// The previous center-point check prevented partial submersion from working,
	// which caused characters standing in shallow water to receive no buoyancy.
	Ogre::Vector3 surfacePosition(bodyPos.x, waterSurfaceY, bodyPos.z);
	Ogre::Vector3 surfaceNormal(0.0f, 1.0f, 0.0f); // Water surface faces up
	Ogre::Vector3 fluidVelocity(0.0f, 0.0f, 0.0f); // Still water for now
	Ogre::Vector3 gravity(0.0f, -waterPhysics->gravity, 0.0f);

	// Apply buoyancy impulse
	m_physics->applyBuoyancyImpulse(bodyID, surfacePosition, surfaceNormal,
					buoyancy, linearDrag, angularDrag,
					fluidVelocity, gravity, deltaTime);
}

bool BuoyancySystem::isBodySubmerged(JPH::BodyID bodyID,
				     float waterSurfaceY) const
{
	if (!m_physics) {
		return false;
	}

	// Get body position
	Ogre::Vector3 bodyPos = m_physics->getPosition(bodyID);

	// Simple check: body is submerged if its Y position is below water surface
	// In a more complete implementation, we would check the actual submerged volume
	// or use Jolt's submerged shape query
	return bodyPos.y < waterSurfaceY;
}

flecs::entity BuoyancySystem::findEntityFromSceneNode(Ogre::SceneNode *node)
{
	auto it = m_nodeToEntity.find(node);
	if (it != m_nodeToEntity.end()) {
		return m_world.entity(it->second);
	}
	return flecs::entity();
}

void BuoyancySystem::debugLogWaterDetection(const Ogre::Vector3 &waterSurfacePos)
{
	static int frameCounter = 0;
	frameCounter++;

	// Log every 60 frames (about 1 second at 60 FPS)
	if (frameCounter % 60 != 0) {
		return;
	}

	Ogre::LogManager::getSingleton().logMessage(
		"=== Buoyancy System Debug Frame " +
		Ogre::StringConverter::toString(frameCounter) + " ===");

	// Log water physics state
	WaterPhysics *waterPhysics = nullptr;
	m_world.query<WaterPhysics>().each(
		[&](flecs::entity entity, WaterPhysics &wp) {
			waterPhysics = &wp;
		});

	if (waterPhysics) {
		Ogre::LogManager::getSingleton().logMessage(
			"WaterPhysics: surfaceY=" +
			Ogre::StringConverter::toString(
				waterPhysics->waterSurfaceY) +
			", enabled=" +
			Ogre::StringConverter::toString(waterPhysics->enabled) +
			", buoyancy=" +
			Ogre::StringConverter::toString(
				waterPhysics->defaultBuoyancy));
	} else {
		Ogre::LogManager::getSingleton().logMessage(
			"WaterPhysics: NOT FOUND");
	}

	// Log camera position
	Ogre::LogManager::getSingleton().logMessage(
		"Camera position: " +
		Ogre::StringConverter::toString(m_cameraPosition));

	// Log water surface position used for detection
	Ogre::LogManager::getSingleton().logMessage(
		"Water detection center: " +
		Ogre::StringConverter::toString(waterSurfacePos));

	// Log all characters and their positions
	int characterCount = 0;
	m_world.query<CharacterComponent, TransformComponent>().each(
		[&](flecs::entity entity, CharacterComponent &cc,
		    TransformComponent &transform) {
			characterCount++;
			Ogre::Vector3 worldPos =
				transform.node ?
					transform.node->_getDerivedPosition() :
					Ogre::Vector3::ZERO;

			// Check if character is in water (simple Y position check)
			bool inWater =
				waterPhysics &&
				(worldPos.y < waterPhysics->waterSurfaceY);

			Ogre::LogManager::getSingleton().logMessage(
				"Character " +
				Ogre::StringConverter::toString(entity.id()) +
				": pos=" +
				Ogre::StringConverter::toString(worldPos) +
				", inWater=" +
				Ogre::StringConverter::toString(inWater) +
				", enabled=" +
				Ogre::StringConverter::toString(cc.enabled));
		});

	Ogre::LogManager::getSingleton().logMessage(
		"Total characters: " +
		Ogre::StringConverter::toString(characterCount));

	// Log bodies detected in water by broadphase query
	Ogre::LogManager::getSingleton().logMessage(
		"Bodies in water (broadphase): " +
		Ogre::StringConverter::toString(m_bodiesInWater.size()));

	for (JPH::BodyID bodyID : m_bodiesInWater) {
		Ogre::Vector3 bodyPos = m_physics->getPosition(bodyID);
		bool isChar = m_physics->bodyIsCharacter(bodyID);
		float gravityFactor = m_physics->getGravityFactor(bodyID);

		Ogre::LogManager::getSingleton().logMessage(
			"  Body " +
			Ogre::StringConverter::toString(bodyID.GetIndex()) +
			": pos=" + Ogre::StringConverter::toString(bodyPos) +
			", isCharacter=" +
			Ogre::StringConverter::toString(isChar) +
			", gravityFactor=" +
			Ogre::StringConverter::toString(gravityFactor));
	}

	Ogre::LogManager::getSingleton().logMessage("=== End Debug Frame ===");
}
