/*
 * Copyright (C) 2023  Christopher J. Howard
 *
 * This file is part of Antkeeper source code.
 *
 * Antkeeper source code is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Antkeeper source code is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef ANTKEEPER_GEOM_CLOSEST_POINT_HPP
#define ANTKEEPER_GEOM_CLOSEST_POINT_HPP

#include <engine/geom/primitives/hypercapsule.hpp>
#include <engine/geom/primitives/hyperplane.hpp>
#include <engine/geom/primitives/hyperrectangle.hpp>
#include <engine/geom/primitives/hypersphere.hpp>
#include <engine/geom/primitives/line-segment.hpp>
#include <engine/geom/primitives/point.hpp>
#include <engine/geom/primitives/ray.hpp>
#include <engine/geom/primitives/triangle.hpp>
#include <algorithm>
#include <cmath>
#include <tuple>

namespace geom {

/**
 * Calculates the closest point on a ray to a point.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param a Ray a.
 * @param b Point b.
 *
 * @return Closest point on ray a to point b.
 */
template <class T, std::size_t N>
[[nodiscard]] constexpr point<T, N> closest_point(const ray<T, N>& a, const point<T, N>& b) noexcept
{
	return a.extrapolate(std::max<T>(T{0}, math::dot(b - a.origin, a.direction)));
}

/**
 * Calculates the closest point on a line segment to a point.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param ab Line segment ab.
 * @param c Point c.
 *
 * @return Closest point on line segment ab to point c.
 */
template <class T, std::size_t N>
[[nodiscard]] constexpr point<T, N> closest_point(const line_segment<T, N>& ab, const point<T, N>& c) noexcept
{
	const auto direction_ab = ab.b - ab.a;
	
	const auto distance_ab = math::dot(c - ab.a, direction_ab);
	if (distance_ab <= T{0})
	{
		return ab.a;
	}
	else
	{
		const auto sqr_length_ab = math::sqr_length(direction_ab);
		if (distance_ab >= sqr_length_ab)
		{
			return ab.b;
		}
		else
		{
			return ab.a + direction_ab * (distance_ab / sqr_length_ab);
		}
	}
}

/**
 * Calculates the closest points on two line segments.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param ab Line segment ab.
 * @param cd Line segment cd.
 *
 * @return Tuple containing the closest point on segment ab to segment cd, followed by the closest point on segment cd to segment ab.
 */
template <class T, std::size_t N>
[[nodiscard]] constexpr std::tuple<point<T, N>, point<T, N>> closest_point(const line_segment<T, N>& ab, const line_segment<T, N>& cd) noexcept
{
	const auto direction_ab = ab.b - ab.a;
	const auto direction_cd = cd.b - cd.a;
	const auto direction_ca = ab.a - cd.a;
	
	const auto sqr_length_ab = math::sqr_length(direction_ab);
	const auto sqr_length_cd = math::sqr_length(direction_cd);
	const auto cd_dot_ca = math::dot(direction_cd, direction_ca);
	
	if (sqr_length_ab <= T{0})
	{
		if (sqr_length_cd <= T{0})
		{
			// Both segments are points
			return {ab.a, cd.a};
		}
		else
		{
			// Segment ab is a point
			return
			{
				ab.a,
				cd.a + direction_cd * std::min<T>(std::max<T>(cd_dot_ca / sqr_length_cd, T{0}), T{1})
			};
		}
	}
	else
	{
		const auto ab_dot_ca = math::dot(direction_ab, direction_ca);
		
		if (sqr_length_cd <= T{0})
		{
			// Segment cd is a point
			return
			{
				ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
				cd.a
			};
		}
		else
		{
			const auto ab_dot_cd = math::dot(direction_ab, direction_cd);
			
			const auto den = sqr_length_ab * sqr_length_cd - ab_dot_cd * ab_dot_cd;
			
			auto distance_ab = (den) ? std::min<T>(std::max<T>((ab_dot_cd * cd_dot_ca - ab_dot_ca * sqr_length_cd) / den, T{0}), T{1}) : T{0};
			auto distance_cd = (ab_dot_cd * distance_ab + cd_dot_ca) / sqr_length_cd;
			
			if (distance_cd < T{0})
			{
				return
				{
					ab.a + direction_ab * std::min<T>(std::max<T>(-ab_dot_ca / sqr_length_ab, T{0}), T{1}),
					cd.a
				};
			}
			else if (distance_cd > T{1})
			{
				return
				{
					ab.a + direction_ab * std::min<T>(std::max<T>((ab_dot_cd - ab_dot_ca) / sqr_length_ab, T{0}), T{1}),
					cd.b
				};
			}
			
			return
			{
				ab.a + direction_ab * distance_ab,
				cd.a + direction_cd * distance_cd
			};
		}
	}
}

