#include "model_class.h"

#include "frame_data.h"
#include "focal_grid.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <chrono>
#include <vector>
#include <list>
#include <ctime>
#include <algorithm>
#include <dirent.h>
#include <armadillo>
#include "json.hpp"

#include <vtkPolyData.h>
#include <vtkSTLReader.h>
#include <vtkSmartPointer.h>
#include <vtkPolyDataMapper.h>
#include <vtkPolyDataSilhouette.h>
#include <vtkMatrix4x4.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkPoints.h>
#include <vtkLine.h>
#include <vtkCellArray.h>
#include <vtkExtractEdges.h>
#include <vtkCleanPolyData.h>
#include <vtkPolyDataConnectivityFilter.h>

#include <vtkPointData.h>
#include <vtkSelectEnclosedPoints.h>
#include <vtkIntArray.h>
#include <vtkDataArray.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkProperty.h>

#include <vtkSurfaceReconstructionFilter.h>
#include <vtkProgrammableSource.h>
#include <vtkContourFilter.h>
#include <vtkReverseSense.h>

#include <vtkBooleanOperationPolyDataFilter.h>

#include <vtkCenterOfMass.h>

#include <CGAL/basic.h>
//#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/algorithm.h>
#include <CGAL/assertions.h>
#include <CGAL/Line_2.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Polygon_set_2.h>
#include <CGAL/connect_holes.h>

#include <CGAL/Cartesian.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Quotient.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_walk_along_line_point_location.h>
//#include "arr_print.h"

#include <boost/math/special_functions/factorials.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel KI;
typedef CGAL::Exact_predicates_exact_constructions_kernel KE;

typedef KI::Point_2 				   Point_2;
typedef KI::Line_2					   Line_2;
typedef CGAL::Polygon_2<KI>            Polygon_2;
typedef CGAL::Polygon_with_holes_2<KI> Polygon_with_holes_2;
typedef CGAL::Polygon_set_2<KI> 	   Polygon_set_2;

typedef Polygon_2::Vertex_iterator 			VertexIterator;
typedef Polygon_2::Edge_const_iterator 		EdgeIterator;
typedef Polygon_2::Edge_const_circulator 	EdgeCirculator;

typedef CGAL::Quotient<int>                                     Number_type;
typedef CGAL::Cartesian<Number_type>                            Kernel;
typedef CGAL::Arr_segment_traits_2<KE>                      	Traits_2;
typedef Traits_2::Point_2                                       Point_arr_2;
typedef Traits_2::X_monotone_curve_2                            Segment_2;
typedef CGAL::Arrangement_2<Traits_2>                           Arrangement_2;
typedef CGAL::Arr_walk_along_line_point_location<Arrangement_2> Walk_pl;
typedef CGAL::Arr_segment_traits_2<KE> 							Segment_traits_2;

#define PI 3.14159265

using namespace std;

using json = nlohmann::json;

ModelClass::ModelClass() {
	// empty
}

bool ModelClass::LoadModel(session_data &session) {
	bool model_loaded = true;
	try {

		string mdl_name = session.model_name;
		size_t dot_pos = mdl_name.find(".");

		cout << mdl_name.erase(dot_pos,5) << endl;

		chdir(session.model_loc.c_str());

		// Load JSON file:
		ifstream in_file(session.model_name.c_str());
		json loaded_file;
		in_file >> loaded_file;

		// Set parameters:
		model_name = mdl_name.erase(dot_pos,5);

		N_components = loaded_file["N_components"];
		N_joints = loaded_file["N_joints"];

		stl_list.clear();
		parent_list.clear();

		for (int i=0; i<N_components; i++) {
			stl_list.push_back(loaded_file["stl_list"][i]);
			int N_temp = loaded_file["parent_list"][i].size();
			vector<int> kin_tree;
			for (int k=0; k<N_temp; k++) {
				kin_tree.push_back(loaded_file["parent_list"][i][k]);
			}
			parent_list.push_back(kin_tree);
		}

		joint_type_list.clear();
		joint_param_parent.clear();
		joint_param_child.clear();

		for (int j=0; j<N_joints; j++) {
			joint_type_list.push_back(loaded_file["joint_type_list"][j]);
			int N_temp = loaded_file["joint_param_parent"][j].size();
			vector<double> parent_param;
			vector<double> child_param;
			for (int k=0; k<N_temp; k++) {
				parent_param.push_back(loaded_file["joint_param_parent"][j][k]);
				child_param.push_back(loaded_file["joint_param_child"][j][k]);
			}
			joint_param_parent.push_back(parent_param);
			joint_param_child.push_back(child_param);
		}

		drag_point_labels.clear();
		drag_point_symbols.clear();
		drag_point_colors.clear();
		drag_point_start_pos.clear();

		int N_drag_pts = loaded_file["drag_point_labels"].size();

		for (int i=0; i<N_drag_pts; i++) {
			drag_point_labels.push_back(loaded_file["drag_point_labels"][i]);
			drag_point_symbols.push_back(loaded_file["drag_point_symbols"][i]);
			vector<int> pt_color;
			vector<double> start_pos;
			for (int k=0; k<3; k++) {
				pt_color.push_back(loaded_file["drag_point_colors"][i][k]);
				start_pos.push_back(loaded_file["drag_point_start_pos"][i][k]);
			}
			drag_point_colors.push_back(pt_color);
			drag_point_start_pos.push_back(start_pos);
		}

		drag_line_connectivity.clear();
		drag_line_colors.clear();
		scale_calc.clear();

		int N_drag_lns = loaded_file["drag_line_connectivity"].size();

		for (int j=0; j<N_drag_lns; j++) {
			vector<int> connectivity;
			vector<int> line_color;
			for (int k=0; k<5; k++) {
				if (k<2) {
					connectivity.push_back(loaded_file["drag_line_connectivity"][j][k]);
				}
				line_color.push_back(loaded_file["drag_line_colors"][j][k]);
			}
			drag_line_connectivity.push_back(connectivity);
			drag_line_colors.push_back(line_color);
		}

		scale_texts.clear();
		scale_calc.clear();

		for (int i=0; i<loaded_file["scale_calc"].size(); i++) {
			scale_texts.push_back(loaded_file["scale_texts"][i]);
			scale_calc.push_back(loaded_file["scale_calc"][i]);
		}

		length_calc.clear();

		for (int j=0; j<loaded_file["length_calc"].size(); j++) {
			length_calc.push_back(loaded_file["length_calc"][j]);
		}

		origin_ind.clear();

		for (int i=0; i<loaded_file["origin_indices"].size(); i++) {
			origin_ind.push_back(loaded_file["origin_indices"][i]);
		}

		contour_calc.clear();

		for (int j=0; j<loaded_file["contour_calc"].size(); j++) {
			contour_calc.push_back(loaded_file["contour_calc"][j]);
		}

		srf_angle = loaded_file["srf_angle"];

		state_calc.clear();

		for (int i=0; i<loaded_file["state_calc"].size(); i++) {
			state_calc.push_back(loaded_file["state_calc"][i]);
		}

	}
	catch(...) {
		model_loaded = false;
	}
	return model_loaded;
}

vector<arma::Mat<double>> ModelClass::ReturnStartState() {
	int N_parents;
	double scale_now;
	double scale_parent;
	double scale_child;
	arma::Col<double> state_parent(7);
	arma::Col<double> state_child(7);
	arma::Mat<double> M_parent(4,4);
	arma::Mat<double> M_child(4,4);
	arma::Mat<double> M_out;
	vector<arma::Mat<double>> M_start;
	for (int i=0; i<N_components; i++) {
		N_parents = parent_list[i].size();
		M_out.eye(4,4);
		for (int j=0; j<N_parents; j++) {
			if (j>0) {
				scale_parent = model_scale[parent_list[i][j-1]];
				state_parent(0) = joint_param_parent[parent_list[i][j]][0];
				state_parent(1) = joint_param_parent[parent_list[i][j]][1];
				state_parent(2) = joint_param_parent[parent_list[i][j]][2];
				state_parent(3) = joint_param_parent[parent_list[i][j]][3];
				state_parent(4) = joint_param_parent[parent_list[i][j]][4]*scale_parent;
				state_parent(5) = joint_param_parent[parent_list[i][j]][5]*scale_parent;
				state_parent(6) = joint_param_parent[parent_list[i][j]][6]*scale_parent;
				M_parent = ModelClass::GetM(state_parent);
				scale_child = model_scale[parent_list[i][j]];
				state_child(0) = joint_param_child[parent_list[i][j]][0];
				state_child(1) = joint_param_child[parent_list[i][j]][1];
				state_child(2) = joint_param_child[parent_list[i][j]][2];
				state_child(3) = joint_param_child[parent_list[i][j]][3];
				state_child(4) = joint_param_child[parent_list[i][j]][4]*scale_child;
				state_child(5) = joint_param_child[parent_list[i][j]][5]*scale_child;
				state_child(6) = joint_param_child[parent_list[i][j]][6]*scale_child;
				M_child = ModelClass::GetM(state_child);
				M_out = ModelClass::MultiplyM(M_out,ModelClass::MultiplyM(M_parent,M_child));
			}
			else {
				//scale_now = model_scale[parent_list[i][j]];
				scale_parent = 1.0;
				state_parent(0) = joint_param_parent[parent_list[i][j]][0];
				state_parent(1) = joint_param_parent[parent_list[i][j]][1];
				state_parent(2) = joint_param_parent[parent_list[i][j]][2];
				state_parent(3) = joint_param_parent[parent_list[i][j]][3];
				state_parent(4) = joint_param_parent[parent_list[i][j]][4]*scale_parent;
				state_parent(5) = joint_param_parent[parent_list[i][j]][5]*scale_parent;
				state_parent(6) = joint_param_parent[parent_list[i][j]][6]*scale_parent;
				M_parent = ModelClass::GetM(state_parent);
				scale_child = model_scale[parent_list[i][j]];
				state_child(0) = joint_param_child[parent_list[i][j]][0];
				state_child(1) = joint_param_child[parent_list[i][j]][1];
				state_child(2) = joint_param_child[parent_list[i][j]][2];
				state_child(3) = joint_param_child[parent_list[i][j]][3];
				state_child(4) = joint_param_child[parent_list[i][j]][4]*scale_child;
				state_child(5) = joint_param_child[parent_list[i][j]][5]*scale_child;
				state_child(6) = joint_param_child[parent_list[i][j]][6]*scale_child;
				M_child = ModelClass::GetM(state_child);
				M_out = ModelClass::MultiplyM(M_out,ModelClass::MultiplyM(M_parent,M_child));
			}
		}
		M_start.push_back(M_out);
	}
	return M_start;
}

vector<double> ModelClass::ReturnSRCState(vector<double> state_in) {

	int N_parents;
	double scale_now;
	double scale_parent;
	double scale_child;
	vector<double> state_out;

	arma::Mat<double> X_mat;
	X_mat.zeros(7,5);

	X_mat(0,0) = state_in[0];
	X_mat(1,0) = state_in[1];
	X_mat(2,0) = state_in[2];
	X_mat(3,0) = state_in[3];
	X_mat(4,0) = state_in[4];
	X_mat(5,0) = state_in[5];
	X_mat(6,0) = state_in[6];
	X_mat(0,3) = state_in[7];
	X_mat(1,3) = state_in[8];
	X_mat(2,3) = state_in[9];
	X_mat(3,3) = state_in[10];
	X_mat(0,4) = state_in[11];
	X_mat(1,4) = state_in[12];
	X_mat(2,4) = state_in[13];
	X_mat(3,4) = state_in[14];

	arma::Col<double> state_now;
	state_now.zeros(7);
	arma::Col<double> state_parent;
	state_parent.zeros(7);
	arma::Col<double> state_child;
	state_child.zeros(7);
	arma::Mat<double> M_parent(4,4);
	arma::Mat<double> M_child(4,4);
	arma::Mat<double> M_state(4,4);
	arma::Mat<double> M_out;
	for (int i=0; i<N_components; i++) {
		N_parents = parent_list[i].size();
		M_out.eye(4,4);
		for (int j=0; j<N_parents; j++) {
			if (j>0) {
				if (state_calc[parent_list[i][j]] >= 0) {
					state_now = X_mat.col(parent_list[i][j]);
				}
				else {
					state_now = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
				}
				M_state = ModelClass::GetM(state_now);
				scale_parent = model_scale[parent_list[i][j-1]];
				state_parent(0) = joint_param_parent[parent_list[i][j]][0];
				state_parent(1) = joint_param_parent[parent_list[i][j]][1];
				state_parent(2) = joint_param_parent[parent_list[i][j]][2];
				state_parent(3) = joint_param_parent[parent_list[i][j]][3];
				state_parent(4) = joint_param_parent[parent_list[i][j]][4]*scale_parent;
				state_parent(5) = joint_param_parent[parent_list[i][j]][5]*scale_parent;
				state_parent(6) = joint_param_parent[parent_list[i][j]][6]*scale_parent;
				M_parent = ModelClass::GetM(state_parent);
				scale_child = model_scale[parent_list[i][j]];
				state_child(0) = joint_param_child[parent_list[i][j]][0];
				state_child(1) = joint_param_child[parent_list[i][j]][1];
				state_child(2) = joint_param_child[parent_list[i][j]][2];
				state_child(3) = joint_param_child[parent_list[i][j]][3];
				state_child(4) = joint_param_child[parent_list[i][j]][4]*scale_child;
				state_child(5) = joint_param_child[parent_list[i][j]][5]*scale_child;
				state_child(6) = joint_param_child[parent_list[i][j]][6]*scale_child;
				M_child = ModelClass::GetM(state_child);
				M_out = ModelClass::MultiplyM(M_out,ModelClass::MultiplyM(M_parent,ModelClass::MultiplyM(M_state,M_child)));
			}
			else {
				if (state_calc[parent_list[i][j]] >= 0) {
					state_now = X_mat.col(parent_list[i][j]);
				}
				else {
					state_now = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
				}
				M_state = ModelClass::GetM(state_now);
				scale_child = model_scale[parent_list[i][j]];
				state_child(0) = joint_param_child[parent_list[i][j]][0];
				state_child(1) = joint_param_child[parent_list[i][j]][1];
				state_child(2) = joint_param_child[parent_list[i][j]][2];
				state_child(3) = joint_param_child[parent_list[i][j]][3];
				state_child(4) = joint_param_child[parent_list[i][j]][4]*scale_child;
				state_child(5) = joint_param_child[parent_list[i][j]][5]*scale_child;
				state_child(6) = joint_param_child[parent_list[i][j]][6]*scale_child;
				M_child = ModelClass::GetM(state_child);
				M_out = ModelClass::MultiplyM(M_out,ModelClass::MultiplyM(M_state,M_child));
			}
		}
		arma::Col<double> state_vec =  ModelClass::GetStateFromM(M_out);
		state_out.push_back(state_vec(0));
		state_out.push_back(state_vec(1));
		state_out.push_back(state_vec(2));
		state_out.push_back(state_vec(3));
		state_out.push_back(state_vec(4));
		state_out.push_back(state_vec(5));
		state_out.push_back(state_vec(6));
	}
	return state_out;
}

