#define CGAL_USE_BASIC_VIEWER

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Delaunay_triangulation_adaptation_traits_2.h>
#include <CGAL/Delaunay_triangulation_adaptation_policies_2.h>

#include <CGAL/Voronoi_diagram_2.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/bounding_box.h>
#include <CGAL/Polygon_2.h>

#include <iostream>
#include <cstdint>
#include <fstream>
#include <iostream>

#include "mex.hpp"
#include "mexAdapter.hpp"
#include "MatlabDataArray.hpp"
#include <omp.h>

typedef CGAL::Exact_predicates_exact_constructions_kernel	K;

typedef K::Iso_rectangle_2									Iso_rectangle2;
typedef K::Segment_2										Segment2;
typedef K::Ray_2											Ray2;
typedef K::Line_2											Line2;
typedef K::Point_2											Point2;
typedef CGAL::Polygon_2<K>									Polygon2;
typedef Polygon2::Edge_const_iterator						EdgeIterator;

typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned int, K> Vb1;
typedef CGAL::Triangulation_data_structure_2<Vb1>					 Tds1;
typedef CGAL::Delaunay_triangulation_2<K, Tds1>						 DT;

typedef CGAL::Delaunay_triangulation_adaptation_traits_2<DT>                 AT;
typedef CGAL::Delaunay_triangulation_caching_degeneracy_removal_policy_2<DT> AP;
typedef CGAL::Voronoi_diagram_2<DT, AT, AP>                                  VD;
typedef VD::Locate_result					Locate_result;
typedef VD::Face::Ccb_halfedge_circulator   Ccb_halfedge_circulator;

//Used to convert otherwise infinite rays into looooong line segments
const int RAY_LENGTH = 100;

//CGAL often returns objects that are either segments or rays. This converts
//these objects into segments. If the object would have resolved into a ray,
//that ray is intersected with the bounding box defined above and returned as
//a segment.
const auto ConvertToSeg = [&](const CGAL::Object seg_obj, bool outgoing) -> K::Segment_2
{
	//One of these will succeed and one will have a NULL pointer
	const K::Segment_2* dseg = CGAL::object_cast<K::Segment_2>(&seg_obj);
	const K::Ray_2* dray = CGAL::object_cast<K::Ray_2>(&seg_obj);
	if (dseg) { //Okay, we have a segment
		return *dseg;
	}
	else {    //Must be a ray
		const auto& source = dray->source();
		const auto dsx = source.x();
		const auto dsy = source.y();
		const auto& dir = dray->direction();
		const auto tpoint = K::Point_2(dsx + RAY_LENGTH * dir.dx(), dsy + RAY_LENGTH * dir.dy());
		if (outgoing)
			return K::Segment_2(
				dray->source(),
				tpoint
			);
		else
			return K::Segment_2(
				tpoint,
				dray->source()
			);
	}
};