/**
 * Calculates the closest point on a hyperplane to a point.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param a Hyperplane a.
 * @param b Point b.
 *
 * @return Closest point on hyperplane a to point b.
 */
template <class T, std::size_t N>
[[nodiscard]] constexpr point<T, N> closest_point(const hyperplane<T, N>& a, const point<T, N>& b) noexcept
{
	return b - a.normal * (math::dot(a.normal, b) + a.constant);
}

/**
 * Calculates the closest point on a triangle to a point.
 *
 * @tparam T Real type.
 *
 * @param tri Triangle.
 * @param a First point of triangle.
 * @param b Second point of triangle.
 * @param c Third point of triangle.
 * @param p Point.
 *
 * @return Closest point on the triangle to point @p p, followed by the index of the edge on which the point lies (`-1` if not on an edge).
 */
/// @{
template <class T>
[[nodiscard]] constexpr point<T, 3> closest_point(const point<T, 3>& a, const point<T, 3>& b, const point<T, 3>& c, const point<T, 3>& p) noexcept
{
	const auto ab = b - a;
	const auto ca = a - c;
	const auto ap = p - a;
	const auto n = math::cross(ab, ca);
	const auto d = math::sqr_length(n);
	const auto q = math::cross(n, ap);
	
	T u, v, w;
	if ((w = math::dot(q, ab) / d) < T{0})
	{
		v = std::min<T>(std::max<T>(math::dot(ab, ap) / math::sqr_length(ab), T{0}), T{1});
		return {a * (T{1} - v) + b * v};
	}
	else if ((v = math::dot(q, ca) / d) < T{0})
	{
		u = std::min<T>(std::max<T>(math::dot(ca, p - c) / math::sqr_length(ca), T{0}), T{1});
		return {a * u + c * (T{1} - u)};
	}
	else if ((u = T{1} - v - w) < T{0})
	{
		const auto bc = c - b;
		w = std::min<T>(std::max<T>(math::dot(bc, p - b) / math::sqr_length(bc), T{0}), T{1});
		return {b * (T{1} - w) + c * w};
	}
	
	return {a * u + b * v + c * w};
}

template <class T>
[[nodiscard]] inline constexpr point<T, 3> closest_point(const triangle<T, 3>& tri, const point<T, 3>& p) noexcept
{
	return closest_point(tri.a, tri.b, tri.c, p);
}
/// @}

/**
 * Calculates the closest point on or in a hypersphere to a point.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param a Hypersphere a.
 * @param b Point b.
 *
 * @return Closest point on or in hypersphere a to point b.
 */
template <class T, std::size_t N>
[[nodiscard]] point<T, N> closest_point(const hypersphere<T, N>& a, const point<T, N>& b)
{
	const auto ab = b - a.center;
	const auto d = math::sqr_length(ab);
	return d > a.radius * a.radius ? a.center + ab * (a.radius / std::sqrt(d)) : b;
}

/**
 * Calculates the closest point on or in a hypercapsule to a point.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param a Hypercapsule a.
 * @param b Point b.
 *
 * @return Closest point on or in hypercapsule a to point b.
 */
template <class T, std::size_t N>
[[nodiscard]] point<T, N> closest_point(const hypercapsule<T, N>& a, const point<T, N>& b)
{
	const auto c = closest_point(a.segment, b);
	const auto cb = b - c;
	const auto d = math::sqr_length(cb);
	return d > a.radius * a.radius ? c + cb * (a.radius / std::sqrt(d)) : b;
}

/**
 * Calculates the closest point on or in a hyperrectangle to a point.
 *
 * @tparam T Real type.
 * @tparam N Number of dimensions.
 *
 * @param a Hyperrectangle a.
 * @param b Point b.
 *
 * @return Closest point on or in hyperrectangle a to point b.
 */
template <class T, std::size_t N>
[[nodiscard]] constexpr point<T, N> closest_point(const hyperrectangle<T, N>& a, const point<T, N>& b) noexcept
{
	return math::min(math::max(b, a.min), a.max);
}

} // namespace geom

#endif // ANTKEEPER_GEOM_CLOSEST_POINT_HPP