#include "mex.hpp"
#include "mexAdapter.hpp"
#include "MatlabDataArray.hpp"

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

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Delaunay_triangulation_cell_base_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>

#include <CGAL/Surface_mesh.h>
#include <CGAL/convex_hull_3.h>

#include <CGAL/Polygon_mesh_processing/corefinement.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
//#include <CGAL/Polygon_mesh_processing/remesh.h>
//#include <CGAL/Polygon_mesh_processing/border.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel         K;
typedef CGAL::Exact_predicates_exact_constructions_kernel			EK;

typedef CGAL::Triangulation_vertex_base_with_info_3<CGAL::IO::Color, K> Vb;
typedef CGAL::Delaunay_triangulation_cell_base_3<K>                 Cb;
typedef CGAL::Triangulation_data_structure_3<Vb, Cb>                Tds;
typedef CGAL::Delaunay_triangulation_3<K, Tds>                      DT;

typedef K::Point_3		Point;
typedef K::Segment_3	Segment;
typedef K::Ray_3		Ray;
typedef K::Vector_3     Vector;


typedef CGAL::Surface_mesh<Point>									Surface_mesh;
typedef Surface_mesh::Edge_index									Edge_index;
typedef boost::graph_traits<Surface_mesh>::vertex_descriptor		vertex_descriptor;
typedef boost::graph_traits<Surface_mesh>::edge_descriptor            edge_descriptor;

typedef Surface_mesh::Property_map<vertex_descriptor, EK::Point_3>	Exact_point_map;

#ifdef CGAL_CONCURRENT_MESH_3
typedef CGAL::Parallel_tag Concurrency_tag;
#else
typedef CGAL::Sequential_tag Concurrency_tag;
#endif

namespace PMP = CGAL::Polygon_mesh_processing;
namespace params = PMP::parameters;
namespace cparas = CGAL::parameters;


const int RAY_LENGTH = 100;
const auto ConvertToSeg = [&](const CGAL::Object seg_obj) -> Segment
{
	//One of these will succeed and one will have a NULL pointer
	const Segment* dseg = CGAL::object_cast<Segment>(&seg_obj);
	const Ray* dray = CGAL::object_cast<Ray>(&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 dsz = source.z();
		const auto& dir = dray->direction();
		const auto tpoint = Point(dsx + RAY_LENGTH * dir.dx(),
			dsy + RAY_LENGTH * dir.dy(),
			dsz + RAY_LENGTH * dir.dz());
		return Segment(
			dray->source(),
			tpoint
		);
	}
};

