#include "sector.hpp"

#include <stdexcept>

#include <glm/gtc/quaternion.hpp>
#include <glm/gtc/matrix_transform.hpp>

game::Sector::Sector(World* world, size_t idx, std::shared_ptr<assets::SectorDef> def) :
	world_(world),
	idx_(idx),
	def_(std::move(def)),
	mesh_(def_->GetMesh()),

	bt_col_dispatcher_(&bt_col_cfg_),
	bt_world_(&bt_col_dispatcher_, &bt_broad_phase_, &bt_col_cfg_)
{
	// Create Bullet collision mesh
	bt_mesh_col_obj_.setCollisionShape(mesh_->GetCollisionMesh()->GetShape());
	bt_world_.addCollisionObject(&bt_mesh_col_obj_);

	auto def_portals = def_->GetPortals();
	size_t num_portals = def_portals.size();

	portals_.reserve(num_portals);
	for (size_t i = 0; i < num_portals; ++i)
	{
		const assets::SectorPortalDef& def_portal = def_portals[i];

		Portal& portal = portals_.emplace_back();
		portal.sector = this;
		portal.def = &assets::PortalDef::portal_defs[def_portal.def_name];

		glm::vec3 angles_rad = glm::radians(def_portal.angles);

		glm::mat4 rotation = glm::mat4((glm::quat(angles_rad)));
		glm::mat4 translation = glm::translate(glm::mat4(1.0f), def_portal.origin);
		glm::mat4 scale = glm::scale(glm::mat4(1.0f), glm::vec3(def_portal.scale));

		portal.trans = translation * rotation * scale;
		portal.scale = def_portal.scale;

		portal.plane = glm::transpose(glm::inverse(portal.trans)) * glm::vec4(0.0f, -1.0f, 0.0f, 0.0f);

		ComputePortalVertex(portal, 0, glm::vec2(-1.0f, -1.0f)); // Bottom-left
		ComputePortalVertex(portal, 1, glm::vec2(-1.0f, 1.0f));  // Top-left
		ComputePortalVertex(portal, 3, glm::vec2(1.0f, -1.0f));  // Bottom-right
		ComputePortalVertex(portal, 2, glm::vec2(1.0f, 1.0f));   // Top-right

		portal.link = nullptr;

		portal_map_[def_portal.name] = i;

		// create portal collision object
		portal.bt_col_shape = std::make_unique<btBoxShape>(
			btVector3(portal.def->size.x * portal.scale * 0.5f, 0.1f, portal.def->size.y * portal.scale * 0.5f)
		);

		portal.bt_col_obj = std::make_unique<btCollisionObject>();
		portal.bt_col_obj->setCollisionShape(portal.bt_col_shape.get());

		btQuaternion bt_rotation(angles_rad.x, angles_rad.y, angles_rad.z);
		btVector3 bt_position(def_portal.origin.x, def_portal.origin.y, def_portal.origin.z);
		btTransform bt_transform(bt_rotation, bt_position);
		portal.bt_col_obj->setWorldTransform(bt_transform);

		portal.col_data.type = CO_PORTAL;
		portal.col_data.portal = &portal;
		portal.bt_col_obj->setUserPointer(&portal.col_data);

		bt_world_.addCollisionObject(portal.bt_col_obj.get());
	}
}

void game::Sector::ComputePortalVertex(Portal& portal, size_t idx, const glm::vec2& base_vert)
{
	glm::vec2 vert_xz = base_vert * portal.def->size * 0.5f; // Scale to portal size
	glm::vec3 vert(vert_xz.x, 0.0f, vert_xz.y); // Convert to 3D vector
	portal.verts[idx] = glm::vec3(portal.trans * glm::vec4(vert, 1.0f));
}

int game::Sector::GetPortalIndex(const std::string& name) const
{
	auto it = portal_map_.find(name);
	if (it != portal_map_.end())
	{
		return static_cast<int>(it->second);
	}

	return -1;
}

static void LinkPortal(game::Portal& p1, game::Portal& p2) {
	// Calculate the traverse transformation
	glm::mat4 rotation = glm::rotate(glm::mat4(1.0f), glm::radians(180.0f), glm::vec3(0, 0, 1));
	glm::mat4 p1_to_p2 = p2.trans * rotation * glm::inverse(p1.trans);

	p1.link = &p2;
	
	// Transforms points from p1's sector space to p2's sector space
	p1.tr_position = p1_to_p2;
	
	// Transforms directions from p1's sector space to p2's sector space
	p1.tr_basis = glm::mat3(p1.tr_position);

	// get rid of scale
	for (size_t i = 0; i < 3 ; i++) 
		p1.tr_basis[i] = glm::normalize(p1.tr_basis[i]);
	
	p1.tr_scale = p2.scale / p1.scale;
}