vector<arma::Mat<double>> ModelClass::ReturnInitState(frame_data &frame_in) {
	int N_parents;
	double scale_now;
	double scale_parent;
	double scale_child;
	arma::Col<double> state_now(7);
	arma::Col<double> state_parent(7);
	arma::Col<double> state_child(7);
	arma::Mat<double> M_parent(4,4);
	arma::Mat<double> M_child(4,4);
	arma::Mat<double> M_state(4,4);
	arma::Mat<double> M_out;
	vector<arma::Mat<double>> M_init;
	for (int i=0; i<N_components; i++) {
		N_parents = parent_list[i].size();
		M_out.eye(4,4);
		for (int j=0; j<N_parents; j++) {
			if (j>0) {
				if (state_calc[parent_list[i][j]] >= 0) {
					state_now = frame_in.init_state.col(state_calc[parent_list[i][j]]);
				}
				else {
					state_now = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
				}
				M_state = ModelClass::GetM(state_now);
				scale_parent = model_scale[parent_list[i][j-1]];
				state_parent(0) = joint_param_parent[parent_list[i][j]][0];
				state_parent(1) = joint_param_parent[parent_list[i][j]][1];
				state_parent(2) = joint_param_parent[parent_list[i][j]][2];
				state_parent(3) = joint_param_parent[parent_list[i][j]][3];
				state_parent(4) = joint_param_parent[parent_list[i][j]][4]*scale_parent;
				state_parent(5) = joint_param_parent[parent_list[i][j]][5]*scale_parent;
				state_parent(6) = joint_param_parent[parent_list[i][j]][6]*scale_parent;
				M_parent = ModelClass::GetM(state_parent);
				scale_child = model_scale[parent_list[i][j]];
				state_child(0) = joint_param_child[parent_list[i][j]][0];
				state_child(1) = joint_param_child[parent_list[i][j]][1];
				state_child(2) = joint_param_child[parent_list[i][j]][2];
				state_child(3) = joint_param_child[parent_list[i][j]][3];
				state_child(4) = joint_param_child[parent_list[i][j]][4]*scale_child;
				state_child(5) = joint_param_child[parent_list[i][j]][5]*scale_child;
				state_child(6) = joint_param_child[parent_list[i][j]][6]*scale_child;
				M_child = ModelClass::GetM(state_child);
				M_out = ModelClass::MultiplyM(M_out,ModelClass::MultiplyM(M_parent,ModelClass::MultiplyM(M_state,M_child)));
			}
			else {
				if (state_calc[parent_list[i][j]] >= 0) {
					state_now = frame_in.init_state.col(state_calc[parent_list[i][j]]);
				}
				else {
					state_now = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
				}
				M_state = ModelClass::GetM(state_now);
				scale_child = model_scale[parent_list[i][j]];
				state_child(0) = joint_param_child[parent_list[i][j]][0];
				state_child(1) = joint_param_child[parent_list[i][j]][1];
				state_child(2) = joint_param_child[parent_list[i][j]][2];
				state_child(3) = joint_param_child[parent_list[i][j]][3];
				state_child(4) = joint_param_child[parent_list[i][j]][4]*scale_child;
				state_child(5) = joint_param_child[parent_list[i][j]][5]*scale_child;
				state_child(6) = joint_param_child[parent_list[i][j]][6]*scale_child;
				M_child = ModelClass::GetM(state_child);
				M_out = ModelClass::MultiplyM(M_out,ModelClass::MultiplyM(M_state,M_child));
			}
		}
		M_init.push_back(M_out);
	}
	return M_init;
}

vector<Polygon_2> ModelClass::ReturnSilhouette(FocalGrid &fg, vector<arma::Mat<double>> M_state, arma::Col<double> view_vec, int cam_nr) {
	vector<Polygon_2> silhouette_vec;

	for (int i=0; i<N_components; i++) {
		double view_vector[3] = {view_vec(0),view_vec(1),view_vec(2)};
		vtkSmartPointer<vtkPolyData> polyDataTrans = ModelClass::TransformPolyData(model_polydata[i],M_state[i],model_scale[i]);
		silhouette_vec.push_back(ModelClass::GetSilhouette(fg, polyDataTrans, view_vector, cam_nr));
	}

	return silhouette_vec;
}

vector<Polygon_2> ModelClass::ReturnSilhouette2(FocalGrid &fg, vector<double> state_vec, int cam_nr) {
	vector<Polygon_2> silhouette_vec;
	arma::Col<double> view_vec = fg.CalculateViewVector(cam_nr);
	for (int i=0; i<N_components; i++) {
		arma::Col<double> state_in = {state_vec[i*7],state_vec[i*7+1],state_vec[i*7+2],state_vec[i*7+3],state_vec[i*7+4],state_vec[i*7+5],state_vec[i*7+6]};
		double view_vector[3] = {view_vec(0),view_vec(1),view_vec(2)};
		vtkSmartPointer<vtkPolyData> polyDataTrans = ModelClass::TransformPolyData(model_polydata[i],ModelClass::GetM(state_in),model_scale[i]);
		silhouette_vec.push_back(ModelClass::GetSilhouette(fg, polyDataTrans, view_vector, cam_nr));
	}

	return silhouette_vec;
}

void ModelClass::SetStrokeBounds(double StrokeBound) {
	stroke_bound = StrokeBound;
}

void ModelClass::SetDeviationBounds(double DeviationBound) {
	deviation_bound = DeviationBound;
}

void ModelClass::SetWingPitchBounds(double WingPitchBound) {
	wing_pitch_bound = WingPitchBound;
}

bool ModelClass::SetModel(session_data &session) {
	bool success = true;
	try {
		ModelClass::LoadModelMesh(session);
	}
	catch(...) {
		success = false;
	}
	return success;
}

bool ModelClass::LoadModelMesh(session_data &session) {
	bool success = true;
	try {
		model_polydata.clear();
		for (int i=0; i<N_components; i++) {
			model_polydata.push_back(ModelClass::LoadSTLFile(session.model_loc,stl_list[i]));
		}
	}
	catch(...) {
		success = false;
	}
	return success;
}

void ModelClass::TestPointInsidePolygonSpeed(arma::Mat<double> &pcl_in) {

	int N_points = pcl_in.n_cols;

	cout << "N_points " << N_points << endl;

	vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();

	for (int i=0; i<N_points; i++) {
		double t_point[3] = {pcl_in(0,i), pcl_in(1,i), pcl_in(2,i)};
		points->InsertNextPoint(t_point);
	}

	vtkSmartPointer<vtkPolyData> pointsPolydata = vtkSmartPointer<vtkPolyData>::New();
  	pointsPolydata->SetPoints(points);

	//Points inside test
	vtkSmartPointer<vtkSelectEnclosedPoints> selectEnclosedPoints = vtkSmartPointer<vtkSelectEnclosedPoints>::New();

	selectEnclosedPoints->SetInputData(pointsPolydata);

	selectEnclosedPoints->SetSurfaceData(model_polydata[0]);

	selectEnclosedPoints->Update();

	// Select enclosed points
	vtkDataArray* insideArray = vtkDataArray::SafeDownCast(selectEnclosedPoints->GetOutput()->GetPointData()->GetArray("SelectedPoints"));

	arma::Row<int> pts_inside_array(insideArray->GetNumberOfTuples());

  	for(vtkIdType j = 0; j < insideArray->GetNumberOfTuples(); j++)
    {
    	pts_inside_array(j) = insideArray->GetComponent(j,0);
    }

}

void ModelClass::Test3DIntersection() {

	vtkSmartPointer<vtkBooleanOperationPolyDataFilter> booleanOperation = vtkSmartPointer<vtkBooleanOperationPolyDataFilter>::New();
	booleanOperation->SetOperationToIntersection();
	booleanOperation->SetInputData( 0, model_polydata[0] );
	booleanOperation->SetInputData( 1, model_polydata[2] );
	booleanOperation->Update();

}

void ModelClass::TestPoissonSurfaceReconstruction(arma::Mat<double> &pcl_in) {

	int N_points = pcl_in.n_cols;

	cout << "N_points " << N_points << endl;

	vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();

	for (int i=0; i<N_points; i++) {
		double t_point[3] = {pcl_in(0,i), pcl_in(1,i), pcl_in(2,i)};
		points->InsertNextPoint(t_point);
	}

	vtkSmartPointer<vtkPolyData> pointsPolydata = vtkSmartPointer<vtkPolyData>::New();
  	pointsPolydata->SetPoints(points);

  	// Construct the surface and create isosurface.
	vtkSmartPointer<vtkSurfaceReconstructionFilter> surf = vtkSmartPointer<vtkSurfaceReconstructionFilter>::New();\
	surf->SetInputData(pointsPolydata);

	vtkSmartPointer<vtkContourFilter> cf = vtkSmartPointer<vtkContourFilter>::New();
	cf->SetInputConnection(surf->GetOutputPort());
	cf->SetValue(0, 0.0);

	cf->Update();

	// Sometimes the contouring algorithm can create a volume whose gradient
  	// vector and ordering of polygon (using the right hand rule) are
  	// inconsistent. vtkReverseSense cures this problem.
  	vtkSmartPointer<vtkReverseSense> reverse = vtkSmartPointer<vtkReverseSense>::New();
	reverse->SetInputConnection(cf->GetOutputPort());
	reverse->ReverseCellsOn();
	reverse->ReverseNormalsOn();

	reverse->Update();

	//vtkSmartPointer<vtkPolyData> poissonPoly = vtkSmartPointer<vtkPolyData>::New();
	//poissonPoly->SetPoints(reverse);

	vtkSmartPointer<vtkCenterOfMass> centerOfMassFilter = vtkSmartPointer<vtkCenterOfMass>::New();

	centerOfMassFilter->SetInputConnection(reverse->GetOutputPort());

	centerOfMassFilter->SetUseScalarsAsWeights(false);
	centerOfMassFilter->Update();

	double center[3];
	centerOfMassFilter->GetCenter(center);

	std::cout << "Center of mass is " << center[0] << " " << center[1] << " " << center[2] << std::endl;

}

vtkSmartPointer<vtkPolyData> ModelClass::LoadSTLFile(string FileLoc, string FileName) {

	vtkSmartPointer<vtkPolyData> polyData = ModelClass::stlReader(FileLoc, FileName);

	return polyData;
}

vector<arma::Mat<double>> ModelClass::ReturnModelPCL(vector<arma::Mat<double>> M_in) {

	vector<arma::Mat<double>> pcl_out;

	arma::Mat<double> pcl_now;

	for (int i=0; i<N_components; i++) {
		pcl_now = model_pcls[i];
		pcl_out.push_back(M_in[i]*pcl_now);
	}

	return pcl_out;
}

