#pragma once

#include <psemek/math/point.hpp>
#include <psemek/math/box.hpp>
#include <psemek/math/math.hpp>
#include <psemek/math/simplex.hpp>
#include <psemek/math/gauss.hpp>
#include <psemek/math/contains.hpp>
#include <psemek/util/range.hpp>

namespace psemek::math
{

	template <typename T>
	T distance(T const & x, interval<T> const & i)
	{
		if (i.min <= x && x <= i.max)
			return T{0};
		return std::min(std::abs(x - i.min), std::abs(x - i.max));
	}

	template <typename T>
	T signed_distance(T const & x, interval<T> const & i)
	{
		return std::max(i.min - x, x - i.max);
	}

	template <typename T, std::size_t N>
	T distance(point<T, N> const & p, box<T, N> const & b)
	{
		T s = T{};
		for (std::size_t i = 0; i < N; ++i)
		{
			if (p[i] < b[i].min)
				s += sqr(b[i].min - p[i]);
			else if (p[i] > b[i].max)
				s += sqr(p[i] - b[i].max);
		}
		return std::sqrt(s);
	}

	template <typename T, std::size_t N>
	T signed_distance(point<T, N> const & p, box<T, N> const & b)
	{
		if (!contains(b, p))
			return distance(p, b);

		T s = limits<T>::min();
		for (std::size_t i = 0; i < N; ++i)
			s = std::max(s, signed_distance(p[i], b[i]));
		return s;
	}

	template <typename T, std::size_t N>
	T distance(point<T, N> const & p, segment<point<T, N>> const & s)
	{
		T const t = dot(p - s[0], s[1] - s[0]) / length_sqr(s[1] - s[0]);

		if (t <= 0)
			return distance(p, s[0]);
		else if (t >= 1)
			return distance(p, s[1]);
		else
			return distance(p, lerp(s[0], s[1], t));
	}

	template <typename T, std::size_t N>
	T distance(point<T, N> const & p, triangle<point<T, N>> const & s)
	{
		auto n = normal(s[0], s[1], s[2]);
		auto v = p - s[0];
		auto l = dot(n, v);
		v -= n * l;

		matrix<T, 3, 2> m;

		for (std::size_t i = 0; i < 3; ++i)
		{
			m[i][0] = (s[1] - s[0])[i];
			m[i][1] = (s[2] - s[0])[i];
		}

		auto res = least_squares(m, v);

		if (res)
		{
			auto u = *res;
			if (u[0] >= 0 && u[1] >= 0 && u[0] + u[1] <= 1)
			{
				return std::abs(l);
			}
		}

		return std::min(distance(p, simplex{s[0], s[1]}), std::min(distance(p, simplex{s[1], s[2]}), distance(p, simplex{s[2], s[0]})));
	}

	template <typename T, typename Iterator>
	std::pair<T, vector<T, 2>> signed_polygon_distance_direction(point<T, 2> const & p, Iterator begin, Iterator end)
	{
		std::pair<T, vector<T, 2>> result{limits<T>::max(), vector<T, 2>::zero()};

		for (auto it = begin; it < end; ++it)
		{
			auto v = p - *it;
			auto l = length(v);
			if (make_min(result.first, l))
				result.second = v / l;
		}

		for (auto it = begin, prev = std::prev(end); it != end; prev = it++)
		{
			auto v = p - *prev;
			auto r = *it - *prev;
			auto t = math::dot(v, r) / math::dot(r, r);

			if (0 <= t && t <= 1)
			{
				v -= r * t;
				auto l = length(v);
				if (make_min(result.first, l))
					result.second = v / l;
			}
		}

		if (polygon_contains(begin, end, p))
		{
			result.first = -result.first;
			result.second = -result.second;
		}

		return result;
	}

	template <typename T, typename Polygon>
	std::pair<T, vector<T, 2>> signed_polygon_distance_direction(point<T, 2> const & p, Polygon const & polygon)
	{
		return signed_polygon_distance_direction(p, util::xbegin(polygon), util::xend(polygon));
	}

	template <typename T, typename Iterator>
	T signed_polygon_distance(point<T, 2> const & p, Iterator begin, Iterator end)
	{
		return signed_polygon_distance_direction(p, begin, end).first;
	}

	template <typename T, typename Polygon>
	T signed_polygon_distance(point<T, 2> const & p, Polygon const & polygon)
	{
		return signed_polygon_distance(p, util::xbegin(polygon), util::xend(polygon));
	}

	template <typename T, typename Iterator>
	T polygon_distance(point<T, 2> const & p, Iterator begin, Iterator end)
	{
		return std::max(T{0}, signed_polygon_distance(p, begin, end));
	}

	template <typename T, typename Polygon>
	T polygon_distance(point<T, 2> const & p, Polygon const & polygon)
	{
		return polygon_distance(p, util::xbegin(polygon), util::xend(polygon));
	}

}