void game::Sector::LinkPortals(Sector& s1, size_t idx1, Sector& s2, size_t idx2)
{
	Portal& p1 = s1.portals_[idx1];
	Portal& p2 = s2.portals_[idx2];

	if (p1.link || p2.link)
	{
		throw std::runtime_error("One of the portals is already linked");
	}

	LinkPortal(p1, p2);

	if (&p1 != &p2)
	{
		LinkPortal(p2, p1);
	}

}

namespace game
{
	struct PortalIgnoringClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
	{
		using Super = btCollisionWorld::ClosestConvexResultCallback;

		PortalIgnoringClosestConvexResultCallback(const btVector3& convexFromWorld, const btVector3& convexToWorld) :
			Super(convexFromWorld, convexToWorld)
		{
		}

		virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) override
		{
			void* ptr = convexResult.m_hitCollisionObject->getUserPointer();
			CollisionObjectData* data = static_cast<CollisionObjectData*>(ptr);

			if (data && data->type == CO_PORTAL)
			{
				// Ignore portal hits
				return m_closestHitFraction;
			} 
			
			return Super::addSingleResult(convexResult, normalInWorldSpace);
		}
	};
}

bool game::Sector::SweepCapsule(
	const btCapsuleShapeZ& capsule,
	const glm::mat3& basis,
	const glm::vec3& start,
	const glm::vec3& end,
	float& hit_fraction,
	glm::vec3& hit_normal)
{
	//const btVector3* bt_start = reinterpret_cast<const btVector3*>(&start);
	//const btVector3* bt_end = reinterpret_cast<const btVector3*>(&end);
	//const btMatrix3x3* bt_basis = reinterpret_cast<const btMatrix3x3*>(&basis);

	btVector3 bt_start(start.x, start.y, start.z);
	btVector3 bt_end(end.x, end.y, end.z);
	btMatrix3x3 bt_basis(
		basis[0][0], basis[0][1], basis[0][2],
		basis[1][0], basis[1][1], basis[1][2],
		basis[2][0], basis[2][1], basis[2][2]
	);

	btTransform start_transform(bt_basis, bt_start);
	btTransform end_transform(bt_basis, bt_end);

	PortalIgnoringClosestConvexResultCallback result_callback(bt_start, bt_end);

	bt_world_.convexSweepTest(&capsule, start_transform, end_transform, result_callback, 0.0f);

	if (result_callback.hasHit())
	{
		hit_fraction = result_callback.m_closestHitFraction;
		hit_normal = glm::vec3(
			result_callback.m_hitNormalWorld.x(),
			result_callback.m_hitNormalWorld.y(),
			result_callback.m_hitNormalWorld.z()
		);

		return true;
	}

	return false;
}

namespace game
{
	struct PortalContactResultCallback : public btCollisionWorld::ContactResultCallback
	{
		const game::Portal* portal = nullptr;

		PortalContactResultCallback()
		{
		}

		virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0,
			const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) override
		{
			void* ptr0 = colObj0Wrap->getCollisionObject()->getUserPointer();
			void* ptr1 = colObj1Wrap->getCollisionObject()->getUserPointer();
			void* ptr = ptr0 ? ptr0 : ptr1;
			CollisionObjectData* data = static_cast<CollisionObjectData*>(ptr);

			if (data && data->type == CO_PORTAL)
			{
				// We hit a portal, store it
				portal = data->portal;
			}

			return 0.0f; // No specific action needed for contact results
		}
	};
}

const game::Portal* game::Sector::TestPortalContact(btCapsuleShapeZ& capsule, const glm::mat3& basis, const glm::vec3& pos)
{
	btCollisionObject capsule_obj;
	capsule_obj.setCollisionShape(&capsule);
	btTransform capsule_transform;

	btVector3 bt_pos(pos.x, pos.y, pos.z);
	btMatrix3x3 bt_basis(
		basis[0][0], basis[0][1], basis[0][2],
		basis[1][0], basis[1][1], basis[1][2],
		basis[2][0], basis[2][1], basis[2][2]
	);

	btTransform bt_transform(bt_basis, bt_pos);

	capsule_obj.setWorldTransform(bt_transform);

	PortalContactResultCallback result_callback;
	bt_world_.contactTest(&capsule_obj, result_callback);

	return result_callback.portal;
}