vector<arma::Mat<double>> ModelClass::ReturnInitPCL(vector<arma::Mat<double>> M_in) {

	arma::Mat<double> init_body_pcl;
	arma::Mat<double> init_wing_L_pcl;
	arma::Mat<double> init_wing_R_pcl;

	arma::Mat<double> pcl_now;
	arma::Mat<double> pcl_scaled;

	for (int i=0; i<N_components; i++) {
		if (state_calc[i] == 0) {
			pcl_now = model_pcls[i];
			pcl_scaled = pcl_now*model_scale[i];
			pcl_scaled.row(3).fill(1.0);
			init_body_pcl = M_in[i]*pcl_scaled;
		}
		else if (state_calc[i] == 1) {
			pcl_now = model_pcls[i];
			pcl_scaled = pcl_now*model_scale[i];
			pcl_scaled.row(3).fill(1.0);
			init_wing_L_pcl = M_in[i]*pcl_scaled;
		}
		else if (state_calc[i] == 2) {
			pcl_now = model_pcls[i];
			pcl_scaled = pcl_now*model_scale[i];
			pcl_scaled.row(3).fill(1.0);
			init_wing_R_pcl = M_in[i]*pcl_scaled;
		}
		else {
			pcl_now = model_pcls[i];
			pcl_scaled = pcl_now*model_scale[i];
			pcl_scaled.row(3).fill(1.0);
			init_body_pcl = arma::join_rows(init_body_pcl,M_in[i]*pcl_scaled);
		}
	}

	vector<arma::Mat<double>> pcl_out;

	pcl_out.push_back(init_body_pcl);
	pcl_out.push_back(init_wing_L_pcl);
	pcl_out.push_back(init_wing_R_pcl);

	return pcl_out;

}

vtkSmartPointer<vtkPolyData> ModelClass::TransformPolyData(vtkSmartPointer<vtkPolyData> polyData, arma::Mat<double> M_in, double scale_in) {

	vtkSmartPointer<vtkMatrix4x4> M_t = vtkSmartPointer<vtkMatrix4x4>::New();

	M_t->SetElement(0,0,M_in(0,0));
	M_t->SetElement(0,1,M_in(0,1));
	M_t->SetElement(0,2,M_in(0,2));
	M_t->SetElement(0,3,M_in(0,3));
	M_t->SetElement(1,0,M_in(1,0));
	M_t->SetElement(1,1,M_in(1,1));
	M_t->SetElement(1,2,M_in(1,2));
	M_t->SetElement(1,3,M_in(1,3));
	M_t->SetElement(2,0,M_in(2,0));
	M_t->SetElement(2,1,M_in(2,1));
	M_t->SetElement(2,2,M_in(2,2));
	M_t->SetElement(2,3,M_in(2,3));
	M_t->SetElement(3,0,M_in(3,0));
	M_t->SetElement(3,1,M_in(3,1));
	M_t->SetElement(3,2,M_in(3,2));
	M_t->SetElement(3,3,M_in(3,3));

	vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();

	transform->SetMatrix(M_t);
	transform->Scale(scale_in,scale_in,scale_in);

	vtkSmartPointer<vtkTransformPolyDataFilter> transformPD = vtkSmartPointer<vtkTransformPolyDataFilter>::New();

	transformPD->SetInputData(polyData);
	transformPD->SetTransform(transform);
	transformPD->Update();

	//vtkPolyData* polyDataOut = transformPD->GetOutput();

	return transformPD->GetOutput();
}

Polygon_2 ModelClass::GetSilhouette(FocalGrid &fg, vtkSmartPointer<vtkPolyData> polyData, double view_vector[3], int cam_nr) {

	vtkSmartPointer<vtkPolyDataSilhouette> silhouette = vtkSmartPointer<vtkPolyDataSilhouette>::New();
	silhouette->SetInputData(polyData);
	silhouette->SetDirectionToSpecifiedVector();
	silhouette->SetVector(view_vector);
	silhouette->SetEnableFeatureAngle(0);
	//silhouette->BorderEdgesOff();
	silhouette->Update();

	vtkPolyData* silhdata = silhouette->GetOutput();

	int N_lines = silhdata->GetNumberOfLines();

	arma::Mat<double> sil_mat(4,N_lines*2);
	arma::Row<int> id_list(N_lines*2);

	double p1[3];
	double p2[3];

	vtkSmartPointer<vtkIdList> idList = vtkSmartPointer<vtkIdList>::New();
	int iter = 0;
	while (silhdata->GetLines()->GetNextCell(idList)) {
		silhdata->GetPoint(idList->GetId(0),p1);
		silhdata->GetPoint(idList->GetId(1),p2);
		id_list(iter*2) = idList->GetId(0);
		id_list(iter*2+1) = idList->GetId(1);
		sil_mat(0,iter*2) = p1[0];
		sil_mat(1,iter*2) = p1[1];
		sil_mat(2,iter*2) = p1[2];
		sil_mat(3,iter*2) = 1.0;
		sil_mat(0,iter*2+1) = p2[0];
		sil_mat(1,iter*2+1) = p2[1];
		sil_mat(2,iter*2+1) = p2[2];
		sil_mat(3,iter*2+1) = 1.0;
		iter++;
	}

	arma::Mat<double> silh_pts = fg.ProjectCloud2UVdouble(sil_mat,cam_nr);

	// Insert the 2d segments in a 2d arrangement
	Arrangement_2 arr;
	Walk_pl pl(arr);
	for (int i=0; i<N_lines; i++) {
		Segment_2 s_now (Point_arr_2 (silh_pts(0,i*2), silh_pts(1,i*2)), Point_arr_2 (silh_pts(0,i*2+1), silh_pts(1,i*2+1)));
		insert(arr, s_now, pl);
	}

	Arrangement_2::Vertex_const_iterator vit;

	arma::Row<int> vertex_degree;
	vertex_degree.zeros(arr.number_of_vertices());
	arma::Mat<double> vertex_mat;
	vertex_mat.zeros(2,arr.number_of_vertices());
	iter = 0;
	for (vit = arr.vertices_begin(); vit != arr.vertices_end(); ++vit) {
  		vertex_degree(iter) = vit->degree();
  		vertex_mat(0,iter) = CGAL::to_double((vit->point()).x());
  		vertex_mat(1,iter) = CGAL::to_double((vit->point()).y());
  		iter++;
  	}

  	//cout << vertex_mat << endl;

  	//cout << vertex_degree << endl;

  	// Find open ends:
  	arma::uvec open_ends = arma::find(vertex_degree==1);

  	//cout << open_ends << endl;

  	if (open_ends.is_empty()) {
  		//cout << "no open ends" << endl;
  	}
  	else {
	  	int N_open_ends = open_ends.n_rows;
	  	//cout << "no of open ends: " << N_open_ends << endl;
	  	if (N_open_ends%2==0) {
	  		int comb_arr[N_open_ends];
	  		for (int k=0; k<N_open_ends; k++) {
	  			comb_arr[k] = k;
	  		}

	  		sort(comb_arr,comb_arr+N_open_ends);

	  		do {
	  			int add_2_mat = 0;
				for (int i=0; i<(N_open_ends/2); i++) {
					if (comb_arr[i*2]<comb_arr[i*2+1]) {
						add_2_mat++;
					}
				}
				if (add_2_mat==(N_open_ends/2)) {
		    		for (int j=0; j<(N_open_ends/2); j++) {
		    			//cout << comb_arr[j*2] << " " << comb_arr[j*2+1] << " ";
		    			Segment_2 s_now (Point_arr_2 (vertex_mat(0,open_ends(comb_arr[j*2])), vertex_mat(1,open_ends(comb_arr[j*2]))),
		    			Point_arr_2 (vertex_mat(0,open_ends(comb_arr[j*2+1])), vertex_mat(1,open_ends(comb_arr[j*2+1]))));
		    			insert(arr, s_now, pl);
		    		}
		    		//cout << endl;
		    	}
	  		} while (next_permutation(comb_arr,comb_arr+N_open_ends-1));

  		}
  		else {
  			cout << "odd number of open ends" << endl;
  		}
  	}

  	// Remove internal edges:
  	Arrangement_2::Face_handle unb_face = arr.unbounded_face();
  	Arrangement_2::Edge_iterator eit;
  	for (eit = arr.edges_begin(); eit!=arr.edges_end(); ++eit) {
  		Arrangement_2::Halfedge_handle he = eit;
  		if ((he->face()!=unb_face)&&(he->twin()->face()!=unb_face)) {
  			arr.remove_edge(eit);
  		}
  	}

  	Polygon_2 P_out;

  	//cout << "Bounded faces: " << endl;
  	Arrangement_2::Face_iterator fit;
  	Arrangement_2::Ccb_halfedge_const_circulator  curr;
  	for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
  		if(fit->is_unbounded()) {
  			//cout << "unbounded face" << endl;
  		}
  		else {
  			//cout << "bounded face" << endl;
  			curr = fit->outer_ccb();
  			//cout << "Direction " << curr->direction() << endl;
      		//cout << curr->source()->point();
      		//P_out.push_back(Point_2 (CGAL::to_double((curr->source()->point()).x()), CGAL::to_double((curr->source()->point()).y())));
      		do {
        		//cout << " --> " << curr->target()->point();
        		//cout << "  direction " << curr->direction();
        		P_out.push_back(Point_2 (CGAL::to_double((curr->target()->point()).x()), CGAL::to_double((curr->target()->point()).y())));
        		++curr;
      		} while (curr != fit->outer_ccb());
      		//cout << endl;
  		}
  	}

	//cout << "N lines = " << N_lines << endl;

	// Print the size of the arrangement.
	//cout << "The arrangement size:" << std::endl
    //	<< "   V = " << arr.number_of_vertices()
    //    << ",  E = " << arr.number_of_edges()
    //    << ",  F = " << arr.number_of_faces() << endl;

    //Polygon_2 P_out;

    //Arrangement_2::Vertex_iterator vit2;

    //for (vit2 = arr.vertices_begin(); vit2 != arr.vertices_end(); ++vit2) {
  		//Arrangement_2::Vertex_handle ve = vit2;
  		//P_out.push_back(Point_2 (CGAL::to_double((vit2->point()).x()), CGAL::to_double((vit2->point()).y())));
  		//cout << vit2->point() << endl;
  	//}

  	//cout << "P_out is simple: " << P_out.is_simple() << endl;

    //arma::uvec unique_ids = arma::find_unique(id_list);
    //arma::Mat<double> unique_pts = silh_pts.cols(unique_ids);

    //for (int j=0; j<unique_pts.n_cols; j++) {
    //	P_out.push_back(Point_2 (unique_pts(0,j), unique_pts(1,j)));
    //}

    return P_out;
}

