#pragma once

#include <psemek/math/intersection.hpp>

#include <cstddef>
#include <algorithm>
#include <variant>
#include <numeric>
#include <queue>
#include <deque>
#include <set>

namespace psemek::cg
{

	template <typename InIterator, typename OutIterator>
	OutIterator segment_intersection(InIterator begin, InIterator end, OutIterator out)
	{
		using segment = std::decay_t<decltype(*begin)>;
		using point = typename segment::point_type;
		using scalar = typename point::scalar_type;

		enum event_type
		{
			BEGIN,
			END,
			CROSS,
		};

		// TODO: robust event comparison predicate

		struct event
		{
			InIterator it1, it2;
			point p;
			event_type type;
		};

		struct event_comparator
		{
			bool operator()(event const & e1, event const & e2) const
			{
				return e1.p > e2.p;
			}
		};

		std::priority_queue<event, std::vector<event>, event_comparator> queue;

		for (auto it = begin; it != end; ++it)
		{
			event e_begin{it, end, (*it)[0], BEGIN};
			event e_end  {it, end, (*it)[1], END  };

			if (e_begin.p > e_end.p) std::swap(e_begin.p, e_end.p);
			queue.push(e_begin);
			queue.push(e_end);
		}

		auto push_cross_event = [&queue](InIterator it, InIterator jt){
			auto ion = intersection(*it, *jt);
			if (auto p = std::get_if<point>(&ion))
			{
				queue.push({it, jt, *p, CROSS});
			}
		};

		scalar sweep_line = -std::numeric_limits<scalar>::infinity();

		struct status_comparator
		{
			scalar & sweep_line;

			bool operator()(InIterator it1, InIterator it2) const
			{
				scalar const eps = 1e-4;
				scalar const y1 = (*it1)[0][1] + ((*it1)[1][1] -(*it1)[0][1]) * (sweep_line + eps - (*it1)[0][0]) / ((*it1)[1][0] - (*it1)[0][0]);
				scalar const y2 = (*it2)[0][1] + ((*it2)[1][1] -(*it2)[0][1]) * (sweep_line + eps - (*it2)[0][0]) / ((*it2)[1][0] - (*it2)[0][0]);

				return y1 < y2;
			}
		};

		std::set<InIterator, status_comparator> status{status_comparator{sweep_line}};

		while (!queue.empty())
		{
			event e = queue.top();
			queue.pop();

			switch (e.type)
			{
			case BEGIN: {
				auto it = status.insert(e.it1).first;
				if (it != status.begin()) push_cross_event(*std::prev(it), *it);
				if (std::next(it) != status.end()) push_cross_event(*it, *std::next(it));
				sweep_line = e.p[0];
				break;
			}
			case END: {
				auto it = status.find(e.it1);
				if (it == status.end())
				{
					int fuck = 42;
				}
				else
				{
					if (it != status.begin() && std::next(it) != status.end()) push_cross_event(*std::prev(it), *std::next(it));
					status.erase(it);
				}
				sweep_line = e.p[0];
				break;
			}
			case CROSS: {
				*out++ = {e.it1, e.it2};

				status.erase(e.it1);
				status.erase(e.it2);
				sweep_line = e.p[0];
				auto it1 = status.insert(e.it1).first;
				auto it2 = status.insert(e.it2).first;
				if (it1 == status.end())
				{
					int fuck_really = 42;
				}
				if (it2 == status.end())
				{
					int fuck_really = 42;
				}

				if (std::next(it2) != it1)
				{
					bool check = std::next(it1) == it2;

					int fuck = 42;
				}
				if (it2 != status.begin()) push_cross_event(*std::prev(it2), *it2);
				if (std::next(it1) != status.end()) push_cross_event(*it1, *std::next(it1));
				break;
			}
			}
		}

		return out;
	}

}
