#pragma once

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

namespace psemek::cg
{

	namespace detail
	{

		template <typename Body, typename T>
		T distance_fast_2d(Body const & b, math::point<T, 2> const & p)
		{
			T result = math::limits<T>::min();

			auto const & vs = vertices(b);

			for (auto const & e : edges(b))
			{
				auto n = math::ort(math::normalized(vs[e[0]] - vs[e[1]]));
				math::make_max(result, math::dot(n, p - vs[e[0]]));
			}

			return result;
		}

		template <typename Body, typename T>
		T distance_fast_3d(Body const & b, math::point<T, 3> const & p)
		{
			T result = math::limits<T>::min();

			auto const & vs = vertices(b);

			for (auto const & f : faces(b))
			{
				auto n = math::normal(vs[f[0]], vs[f[1]], vs[f[2]]);
				math::make_max(result, math::dot(n, p - vs[f[0]]));
			}

			return result;
		}

	}

	// Returns an upper bound of the distance between b and p
	// using only edge/face normals
	template <typename Body, typename Point>
	auto distance_fast(Body const & b, Point const & p)
	{
		constexpr auto dim = dimension<Body>;

		static_assert(dim == 2 || dim == 3);

		if constexpr (dim == 2)
		{
			return detail::distance_fast_2d(b, p);
		}
		else if constexpr (dim == 3)
		{
			return detail::distance_fast_3d(b, p);
		}
		else
		{
			return;
		}
	}

}