class MexFunction : public matlab::mex::Function {
public:
	void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) 
	{
		checkArguments(outputs, inputs);

		matlab::data::TypedArray<double> pnts = std::move(inputs[0]);
		matlab::data::TypedArray<int> idx = std::move(inputs[1]);
		matlab::data::TypedArray<double> pgon_pnts = std::move(inputs[2]);

		std::vector<std::vector<Segment2>> interPoly;
		//std::vector<int> sid;
		mexCVT(interPoly, pnts, idx, pgon_pnts);

		// write to matlab version
		matlab::data::ArrayFactory factory;
		matlab::data::CellArray c_edges = factory.createCellArray({ interPoly.size(), 1 });
		//matlab::data::TypedArray<double> sidMat = factory.createArray<double>({ sid.size(), 1 });

		for (int i = 0; i < interPoly.size(); ++i)
		{
			std::vector<Segment2> polyi = interPoly[i];
			matlab::data::TypedArray<double> edgesM = factory.createArray<double>({ polyi.size(), 4 });

			for (int j = 0; j < polyi.size(); ++j)
			{
				edgesM[j][0] = CGAL::to_double(polyi[j].point(0).x());
				edgesM[j][1] = CGAL::to_double(polyi[j].point(0).y());
				edgesM[j][2] = CGAL::to_double(polyi[j].point(1).x());
				edgesM[j][3] = CGAL::to_double(polyi[j].point(1).y());
			}
			c_edges[i] = edgesM;
			//sidMat[i] = sid[i];
		}

		outputs[0] = c_edges;
		//outputs[1] = sidMat;
	}

	void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) {
		std::shared_ptr<matlab::engine::MATLABEngine> matlabPtr = getEngine();
		matlab::data::ArrayFactory factory;

		if (inputs.size() != 3) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Three inputs required") }));
		}

		if (inputs[0].getDimensions()[1] != 2) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Seeds input 1 must be NX2 dimension") }));
		}

		if (inputs[0].getType() != matlab::data::ArrayType::DOUBLE ||
			inputs[0].getType() == matlab::data::ArrayType::COMPLEX_DOUBLE) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Seeds Input 1 must be a noncomplex scalar double") }));
		}

		if (inputs[1].getType() != matlab::data::ArrayType::INT32) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Index Input 2 must be type INT32") }));
		}

		if (inputs[1].getDimensions()[1] != 1) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Index Input 2 must be NX1 dimension") }));
		}

		if (inputs[2].getDimensions()[1] != 2) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Bpoly input 3 must be NX2 dimension") }));
		}

		if (inputs[2].getType() != matlab::data::ArrayType::DOUBLE ||
			inputs[2].getType() == matlab::data::ArrayType::COMPLEX_DOUBLE) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Bpoly Input 3 must be a noncomplex scalar double") }));
		}
		if (outputs.size() != 1) {
			matlabPtr->feval(u"error", 0,
				std::vector<matlab::data::Array>({ factory.createScalar("Only one output is returned") }));
		}
	}

	void mexCVT(std::vector<std::vector<Segment2>> & interPoly, 
		matlab::data::TypedArray<double> seeds, matlab::data::TypedArray<int> idx, 
		matlab::data::TypedArray<double> pgon_pnts)
	{
		std::vector<Point2> points;

		unsigned int pnts_num = seeds.getDimensions()[0];

		//#pragma omp parallel for
		for (int i = 0; i < pnts_num; ++i)
		{
			double x = seeds[i][0];
			double y = seeds[i][1];
			Point2 p(x, y);
			points.push_back(p);
		}
		DT dt = DT(points.begin(), points.end());

		std::shared_ptr<matlab::engine::MATLABEngine> matlabPtr = getEngine();
		matlab::data::ArrayFactory factory;
		if (!dt.is_valid()) {
			matlabPtr->feval(u"error", 0,
				std::vector<matlab::data::Array>({ factory.createScalar("Delaunay triangulation is not valid!") }));
		}

		VD vd(dt);

		/// step.2 generate the outer-bounding
		//In order to crop the Voronoi diagram, we need to convert the bounding box into a polygon. 
		CGAL::Polygon_2<K> bpoly;
		for (int i = 0; i < pgon_pnts.getDimensions()[0]; ++i)
		{
			double x = pgon_pnts[i][0];
			double y = pgon_pnts[i][1];
			bpoly.push_back(Point2(x, y));
		}
		
		// step.3 get voronoi
		unsigned int nidx = idx.getDimensions()[0];
		int j_real = 0;

		//#pragma omp parallel for
		for (int j = 0; j < nidx; ++j)
		{
			int seed_id = idx[j] - 1;
			if (seed_id < 0 || seed_id > pnts_num) {
				matlabPtr->feval(u"error", 0,
					std::vector<matlab::data::Array>({ factory.createScalar("The input idx is out of boundary!") }));
			}
			Point2 p = points[seed_id];

			Locate_result lr = vd.locate(p); // the face of p located
			VD::Face_handle* f = boost::get<VD::Face_handle>(&lr);

			Ccb_halfedge_circulator ec_start = (*f)->ccb(); // traversing the halfedges on the boundary of f
			Ccb_halfedge_circulator ec = ec_start;

			CGAL::Polygon_2<K> pgon;

			do {
				//A half edge circulator representing a ray doesn't carry direction
				//information. To get it, we take the dual of the dual of the half-edge.
				//The dual of a half-edge circulator is the edge of a Delaunay triangle.
				//The dual of the edge of Delaunay triangle is either a segment or a ray.
				const CGAL::Object seg_dual = vd.dual().dual(ec->dual());

				//Convert the segment/ray into a segment
				const auto this_seg = ConvertToSeg(seg_dual, ec->has_target());

				pgon.push_back(this_seg.source());

				//If the segment has no target, it's a ray. This means that the next
				//segment will also be a ray. We need to connect those two rays with a
				//segment. The following accomplishes this.
				if (!ec->has_target()) {
					const CGAL::Object nseg_dual = vd.dual().dual(ec->next()->dual());
					const auto next_seg = ConvertToSeg(nseg_dual, ec->next()->has_target());
					pgon.push_back(next_seg.target());
				}
			} while (++ec != ec_start); //Loop until we get back to the beginning

			if (CGAL::do_intersect(pgon, bpoly)) // in fact, not necessarily
			{
				//Perform the intersection. Since CGAL is very general, it believes the
				//result might be multiple polygons with holes.
				std::list<CGAL::Polygon_with_holes_2<K>> isect;
				CGAL::intersection(pgon, bpoly, std::back_inserter(isect));

				std::vector<Segment2> edges; // each intersection

				//The intersection of a convex polygon and a box may be two polygon
				//since the seed might move to the outer of the box
				std::list<CGAL::Polygon_with_holes_2<K>>::iterator it;
				for (it = isect.begin(); it != isect.end(); it++) {
					//And recover the polygon of interest
					auto& poly_w_holes = *it;
					auto& poly_outer = poly_w_holes.outer_boundary();


					for (auto v = poly_outer.vertices_begin(); v != poly_outer.vertices_end(); v++)
					{
						auto v1 = v + 1;
						if (v + 1 == poly_outer.vertices_end())
							v1 = poly_outer.vertices_begin();

						Point2 p1 = *v;
						Point2 p2 = *v1;
						if (CGAL::squared_distance(p1, p2) < 1e-5)
							continue;

						edges.push_back(Segment2(p1, p2));
					}
					
				}
				// maybe the voronoi-cell and box has none or two intersection
				// so we need to record the voronoi-id for each intersection
				interPoly.push_back(edges);
				//sid.push_back(seed_id + 1); // matlab index started with 1
			}
			else
			{
				std::cout << "no intersection for this cell with bounding box!" << std::endl;
				continue;
			}
		}
	}
};