#pragma once

#include <psemek/util/empty.hpp>

#include <psemek/math/simplex.hpp>
#include <psemek/math/ray.hpp>
#include <psemek/math/box.hpp>
#include <psemek/math/sphere.hpp>
#include <psemek/math/cylinder.hpp>
#include <psemek/math/orientation.hpp>
#include <psemek/math/contains.hpp>
#include <psemek/math/gauss.hpp>

#include <optional>
#include <variant>
#include <type_traits>

namespace psemek::math
{

	template <typename RobustTag, typename T>
	bool intersect(RobustTag robust_tag, segment<point<T, 2>> const & s0, segment<point<T, 2>> const & s1)
	{
		auto const o00 = orientation(robust_tag, s0[0], s0[1], s1[0]);
		auto const o01 = orientation(robust_tag, s0[0], s0[1], s1[1]);
		auto const o10 = orientation(robust_tag, s1[0], s1[1], s0[0]);
		auto const o11 = orientation(robust_tag, s1[0], s1[1], s0[1]);

		return ((o00 != o01) || (o00 == sign_t::zero)) && ((o10 != o11) || (o10 == sign_t::zero));
	}

	template <typename T>
	bool intersect(segment<point<T, 2>> const & s0, segment<point<T, 2>> const & s1)
	{
		return intersect(default_robust_tag, s0, s1);
	}

	// TODO: robust implementation
	template <typename T>
	std::variant<util::empty, point<T, 2>, segment<point<T, 2>>> intersection(segment<point<T, 2>> const & s0, segment<point<T, 2>> const & s1)
	{
		auto const a0 = -det(s1[0] - s0[0], s1[1] - s1[0]);
		auto const a1 =  det(s0[1] - s0[0], s1[0] - s0[0]);
		auto const b  = -det(s0[1] - s0[0], s1[1] - s1[0]);

		if (b != 0)
		{
			// general case

			auto const t0 = a0 / b;
			auto const t1 = a1 / b;

			if (t0 < 0 || t0 > 1 || t1 < 0 || t1 > 1)
				return util::empty{};

			return s0[0] + t0 * (s0[1] - s0[0]);
		}
		else
		{
			// collinear segments

			if (a0 != 0)
			{
				// segments do not lie on the same line: no intersection
				return util::empty{};
			}

			// if segments are not Y-axis aligned, safe to use X-coordinates to sort them (k = 0)
			// otherwise use Y-coordinates (k = 1)
			std::size_t const k = (s0[0][0] != s0[1][0]) ? 0 : 1;

			if (s0[0][k] > s0[1][k])
				std::swap(s0[0], s0[1]);

			if (s1[0][k] > s1[1][k])
				std::swap(s1[0], s1[1]);

			auto const r0 = std::max(s0[0][k], s1[0][k]);
			auto const r1 = std::min(s0[1][k], s1[1][k]);

			if (r0 > r1)
				return util::empty{};

			bool const s0_is_first = s0[0][k] < s1[0][k];

			if (r0 == r1)
			{
				point<T, 2> p;
				p[k] = r0;
				p[1 - k] = s0_is_first ? s0[1][1 - k] : s1[1][1 - k];
				return p;
			}
			else if (s0_is_first)
				return simplex{ s1[0], s0[1] };
			else
				return simplex{ s0[0], s1[1] };
		}
	}

	template <typename RobustTag, typename T>
	bool intersect(RobustTag robust_tag, triangle<point<T, 2>> const & t0, triangle<point<T, 2>> const & t1)
	{
		if (contains(robust_tag, t0, t1[0]) || contains(robust_tag, t0, t1[1]) || contains(robust_tag, t0, t1[2])) return true;

		if (contains(robust_tag, t1, t0[0]) || contains(robust_tag, t1, t0[1]) || contains(robust_tag, t1, t0[2])) return true;

		if (intersect(robust_tag, simplex{t0[0], t0[1]}, simplex{t1[0], t1[1]})) return true;
		if (intersect(robust_tag, simplex{t0[0], t0[1]}, simplex{t1[1], t1[2]})) return true;
		if (intersect(robust_tag, simplex{t0[1], t0[2]}, simplex{t1[0], t1[1]})) return true;
		if (intersect(robust_tag, simplex{t0[1], t0[2]}, simplex{t1[1], t1[2]})) return true;
		if (intersect(robust_tag, simplex{t0[2], t0[0]}, simplex{t1[0], t1[1]})) return true;
		if (intersect(robust_tag, simplex{t0[2], t0[0]}, simplex{t1[1], t1[2]})) return true;

		return false;
	}

	template <typename T>
	bool intersect(triangle<point<T, 2>> const & t0, triangle<point<T, 2>> const & t1)
	{
		return intersect(default_robust_tag, t0, t1);
	}

	template <typename T, std::size_t N>
	interval<T> intersection(ray<T, N> const & r, box<T, N> const & b)
	{
		auto t = interval<T>::full();

		for (std::size_t i = 0; i < N; ++i)
		{
			T tmin = (b[i].min - r.origin[i]) / r.direction[i];
			T tmax = (b[i].max - r.origin[i]) / r.direction[i];

			if (tmin > tmax) std::swap(tmin, tmax);

			t &= interval<T>{tmin, tmax};
		}

		return t;
	}

	template <typename T, std::size_t N>
	bool intersect(ray<T, N> const & r, box<T, N> const & b)
	{
		return !intersection(r, b).empty();
	}