/*

Polygon_2 ModelClass::GetSilhouette(FocalGrid &fg, vtkSmartPointer<vtkPolyData> polyData, double view_vector[3], int cam_nr) {

	vtkSmartPointer<vtkPolyDataSilhouette> silhouette = vtkSmartPointer<vtkPolyDataSilhouette>::New();
	silhouette->SetInputData(polyData);
	silhouette->SetDirectionToSpecifiedVector();
	silhouette->SetVector(view_vector);
	silhouette->SetEnableFeatureAngle(0);
	//silhouette->BorderEdgesOff();
	silhouette->Update();

	vtkPolyData* silhdata = silhouette->GetOutput();

	//vtkSmartPointer<vtkPolyDataConnectivityFilter> connectivityFilter =
    //vtkSmartPointer<vtkPolyDataConnectivityFilter>::New();
  	//connectivityFilter->SetInputConnection(silhouette->GetOutputPort());
  	//connectivityFilter->SetExtractionModeToLargestRegion();
  	//connectivityFilter->SetExtractionModeToClosestPointRegion();
  	//connectivityFilter->SetExtractionModeToAllRegions();
  	//connectivityFilter->Update();

  	//vtkPolyData* silhdata = connectivityFilter->GetOutput();

  	//vtkSmartPointer<vtkCleanPolyData> cleaner = vtkSmartPointer<vtkCleanPolyData>::New();
  	//cleaner->SetInputConnection(silhouette->GetOutputPort());
  	//cleaner->Update();

  	//vtkPolyData* silhdata = cleaner->GetOutput();

	int N_lines = silhdata->GetNumberOfLines();

	arma::Mat<double> sil_mat(4,N_lines*2);
	arma::Row<int> id_list(N_lines*2);

	double p1[3];
	double p2[3];

	vtkSmartPointer<vtkIdList> idList = vtkSmartPointer<vtkIdList>::New();
	int iter = 0;
	while (silhdata->GetLines()->GetNextCell(idList)) {
		silhdata->GetPoint(idList->GetId(0),p1);
		silhdata->GetPoint(idList->GetId(1),p2);
		id_list(iter*2) = idList->GetId(0);
		id_list(iter*2+1) = idList->GetId(1);
		sil_mat(0,iter*2) = p1[0];
		sil_mat(1,iter*2) = p1[1];
		sil_mat(2,iter*2) = p1[2];
		sil_mat(3,iter*2) = 1.0;
		sil_mat(0,iter*2+1) = p2[0];
		sil_mat(1,iter*2+1) = p2[1];
		sil_mat(2,iter*2+1) = p2[2];
		sil_mat(3,iter*2+1) = 1.0;
		iter++;
	}

	cout << "id list" << endl;
	cout << id_list << endl;

	arma::Mat<double> silh_pts = fg.ProjectCloud2UVdouble(sil_mat,cam_nr);

	vector<list<int>> line_seg_vec;
	list<int> line_seg;

	int prev_id = id_list(0);
	int next_id = id_list(1);
	id_list(0) = -1;
	id_list(1) = -1;
	int count = 0;

	while (count<(silh_pts.n_cols/2)) {

		if (prev_id<0 && next_id<0) {
			line_seg_vec.push_back(line_seg);
			line_seg.clear();
			arma::uvec ids_left = arma::find(id_list>-1);
			if (ids_left.is_empty()) {
				count = silh_pts.n_cols/2;
			}
			else {
				prev_id = id_list(ids_left(0));
				next_id = id_list(ids_left(0)+1);
			}
		}
		else {
			arma::uvec found_prev_ids = arma::find(id_list==prev_id);

			if (found_prev_ids.is_empty()) {
				prev_id = -10;
			}
			else {
					if (found_prev_ids(0)%2==0) {
						line_seg.push_front(found_prev_ids(0));
						prev_id = id_list(found_prev_ids(0)+1);
						id_list(found_prev_ids(0)) = -1;
						id_list(found_prev_ids(0)+1) = -1;
					}
					else {
						line_seg.push_front(found_prev_ids(0)-1);
						prev_id = id_list(found_prev_ids(0)-1);
						id_list(found_prev_ids(0)) = -1;
						id_list(found_prev_ids(0)-1) = -1;
					}
					count++;
			}

			arma::uvec found_next_ids = arma::find(id_list==next_id);

			if (found_next_ids.is_empty()) {
				next_id = -10;
			}
			else {
					if (found_next_ids(0)%2==0) {
						line_seg.push_back(found_next_ids(0)+1);
						next_id = id_list(found_next_ids(0)+1);
						id_list(found_next_ids(0)) = -1;
						id_list(found_next_ids(0)+1) = -1;
					}
					else {
						line_seg.push_back(found_next_ids(0)-1);
						next_id = id_list(found_next_ids(0)-1);
						id_list(found_next_ids(0)) = -1;
						id_list(found_next_ids(0)-1) = -1;
					}
					count++;
			}
		}

		if (prev_id==next_id && prev_id+next_id>=0) {
			list<int>::iterator it1 = line_seg.begin();
			it1 = line_seg.erase(it1);
		}

	}

	cout << "finished" << endl;

	int N_seg = line_seg_vec.size();

	arma::Row<int> connectivity_vec;
	connectivity_vec.zeros(2*N_seg);

	if (N_seg>1) {

		arma::Mat<double> connectivity_pts(3,2*N_seg);
		arma::Row<int> connectivity_ids(2*N_seg);
		int connectivity_arr[2*(N_seg-1)];
		for (int i=0; i<N_seg; i++) {
			if (i==0) {
				connectivity_pts.col(i*2) = silh_pts.col(line_seg_vec[i].front());
				connectivity_pts.col(i*2+1) = silh_pts.col(line_seg_vec[i].back());
				connectivity_ids(i*2) = line_seg_vec[i].front();
				connectivity_ids(i*2+1) = line_seg_vec[i].back();
			}
			else {
				connectivity_arr[(i-1)*2] = i*2;
				connectivity_arr[(i-1)*2+1] = i*2+1;
				connectivity_pts.col(i*2) = silh_pts.col(line_seg_vec[i].front());
				connectivity_pts.col(i*2+1) = silh_pts.col(line_seg_vec[i].back());
				connectivity_ids(i*2) = line_seg_vec[i].front();
				connectivity_ids(i*2+1) = line_seg_vec[i].back();
			}
		}

		sort(connectivity_arr, connectivity_arr+2*(N_seg-1));

		arma::Mat<int> connectivity_mat;
		connectivity_mat.zeros(1,2*N_seg);

		int row_count = 0;
		do {
			int add_2_mat = 0;
			for (int j=0; j<(N_seg-1); j++) {
				if (abs(connectivity_arr[j*2]-connectivity_arr[j*2+1])==1) {
					add_2_mat++;
				}
			}
			if (add_2_mat==(N_seg-1)) {
				if (row_count==0) {
					connectivity_mat(row_count,0) = 0;
					connectivity_mat(row_count,1) = 1;
					for (int j=0; j<(N_seg-1); j++) {
						connectivity_mat(row_count,(j+1)*2) = connectivity_arr[j*2];
						connectivity_mat(row_count,(j+1)*2+1) = connectivity_arr[j*2+1];
					}
				}
				else {
					arma::Row<int> temp_row(2*N_seg);
					temp_row(0) = 0;
					temp_row(1) = 1;
					for (int j=0; j<(N_seg-1); j++) {
						temp_row((j+1)*2) = connectivity_arr[j*2];
						temp_row((j+1)*2+1) = connectivity_arr[j*2+1];
					}
					connectivity_mat.insert_rows(row_count,temp_row);
				}

				// Check if lines intersect:
				Polygon_2 P_test;
				double pt_diff;
				for (int i=0; i<N_seg; i++) {
					if (connectivity_ids(connectivity_mat(row_count,i*2))==connectivity_ids(connectivity_mat(row_count,i*2+1))) {
						P_test.push_back(Point_2 (connectivity_pts(0,connectivity_mat(row_count,i*2)), connectivity_pts(1,connectivity_mat(row_count,i*2))));
					}
					else {
						P_test.push_back(Point_2 (connectivity_pts(0,connectivity_mat(row_count,i*2)), connectivity_pts(1,connectivity_mat(row_count,i*2))));
						P_test.push_back(Point_2 (connectivity_pts(0,connectivity_mat(row_count,i*2+1)), connectivity_pts(1,connectivity_mat(row_count,i*2+1))));
					}
				}

				if (P_test.is_simple()==true) {
					connectivity_vec = connectivity_mat.row(row_count);
					break;
				}

				row_count++;
			}
		} while (next_permutation(connectivity_arr, connectivity_arr+2*(N_seg-1)));

	}
	else {
		connectivity_vec(0) = 0;
		connectivity_vec(1) = 1;
	}

	cout << connectivity_vec << endl;

	Polygon_2 P_temp;

	int list_nr;

	list<Segment_2> segment_list;

	cout << "printing segments" << endl;
	cout << N_seg << endl;

	for (int i=0; i<N_seg; i++) {
		if (connectivity_vec(i*2)<connectivity_vec(i*2+1)) {
			list_nr = connectivity_vec(i*2)/2;
			for (list<int>::iterator it=line_seg_vec[i].begin(); it!=line_seg_vec[i].end(); ++it) {
				cout << " " << *it;
				Point_2 t_p1 (silh_pts(0,*it), silh_pts(1,*it));
				Point_2 t_p2 (silh_pts(0,next(*it)), silh_pts(1,next(*it)));
				segment_list.push_back(Segment_2 (t_p1, t_p2));
				P_temp.push_back(Point_2 (silh_pts(0,*it), silh_pts(1,*it)));
			}
		}
		else {
			list_nr = (connectivity_vec(i*2)-1)/2;
			for (list<int>::reverse_iterator rit=line_seg_vec[i].rbegin(); rit!=line_seg_vec[i].rend(); ++rit) {
				cout << " " << *rit;
				segment_list.push_back(Point_2 (silh_pts(0,*rit), silh_pts(1,*rit)));
				P_temp.push_back(Point_2 (silh_pts(0,*rit), silh_pts(1,*rit)));
			}
		}
	}

	Arrangement_2 arr;
	insert(arr, segment_list.begin(), segment_list.end());
	for (auto it = arr.begin_vertices(); it != arr.end_vertices(); ++it) {
		if (4 == it->degree()) {
			cout << "intersection vertex " << *it << endl;
		}
	}

	Polygon_2 P_temp;

	cout << "printing segments" << endl;
	cout << N_seg << endl;

	int list_nr;

	for (int i=0; i<N_seg; i++) {
		if (connectivity_vec(i*2)<connectivity_vec(i*2+1)) {
			list_nr = connectivity_vec(i*2)/2;
			for (list<int>::iterator it=line_seg_vec[i].begin(); it!=line_seg_vec[i].end(); ++it) {
				cout << " " << *it;
				P_temp.push_back(Point_2 (silh_pts(0,*it), silh_pts(1,*it)));
			}
		}
		else {
			list_nr = (connectivity_vec(i*2)-1)/2;
			for (list<int>::reverse_iterator rit=line_seg_vec[i].rbegin(); rit!=line_seg_vec[i].rend(); ++rit) {
				cout << " " << *rit;
				P_temp.push_back(Point_2 (silh_pts(0,*rit), silh_pts(1,*rit)));
			}
		}
	}

	// Iterate through the vertices, remove vertices which lay inside the contour:
	//for (VertexIterator vi = P_temp.vertices_begin(); vi != P_temp.vertices_end(); ++vi) {
		//cout << P_temp.bounded_side(*vi) << endl;
		//cout << CGAL::bounded_side_2(P_temp.vertices_begin(), P_temp.vertices_end(), *vi, KI()) << endl;
	//}

	//Polygon_with_holes_2 P_holes(P_temp);

	Polygon_2 P_out = P_temp;

	cout << " " << endl;

	cout << "P_out is simple : " << P_out.is_simple() << endl;

	return P_out;
}

*/

vtkSmartPointer<vtkPolyData> ModelClass::stlReader(string FileLoc, string FileName) {

	vtkSmartPointer<vtkPolyData> polyData;

	string inputFilename = FileLoc + "/" + FileName;
	vtkSmartPointer<vtkSTLReader> reader = vtkSmartPointer<vtkSTLReader>::New();
	reader->SetFileName(inputFilename.c_str());
	reader->Update();

	vtkSmartPointer<vtkCleanPolyData> clean = vtkSmartPointer<vtkCleanPolyData>::New();
	clean->SetInputConnection(reader->GetOutputPort());
	clean->Update();

	polyData = clean->GetOutput();

	return polyData;
}

void ModelClass::SetScale(vector<double> scale_in) {
	model_scale.clear();
	cout << "" << endl;
	cout << "------------------------------------------------" << endl;
	cout << "" << endl;
	cout << "model scales: " << endl;
	for (int i=0; i<N_components; i++) {
		cout << stl_list[i] << ": " << to_string(scale_in[i]) << endl;
		model_scale.push_back(scale_in[i]);
	}
	cout << "" << endl;
	cout << "------------------------------------------------" << endl;
	cout << "" << endl;
}

vector<double> ModelClass::GetModelScale() {
	return model_scale;
}

bool ModelClass::ScaleModelPCL() {
	bool success = true;
	try {
		for (int i=0; i<N_components; i++) {
			arma::Mat<double> scale_mat;
			scale_mat.eye(4,4);
			scale_mat(0,0) = model_scale[i];
			scale_mat(1,1) = model_scale[i];
			scale_mat(2,2) = model_scale[i];
			model_pcls[i] = scale_mat*model_pcls[i];
		}
	}
	catch(...) {
		success = false;
	}
	return success;
}

arma::Mat<double> ModelClass::GetM(arma::Col<double> state_in) {

	arma::Col<double> q_vec = {state_in(0),state_in(1),state_in(2),state_in(3)};
	q_vec = q_vec/sqrt(pow(q_vec(0),2)+pow(q_vec(1),2)+pow(q_vec(2),2)+pow(q_vec(3),2));

	double q0 = q_vec(0);
	double q1 = q_vec(1);
	double q2 = q_vec(2);
	double q3 = q_vec(3);
	double tx = state_in(4);
	double ty = state_in(5);
	double tz = state_in(6);

	arma::Mat<double> M = {{2.0*pow(q0,2.0)-1.0+2.0*pow(q1,2.0), 2.0*q1*q2+2.0*q0*q3, 2.0*q1*q3-2.0*q0*q2, tx},
		{2.0*q1*q2-2.0*q0*q3, 2.0*pow(q0,2.0)-1.0+2.0*pow(q2,2.0), 2.0*q2*q3+2.0*q0*q1, ty},
		{2.0*q1*q3+2.0*q0*q2, 2.0*q2*q3-2.0*q0*q1, 2.0*pow(q0,2.0)-1.0+2.0*pow(q3,2.0), tz},
		{0.0, 0.0, 0.0, 1.0}};

	return M;
}

arma::Col<double> ModelClass::GetStateFromM(arma::Mat<double> M_in) {

	double q0 = 0.5*sqrt(M_in(0,0)+M_in(1,1)+M_in(2,2)+1.0);
	double q1 = (M_in(1,2)-M_in(2,1))/(4.0*q0);
	double q2 = (M_in(2,0)-M_in(0,2))/(4.0*q0);
	double q3 = (M_in(0,1)-M_in(1,0))/(4.0*q0);
	double tx = M_in(0,3);
	double ty = M_in(1,3);
	double tz = M_in(2,3);

	arma::Col<double> q = {q0, q1, q2, q3};
	q = q/arma::norm(q);

	arma::Col<double> state_out = {q(0), q(1), q(2), q(3), tx, ty, tz};

	return state_out;
}

