#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/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_triangle_primitive.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel         K;
//typedef CGAL::Exact_predicates_exact_constructions_kernel			K;

typedef K::Point_3		Point;
typedef K::Segment_3	Segment;
typedef K::Triangle_3 Triangle;

typedef std::list<Triangle>::iterator Iterator;
typedef CGAL::AABB_triangle_primitive<K, Iterator> Primitive;
typedef CGAL::AABB_traits<K, Primitive> AABB_triangle_traits;
typedef CGAL::AABB_tree<AABB_triangle_traits> Tree;

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

		matlab::data::TypedArray<double> mmcsMat  = std::move(inputs[0]);
		matlab::data::CellArray c_polyPnts = std::move(inputs[1]);
		matlab::data::CellArray c_polyMesh = std::move(inputs[1]);

		// transfer to segments
		unsigned int nmmc = mmcsMat.getDimensions()[0];
		std::vector<Segment> mmcs;
		for (int i = 0; i < nmmc; ++i)
		{
			double x1 = mmcsMat[i][0];
			double y1 = mmcsMat[i][1];
			double z1 = mmcsMat[i][2];
			double x2 = mmcsMat[i][3];
			double y2 = mmcsMat[i][4];
			double z2 = mmcsMat[i][5];
			mmcs.push_back( Segment(Point(x1, y1, z1), Point(x2, y2, z2)) );
		}

		// transfer to triangulation list
		unsigned int npoly = c_polyPnts.getDimensions()[0];
		std::vector<std::list<Triangle>> triList;
		for (int i = 0; i < npoly; ++i)
		{
			matlab::data::TypedArray<double> pnts = c_polyPnts[i];
			matlab::data::TypedArray<int> trimesh = c_polyMesh[i];

			std::vector<Point> pnts;
			for (int j = 0; j < poly.getDimensions()[0]; ++j)
			{
				pnts.push_back( Point(poly[j][0], poly[j][2], poly[j][2]) );
			}
			Surface_mesh sm;
			CGAL::convex_hull_3(pnts.begin(), pnts.end(), sm);

			mesh_lists.push_back(sm);			
		}

		std::vector<std::vector<int>> interP = mexInterpolation(mesh_lists, mmcs);

		matlab::data::ArrayFactory factory;
		matlab::data::CellArray c_interP = factory.createCellArray({ nmmc, 1 });
		for (int i = 0; i < nmmc; ++i)
		{
			std::vector<int> Pi = interP[i];
			matlab::data::TypedArray<double> Pmat = factory.createArray<double>({ Pi.size()-1, 1 });

			// j start with 1, to remove the pre-filled value [-1]
			for (int j = 1; j < Pi.size(); ++j)
				Pmat[j-1] = Pi[j] + 1; // matlab start with 1

			c_interP[i] = Pmat;
		}


		outputs[0] = c_interP;
	}

	std::vector<std::vector<int>> mexInterpolation(std::vector<Surface_mesh> mesh_lists, 
		std::vector<Segment> mmcs)
	{
		int nmmc = mmcs.size();
		std::vector<int> v(1, -1);
		std::vector<std::vector<int>> interP(nmmc, v);

		double target_edge_length = 10;
		unsigned int nb_iter = 10;

		for (int i = 0; i < mesh_lists.size(); ++i)
		{
			Surface_mesh mesh = mesh_lists[i];		
			Tree tree(faces(mesh).first, faces(mesh).second, mesh);

			for (int j = 0; j < nmmc; ++j)
			{
				Segment segment_query = mmcs[j];

				if (tree.do_intersect(segment_query))
					interP[j].push_back(i);
			}
		}
		return interP;
	}

	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("Two inputs required") }));
		}

		if (inputs[0].getDimensions()[1] != 6) {
			matlabPtr->feval(u"error", 0, std::vector<matlab::data::Array>({
				factory.createScalar("MMCs input 1 must be NX6 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("MMCs Input 1 must be noncomplex scalar double") }));
		}
		if (outputs.size() != 1) {
			matlabPtr->feval(u"error", 0,
				std::vector<matlab::data::Array>({ factory.createScalar("One output is returned") }));
		}
	}
};