#pragma once

#include <psemek/cg/body/body.hpp>

#include <psemek/math/interval.hpp>
#include <psemek/math/homogeneous.hpp>

namespace psemek::cg
{

	template <typename T, std::size_t N>
	struct separation_info
	{
		T distance = -std::numeric_limits<T>::infinity();
		math::vector<T, N> direction = math::vector<T, N>::zero();
		math::point<T, N> contact_point = math::point<T, N>::zero();
		int contact_point_body = 0;

		separation_info & operator |= (separation_info const & i);
	};

	template <typename T, std::size_t N>
	separation_info<T, N> operator | (separation_info<T, N> const & i1, separation_info<T, N> const & i2)
	{
		if (i1.distance > i2.distance)
			return i1;
		return i2;
	}

	template <typename T, std::size_t N>
	separation_info<T, N> & separation_info<T, N>::operator |= (separation_info<T, N> const & i)
	{
		return *this = *this | i;
	}

	namespace detail
	{

		template <bool EarlyExit, typename Body1, typename Body2>
		auto separation_2d(Body1 const & b1, Body2 const & b2)
		{
			auto const & vs1 = vertices(b1);
			auto const & vs2 = vertices(b2);

			auto const & es1 = edges(b1);
			auto const & es2 = edges(b2);

			using scalar_type = std::remove_cvref_t<decltype(vs1[0][0])>;

			using result_type = separation_info<scalar_type, 2>;

			auto process_edges = [](auto const & vs1, auto const & es1, auto const & vs2, int contact_body)
			{
				result_type result;
				result.contact_point_body = contact_body;

				for (auto const & e : es1)
				{
					result_type edge_result;
					edge_result.direction = -math::ort(math::normalized(vs1[e[1]] - vs1[e[0]]));
					edge_result.distance = std::numeric_limits<scalar_type>::infinity();

					for (auto const & v : vs2)
					{
						auto const d = math::dot(math::homogeneous(edge_result.direction), math::homogeneous(v - vs1[e[0]]));
						if (math::make_min(edge_result.distance, d))
							edge_result.contact_point = v;
					}

					result |= edge_result;
					if (EarlyExit && result.distance > 0)
						break;
				}

				if (contact_body == 0)
					result.direction = -result.direction;

				return result;
			};

			result_type result;
			result |= process_edges(vs1, es1, vs2, 1);
			if (EarlyExit && result.distance > 0)
				return result;
			result |= process_edges(vs2, es2, vs1, 0);
			return result;
		}

