#include "entity.hpp"
#include "world.hpp"
#include <algorithm>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>

game::Entity::Entity(World* world, size_t sector_idx, const CapsuleShape& capsule_shape, const glm::vec3& position) :
	world_(world),
	capsule_(capsule_shape),
	velocity_(0.0f),
	touching_portal_(nullptr),
	light_trace_offset_(0.0f, 0.0f, capsule_shape.height * 0.5f),
	lights_valid_(false)
{
	CreateOccurrenceParams occu_params;
	occu_params.sector = &world->GetSector(sector_idx);
	occu_params.position = position;
	occu_params.scale = 1.0f; // Default scale
	occu_params.basis = glm::mat3(1.0f); // Identity basis
	
	// Create initial occurence in given sector
	occu_ = CreateOccurrence(occu_params);
}

void game::Entity::Move(glm::vec3& velocity, float dt)
{
	glm::vec3 u = velocity * dt; // Calculate the movement vector

	const int MAX_ITERS = 16;
	for (size_t i = 0; i < MAX_ITERS && glm::dot(u, u) > 0.0f; ++i)
	{
		//printf("Entity::Move: Iteration %zu, u = (%f, %f, %f)\n", i, u.x, u.y, u.z);

		// offset in occu's sector space
		glm::vec3 occu_offset = occu_->basis_ * u;
		glm::vec3 to = occu_->position_ + occu_offset;

		float occu_hit_fraction = 1.0f;
		glm::vec3 occu_hit_normal;
		const Portal* hit_portal = nullptr;

		bool hit = occu_->Sweep(to, occu_hit_fraction, occu_hit_normal, &hit_portal);

		if (hit_portal != touching_portal_)
		{
			touching_portal_ = hit_portal;

			if (hit_portal)
			{
				// Beginning to touch a portal, create other occurrence in the linked sector
				CreateOtherOccurence(*hit_portal);
			}
			else
			{
				// Not touching the portal anymore, destroy the other occurrence
				other_occu_.reset();
			}
		}

		float other_hit_fraction = 1.0f;
		glm::vec3 other_hit_normal;

		bool other_hit = false;

		if (other_occu_)
		{
			glm::vec3 other_offset = other_occu_->basis_ * u;
			glm::vec3 other_to = other_occu_->position_ + other_offset;

			// Sweep the other occurrence if it exists
			other_hit = other_occu_->Sweep(other_to, other_hit_fraction, other_hit_normal, nullptr);
		}

		bool any_hit = hit || other_hit;
		float hit_fraction = 1.0f; // Default to no hit
		glm::vec3 hit_normal;

		if (hit && (!other_hit || occu_hit_fraction <= other_hit_fraction))
		{
			// Primary occurrence hit firstly
			hit_fraction = occu_hit_fraction;
			hit_normal = occu_->inv_normal_basis_ * occu_hit_normal;
		}
		else if (other_hit)
		{
			// Other occurrence hit firstly
			hit_fraction = other_hit_fraction;
			hit_normal = other_occu_->inv_normal_basis_ * other_hit_normal;
		}

		// Update the position based on the hit fraction
		occu_->position_ += hit_fraction * occu_offset;

		//if (any_hit)
		//{
		//	occu_->position_ += hit_normal * 0.00001f;
		//}

		if (other_occu_ && touching_portal_)
		{
			other_occu_->position_ = touching_portal_->tr_position * glm::vec4(occu_->position_, 1.0f);

			// TELEPORT
			float sd = glm::dot(glm::vec3(touching_portal_->plane), occu_->position_) + touching_portal_->plane.w;
			if (sd < 0.0f)
			{
				std::swap(occu_, other_occu_); // Swap occurrences
				touching_portal_ = touching_portal_->link; // Switch to the linked portal
			}
		}

		if (!any_hit)
		{
			break;
		}

		//hit_normal *= -1.0f; // Invert the normal to point outwards
		//printf("Entity::Move: Hit detected, hit_fraction = %f, hit_normal = (%f, %f, %f)\n", hit_fraction, hit_normal.x, hit_normal.y, hit_normal.z);

		u -= hit_fraction * u; // Reduce the movement vector by the hit fraction
		u -= glm::dot(u, hit_normal) * hit_normal; // Reflect the velocity along the hit normal
		velocity -= glm::dot(velocity, hit_normal) * hit_normal; // Adjust the velocity
	}

}

void game::Entity::Update(float dt)
{
	Move(velocity_, dt);
	lights_valid_ = false;
}

void game::Entity::UpdateLights()
{
	if (lights_valid_)
	{
		return; // Already updated since last update
	}

	const SectorEnvironment& env = occu_->sector_->GetEnvironment();
	glm::vec3 trace_pos = occu_->position_ + occu_->basis_ * light_trace_offset_;

	static std::vector<Light> lights;
	lights.clear();
	occu_->sector_->GetLightsAt(trace_pos, lights);

	// select only closest SD_MAX_LIGHTS lights
	if (lights.size() > SD_MAX_LIGHTS)
	{
		std::partial_sort(
			lights.begin(),
			lights.begin() + SD_MAX_LIGHTS,
			lights.end(),
			[&](const Light& a, const Light& b)
			{
				float aa = glm::distance2(a.position, trace_pos) / (a.radius * a.radius);
				float ab = glm::distance2(b.position, trace_pos) / (b.radius * b.radius);
				return aa < ab;
			}
		);

		lights.resize(SD_MAX_LIGHTS);
	}

	// Primary occurence
	occu_->lights_.ambient = env.ambient;
	occu_->lights_.num_lights = lights.size();
	for (size_t i = 0; i < lights.size(); ++i)
	{
		const Light& light = lights[i];
		occu_->lights_.colors_rs[i] = glm::vec4(light.color, light.radius);
		occu_->lights_.positions[i] = light.position;
	}

	// Other occurence
	if (other_occu_ && touching_portal_)
	{
		other_occu_->lights_.ambient = env.ambient;
		other_occu_->lights_.num_lights = lights.size();
		for (size_t i = 0; i < lights.size(); ++i)
		{
			const Light& light = lights[i];

			// transform pos through portal
			glm::vec3 new_pos = touching_portal_->tr_position * glm::vec4(light.position, 1.0f);
			float new_radius = light.radius * touching_portal_->tr_scale;

			other_occu_->lights_.colors_rs[i] = glm::vec4(light.color, new_radius);
			other_occu_->lights_.positions[i] = new_pos;
		}
	}

	lights_valid_ = true;
}

void game::Entity::CreateOtherOccurence(const Portal& portal)
{
	other_occu_.reset();

	if (!portal.link)
	{
		// No linked sector, no occurence :(
		return;
	}

	CreateOccurrenceParams other_occu_params;
	other_occu_params.sector = portal.link->sector;
	other_occu_params.position = portal.tr_position * glm::vec4(occu_->position_, 1.0f);
	other_occu_params.scale = occu_->scale_ * portal.tr_scale;
	other_occu_params.basis = portal.tr_basis * occu_->basis_;

	other_occu_ = CreateOccurrence(other_occu_params);
}

game::EntityOccurrence::EntityOccurrence(Entity* entity, const CreateOccurrenceParams& params) :
	entity_(entity),
	sector_(params.sector),
	position_(params.position),
	scale_(params.scale),
	basis_(params.basis),
	inv_basis_(glm::inverse(params.basis)),
	bt_capsule_(entity->GetCapsuleShape().radius * params.scale, entity->GetCapsuleShape().height * params.scale)
{

	// compute normalized basis
	for (size_t i = 0; i < 3; ++i)
	{
		normal_basis_[i] = glm::normalize(basis_[i]);
	}

	// compute inverse normalized basis
	inv_normal_basis_ = glm::inverse(normal_basis_);
}

bool game::EntityOccurrence::Sweep(const glm::vec3& target_position, float& hit_fraction, glm::vec3& hit_normal, const Portal** hit_portal)
{
	if (hit_portal)
	{
		*hit_portal = sector_->TestPortalContact(bt_capsule_, inv_normal_basis_, position_);
	
		if (!*hit_portal)
		{
			*hit_portal = sector_->TestPortalContact(bt_capsule_, inv_normal_basis_, target_position);
		}
	}

	return sector_->SweepCapsule(
		bt_capsule_,
		inv_normal_basis_,
		position_,
		target_position,
		hit_fraction,
		hit_normal
	);
}

const game::LightInfluences& game::EntityOccurrence::GetLights()
{
	entity_->UpdateLights();
	return lights_;
}