struct Exact_vertex_point_map
{
	// typedef for the property map
	typedef boost::property_traits<Exact_point_map>::value_type value_type;
	typedef boost::property_traits<Exact_point_map>::reference reference;
	typedef boost::property_traits<Exact_point_map>::key_type key_type;
	typedef boost::read_write_property_map_tag category;
	// exterior references
	Exact_point_map exact_point_map;
	Surface_mesh* tm_ptr;
	// Converters
	CGAL::Cartesian_converter<K, EK> to_exact;
	CGAL::Cartesian_converter<EK, K> to_input;
	Exact_vertex_point_map()
		: tm_ptr(nullptr)
	{}
	Exact_vertex_point_map(const Exact_point_map& ep, Surface_mesh& tm)
		: exact_point_map(ep)
		, tm_ptr(&tm)
	{
		for (Surface_mesh::Vertex_index v : vertices(tm))
			exact_point_map[v] = to_exact(tm.point(v));
	}
	friend
		reference get(const Exact_vertex_point_map& map, key_type k)
	{
		CGAL_precondition(map.tm_ptr != nullptr);
		return map.exact_point_map[k];
	}
	friend
		void put(const Exact_vertex_point_map& map, key_type k, const EK::Point_3& p)
	{
		CGAL_precondition(map.tm_ptr != nullptr);
		map.exact_point_map[k] = p;
		// create the input point from the exact one
		map.tm_ptr->point(k) = map.to_input(p);
	}
};

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]);
		double nelx = inputs[2][0];
		double nely = inputs[3][0];
		double nelz = inputs[4][0];

		std::vector<Point> seeds;
		DT dt;
		preparation(pnts, dt, seeds);

		matlab::data::ArrayFactory factory;
		matlab::data::CellArray c_edges = factory.createCellArray({ idx.getDimensions()[0], 1 });
		matlab::data::TypedArray<double> cenP = factory.createArray<double>({ idx.getDimensions()[0], 3 });
		mexVD(c_edges, cenP, idx, seeds, dt, nelx, nely, nelz);

		outputs[0] = c_edges;
		outputs[1] = cenP;
	}

	void preparation(matlab::data::TypedArray<double> seeds, DT & dt, std::vector<Point> & points)
	{
		
		// seeds & DT
		unsigned int pnts_num = seeds.getDimensions()[0];
		for (int i = 0; i < pnts_num; ++i)
		{
			double x = seeds[i][0];
			double y = seeds[i][1];
			double z = seeds[i][2];
			points.push_back(Point(x, y, z));
		}
		dt = DT(points.begin(), points.end());
		assert(dt.is_valid());
	}

	void cur_voronoi_cell(Point p, DT dt, std::vector<Point> & vor_pnts)
	{
		// Starts at an arbitrary facet incident to edge *fei.
		std::vector<DT::Facet> incident_facet;
		DT::Vertex_handle vh = dt.nearest_vertex(p);
		dt.finite_incident_facets(vh, std::back_inserter(incident_facet));

		for (size_t j = 0; j < incident_facet.size(); j++)
		{
			DT::Facet fi = incident_facet[j];
			CGAL::Object dualedge = dt.dual(fi);

			// either segment or ray
			const auto this_seg = ConvertToSeg(dualedge);

			vor_pnts.push_back(this_seg.source());
			vor_pnts.push_back(this_seg.target());
		}
	}

	bool point_equal(Point p1, Point p2, double tol)
	{
		return (abs(p1.x() - p2.x()) < tol &&
			abs(p1.y() - p2.y()) < tol &&
			abs(p1.z() - p2.z()) < tol);
	}


	std::vector<Segment> merge_conlinear(std::vector<Segment> segs)
	{
		std::vector<Segment> newlines;

		while (!segs.empty())
		{
			Segment s1 = segs[0];
			Vector v1(s1);

			segs.erase(segs.begin());

			int j = 0;
			int len = segs.size();
			bool found = 0;
			while (j < len)
			{
				Segment s2 = segs[j];
				Vector v2(s2);
				Vector v3(s1.point(0), s2.point(0));

				//Vector v = CGAL::cross_product(v1, v2);
				//double len = v.squared_length();

				if (CGAL::cross_product(v1, v2).squared_length() < 1e-10 &&
					CGAL::cross_product(v3, v2).squared_length() < 1e-10) // colinear or parallel
				{
					for (int ii = 0; ii < 2; ++ii)
						for (int jj = 0; jj < 2; ++jj)
							if (point_equal(s1.point(ii), s2.point(jj), 1e-5) && !found)
							{
								s1 = Segment(s1.point(1 - ii), s2.point(1 - jj));
								found = 1;

								segs.erase(segs.begin() + j);
							}
				}
				len = segs.size();

				if (found)
				{
					j = 0;
					found = 0;
				}
				else
					j++;
			}
			newlines.push_back(s1);
		}
		return newlines;
	}

	void mexVD(matlab::data::CellArray& c_edges, matlab::data::TypedArray<double> & cenP, 
		matlab::data::TypedArray<int> query_idx, std::vector<Point> seeds, DT dt,
		double nelx, double nely, double nelz)
	{
		std::shared_ptr<matlab::engine::MATLABEngine> matlabPtr = getEngine();
		matlab::data::ArrayFactory factory;

		/*
		*  step.0 generate bounding box
		*/
		double halfx = nelx / 2.;
		double halfy = nely / 2.;
		double halfz = nelz / 2.;
		const char* filename = "data/L.off";
		std::ifstream input(filename);
		if (!input)
		{
			std::cerr << "Cannot open file " << std::endl;
		}
		Surface_mesh bmesh;
		CGAL::IO::read_OFF(input, bmesh);
		//Surface_mesh bmesh;
		//CGAL::convex_hull_3(bb_pnts.begin(), bb_pnts.end(), bmesh);
		Exact_point_map mesh2_exact_points =
			bmesh.add_property_map<vertex_descriptor, EK::Point_3>("v:exact_point").first;
		Exact_vertex_point_map mesh2_vpm(mesh2_exact_points, bmesh);

		/*
		*  step.1 get each voronoi-cell
		*/
		int npnts = seeds.size();

		for (int i = 0; i < query_idx.getDimensions()[0]; ++i)
		{
			int k = query_idx[i] - 1;
			if (k < 0 || k >= npnts) {
				matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({
					factory.createScalar("The input idx is out of boundary!") }));
			}
			Point p = seeds[k];

			std::vector<Point> vor_pnts; // all points of this voronoi-cell
			cur_voronoi_cell(p, dt, vor_pnts);

			/*
			*  step.2 intersection with bbx
			*/			
			Surface_mesh sm; //define polyhedron to hold convex hull
			CGAL::convex_hull_3(vor_pnts.begin(), vor_pnts.end(), sm);

			Exact_point_map mesh1_exact_points =
				sm.add_property_map<vertex_descriptor, EK::Point_3>("v:exact_point").first;
			Exact_vertex_point_map mesh1_vpm(mesh1_exact_points, sm);

			bool flag = PMP::corefine_and_compute_intersection( sm,
																bmesh,
																sm,
																params::vertex_point_map(mesh1_vpm),
																params::vertex_point_map(mesh2_vpm),
																params::vertex_point_map(mesh1_vpm));
			if (!flag) {
				matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({
					factory.createScalar("None intersection for cell-"+std::to_string(k+1)) }));
			}

			Point c = PMP::centroid(sm, params::all_default());
			cenP[i][0] = c.x();
			cenP[i][1] = c.y();
			cenP[i][2] = c.z();

			/*
			*  step.3 get segments of each voronoi cell
			*/
			std::vector<Segment> feature_lines; // to preserve		

			Surface_mesh::Property_map<edge_descriptor, bool> is_constrained =
				sm.add_property_map<edge_descriptor, bool>("e:is_constrained", false).first;

			auto fnormals = sm.add_property_map<Surface_mesh::Face_index, EK::Vector_3>("f:normals",
				CGAL::NULL_VECTOR).first;
			PMP::compute_face_normals(sm, fnormals, params::vertex_point_map(mesh1_vpm));

			for (Edge_index ei : sm.edges())
			{
				Surface_mesh::Halfedge_index h1 = sm.halfedge(ei, 0);
				Surface_mesh::Halfedge_index h2 = sm.halfedge(ei, 1);

				Surface_mesh::Vertex_index v0 = sm.source(h1);
				Surface_mesh::Vertex_index v1 = sm.source(h2);
				Point p1 = sm.point(v0);
				Point p2 = sm.point(v1);

				// skip if this segs is too short
				if (point_equal(p1, p2, 1e-5))
					continue;

				Surface_mesh::Face_index f1 = face(h1, sm);
				Surface_mesh::Face_index f2 = face(h2, sm);
				EK::Vector_3 n1 = fnormals[f1];
				EK::Vector_3 n2 = fnormals[f2];

				if (n1.squared_length() < 1e-10 || n2.squared_length() < 1e-10) {
					matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({
						factory.createScalar("The face norm may be (0,0,0) for cell-" + 
						std::to_string(k + 1)) }));
				}
				double ang = CGAL::to_double(CGAL::approximate_angle(n1, n2));

				if (abs(ang) > 1e-5 && abs(abs(ang) - 180) > 1e-5)
				{
					feature_lines.push_back(Segment(p1, p2));

					is_constrained[ei] = true;
				}
			}
			std::vector<Segment> final_lines = merge_conlinear(feature_lines);
			//std::vector<Segment> final_lines = feature_lines;

			/*
			*  step.4 change to matlab version
			*/
			matlab::data::TypedArray<double> linesMat = factory.createArray<double>({ final_lines.size(), 6 });
			for (int j = 0; j < final_lines.size(); ++j)
			{
				linesMat[j][0] = final_lines[j].point(0).x();
				linesMat[j][1] = final_lines[j].point(0).y();
				linesMat[j][2] = final_lines[j].point(0).z();
				linesMat[j][3] = final_lines[j].point(1).x();
				linesMat[j][4] = final_lines[j].point(1).y();
				linesMat[j][5] = final_lines[j].point(1).z();
			}
			c_edges[i] = linesMat;
		}

	}

	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() != 5) {
			matlabPtr->feval(u"error",
				0, std::vector<matlab::data::Array>({ factory.createScalar("Four inputs required") }));
		}

		if (inputs[0].getDimensions()[1] != 3) {
			matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({ 
				factory.createScalar("Seeds input 1 must be NX3 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") }));
		}

		for (int j = 2; j < 5; ++j)
		{
			if (inputs[j].getDimensions()[0] != 1 || inputs[j].getDimensions()[1] != 1) {
				matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({ 
					factory.createScalar("nelx/nely/nelz input 3/4/5 must be a scalar") }));
			}
			if (inputs[j].getType() != matlab::data::ArrayType::DOUBLE ||
				inputs[j].getType() == matlab::data::ArrayType::COMPLEX_DOUBLE) {
				matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({ 
					factory.createScalar("nelx/nely/nelz input 3/4/5 must be a noncomplex scalar double") }));
			}
		}
		if (outputs.size() != 2) {
			matlabPtr->feval(u"error", 0,
				std::vector<matlab::data::Array>({ factory.createScalar("Two output is returned") }));
		}
	}
};