		template <bool EarlyExit, typename Body1, typename Body2>
		auto separation_3d(Body1 const & b1, Body2 const & b2)
		{
			auto const & vs1 = vertices(b1);
			auto const & vs2 = vertices(b2);

			auto const & fs1 = faces(b1);
			auto const & fs2 = faces(b2);

			auto const & es1 = edges(b1);
			auto const & es2 = edges(b2);

			auto const & eds1 = edge_directions(b1);
			auto const & eds2 = edge_directions(b2);

			using scalar_type = std::remove_cvref_t<decltype(vs1[0][0])>;

			using result_type = separation_info<scalar_type, 3>;

			auto process_faces = [](auto const & vs1, auto const & fs1, auto const & vs2, int contact_body)
			{
				result_type result;
				result.contact_point_body = contact_body;

				for (auto const & f : fs1)
				{
					result_type face_result;
					face_result.direction = math::normal(vs1[f[0]], vs1[f[1]], vs1[f[2]]);
					face_result.distance = std::numeric_limits<scalar_type>::infinity();

					auto const face_p = vs1[f[0]];

					for (auto const & v : vs2)
					{
						auto const d = math::dot(face_result.direction, v - face_p);
						if (math::make_min(face_result.distance, d))
							face_result.contact_point = v;
					}

					result |= face_result;
					if (EarlyExit && result.distance > 0)
						break;
				}

				if (contact_body == 0)
					result.direction = -result.direction;

				return result;
			};

			auto process_edges = [](auto const & vs1, auto const & eds1, auto const & vs2, auto const & eds2)
			{
				result_type result;

				for (auto const & ed1 : eds1)
				{
					for (auto const & ed2 : eds2)
					{
						result_type edge_result;
						edge_result.direction = math::cross(ed1, ed2);
						auto l = math::length(edge_result.direction);
						if (l == 0) continue;
						edge_result.direction /= l;

						math::interval<scalar_type> i1, i2;

						for (auto const & v : vs1)
						{
							i1 |= math::dot(math::homogeneous(v), math::homogeneous(edge_result.direction));
						}

						for (auto const & v : vs2)
						{
							i2 |= math::dot(math::homogeneous(v), math::homogeneous(edge_result.direction));
						}

						auto edge_d12 = i2.min - i1.max;
						auto edge_d21 = i1.min - i2.max;

						if (edge_d12 > edge_d21)
						{
							edge_result.distance = edge_d12;
						}
						else
						{
							edge_result.distance = edge_d21;
							edge_result.direction = -edge_result.direction;
						}

						result |= edge_result;

						if (EarlyExit && result.distance > 0)
							return result;
					}
				}

				return result;
			};

			result_type result;
			result |= process_faces(vs1, fs1, vs2, 1);
			if (EarlyExit && result.distance > 0)
				return result;
			result |= process_faces(vs2, fs2, vs1, 0);
			if (EarlyExit && result.distance > 0)
				return result;

			auto edge_result = process_edges(vs1, eds1, vs2, eds2);
			if (math::make_max(result.distance, edge_result.distance))
			{
				result = edge_result;

				auto e1 = es1[0];
				auto e2 = es2[0];

				{
					auto max_d = math::limits<scalar_type>::min();
					for (auto const & e : es1)
					{
						auto d = math::dot(math::homogeneous(vs1[e[0]]), math::homogeneous(result.direction)) + math::dot(math::homogeneous(vs1[e[1]]), math::homogeneous(result.direction));
						if (math::make_max(max_d, d))
							e1 = e;
					}
				}

				{
					auto min_d = math::limits<scalar_type>::max();
					for (auto const & e : es2)
					{
						auto d = math::dot(math::homogeneous(vs2[e[0]]), math::homogeneous(result.direction)) + math::dot(math::homogeneous(vs2[e[1]]), math::homogeneous(result.direction));
						if (math::make_min(min_d, d))
							e2 = e;
					}
				}

				result.contact_point_body = 0;

				auto m = math::cross(result.direction, vs2[e2[1]] - vs2[e2[0]]);
				auto v = math::dot(math::homogeneous(vs2[e2[0]]), math::homogeneous(m));
				auto t = (v - math::dot(math::homogeneous(vs1[e1[0]]), math::homogeneous(m))) / math::dot(math::homogeneous(vs1[e1[1]] - vs1[e1[0]]), math::homogeneous(m));
				result.contact_point = math::lerp(vs1[e1[0]], vs1[e1[1]], math::clamp(t, math::interval(scalar_type(0), scalar_type(1))));
			}

			return result;
		}

	}

	template <typename Body1, typename Body2>
	auto separation(Body1 const & b1, Body2 const & b2)
	{
		constexpr auto dim1 = dimension<Body1>;
		constexpr auto dim2 = dimension<Body2>;

		static_assert(dim1 == dim2);
		static_assert(dim1 == 2 || dim1 == 3);

		if constexpr (dim1 == 2)
		{
			return detail::separation_2d<false>(b1, b2);
		}
		else if constexpr (dim1 == 3)
		{
			return detail::separation_3d<false>(b1, b2);
		}
		else
		{
			return;
		}
	}

	template <typename Body1, typename Body2>
	auto intersect(Body1 const & b1, Body2 const & b2)
	{
		constexpr auto dim1 = dimension<Body1>;
		constexpr auto dim2 = dimension<Body2>;

		static_assert(dim1 == dim2);
		static_assert(dim1 == 2 || dim1 == 3);

		if constexpr (dim1 == 2)
		{
			return detail::separation_2d<true>(b1, b2).distance <= 0;
		}
		else if constexpr (dim1 == 3)
		{
			return detail::separation_3d<true>(b1, b2).distance <= 0;
		}
		else
		{
			return;
		}
	}

}