arma::Mat<double> ModelClass::MultiplyM(arma::Mat<double> MA, arma::Mat<double> MB) {

	arma::Mat<double> RA = MA.submat(0,0,2,2);
	arma::Mat<double> RB = MB.submat(0,0,2,2);
	arma::Mat<double> RC = RA*RB;
	arma::Mat<double> MC;
	MC.eye(4,4);
	MC.submat(0,0,2,2) = RC;
	arma::Col<double> TA(3);
	arma::Col<double> TB(3);
	TA(0) = MA(0,3);
	TA(1) = MA(1,3);
	TA(2) = MA(2,3);
	TB(0) = MB(0,3);
	TB(1) = MB(1,3);
	TB(2) = MB(2,3);
	arma::Col<double> TC = TA+RA*TB;
	MC(0,3) = TC(0);
	MC(1,3) = TC(1);
	MC(2,3) = TC(2);
	return MC;
}

arma::Mat<double> ModelClass::TransposeM(arma::Mat<double> M_in) {
	arma::Mat<double> M_out;
	M_out.eye(4,4);
	M_out.submat(0,0,2,2) = M_in.submat(0,0,2,2).t();
	M_out(0,3) = M_in(0,3);
	M_out(1,3) = M_in(1,3);
	M_out(2,3) = M_in(2,3);
	return M_out;
}

vector<string> ModelClass::GetPointLabels() {
	return drag_point_labels;
}

vector<string> ModelClass::GetPointSymbols() {
	return drag_point_symbols;
}

vector<vector<int>> ModelClass::GetPointColors() {
	return drag_point_colors;
}

vector<vector<double>> ModelClass::GetPointStartPos() {
	return drag_point_start_pos;
}

vector<vector<int>> ModelClass::GetLineConnectivity() {
	return drag_line_connectivity;
}

vector<vector<int>> ModelClass::GetLineColors() {
	return drag_line_colors;
}

vector<string> ModelClass::GetScaleTexts() {
	return scale_texts;
}

vector<vector<int>> ModelClass::GetScaleCalc() {
	return scale_calc;
}

vector<vector<int>> ModelClass::GetLengthCalc() {
	return length_calc;
}

vector<int> ModelClass::GetContourCalc() {
	return contour_calc;
}

vector<int> ModelClass::GetOriginInd() {
	return origin_ind;
}


#include <stdexcept>
#include <algorithm>
#include <functional>
#include <iterator>
#include <cmath>

#ifdef DEBUG
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#endif

#include "maxtri.h"
#include "intersection.h"

#include <boost/graph/bron_kerbosch_all_cliques.hpp>
#ifdef MAXTRI_DEBUG
#include <boost/graph/graphviz.hpp>
#endif

using namespace std;
using namespace boost;

// competitive facility location algorithms
namespace cfla { namespace tri
{

template<class Tp_>
MaxTri<Tp_>::~MaxTri(void)
{
  status.clear();
  tris.clear();
  polys.clear();
  if (graph)
  {
    delete graph;
    graph = nullptr;
  }
}

template<class Tp_>
template<typename Iter>
void
MaxTri<Tp_>::
intersect_range(status_seg_type const& segment, Iter begin, Iter end)
{
  // Check intersection with each segment from [neighbor, end).
  point_type current_point = status_compare::isect_sweep(segment, current_y());

  // See the comments on IXEvent for why this is a loop.
  while (begin != end)
  {
#ifdef MAXTRI_DEBUG_INTERSECT
    cerr << "    checking intersection with " << *begin << endl;
#endif

    // Check for intersections and queue any we find.
    // Do not queue filthy degenerates or points we've already visited.
    point_type pt;
    char code = intersect(segment.edge(), begin->edge(), pt);
    if ('0' != code && 'v' != code && 'e' != code
        && event_point_ycompare()(pt, current_point))
    {
#ifdef MAXTRI_DEBUG_INTERSECT
      cerr << "    found intersection '" << code << "' " << pt << endl;
#endif

      // We need to look up the intersection in the intersection map to see if
      // we've already found an intersection here.
      auto ixit = intersections.find(pt);

      // If it doesn't exist yet, this is the first intersection at this point.
      if (ixit == intersections.end())
      {
        auto insert = intersections.insert(make_pair(pt, isect_event_type(pt)));
        assert(insert.second);
        ixit = insert.first; // ref to the newly inserted pair.

        // Queue the intersection the first time we find it.
        // Subsequent intersections at the same point will just add segments.
        queue().emplace(ixit->second);
      }

      // Record that these segments also form this intersection.
      ixit->second.insert(segment, *begin);
    }

    ++begin;
  }
}

// Check new intersections for a segment based on the current status.
template<class Tp_>
void MaxTri<Tp_>::
check_intersections(status_iterator center)
{
  // don't call us with a bad segment!
  assert(center != status.end());

#ifdef MAXTRI_DEBUG_INTERSECT
  cerr << "    looking for intersections around " << *center << endl;
#endif

  auto range = status.equal_range(*center);

  // Innner bounds.
  status_iterator right = range.second;
  status_iterator left = range.first;

  /* The following diagram illustrates the iterators below referring to
   * segments in the sweep status, where the same letter indicates segments in
   * the same equal_range:
   *
   *      0  1  2      3  4  5      6  7  8      9 10 11     12 13 14
   *    ---------------------------------------------------------------
   *   |  X  X  X      L  L  L      C  C  C      R  R  R      Z  Z  Z  |
   *    ---------------------------------------------------------------
   *            ^      ^     ^      ^  ^         ^            ^
   *     left_end      |     |      |  | center  |            right_end
   *         left_last |     |      | left       | right (upper_bound)
   *              left_begin |    (lower_bound)  | right_begin
   *
   * Ultimately, we want to check *center for intersections with all the
   * left (L) and right (R) segments, but not the segments in its own range (C).
   * We use reverse iterators in the left direction, and forward iterators in
   * the right direction so the actual checking code in intersect_range can be
   * the same.
   */

  // Look for intersections with the left-adjacent edge(s).
  if (left != status.end() && left != status.begin())
  {
    // Move out of the lower_bound on center into the range to the left.
    // Nb. converting to reverse_iterator subtracts 1 from the pointer.
    status_riterator left_begin = status_riterator(left);

    status_iterator left_last = status.lower_bound(*left_begin);
    status_riterator left_end = status.rend();

    if (left_last != status.end())
      left_end = status_riterator(left_last);

#ifdef MAXTRI_DEBUG_INTERSECT
      cerr << "      left: from " << *left_begin << " up to ";
      if (left_end != status.rend())
        cerr << *left_end;
      else
        cerr << "REND" << endl;
#endif

    intersect_range(*center, left_begin, left_end);
  }

  // Look for intersections with the right-adjacent edge(s).
  if (right != status.end())
  {
    // The upper_bound is already in the right-adjacent range.
    // Upper-bounding that range already puts us past the end like we want.
    status_iterator right_begin = right;
    status_iterator right_end = status.upper_bound(*right_begin);

#ifdef MAXTRI_DEBUG_INTERSECT
    cerr << "      right: from " << *right_begin << " up to ";
    if (right_end != status.end())
      cerr << *right_end;
    else
      cerr << "END" << endl;
#endif

    intersect_range(*center, right_begin, right_end);
  }
}

template<class Tp_>
void MaxTri<Tp_>::
insert_edge(edge_type const& e)
{
#ifdef MAXTRI_DEBUG_INTERSECT
  cerr << "  inserting " << e << endl;
#endif

  // Insert the edge by its top point.
  // It is always inserted as the upper bound of any equal_range.
  status_iterator eit = status.insert(e);
  assert(eit != status.end());

  check_intersections(eit);
}

template<class Tp_>
void MaxTri<Tp_>::
remove_edge(edge_type const& e)
{
#ifdef MAXTRI_DEBUG_INTERSECT
  size_t status_size = status.size();
  cerr << "  removing " << e << endl;
#endif

  // Remove the edge from the status.
  status_seg_type edge_segment(e);
  status_iterator eub, elb = find_unique(edge_segment);

  // Nb. If this is not the bottom edge ([2]) in the triangle, we will
  // immediately be adding another edge, but it is already done in
  // handle_tripoint.

#ifndef MAXTRI_DEBUG_INTERSECT
  assert (elb != status.end());
#else
  // this should always be true: if it's not, handle_tripoint should catch it
  if (elb == status.end())
    MTFAIL("edge not found!");
#endif

  eub = elb = status.erase(elb);
  if (elb != status.begin())
    --elb;

#ifdef MAXTRI_DEBUG_INTERSECT
  if (status.size() != status_size - 1)
    MTFAIL("failed to remove edge!");
#endif

  // Now elb, eub refer to the edges immediately left and right of the removed
  // edge. Theoretically we only need to check for intersections between these
  // two edges, but pass them both to the helper anyway.
  // It's okay if we find the same intersection twice, because it should end up
  // collapsing in the unordered_set used by IXEvent.
  if (elb != status.end())
    check_intersections(elb);

  if (eub != status.end() && elb != eub)
    check_intersections(eub);
}

template<class Tp_>
void MaxTri<Tp_>::
handle_intersection(point_type const& isect_point)
{
  // The first step is to reorder all the segments which form the intersection.
  //
  // Because we don't have access to the internal tree nodes of a C++ set,
  // we have to do this by removing the segments, modifying the comparator,
  // then re-inserting the segments so they'll be sorted correctly by the
  // updated comparator.
  //
  // This sounds like bad news, but it's actually fine, since the algorithm is
  // designed so that the change in comparison criteria happens only after each
  // intersection point, and affects only the intersecting segments! Therefore
  // we are not modifying the sort order of any nodes existing in the tree.

#ifdef MAXTRI_DEBUG_INTERSECT
  size_t status_size;
#endif

  // Lookup the actual intersection event from its point.
  auto ixit = intersections.find(isect_point);
  assert(ixit != intersections.end());
  isect_event_type const& isect = ixit->second;

#ifdef MAXTRI_DEBUG_INTERSECT
  cerr << "handling " << isect;
#endif

  // Remove the segments forming the intersection (so we can add them later).
  for (auto it = isect.begin(); it != isect.end(); ++it)
  {
#ifdef MAXTRI_DEBUG_INTERSECT
    status_size = status.size();
    cerr << "    removing old segment " << *it << endl;
#endif

    /* While we're at it, mark every triangle composing this point as
     * intersecting if their lines actually do intersect.
     *
     * Many of the iterations of this loop will be duplicates, but an
     * undirected adjacency matrix will squash those.
     * In the common case, this line runs twice, once for each line.  */
    for (auto it2 = isect.begin(); it2 != isect.end(); ++it2)
      if (it2 != it && (status.key_comp()(*it, *it2)
            || status.key_comp()(*it2, *it)))
        add_edge(it->edge().tridata()->id, it2->edge().tridata()->id, *graph);

    status_iterator segit = find_unique(*it);

#ifdef MAXTRI_DEBUG_INTERSECT
    if (segit == status.end())
      MTFAIL("failed to find old segment " << *it);
#endif

    status.erase(segit);

#ifdef MAXTRI_DEBUG_INTERSECT
    if (status.size() != status_size - 1)
      MTFAIL("failed to remove old segment " << *it);
#endif
  }

  // Update the sweep line to the intersection point now that we've removed the
  // intersecting segments. This changes the sorting criteria, since the
  // comparator refers to us for the y-position of the sweep line.
  // Now when we insert the segments again they should be in the correct order.
  // Classically this equates to a single swap.
  update_sweep(isect_point);

  // Perform the reordering by inserting the intersecting segments again.
  // This time they will follow the new world order.
  // We need to remember each segment that is inserted, so we can check for
  // intersections after they're all added again.
  std::vector<status_iterator> new_segments;
  for (auto it = isect.begin(); it != isect.end(); ++it)
  {
#ifdef MAXTRI_DEBUG_INTERSECT
    status_size = status.size();
    cerr << "    re-inserting segment " << *it << endl;
#endif

    status_iterator newit = status.insert(*it);
    new_segments.push_back(newit);

#ifdef MAXTRI_DEBUG_INTERSECT
    if (status.size() != status_size + 1)
      MTFAIL("failed to insert new segment!");
#endif
  }

  // Now that the sweep line has been updated to the intersection point and the
  // intersecting segments have been reordered, we have to check each of these
  // segments for their next point of intersection below the sweep line.
  // Technically this will make too many comparisons, as each newly inserted
  // segment will be compared to the segment it just intersected, but
  // classically they should only look in the opposite direction.
  // That's fine, as it's easier than keeping track of which segments ended up
  // on which side of the intersection.
  for (auto const& segiter : new_segments)
    check_intersections(segiter);
}

template<class Tp_>
void MaxTri<Tp_>::
handle_tripoint(tri_point_type const& tpoint)
{
  // Update the sweep line to the point of this event.
  // Though this implicitly modifies the status comparator, no segments can be
  // reordered in the status as a result because of the invariants of the
  // algorithm.
  update_sweep(tpoint.value());

  // We could queue all points twice (once for the top of each segment, once
  // for the bottom of each segment) but handling each point of the triangle
  // is clearer and simplifies event ordering. The diagram from the header
  // is reproduced below for reference.
  switch (tpoint.height)               /*                                 */
  {                                    /*       [0]                TOP    */
  case TOP:                            /* (0,1)  *                        */
    insert_edge(tpoint.top_edge());    /* V E0  /  \  E1 V                */
    insert_edge(tpoint.bottom_edge()); /*      /     \  (0,2)             */
    break;                             /* [1] *- _    \            MIDDLE */
                                       /*         - _   \                 */
  case MIDDLE:                         /*             - _\                */
    remove_edge(tpoint.top_edge());    /*        E2 >     * [2]    BOTTOM */
    insert_edge(tpoint.bottom_edge()); /*        (1,2) or (2,1)           */
    break;

  case BOTTOM:
    remove_edge(tpoint.top_edge());
    remove_edge(tpoint.bottom_edge());
    break;

  default:
    MTFAIL("bad tpoint index!");
  }
}

#ifdef MAXTRI_DEBUG

template<class Tp_>
void MaxTri<Tp_>::
dump_tris_to_octave(ostream& os) const
{
  os << "hold on;" << endl;

  int color_id = 1;
  for (const triangle_type &t : tris)
  {
    cerr << "    [" << setw(2) << setfill(' ') << (color_id-1) << "] "
      << t.point(0) << " " << t.point(1) << " " << t.point(2) << endl;

    os << "tri = [ ";
    for (auto const& point : t.points())
      os << getx(point) << " , " << gety(point) << " ; ";
    os << " ];" << endl
       << "fill(tri(:,1), tri(:,2), " << color_id++ << ");" << endl;
  }
}

template<class Tp_>
void MaxTri<Tp_>::
dump_solution_to_octave(ostream& os, std::set<int> const& indexes,
    solution_cell_type const& solution) const
{
  os << "hold on;" << endl;

  // Fill all composite triangles
  int color_id = 1;
  for (int idx : indexes)
  {
    os << "tri = [ ";
    const triangle_type &t = tris[idx];
    for (auto const& point : t.points())
      os << getx(point) << " , " << gety(point) << " ; ";
    os << " ];" << endl
       << "fill(tri(:,1), tri(:,2), " << color_id++ << ");" << endl;
  }

  // Then fill the solution
  os << "sol = [ ";
  for (auto const& point : solution)
    os << getx(point) << " , " << gety(point) << " ; ";
  os << " ];" << endl
    << "fill(sol(:,1), sol(:,2), 'k');" << endl;
}

#endif

#ifdef MAXTRI_DEBUG_INTERSECT

template<class Tp_>
void MaxTri<Tp_>::
dump_status_to_octave(ostream& os) const
{
  vector<coordinate_type> xs, ys;
  coordinate_type minx = numeric_limits<coordinate_type>::max()
                , maxx = numeric_limits<coordinate_type>::lowest();
  xs.reserve(status.size());
  ys.reserve(status.size());
  for (auto const& segment : status)
  {
    coordinate_type x1 = getx(segment.first()), x2 = getx(segment.second());

    if (x1 < minx)
      minx = x1;
    if (x1 > maxx)
      maxx = x1;
    if (x2 < minx)
      minx = x2;
    if (x2 > maxx)
      maxx = x2;

    xs.push_back(x1);
    xs.push_back(x2);
    ys.push_back(gety(segment.first()));
    ys.push_back(gety(segment.second()));
  }

  os << "figure();" << endl
    << "hold on;" << endl;
  // draw the sweep line itself if we have anything
  if (status.size() > 0)
  {
    os << "plot([ " << minx << " ; " << maxx << " ], "
               "[ " << current_y() << " ; " << current_y() << " ]"
               ", 'Color', 'red', 'LineWidth', 2);" << endl
       << "xlim([ " << minx << " ; " << maxx << " ]);" << endl;
  }

  // scatter plot for all endpoints
  os << "scatter([";
  for (auto const& x : xs)
    os << x << " ; ";
  os << "] , [";
  for (auto const& y : ys)
    os << y << " ; ";
  os << "]);" << endl;

  // also, plot each segment separately
  for (auto const& segment : status)
  {
    os << "plot(["
      << getx(segment.first()) << " ; "
      << getx(segment.second())
      << "], ["
      << gety(segment.first()) << " ; "
      << gety(segment.second())
      << "]);" << endl;
  }
}

template<class Tp_>
void MaxTri<Tp_>::
debug_status(void) const
{
  static int status_cnt = 0;

  stringstream ofname_s;
  ofname_s << "./scripts/status_" // %02d
    << dec << setw(2) << setfill('0') << status_cnt << ".m";
  string ofname(ofname_s.str());

  cerr << ofname << ":" << endl;
  int i = 0;
  for (auto const& segment : status)
  {
    cerr << "    [" << setw(2) << dec << setfill(' ') << i << "] "
      << segment << endl;
    ++i;
  }

  ofstream ofstatus(ofname);
  dump_status_to_octave(ofstatus);
  ofstatus.close();

  ++status_cnt;
}

#endif

template<class Tp_>
void MaxTri<Tp_>::
handle_event(EventType type, event_point_type const& event)
{
  switch (type)
  {
  case INTERSECTION:
    {
      point_type const& isect_point(event.isect());

      handle_intersection(isect_point);
    }
    break;

  case TRIPOINT:
    {
      tri_point_type const& tri_point(event.point());

#ifdef MAXTRI_DEBUG_INTERSECT
      cerr << "handling point " << tri_point << endl;
#endif

      handle_tripoint(tri_point);
    }
    break;

  default:
    MTFAIL("unhandled event type!");
  }

#ifdef MAXTRI_DEBUG_INTERSECT
  debug_status();
#endif
}

template<class Tp_>
void MaxTri<Tp_>::
initialize(void)
{
  if (graph)
    delete graph;

#ifdef MAXTRI_DEBUG
  string tri_path("./scripts/triangles.m");
  cerr << "dumping triangles to " << tri_path << endl;
  ofstream trif(tri_path);
  dump_tris_to_octave(trif);
  trif.close();
#endif

  // One slot per triangle.
  graph = new components_graph(tris.size());
}

struct clique_visitor
{
  clique_visitor(std::set<int> &clique_ref, size_t &depth_ref)
    : max_clique(clique_ref), max_depth(depth_ref)
  {}