	template <typename T, std::size_t N>
	interval<T> intersection(segment<point<T, N>> const & s, box<T, N> const & b)
	{
		auto t = interval<T>{T(0), T(1)};

		for (std::size_t i = 0; i < N; ++i)
		{
			if (s[1][i] != s[0][i])
			{
				T tmin = (b[i].min - s[0][i]) / (s[1][i] - s[0][i]);
				T tmax = (b[i].max - s[0][i]) / (s[1][i] - s[0][i]);

				if (tmin > tmax) std::swap(tmin, tmax);

				t &= interval<T>{tmin, tmax};
			}
			else
			{
				if (!contains(b[i], s[0][i]))
					return interval<T>{};
			}
		}

		return t;
	}

	template <typename T, std::size_t N>
	bool intersect(segment<point<T, N>> const & s, box<T, N> const & b)
	{
		return !intersection(s, b).empty();
	}

	template <typename T, std::size_t N>
	std::optional<T> intersection(ray<T, N> const & r, simplex<point<T, N>, N - 1> const & s)
	{
		math::matrix<T, N, N> m;
		math::vector<T, N> b;

		for (std::size_t i = 0; i < N; ++i)
		{
			m[i][0] = r.direction[i];
			b[i] = s[0][i] - r.origin[i];

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

		if (!math::gauss(m, b))
			return std::nullopt;

		T sum{};
		for (std::size_t j = 1; j < N; ++j)
		{
			if (b[j] >= T{}) return std::nullopt;
			sum += (-b[j]);
		}

		if (sum > T{1}) return std::nullopt;

		if (b[0] < T{}) return std::nullopt;

		return b[0];
	}

	template <typename T, std::size_t N>
	bool intersect(ray<T, N> const & r, simplex<point<T, N>, N - 1> const & s)
	{
		return static_cast<bool>(intersection(r, s));
	}

	template <typename T, std::size_t N, typename = std::enable_if_t<(N > 2)>>
	std::optional<T> intersection(segment<point<T, N>> const & seg, simplex<point<T, N>, N - 1> const & s)
	{
		auto i = intersection(ray{seg[0], seg[1] - seg[0]}, s);
		if (!i || *i < 0 || *i > 1)
			return std::nullopt;
		return i;
	}

	template <typename T, std::size_t N, typename = std::enable_if_t<(N > 2)>>
	bool intersect(segment<point<T, N>> const & seg, simplex<T, N> const & s)
	{
		return static_cast<bool>(intersection(seg, s));
	}

	template <typename T, std::size_t N>
	interval<T> intersection(segment<point<T, N>> const & seg, sphere<T, N> const & s)
	{
		auto d = seg[0] - s.center;
		auto dir = seg[1] - seg[0];

		auto ir = solve_quadratic(length_sqr(dir), T{2} * dot(dir, d), length_sqr(d) - sqr(s.radius));
		if (!ir)
			return {};

		return {std::max(T{0}, ir->first), std::min(T{1}, ir->second)};
	}

	template <typename T, std::size_t N>
	bool intersect(segment<point<T, N>> const & seg, sphere<T, N> const & s)
	{
		return !intersection(seg, s).empty();
	}

	template <typename T, std::size_t N>
	interval<T> intersection(ray<T, N> const & r, sphere<T, N> const & s)
	{
		auto d = r.origin - s.center;

		auto ir = solve_quadratic(length_sqr(r.direction), T{2} * dot(r.direction, d), length_sqr(d) - sqr(s.radius));
		if (!ir)
			return {};

		return {ir->first, ir->second};
	}

	template <typename T, std::size_t N>
	interval<T> intersection(ray<T, N> const & r, cylinder<T, N> const & c)
	{
		auto Z2 = length_sqr(c.axis);
		auto CZ = dot(c.center - r.origin, c.axis);
		auto DZ = dot(r.direction, c.axis);
		if (DZ == 0)
			return {};

		auto prmin = (-Z2 + CZ) / DZ;
		auto prmax = ( Z2 + CZ) / DZ;
		if (DZ < 0)
			std::swap(prmin, prmax);

		auto n = normalized(c.axis);
		auto q = r.origin - c.center;
		auto ir = solve_quadratic(length_sqr(r.direction) - sqr(dot(r.direction, n)),
			2 * dot(q, r.direction) - 2 * dot(q, n) * dot(r.direction, n),
			length_sqr(q) - sqr(dot(q, n)) - sqr(c.radius));

		if (!ir)
			return {};

		return {std::max(ir->first, prmin), std::min(ir->second, prmax)};
	}

	template <typename T, std::size_t N>
	bool intersect(ray<T, N> const & r, sphere<T, N> const & s)
	{
		return !intersection(r, s).empty();
	}

	template <typename T, std::size_t N>
	bool intersect(ray<T, N> const & r, cylinder<T, N> const & c)
	{
		return !intersection(r, c).empty();
	}

	template <typename T, std::size_t N>
	interval<T> intersection(segment<point<T, N>> const & s, cylinder<T, N> const & c)
	{
		return intersection(ray{s[0], s[1] - s[0]}, c) & interval<T>{T{0}, T{1}};
	}

	template <typename T, std::size_t N>
	bool intersect(segment<point<T, N>> const & s, cylinder<T, N> const & c)
	{
		return !intersection(s, c).empty();
	}

}
