#pragma once

#include <psemek/math/vector.hpp>
#include <psemek/math/point.hpp>
#include <psemek/math/orientation.hpp>
#include <psemek/math/simplex.hpp>
#include <psemek/math/math.hpp>

#include <vector>
#include <list>
#include <queue>
#include <map>
#include <algorithm>

namespace psemek::cg
{

	template <typename RobustTag, typename T, typename Index = std::size_t>
	std::vector<math::triangle<Index>> quickhull(RobustTag robust_tag, std::vector<math::point<T, 3ul>> const & points)
	{
		struct face;

		using handle = typename std::list<face>::iterator;
		using edge = math::segment<Index>;
		using triangle = math::triangle<Index>;

		using point = math::point<T, 3ul>;

		struct face
		{
			triangle tri;
			handle neigh[3];
			bool valid;
			std::vector<Index> out;
		};

		std::list<face> hull;
		std::list<face> deleted;

		handle const null = hull.end();

		std::queue<handle> face_queue;

		{
			// lowest 'x'
			Index i0 = std::min_element(points.begin(), points.end(), [](point const & p1, point const & p2){
				return p1[0] < p2[0];
			}) - points.begin();

			// furthest from i0
			Index i1 = std::max_element(points.begin(), points.end(), [&](point const & p1, point const & p2){
				return distance(points[i0], p1) < distance(points[i0], p2);
			}) - points.begin();

			auto n01 = math::normalized(points[i1] - points[i0]);
			auto dist01 = [&](point const & p){
				auto l = p - points[i0];
				return math::length_sqr(l) - math::sqr(math::dot(l, n01));
			};

			// furthest from i0-i1 line
			Index i2 = std::max_element(points.begin(), points.end(), [&](point const & p1, point const & p2){
				return dist01(p1) < dist01(p2);
			}) - points.begin();

			auto dist012 = [&](point const & p){
				return std::abs(math::volume(p, points[i0], points[i1], points[i2]));
			};

			// highest 'z'
			Index i3 = std::max_element(points.begin(), points.end(), [&](point const & p1, point const & p2){
				return dist012(p1) < dist012(p2);
			}) - points.begin();

			if (math::orientation(robust_tag, points[i0], points[i1], points[i2], points[i3]) == math::sign_t::negative)
				std::swap(i2, i3);

			triangle t0 { i0, i1, i2 };
			triangle t1 { i0, i3, i1 };
			triangle t2 { i0, i2, i3 };
			triangle t3 { i1, i3, i2 };

			std::vector<Index> out0, out1, out2, out3;

			for (Index i = 0; i < points.size(); ++i)
			{
				if (i == i0) continue;
				if (i == i1) continue;
				if (i == i2) continue;
				if (i == i3) continue;

				if (math::orientation(robust_tag, points[t0[0]], points[t0[1]], points[t0[2]], points[i]) == math::sign_t::negative)
					out0.push_back(i);
				else if (math::orientation(robust_tag, points[t1[0]], points[t1[1]], points[t1[2]], points[i]) == math::sign_t::negative)
					out1.push_back(i);
				else if (math::orientation(robust_tag, points[t2[0]], points[t2[1]], points[t2[2]], points[i]) == math::sign_t::negative)
					out2.push_back(i);
				else if (math::orientation(robust_tag, points[t3[0]], points[t3[1]], points[t3[2]], points[i]) == math::sign_t::negative)
					out3.push_back(i);
			}

			auto it0 = hull.insert(hull.end(), {t0, {}, true, std::move(out0)});
			auto it1 = hull.insert(hull.end(), {t1, {}, true, std::move(out1)});
			auto it2 = hull.insert(hull.end(), {t2, {}, true, std::move(out2)});
			auto it3 = hull.insert(hull.end(), {t3, {}, true, std::move(out3)});

			it0->neigh[0] = it1;
			it0->neigh[1] = it3;
			it0->neigh[2] = it2;

			it1->neigh[0] = it2;
			it1->neigh[1] = it3;
			it1->neigh[2] = it0;

			it2->neigh[0] = it0;
			it2->neigh[1] = it3;
			it2->neigh[2] = it1;

			it3->neigh[0] = it1;
			it3->neigh[1] = it2;
			it3->neigh[2] = it0;

			face_queue.push(it0);
			face_queue.push(it1);
			face_queue.push(it2);
			face_queue.push(it3);
		}

		while (!face_queue.empty())
		{
			handle h = face_queue.front();
			face_queue.pop();

			if (!h->valid)
				continue;

			std::vector<Index> const & out_set = h->out;

			if (out_set.empty())
				continue;

			triangle const t = h->tri;

			float dist = std::numeric_limits<float>::infinity();
			Index max_i = out_set[0];

			for (auto i : out_set)
			{
				float const d = math::volume(points[t[0]], points[t[1]], points[t[2]], points[i]);

				if (d < dist)
				{
					dist = d;
					max_i = i;
				}
			}

			std::vector<handle> visible;

			std::queue<handle> visible_queue;
			visible_queue.push(h);
			h->valid = false;

			while (!visible_queue.empty())
			{
				auto h = visible_queue.front();
				visible_queue.pop();
				visible.push_back(h);

				for (auto i : {0, 1, 2})
				{
					handle it = h->neigh[i];
					triangle const & t = it->tri;
					if (it->valid && (math::orientation(robust_tag, points[t[0]], points[t[1]], points[t[2]], points[max_i]) == math::sign_t::negative))
					{
						it->valid = false;
						visible_queue.push(it);
					}
				}
			}

			std::vector<Index> unassigned;
			std::vector<std::pair<handle, Index>> boundary;

			for (auto h : visible)
			{
				auto & out = h->out;
				unassigned.insert(unassigned.end(), out.begin(), out.end());

				for (auto i : {0, 1, 2})
				{
					handle n = h->neigh[i];
					if (n->valid)
					{
//						bool found = false;

						for (auto j : {0, 1, 2})
						{
							if (n->neigh[j] == h)
							{
								boundary.push_back({n, j});
//								found = true;
								break;
							}
						}
					}
				}

				deleted.splice(deleted.end(), hull, h);
			}

			auto unassigned_end = std::prev(unassigned.end());

			std::iter_swap(std::find(unassigned.begin(), unassigned.end(), max_i), unassigned_end);

			std::map<edge, std::pair<handle, Index>> edge_map;

			for (auto p : boundary)
			{
				handle h = p.first;
				std::size_t i = p.second;
				triangle const t { h->tri[(i+1)%3], h->tri[i], max_i };

				auto it = std::partition(unassigned.begin(), unassigned_end, [&](Index i){
					return math::orientation(robust_tag, points[t[0]], points[t[1]], points[t[2]], points[i]) == math::sign_t::positive;
				});

				std::vector<Index> out_set(it, unassigned_end);
				unassigned_end = it;

				hull.push_back({ t, { h, null, null }, true, std::move(out_set) });
				handle n = std::prev(hull.end());
				h->neigh[i] = n;
				face_queue.push(n);

				edge_map[{t[1], t[2]}] = {n, 1};
				edge_map[{t[2], t[0]}] = {n, 2};
			}

			for (auto const & p : edge_map)
			{
				p.second.first->neigh[p.second.second] = edge_map.at({p.first[1], p.first[0]}).first;
			}
		}

		std::vector<triangle> result;
		result.reserve(hull.size());

		for (auto const & f : hull)
			result.push_back(f.tri);

		return result;
	}

	template <typename T, typename Index = std::size_t>
	auto quickhull(std::vector<math::point<T, 3ul>> const & points)
	{
		return quickhull(math::default_robust_tag, points);
	}

}