  template<typename Clique, typename Graph>
  void clique(Clique const& c, Graph const& g)
  {
    std::set<int> current_clique;
    size_t current_depth = 0u;

    for (auto it = c.begin(); it != c.end(); ++it)
    {
      current_clique.insert(*it);
      ++current_depth;
    }

    if (current_depth > max_depth)
    {
      max_depth = current_depth;
      max_clique = current_clique;
    }
  }

  std::set<int> &max_clique;
  size_t &max_depth;
};

template<class Tp_>
void MaxTri<Tp_>::
finalize(void)
{
  // Re-orient triangles to make boost happy.
  std::vector<solution_cell_type> new_triangles;
  new_triangles.reserve(tris.size());
  for (triangle_type const &t : tris)
  {
    new_triangles.emplace_back();
    bg::assign(new_triangles.back(), t);
    bg::correct(new_triangles.back());
  }

  // Get the index set of the max clique.
  // This is the group of maximally intersecting triangles.
  // We could modify our clique visitor to track all cells at the max depth...
  // But for now we'll just choose the first one we encounter.
#ifdef MAXTRI_DEBUG
  string graph_path("./data/adjacency.gvz");
  cerr << "dumping graph to " << graph_path << endl;
  ofstream graphf(graph_path);
  boost::write_graphviz(graphf, *graph);
  graphf.close();
#endif

  std::set<int> max_clique;
  size_t max_size = 0u;
  clique_visitor visitor(max_clique, max_size);
  bron_kerbosch_all_cliques(*graph, visitor);

  // Pull the solution triangles from the index set.
  std::vector<solution_cell_type> max_clique_triangles;
  max_clique_triangles.reserve(new_triangles.size());
  for (int triangle_index : max_clique)
    max_clique_triangles.push_back(new_triangles[triangle_index]);

  // Intersect all the triangles together.
  solution_cell_type s;
  intersection(max_clique_triangles.begin(), max_clique_triangles.end(), s);

#ifdef MAXTRI_DEBUG
  cerr << "solution from triangles: " << endl;
  for (int index : max_clique)
  {
    triangle_type const& tri = tris[index];
    cerr << "    [" << setw(2) << setfill(' ') << index << "] "
      << tri.point(0) << "  " << tri.point(1) << "  " << tri.point(2)
      << endl;
  }

  cerr << "with bounds: ";
  for (auto const& point : s)
    cerr << point << " ";
  cerr << endl;

  ofstream ofsol("./scripts/solution.m");
  dump_solution_to_octave(ofsol, max_clique, s);
  ofsol.close();
#endif

  solutions().push_back(s);
}

template class MaxTri<double>;
template class MaxTri<float>;

} } // end namespace cfla::tri


// Copyright (c) 2019, Tom Westerhout
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#pragma once

#include "detail/lattice.hpp"
#include "detail/magnetic_cluster.hpp"
#include "detail/simulated_annealing.hpp"
#include "detail/utility.hpp"
#include <boost/align/aligned_allocator.hpp>
#include <gsl/gsl-lite.hpp>
#include <sleef.h>
#include <sys/user.h>
#include <vector>

#if !defined(__AVX__)
#    error "Thermalisation relies on AVX"
#endif

TCM_NAMESPACE_BEGIN

using size_t = std::size_t;

namespace detail {
/// Wrapping to [0, 2pi).
struct to_0_2pi_fn {
    static constexpr float one_over_two_pi = 0.15915494f;
    static constexpr float two_pi          = 6.2831855f;

    auto operator()(float const x) const noexcept -> float
    {
        return x - two_pi * std::floor(one_over_two_pi * x);
    }

