//#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/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 std::pair<EK::Vector_3, EK::Vector_3> Vector_pair;
//
//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> bb_pnts, seeds;
//		DT dt;
//		preparation(nelx, nely, nelz, pnts, bb_pnts, dt, seeds);
//
//		matlab::data::ArrayFactory factory;
//		matlab::data::CellArray c_edges = factory.createCellArray({ idx.getDimensions()[0], 1 });
//		matlab::data::CellArray c_norms = factory.createCellArray({ idx.getDimensions()[0], 1 });
//		mexVD(c_edges, c_norms, idx, bb_pnts, seeds, dt);
//
//		outputs[0] = c_edges;
//		outputs[1] = c_norms;
//	}
//
//	void preparation(double nelx, double nely, double nelz, matlab::data::TypedArray<double> seeds,
//		std::vector<Point> & bb_pnts, DT & dt, std::vector<Point> & points)
//	{
//		// bounding-box
//		double halfx = nelx / 2.;
//		double halfy = nely / 2.;
//		double halfz = nelz / 2.;
//				
//		bb_pnts.push_back(Point(-halfx, -halfy, -halfz));
//		bb_pnts.push_back(Point(halfx, -halfy, -halfz));
//		bb_pnts.push_back(Point(halfx, halfy, -halfz));
//		bb_pnts.push_back(Point(-halfx, halfy, -halfz));
//		bb_pnts.push_back(Point(-halfx, -halfy, halfz));
//		bb_pnts.push_back(Point(halfx, -halfy, halfz));
//		bb_pnts.push_back(Point(halfx, halfy, halfz));
//		bb_pnts.push_back(Point(-halfx, halfy, halfz));
//
//		// 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<Vector_pair> incident_fnorms, std::vector<Vector_pair>& newnorms)
//	{
//		std::vector<Segment> newlines;
//
//		while (!segs.empty())
//		{
//			Segment s1 = segs[0];
//			Vector v1(s1);
//
//			segs.erase(segs.begin());
//
//			Vector_pair f1 = incident_fnorms[0];
//			incident_fnorms.erase(incident_fnorms.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);
//								incident_fnorms.erase(incident_fnorms.begin() + j);
//							}
//				}
//				len = segs.size();
//
//				if (found)
//				{
//					j = 0;
//					found = 0;
//				}
//				else
//					j++;
//			}
//			newlines.push_back(s1);
//			newnorms.push_back(f1);
//		}
//		return newlines;
//	}
//
//	void mexVD(matlab::data::CellArray& c_edges, matlab::data::CellArray& c_norms,
//		matlab::data::TypedArray<int> query_idx, 
//		std::vector<Point> bb_pnts, std::vector<Point> seeds, DT dt)
//	{
//		std::shared_ptr<matlab::engine::MATLABEngine> matlabPtr = getEngine();
//		matlab::data::ArrayFactory factory;
//
//		/*
//		*  step.0 generate bounding box
//		*/
//		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)) }));
//			}
//
//			/*
//			*  step.3 get segments of each voronoi cell
//			*/
//			std::vector<Segment> feature_lines; // to preserve	
//			std::vector<Vector_pair> incident_fnorms;
//
//			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));
//
//					std::pair<EK::Vector_3, EK::Vector_3> fn;
//					fn.first = n1;
//					fn.second = n2;
//					incident_fnorms.push_back(fn);
//				}
//			}
//			std::vector<Vector_pair> final_fnorms;
//			std::vector<Segment> final_lines = merge_conlinear(feature_lines, 
//				incident_fnorms, final_fnorms);
//			//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 });
//			matlab::data::TypedArray<double> fnormMat = 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();
//
//				fnormMat[j][0] = CGAL::to_double(final_fnorms[j].first.x());
//				fnormMat[j][1] = CGAL::to_double(final_fnorms[j].first.y());
//				fnormMat[j][2] = CGAL::to_double(final_fnorms[j].first.z());
//				fnormMat[j][3] = CGAL::to_double(final_fnorms[j].second.x());
//				fnormMat[j][4] = CGAL::to_double(final_fnorms[j].second.y());
//				fnormMat[j][5] = CGAL::to_double(final_fnorms[j].second.z());
//			}
//			c_edges[i] = linesMat;
//			c_norms[i] = fnormMat;
//		}
//
//	}
//
//	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("Only two output is returned") }));
//		}
//	}
//};