#include "geometry/normal_estimation.hpp"

#include <glm/glm.hpp>
#include <algorithm>

void estimate_normals(
	std::span<const components::mesh_vertex::position> vertices,
	std::span<const std::array<ztu::u32, 3>> triangles,
	std::vector<components::mesh_vertex::normal>& normals
) {

	normals.resize(vertices.size());

	std::ranges::fill(normals, std::array{ 0.0f, 0.0f, 0.0f });

	for (const auto& triangle : triangles)
	{
		auto abc =  std::array<glm::vec3, 3>{};

		std::ranges::transform(
			triangle,
			abc.begin(),
			[&](const auto& index)
			{
				const auto& [ x, y, z ] = vertices[index];
				return glm::vec3{ x, y, z };
			}
		);

		const auto [ A, B, C ] = abc;

		// TODO normalization can be done more efficiently
		const auto normal = glm::normalize(glm::vec3{
			A.y * B.z - A.z * B.y,
			A.z * B.x - A.x * B.z,
			A.x * B.y - A.y * B.x
		});

		const auto a_length = glm::length(B - C);
		const auto b_length = glm::length(C - A);
		const auto c_length = glm::length(A - B);

		const auto area = 0.25f * std::sqrt(
			(a_length + b_length + c_length) *
			(-a_length + b_length + c_length) *
			(a_length - b_length + c_length) *
			(a_length + b_length - c_length)
		);

		const auto weighted_normal = normal * area;

		for (const auto& index : triangle)
		{
			auto& normal_avg = normals[index];
			for (int i{}; i != 3; ++i)
			{
				normal_avg[i] += weighted_normal[i];
			}
		}
	}

	using normal_component_type = components::mesh_vertex::normal::component_type;
	constexpr auto epsilon = std::numeric_limits<normal_component_type>::epsilon();

	for (auto& [ x, y, z ] : normals)
	{
		const auto length = glm::length(glm::vec3{ x, y, z });
		if (length <= epsilon)
		{
			x = 0;
			y = 1;
			z = 0;
		}
		else
		{
			const auto scale = 1.0f / length;
			x *= scale;
			y *= scale;
			z *= scale;
		}
	}
}