    auto operator()(__m256 const x) const noexcept -> __m256
    {
        return x
               - _mm256_set1_ps(two_pi)
                     * Sleef_floorf8_avx(_mm256_set1_ps(one_over_two_pi) * x);
    }
};

#if 0
/// Performs the actual computation of energy
///
/// NOTE: Uses AVX intrinsics.
inline auto energy_kernel(
    gsl::span<float const> const                     spin,
    gsl::span<std::pair<size_t, size_t> const> const edges) TCM_NOEXCEPT
    -> float
{ // {{{
    auto const load_angles =
        [s = spin.data()](auto const* const d, auto const n)
            TCM_NOEXCEPT -> __m256 {
        switch (n) {
        case 0: return _mm256_set1_ps(0);
        case 1:
            return _mm256_set_ps(0, 0, 0, 0, 0, 0, 0, //
                                 s[d[0].first] - s[d[0].second]);
        case 2:
            return _mm256_set_ps(0, 0, 0, 0, 0, 0,               //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);
        case 3:
            return _mm256_set_ps(0, 0, 0, 0, 0,                  //
                                 s[d[2].first] - s[d[2].second], //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);
        case 4:
            return _mm256_set_ps(0, 0, 0, 0,                     //
                                 s[d[3].first] - s[d[3].second], //
                                 s[d[2].first] - s[d[2].second], //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);
        case 5:
            return _mm256_set_ps(0, 0, 0,                        //
                                 s[d[4].first] - s[d[4].second], //
                                 s[d[3].first] - s[d[3].second], //
                                 s[d[2].first] - s[d[2].second], //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);
        case 6:
            return _mm256_set_ps(0, 0,                           //
                                 s[d[5].first] - s[d[5].second], //
                                 s[d[4].first] - s[d[4].second], //
                                 s[d[3].first] - s[d[3].second], //
                                 s[d[2].first] - s[d[2].second], //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);
        case 7:
            return _mm256_set_ps(0,                              //
                                 s[d[6].first] - s[d[6].second], //
                                 s[d[5].first] - s[d[5].second], //
                                 s[d[4].first] - s[d[4].second], //
                                 s[d[3].first] - s[d[3].second], //
                                 s[d[2].first] - s[d[2].second], //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);

        case 8:
            return _mm256_set_ps(s[d[7].first] - s[d[7].second], //
                                 s[d[6].first] - s[d[6].second], //
                                 s[d[5].first] - s[d[5].second], //
                                 s[d[4].first] - s[d[4].second], //
                                 s[d[3].first] - s[d[3].second], //
                                 s[d[2].first] - s[d[2].second], //
                                 s[d[1].first] - s[d[1].second], //
                                 s[d[0].first] - s[d[0].second]);
        default: TCM_ASSERT(false, "Bug! This should never have happened.");
        } // end switch
    };

    constexpr auto vector_size = 8;
    auto const     chunks      = edges.size() / vector_size;
    auto const     rest        = edges.size() % vector_size;

    auto        energy = _mm256_set1_ps(0);
    auto const* data   = edges.data();

    for (auto i = std::size_t{0}; i < chunks; ++i, data += vector_size) {
        energy += Sleef_cosf8_u35avx(load_angles(data, vector_size));
    }
    if (rest != 0) { energy += Sleef_cosf8_u35avx(load_angles(data, rest)); }

    return energy[0] + energy[1] + energy[2] + energy[3] + energy[4] + energy[5]
           + energy[6] + energy[7] - static_cast<float>(rest);
} // }}}
#endif

inline auto
energy_kernel(gsl::span<float const> const                         spin,
              gsl::span<std::pair<unsigned, unsigned> const> const edges,
              gsl::span<float const> const coeffs) TCM_NOEXCEPT -> float
{ // {{{
    auto const load_chunk = [s = spin, d = edges](auto const n)
                                TCM_NOEXCEPT -> __m256 {
        TCM_ASSERT(8 * (n + 1) <= d.size(), "Index out of bounds");
        auto const i = 8 * n;
        auto const x = _mm256_set_ps(s[d[i + 7].first], s[d[i + 6].first],
                                     s[d[i + 5].first], s[d[i + 4].first],
                                     s[d[i + 3].first], s[d[i + 2].first],
                                     s[d[i + 1].first], s[d[i + 0].first]);
        auto const y = _mm256_set_ps(s[d[i + 7].second], s[d[i + 6].second],
                                     s[d[i + 5].second], s[d[i + 4].second],
                                     s[d[i + 3].second], s[d[i + 2].second],
                                     s[d[i + 1].second], s[d[i + 0].second]);
        return _mm256_sub_ps(x, y);
    };

    auto const load_part = [s = spin, d = edges](auto const n, auto const rest)
                               TCM_NOEXCEPT -> __m256 {
        TCM_ASSERT(0 < rest && rest < 8, "Precondition violated");
        TCM_ASSERT(8 * n + rest <= d.size(), "Index out of bounds");
        auto const i = 8 * n;
        switch (rest) {
        case 1:
            return _mm256_set_ps(0, 0, 0, 0, 0, 0, 0, //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        case 2:
            return _mm256_set_ps(0, 0, 0, 0, 0, 0,                       //
                                 s[d[i + 1].first] - s[d[i + 1].second], //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        case 3:
            return _mm256_set_ps(0, 0, 0, 0, 0,                          //
                                 s[d[i + 2].first] - s[d[i + 2].second], //
                                 s[d[i + 1].first] - s[d[i + 1].second], //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        case 4:
            return _mm256_set_ps(0, 0, 0, 0,                             //
                                 s[d[i + 3].first] - s[d[i + 3].second], //
                                 s[d[i + 2].first] - s[d[i + 2].second], //
                                 s[d[i + 1].first] - s[d[i + 1].second], //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        case 5:
            return _mm256_set_ps(0, 0, 0,                                //
                                 s[d[i + 4].first] - s[d[i + 4].second], //
                                 s[d[i + 3].first] - s[d[i + 3].second], //
                                 s[d[i + 2].first] - s[d[i + 2].second], //
                                 s[d[i + 1].first] - s[d[i + 1].second], //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        case 6:
            return _mm256_set_ps(0, 0,                                   //
                                 s[d[i + 5].first] - s[d[i + 5].second], //
                                 s[d[i + 4].first] - s[d[i + 4].second], //
                                 s[d[i + 3].first] - s[d[i + 3].second], //
                                 s[d[i + 2].first] - s[d[i + 2].second], //
                                 s[d[i + 1].first] - s[d[i + 1].second], //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        case 7:
            return _mm256_set_ps(0,                                      //
                                 s[d[i + 6].first] - s[d[i + 6].second], //
                                 s[d[i + 5].first] - s[d[i + 5].second], //
                                 s[d[i + 4].first] - s[d[i + 4].second], //
                                 s[d[i + 3].first] - s[d[i + 3].second], //
                                 s[d[i + 2].first] - s[d[i + 2].second], //
                                 s[d[i + 1].first] - s[d[i + 1].second], //
                                 s[d[i + 0].first] - s[d[i + 0].second]);
        default: TCM_ASSERT(false, "Bug! This should never have happened.");
        } // end switch
    };

    constexpr auto vector_size = 8;
    auto const     chunks      = edges.size() / vector_size;
    auto const     rest        = edges.size() % vector_size;

    auto energy = _mm256_set1_ps(0.0f);
    for (auto i = size_t{0}; i < chunks; ++i) {
        energy += _mm256_mul_ps(_mm256_load_ps(coeffs.data() + i * vector_size),
                                Sleef_cosf8_u35avx(load_chunk(i)));
    }
    auto energy_rest = 0.0f;
    if (rest != 0) {
        auto const begin = chunks * vector_size;
        auto const end   = begin + rest;
        for (auto i = begin; i < end; ++i) {
            energy_rest +=
                coeffs[i]
                * Sleef_cosf_u35(spin[edges[i].first] - spin[edges[i].second]);
        }
    }
    return energy[0] + energy[1] + energy[2] + energy[3] + energy[4] + energy[5]
           + energy[6] + energy[7] + energy_rest;
} // }}}
} // namespace detail

#if 0
struct energy_buffers_t {

    struct energy_fn_type {
        gsl::span<std::pair<unsigned, unsigned> const> edges;
        gsl::span<float const>                         coeffs;
        unsigned                                       number_sites;

        auto operator()(gsl::span<float const> const spin) TCM_NOEXCEPT -> float
        {
            TCM_ASSERT(spin.size() == number_sites, "");
            return detail::energy_kernel(spin, edges, coeffs);
        }
    };

  private:
    template <class T>
    using buffer_type =
        std::vector<T, boost::alignment::aligned_allocator<T, PAGE_SIZE>>;

    static constexpr auto empty = std::numeric_limits<unsigned>::max();

    buffer_type<unsigned> _g_to_l; ///< Mapping of global to local indices.
    buffer_type<std::pair<unsigned, unsigned>>
                       _edges;  ///< Edges (given in terms of local indices)
    buffer_type<float> _coeffs; ///< Couplings (+1 for antiferromagnetic
                                ///< connections and -1 for ferromagnetic ones)

  public:
    explicit energy_buffers_t(size_t const size)
        : _g_to_l(size)
        , _edges(size)  // NOTE: This is just an initial estimate
        , _coeffs(size) // And this as well
    {}

    energy_buffers_t(const energy_buffers_t&)     = delete;
    energy_buffers_t(energy_buffers_t&&) noexcept = default;
    energy_buffers_t& operator=(energy_buffers_t const&) = delete;
    energy_buffers_t& operator=(energy_buffers_t&&) noexcept = default;

    template <class System>
    auto energy_fn(System const& system) -> energy_fn_type;

  private:
    template <class System>
    auto set_initial_state(sa_buffers_t const& sa_buffers) -> energy_fn_type;

    auto global_to_local(size_t const i) const TCM_NOEXCEPT -> size_t
    {
        TCM_ASSERT(i < _g_to_l.size(), "Index out of bounds");
        TCM_ASSERT(_g_to_l[i] != empty, "Site does not belong to the cluster");
        return _g_to_l[i];
    }
};
#endif

#if 0
// {{{ energy_buffers_t::energy_fn IMPLEMENTATION
template <class Lattice>
TCM_NOINLINE auto energy_buffers_t::energy_fn(gsl::span<size_t const> sites,
                                              Lattice const&          lattice)
    -> energy_fn_type
{
    using std::begin, std::end;
    if (::TCM_NAMESPACE::size(lattice) != _g_to_l.size()) {
        _g_to_l.resize(::TCM_NAMESPACE::size(lattice));
    }

    std::fill(begin(_g_to_l), end(_g_to_l), empty);
    for (auto local_i = size_t{0}; local_i < sites.size(); ++local_i) {
        auto const global_i = sites[local_i];
        _g_to_l[global_i]   = local_i;
    }

    _fm_edges.clear();
    _afm_edges.clear();
    for (auto local_i = size_t{0}; local_i < sites.size(); ++local_i) {
        auto const global_i = sites[local_i];
        for (std::int64_t const _global_j :
             ::TCM_NAMESPACE::neighbours(lattice, global_i)) {
            // We want signed comparison here, because j may denote a
            // boundary and thus be negative. This condition ensures that
            // every edge is counted only once.
            if (static_cast<std::int64_t>(global_i) < _global_j) {
                // Here we know that _global_j > 0
                TCM_ASSERT(_global_j > 0, "");
                auto const global_j = static_cast<size_t>(_global_j);
                auto const local_j  = _g_to_l[global_j];
                if (local_j != empty) {
                    switch (::TCM_NAMESPACE::interaction(lattice, global_i,
                                                         global_j)) {
                    case interaction_t::Ferromagnetic:
                        _fm_edges.emplace_back(local_i, local_j);
                        break;
                    case interaction_t::Antiferromagnetic:
                        _afm_edges.emplace_back(local_i, local_j);
                        break;
                    } // end switch
                }
            }
        }
    }

    return {{_fm_edges}, {_afm_edges}};
}
#endif

#if 0
template <class System>
TCM_NOINLINE auto energy_buffers_t::energy_fn(System const& system)
    -> energy_fn_type
{
    using std::begin, std::end;

    auto const& lattice = system.lattice();
    if (::TCM_NAMESPACE::size(system.lattice()) != _g_to_l.size()) {
        _g_to_l.resize(::TCM_NAMESPACE::size(lattice));
    }
    std::fill(begin(_g_to_l), end(_g_to_l), empty);
    _edges.clear();
    _coeffs.clear();

    auto number_sites = unsigned{0};
    for (auto i = size_t{0}; i < ::TCM_NAMESPACE::size(lattice); ++i) {
        if (!system.is_empty(i)) {
            for (auto const _j : ::TCM_NAMESPACE::neighbours(lattice, i)) {
                if (_j >= 0) {
                    auto const j = static_cast<size_t>(_j);
                    if (!system.is_empty(j)
                        && system.get_magnetic_cluster(i)
                               == system.get_magnetic_cluster(j)) {
                        if (_g_to_l[i] == empty) {
                            _g_to_l[i] = number_sites++;
                        }
                        if (j < i) {
                            TCM_ASSERT(
                                _g_to_l[j] != empty && _g_to_l[i] != empty, "");
                            _coeffs.emplace_back(
                                ::TCM_NAMESPACE::coupling(lattice, j, i));
                            _edges.emplace_back(_g_to_l[j], _g_to_l[i]);
                        }
                    }
                }
            }
        }
    }

    return {{_edges}, {_coeffs}, number_sites};
}
// }}}
#endif

struct thermaliser_t {

    struct energy_fn_type {
        gsl::span<std::pair<unsigned, unsigned> const> edges;
        gsl::span<float const>                         coeffs;
        size_t                                         number_sites;

        auto operator()(gsl::span<float const> const spin) TCM_NOEXCEPT -> float
        {
            TCM_ASSERT(spin.size() == number_sites, "");
            return detail::energy_kernel(spin, edges, coeffs);
        }
    };

  private:
    template <class T>
    using buffer_type =
        std::vector<T, boost::alignment::aligned_allocator<T, PAGE_SIZE>>;

    static constexpr auto empty = std::numeric_limits<unsigned>::max();

    sa_buffers_t          _sa_buffers; ///< Simulated Annealing buffers.
    buffer_type<unsigned> _l_to_g; ///< Mapping from local to global indices.
    buffer_type<unsigned> _g_to_l; ///< Mapping from global to local indices.
    buffer_type<std::pair<unsigned, unsigned>>
                       _edges;  ///< Edges (given in terms of local indices)
    buffer_type<float> _coeffs; ///< Couplings (+1 for antiferromagnetic
                                ///< connections and -1 for ferromagnetic ones)

  public:
    explicit thermaliser_t(size_t const size)
        : _sa_buffers{size}
        , _l_to_g(size)
        , _g_to_l(size)
        , _edges(size)  // NOTE: This is just an initial estimate
        , _coeffs(size) // And this as well
    {}

    thermaliser_t(const thermaliser_t&)     = delete;
    thermaliser_t(thermaliser_t&&) noexcept = default;
    thermaliser_t& operator=(thermaliser_t const&) = delete;
    thermaliser_t& operator=(thermaliser_t&&) noexcept = default;

    template <class System>
    TCM_NOINLINE auto thermalise(magnetic_cluster_t<System>& cluster,
                                 sa_pars_t const& parameters) -> void;

    template <class System>
    TCM_NOINLINE auto thermalise(System& system, sa_pars_t const& parameters)
        -> void;

  private:
    template <class System>
    TCM_NOINLINE auto reset(System const& system) -> void;

    template <class System>
    TCM_NOINLINE auto reset(magnetic_cluster_t<System> const& cluster) -> void;

    template <class System>
    TCM_NOINLINE auto store(gsl::span<float const>      best,
                            magnetic_cluster_t<System>& cluster) -> void;

    template <class System>
    TCM_NOINLINE auto store(gsl::span<float const> best, System& system)
        -> void;
};

template <class System>
auto thermaliser_t::thermalise(magnetic_cluster_t<System>& cluster,
                               sa_pars_t const&            parameters) -> void
{
    reset(cluster);
    auto&      system    = cluster.system();
    auto       energy_fn = energy_fn_type{{_edges}, {_coeffs}, _l_to_g.size()};
    auto       wrap_fn   = detail::to_0_2pi_fn{};
    sa_chain_t chain{_sa_buffers, parameters, energy_fn, wrap_fn,
                     system.rng_stream()};
    for (auto i = 0u; i < parameters.n; ++i) {
        chain();
    }
    store(chain.best(), system);
}

template <class System>
auto thermaliser_t::thermalise(System& system, sa_pars_t const& parameters)
    -> void
{
    reset(system);
    auto       energy_fn = energy_fn_type{{_edges}, {_coeffs}, _l_to_g.size()};
    auto       wrap_fn   = detail::to_0_2pi_fn{};
    sa_chain_t chain{_sa_buffers, parameters, energy_fn, wrap_fn,
                     system.rng_stream()};
    for (auto i = 0u; i < parameters.n; ++i) {
        chain();
    }
    store(chain.best(), system);
}

template <class System>
auto thermaliser_t::reset(magnetic_cluster_t<System> const& cluster) -> void
{
    auto const& system  = cluster.system();
    auto const& lattice = system.lattice();
    if (::TCM_NAMESPACE::size(lattice) != _g_to_l.size()) {
        _g_to_l.resize(::TCM_NAMESPACE::size(lattice));
    }
    std::fill(begin(_g_to_l), end(_g_to_l), empty);
    _l_to_g.clear();
    _edges.clear();
    _coeffs.clear();

    for (auto const i : cluster.sites()) {
        if (!system.is_empty(i)) {
            for (auto const _j : ::TCM_NAMESPACE::neighbours(lattice, i)) {
                if (_j >= 0) {
                    auto const j = static_cast<unsigned>(_j);
                    if (!system.is_empty(j)
                        && (&system.get_magnetic_cluster(j) == &cluster)) {
                        if (_g_to_l[i] == empty) {
                            _g_to_l[i] = static_cast<unsigned>(_l_to_g.size());
                            _l_to_g.push_back(i);
                        }
                        if (j < i) {
                            TCM_ASSERT(
                                _g_to_l[j] != empty && _g_to_l[i] != empty, "");
                            _coeffs.push_back(
                                ::TCM_NAMESPACE::coupling(lattice, j, i));
                            _edges.emplace_back(_g_to_l[j], _g_to_l[i]);
                        }
                    }
                }
            }
        }
    }

    _sa_buffers.resize(_l_to_g.size());
    auto initial = _sa_buffers.current();
    for (auto const i : _l_to_g) {
        initial[i] = system.get_angle(i);
    }
}

template <class System> auto thermaliser_t::reset(System const& system) -> void
{
    auto const& lattice = system.lattice();
    if (::TCM_NAMESPACE::size(lattice) != _g_to_l.size()) {
        _g_to_l.resize(::TCM_NAMESPACE::size(lattice));
    }
    std::fill(begin(_g_to_l), end(_g_to_l), empty);
    _l_to_g.clear();
    _edges.clear();
    _coeffs.clear();

    for (auto i = 0u; i < ::TCM_NAMESPACE::size(lattice); ++i) {
        if (!system.is_empty(i)) {
            for (auto const _j : ::TCM_NAMESPACE::neighbours(lattice, i)) {
                if (_j >= 0) {
                    auto const j = static_cast<unsigned>(_j);
                    if (!system.is_empty(j)
                        && system.get_magnetic_cluster(i)
                               == system.get_magnetic_cluster(j)) {
                        if (_g_to_l[i] == empty) {
                            _g_to_l[i] = static_cast<unsigned>(_l_to_g.size());
                            _l_to_g.push_back(i);
                        }
                        if (j < i) {
                            TCM_ASSERT(
                                _g_to_l[j] != empty && _g_to_l[i] != empty, "");
                            _coeffs.push_back(
                                ::TCM_NAMESPACE::coupling(lattice, j, i));
                            _edges.emplace_back(_g_to_l[j], _g_to_l[i]);
                        }
                    }
                }
            }
        }
    }

    _sa_buffers.resize(_l_to_g.size());
    auto initial = _sa_buffers.current();
    for (auto const i : _l_to_g) {
        initial[i] = system.get_angle(i);
    }
}

template <class System>
auto thermaliser_t::store(gsl::span<float const>      best,
                          magnetic_cluster_t<System>& cluster) -> void
{
    auto const& system = cluster.system();
    for (auto i = 0u; i < _l_to_g.size(); ++i) {
        system.set_angle(_l_to_g[i], angle_t{best[i]});
    }
}

template <class System>
auto thermaliser_t::store(gsl::span<float const> best, System& system) -> void
{
    for (auto i = 0u; i < _l_to_g.size(); ++i) {
        system.set_angle(_l_to_g[i], angle_t{best[i]});
    }
}

#if 0
template <class EnergyFn, class InitFn>
inline auto optimise(EnergyFn&& energy, sa_pars_t const& parameters,
                     sa_buffers_t& buffers, VSLStreamStatePtr stream,
                     InitFn&& init_fn)
{
    std::forward<InitFn>(init_fn)(buffers.current());
    sa_chain_t chain{buffers, parameters, std::forward<EnergyFn>(energy),
                     detail::to_0_2pi_fn{}, stream};
    for (auto i = 0u; i < parameters.n; ++i) {
        chain();
    }
    return chain.best();
}
#endif

TCM_NAMESPACE_END


#include "camera.h"

#include <atomic>
#include <glm/common.hpp>
#include <memory>
#include <boost/asio/executor.hpp>
#include <boost/asio/post.hpp>
#include <glm/glm.hpp>
#include "light.h"
#include "scene.h"
#include "ray.h"
#include "object.h"

namespace {

glm::vec3 shadow(const Scene &scene, const Ray &r,
				 const PointLight *source) noexcept {
	Intersection i;
	if (!scene.intersect(r, i))
		return {1, 1, 1};
	if (i.object == source)
		return {1, 1, 1};
	if (glm::length(i.object->transmissivity) < 1e-5)
		return {0, 0, 0};
	return i.object->transmissivity
			* shadow(scene, Ray{r.at(i.t) + 1e-5f*r.dir, r.dir}, source);
}

glm::vec3 shade(const Object &o, float shininess,
                const Scene &scene,
                glm::vec3 Q, glm::vec3 N, glm::vec3 V) noexcept {
    glm::vec3 I = o.emissive;                            // emissive
    I += scene.ambiant * o.color;                        // ambient
    for (const PointLight *pl : scene.point_lights) {
		glm::vec3 diff = pl->pos() - Q;
		glm::vec3 L = glm::normalize(diff);
		float dis = glm::length(diff);
		if (glm::dot(N, L) >= 0) {
			float atten = 1.0f / (dis * dis);
			glm::vec3 I_l = atten * pl->emissive
					* shadow(scene, Ray(Q + 1e-5f*L, L), pl);
			glm::vec3 H = glm::normalize(V + L);
			I += glm::dot(N, L) * I_l * o.color;          // diffuse
			I += glm::pow(glm::max(0.0f, glm::dot(H, N)),
						  shininess) * I_l * o.specular;  // specular
		}
	}
	for (const auto &dl : scene.dir_lights) {
		glm::vec3 L = -glm::normalize(dl->direction);
		if (glm::dot(N, L) >= 0) {
			glm::vec3 I_l = dl->color * shadow(scene, Ray(Q + 1e-5f*L, L), nullptr);
			glm::vec3 H = glm::normalize(V + L);
			I += glm::dot(N, L) * I_l * o.color;          // diffuse
			I += glm::pow(glm::max(0.0f, glm::dot(H, N)),
						  shininess) * I_l * o.specular;  // specular
		}
	}
    return I;
}

glm::vec3 trace(const Scene &scene,
				const Ray &r,
				int32_t depth) noexcept {
	if (depth == 0)
		return {0, 0, 0};
	Intersection i;
	if (scene.intersect(r, i)) {
		const Object &o = *i.object;
		const float shininess = i.object->shininess;
		const glm::vec3 Q = r.at(i.t);
		const glm::vec3 D = r.dir;
		const bool exit = glm::dot(D, i.normal) > 0.0;
		const glm::vec3 N = exit ? -i.normal : i.normal;

		// direct
		const glm::vec3 I_direct = shade(o, shininess, scene, Q, N, -D);

		const float d_dot_n = glm::dot(D, N);

		// reflection
		glm::vec3 I_reflected = {0, 0, 0};
		if (glm::length(o.specular) > 1e-5) {
			glm::vec3 R = D - 2.0f * d_dot_n * N;
			I_reflected = o.specular * trace(scene, Ray(Q, R), depth - 1);
		}

		// refraction
		glm::vec3 I_refracted = {0, 0, 0};
		if (glm::length(o.transmissivity) > 1e-5) {
			float ni, nt;
			if (exit) {
				// exiting object
				ni = o.ior;
				nt = 1.00029;
			} else {
				// entering object
				ni = 1.00029;
				nt = o.ior;
			}
			const auto sqr = [](float x) { return x * x; };
			float inside_sqrt = 1.0f -
					sqr(ni) / sqr(nt) * (1.0f - sqr(d_dot_n));
			if (inside_sqrt > 0.0f) {
				// not total internal reflection
				glm::vec3 T =
						(ni / nt) * (D - d_dot_n * N)
						- (glm::sqrt(inside_sqrt) * N);
				I_refracted = o.transmissivity
						* trace(scene, Ray(Q, T), depth - 1);
			}
		}

		return glm::clamp(I_direct + I_reflected + I_refracted, 0.0f, 1.0f);
	} else if (scene.envmap != nullptr) {
		return scene.envmap->get_color(r.dir);
	} else {
		return {0, 0, 0};
	}
}

template<typename It, typename F>
void parallelForEach(boost::asio::executor exec, It begin, It end, F f) {
	if (begin + 1 == end) {
		f(begin);
	} else {
		It mid = begin + (end - begin) / 2;
		boost::asio::post(
				exec,
				[=]() { parallelForEach(exec, begin, mid, f); });
		parallelForEach(exec, mid, end, f);
	}
}

}  // namespace

void Camera::trace_rays(
		const Scene &root,
		std::function<void(std::vector<glm::vec3>)> completion) const noexcept {
	auto results = std::make_shared<std::vector<glm::vec3>>(rays.size());
	auto finished = std::make_shared<std::atomic_int32_t>(0);
		auto task = [this, finished, results, &root, completion](int32_t i) {
			(*results)[i] = trace(
					root,
					Ray{inner_to_world({0, 0, 0, 1}), inner_to_world({rays[i], 0})},
					depth);
			if (finished->fetch_add(1, std::memory_order_relaxed) + 1
					== rays.size())
				completion(*results);
		};
	parallelForEach<int32_t>(exec_, 0, rays.size(), task);
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/functional/lockstep.hpp>
#include <boost/hana/plus.hpp>
namespace hana = boost::hana;

constexpr int to_int(char c) {
    return static_cast<int>(c) - 48;
}

constexpr int increment(int i) {
    return i + 1;
}

static_assert(hana::lockstep(hana::plus)(to_int, increment)('3', 4) == 3 + 5, "");

int main() { }


//
// Copyright (c) 2009--2010
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_GLAS_COMPRESSED_HPP
#define BOOST_NUMERIC_BINDINGS_GLAS_COMPRESSED_HPP

#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/end.hpp>
#include <boost/numeric/bindings/detail/adaptor.hpp>
#include <boost/numeric/bindings/detail/copy_const.hpp>
#include <boost/numeric/bindings/glas/detail/convert_to.hpp>
#include <boost/numeric/bindings/glas/dense_vector.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <glas/sparse/compressed.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, typename O, typename IndexType, typename NNZType, int IB, typename Id, typename Enable >
struct adaptor< glas::sparse_matrix< T, glas::compressed_sparse_structure<O, IndexType, NNZType, IB> >, Id, Enable > {

    typedef typename copy_const< Id, T >::type value_type;
    typedef typename copy_const< Id, IndexType >::type index_type;
    typedef typename convert_to< tag::data_order, O >::type data_order;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::index_type, index_type >,
        mpl::pair< tag::entity, tag::matrix >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::size_type<2>, std::ptrdiff_t >,
        mpl::pair< tag::matrix_type, tag::general >,
        mpl::pair< tag::data_structure, tag::compressed_sparse >,
        mpl::pair< tag::data_order, data_order >,
        mpl::pair< tag::index_base, mpl::int_<IB> >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.num_rows();
    }

    static std::ptrdiff_t size2( const Id& id ) {
        return id.num_columns();
    }

    static value_type* begin_value( Id& id ) {
        return bindings::begin_value( id.value_array() );
    }

    static value_type* end_value( Id& id ) {
        return bindings::begin_value( id.value_array() ) + id.nnz();
    }

    static index_type* begin_compressed_index_major( Id& id ) {
        return bindings::begin_value( id.sparse_structure().compressed_index_array() );
    }

    static index_type* end_compressed_index_major( Id& id ) {
        return bindings::end_value( id.sparse_structure().compressed_index_array() );
    }

    static index_type* begin_index_minor( Id& id ) {
        return bindings::begin_value( id.sparse_structure().index_array() );
    }

    static index_type* end_index_minor( Id& id ) {
        return bindings::begin_value( id.sparse_structure().index_array() ) + id.nnz();
    }

};

} // namespace detail
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif