#include "Defined_Tensor.h"
#include <Eigen\Dense>
#include <queue>

defined_tensor::defined_tensor()
{
	m_tensor_name = Utility_Definition::T_IDENTITY;
	AABB_Tree = NULL;
	AABB_Segment_Tree = NULL;
	bm = NULL;
	ref_mesh_ = NULL;
	kdTree = NULL;
}

defined_tensor::~defined_tensor()
{
	if(AABB_Tree) delete AABB_Tree;
	if(AABB_Segment_Tree) delete AABB_Segment_Tree;
	if(bm) delete bm;
	if(kdTree) {delete kdTree; annClose();}
}

void defined_tensor::get_tensor( const OpenVolumeMesh::Geometry::Vec3d& p, OpenVolumeMesh::Geometry::Vec6d& M )
{
	double det = 1.0; double t = 1.0;
	switch (m_tensor_name)
	{
	case Utility_Definition::T_IDENTITY:
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 1.0;
		break;
	case Utility_Definition::T_10_1_1:
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 100.0;
		break;
	case Utility_Definition::T_X2_Y2_Z4: //0.5*(x^2 + y^2 + z^4)
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 6*p[2]*p[2];
		det = M[0]*M[3]*M[5] + M[1]*M[4]*M[2] + M[2]*M[1]*M[4] - M[2]*M[3]*M[2] - M[4]*M[4]*M[1]- M[5]*M[1]*M[1];
		t = std::pow( det, -0.2);
		M *= t;
		break;
	case Utility_Definition::T_EXP:
		M[0] = std::exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10)/5 + (p[0]*p[0]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[3] = std::exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10)/5 + (p[1]*p[1]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[5] = std::exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10)/5 + (p[2]*p[2]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[1] = (p[0]*p[1]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[4] = (p[2]*p[1]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		M[2] = (p[2]*p[0]*exp(p[0]*p[0]/10 + p[1]*p[1]/10 + p[2]*p[2]/10))/25;
		det = M[0]*M[3]*M[5] + M[1]*M[4]*M[2] + M[2]*M[1]*M[4] - M[2]*M[3]*M[2] - M[4]*M[4]*M[1]- M[5]*M[1]*M[1];
		t = std::pow( det, -0.2);
		M *= t;
		break;
	default:
		M[0] = 1.0; M[1] = 0.0; M[2] = 0.0; M[3] = 1.0; M[4] = 0.0; M[5] = 1.0;
		break;
	}

	if(Utility_Definition::T_SPHERICAL_SHOCK == m_tensor_name)
	{
		double t = 7;
		Eigen::Matrix3d R;
		double r = p.norm();
		OpenVolumeMesh::Geometry::Vec3d e_r = p / r;
		local_frame lf; lf.n = e_r; lf.find_e_x_y();
		R(0,0) =    e_r[0]; R(0,1) =    e_r[1]; R(0,2) =    e_r[2];
		R(1,0) = lf.e_y[0]; R(1,1) = lf.e_y[1]; R(1,2) = lf.e_y[2];
		R(2,0) = lf.e_x[0]; R(2,1) = lf.e_x[1]; R(2,2) = lf.e_x[2];

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0/( 0.025 + 1.0 * (1.0 - std::exp( -0.01*std::abs(r*r - t*t) ) ) );
		D(1,1) = 2.0 / 2.0; D(2,2) = 2.0 / 2.0;
		Eigen::Matrix3d TM  = R.transpose() *D * D * R;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}
	else if (Utility_Definition::T_CYLINDER_SHOCK == m_tensor_name)
	{
		double t = 7; Eigen::Matrix3d R;
		double r = std::sqrt(p[0]*p[0] + p[1]*p[1]);
		R(0,0) =  p[0]/r; R(0,1) = p[1]/r; R(0,2) = 0;
		R(1,0) = -p[1]/r; R(1,1) = p[0]/r; R(1,2) = 0;
		R(2,0) = 0; R(2,1) = 0; R(2,2) = 1;

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 2.0/( 0.1 + 2.0 * (1.0 - std::exp( -0.01*std::abs(r*r - t*t) ) ) );
		D(1,1) = 2.0 / 2.0; D(2,2) = 2.0 / 2.0;
		Eigen::Matrix3d TM  = R.transpose() *D * D * R;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}

	if(Utility_Definition::T_PLANAR_SHOCK == m_tensor_name)
	{
		double t = 0.6;
		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0/( 0.0025 + 0.2 * (1.0 - std::exp( -1.0*std::abs(p[0] - t) ) ) );
		D(1,1) = 1.0 / 0.2; D(2,2) = 1.0 / 0.2;
		Eigen::Matrix3d TM = D * D;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}

	if(Utility_Definition::T_SINE == m_tensor_name)
	{
		//printf("Sine tensor!!!!!!!\n");
		Eigen::Matrix3d R;
		OpenVolumeMesh::Geometry::Vec3d e_1( 2.0*std::cos(p[0]*6.0), 1.0, 0.0 ); e_1.normalize();
		OpenVolumeMesh::Geometry::Vec3d e_2(0.0, 1.0, 0.0);
		e_2 = OpenVolumeMesh::Geometry::cross(e_1, e_2); e_2.normalize();
		OpenVolumeMesh::Geometry::Vec3d e_3 = OpenVolumeMesh::Geometry::cross(e_1, e_2);

		R(0, 0) = e_1[0]; R(0, 1) = e_1[1]; R(0, 2) = e_1[2];
		R(1, 0) = e_2[0]; R(1, 1) = e_2[1]; R(1, 2) = e_2[2];
		R(2, 0) = e_3[0]; R(2, 1) = e_3[1]; R(2, 2) = e_3[2];

		//double R_det = R.determinant();

		Eigen::Matrix3d D; D.setZero();
		D(0, 0) = 100;
		D(1, 1) = 1.0; D(2, 2) = 1.0;
		D *= 10;
		Eigen::Matrix3d TM = R.transpose()* D * R;

		M[0] = TM(0, 0); M[1] = TM(0, 1); M[2] = TM(0, 2);
		M[3] = TM(1, 1); M[4] = TM(1, 2); M[5] = TM(2, 2);

		double D00 = 1000; double D11 = 10.0; double D22 = 10.0;

		/*M[0] = D00*e_1[0] * e_1[0] + D11*e_2[0] * e_2[0] + D22*e_3[0] * e_3[0];
		M[1] = e_1[0] * e_1[1] * D00 + e_2[0] * e_2[1] * D11 + e_3[0] * e_3[1] * D22;
		M[2] = e_1[0] * e_1[2] * D00 + e_2[0] * e_2[2] * D11 + e_3[0] * e_3[2] * D22;
		M[3] = D00*e_1[1] * e_1[1] + D11*e_2[1] * e_2[1] + D22*e_3[1] * e_3[1];
		M[4] = e_1[1] * e_1[2] * D00 + e_2[1] * e_2[2] * D11 + e_3[1] * e_3[2] * D22;
		M[5] = D00*e_1[2] * e_1[2] + D11*e_2[2] * e_2[2] + D22*e_3[2] * e_3[2];*/

		double M0 = D00*e_1[0] * e_1[0] + D11*e_2[0] * e_2[0] + D22*e_3[0] * e_3[0];
		double M1 = e_1[0] * e_1[1] * D00 + e_2[0] * e_2[1] * D11 + e_3[0] * e_3[1] * D22;
		double M2 = e_1[0] * e_1[2] * D00 + e_2[0] * e_2[2] * D11 + e_3[0] * e_3[2] * D22;
		double M3 = D00*e_1[1] * e_1[1] + D11*e_2[1] * e_2[1] + D22*e_3[1] * e_3[1];
		double M4 = e_1[1] * e_1[2] * D00 + e_2[1] * e_2[2] * D11 + e_3[1] * e_3[2] * D22;
		double M5 = D00*e_1[2] * e_1[2] + D11*e_2[2] * e_2[2] + D22*e_3[2] * e_3[2];

		/*M[0] = D00*e_1[0] * e_1[0] + D11*e_1[1] * e_1[1] + D22*e_1[2] * e_1[2];
		M[1] = e_1[0] * e_2[0] * D00 + e_1[1] * e_2[1] * D11 + e_1[2] * e_2[2] * D22;
		M[2] = e_1[0] * e_3[0] * D00 + e_1[1] * e_3[1] * D11 + e_1[2] * e_3[2] * D22;
		M[3] = D00*e_2[0] * e_2[0] + D11*e_2[1] * e_2[1] + D22*e_2[2] * e_2[2];
		M[4] = e_2[0] * e_3[0] * D00 + e_2[1] * e_3[1] * D11 + e_2[2] * e_3[2] * D22;
		M[5] = D00*e_3[0] * e_3[0] + D11*e_3[1] * e_3[1] + D22*e_3[2] * e_3[2];*/
	}

	if(Utility_Definition::T_SINK == m_tensor_name)
	{
		Eigen::Matrix3d R;
		double r = std::sqrt(p[0]*p[0] + p[1]*p[1]);
		R(0,0) =  p[0]/r; R(0,1) = p[1]/r; R(0,2) = 0;
		R(1,0) = -p[1]/r; R(1,1) = p[0]/r; R(1,2) = 0;
		R(2,0) = 0; R(2,1) = 0; R(2,2) = 1;

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0;
		D(1,1) = 1.0 + r*5;
		D(2,2) = 1.0 + r*5;
		D *= 1.2/(0.5 + 1 - std::exp(-0.05*(r*r - 2.56)) );
		Eigen::Matrix3d TM = R.transpose() *D * D * R ;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}

	if(Utility_Definition::T_TEST == m_tensor_name)
	{
		double t = 4;
		Eigen::Matrix3d R;
		double r = p.norm();
		OpenVolumeMesh::Geometry::Vec3d e_r = p / r;
		local_frame lf; lf.n = e_r; lf.find_e_x_y();
		R(0,0) =    e_r[0]; R(0,1) =    e_r[1]; R(0,2) =    e_r[2];
		R(1,0) = lf.e_y[0]; R(1,1) = lf.e_y[1]; R(1,2) = lf.e_y[2];
		R(2,0) = lf.e_x[0]; R(2,1) = lf.e_x[1]; R(2,2) = lf.e_x[2];

		Eigen::Matrix3d D; D.setZero();
		D(0,0) = 1.0/( 0.05 + 1.0 * (1.0 - std::exp( -0.01*std::abs(r*r - t*t) ) ) );
		D(1,1) = 1; D(2,2) = 1;
		D *= 1.25;
		Eigen::Matrix3d TM = R.transpose() *D * D * R ;

		M[0] = TM(0,0); M[1] = TM(0,1); M[2] = TM(0,2);
		M[3] = TM(1,1); M[4] = TM(1,2); M[5] = TM(2,2);
	}
}

void defined_tensor::build_AABB_Tree_using_ref_mesh(SurfaceMesh* mesh_)
{
	if(AABB_Tree) {delete AABB_Tree; AABB_Tree = NULL;}

	unsigned nf = mesh_->n_faces();
	if (nf == 0) return;
	opp_face_id.resize(nf); face_pos.resize(nf);
	std::vector<int> one_face_opp_face_id(3); std::vector<OpenVolumeMesh::Geometry::Vec3d> one_face_pos(3);
	visited_ref_face_id.resize(nf, -1); visited_ref_face.resize(nf, -1);

	std::vector<CGAL_double_3_Point> v_pos( mesh_->n_vertices() ); OpenMesh::Vec3d p;
	for(SurfaceMesh::VertexIter v_it = mesh_->vertices_begin(); v_it != mesh_->vertices_end(); ++v_it)
	{
		int vertex_id = v_it.handle().idx();
		p = mesh_->point( v_it );
		v_pos[vertex_id] = CGAL_double_3_Point( p[0], p[1], p[2] );
	}

	triangle_vectors.clear();
	triangle_vectors.resize(nf); int fv_id[3];
	for(SurfaceMesh::FaceIter f_it = mesh_->faces_begin(); f_it != mesh_->faces_end(); ++f_it)
	{
		SurfaceMesh::FaceVertexIter fv_it = mesh_->fv_iter(f_it);
		fv_id[0] = fv_it.handle().idx(); ++fv_it;
		fv_id[1] = fv_it.handle().idx(); ++fv_it;
		fv_id[2] = fv_it.handle().idx(); ++fv_it;
		one_face_pos[0] = OpenVolumeMesh::Geometry::Vec3d(v_pos[fv_id[0]].x(), v_pos[fv_id[0]].y(), v_pos[fv_id[0]].z());
		one_face_pos[1] = OpenVolumeMesh::Geometry::Vec3d(v_pos[fv_id[1]].x(), v_pos[fv_id[1]].y(), v_pos[fv_id[1]].z());
		one_face_pos[2] = OpenVolumeMesh::Geometry::Vec3d(v_pos[fv_id[2]].x(), v_pos[fv_id[2]].y(), v_pos[fv_id[2]].z());
		face_pos[f_it.handle().idx()] = one_face_pos;
		SurfaceMesh::FaceHalfedgeIter fhe_it = mesh_->fh_iter(f_it);
		one_face_opp_face_id[0] = mesh_->face_handle(mesh_->opposite_halfedge_handle(fhe_it)).idx();
		++fhe_it; one_face_opp_face_id[1] = mesh_->face_handle(mesh_->opposite_halfedge_handle(fhe_it)).idx();
		++fhe_it; one_face_opp_face_id[2] = mesh_->face_handle(mesh_->opposite_halfedge_handle(fhe_it)).idx();
		opp_face_id[f_it.handle().idx()] = one_face_opp_face_id;

		triangle_vectors[f_it.handle().idx()] = CGAL_3_Triangle(v_pos[fv_id[0]], v_pos[fv_id[1]], v_pos[fv_id[2]]);

		if (fv_it)
		{
			fv_id[1] = fv_id[2];
			fv_id[2] = fv_it.handle().idx();
			triangle_vectors.push_back(CGAL_3_Triangle(v_pos[fv_id[0]], v_pos[fv_id[1]], v_pos[fv_id[2]]));
		}
	}
	AABB_Tree = new CGAL_AABB_Tree(triangle_vectors.begin(), triangle_vectors.end());
	AABB_Tree->accelerate_distance_queries();

	printf("Finish Constructing AABB Tree.\n");
}

void defined_tensor::build_AABB_Tree_using_ref_mesh(VolumeMesh* mesh_)
{
	if (AABB_Tree) { delete AABB_Tree; AABB_Tree = NULL; }
	triangle_vectors.clear();
	for (OpenVolumeMesh::FaceIter f_it = mesh_->faces_begin(); f_it != mesh_->faces_end(); ++f_it)
	{
		if (!mesh_->is_boundary(*f_it)) continue;

		OpenVolumeMesh::HalfFaceHandle hfHandle = mesh_->halfface_handle(*f_it, 0);
		OpenVolumeMesh::HalfFaceHandle hfHandle2 = mesh_->halfface_handle(*f_it, 1);
		int cell_id = mesh_->incident_cell(hfHandle2).idx();
		if (cell_id < 0)
		{
			hfHandle2 = hfHandle;
			cell_id = mesh_->incident_cell(hfHandle2).idx();
			hfHandle = mesh_->halfface_handle(*f_it, 1);
		}

		OpenVolumeMesh::HalfFaceVertexIter hfv_it = mesh_->hfv_iter(hfHandle);
		OpenVolumeMesh::Geometry::Vec3d p0 = mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p1 = mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p2 = mesh_->vertex(*hfv_it);

		CGAL_double_3_Point q0(p0[0], p0[1], p0[2]);
		CGAL_double_3_Point q1(p1[0], p1[1], p1[2]);
		CGAL_double_3_Point q2(p2[0], p2[1], p2[2]);

		triangle_vectors.push_back( CGAL_3_Triangle(q0, q1, q2) );
	}

	AABB_Tree = new CGAL_AABB_Tree(triangle_vectors.begin(), triangle_vectors.end());
	AABB_Tree->accelerate_distance_queries();

	printf("Finish Constructing AABB Tree.\n");
}

void defined_tensor::build_AABB_Segment_Tree_using_ref_mesh(VolumeMesh* mesh_, const std::vector<int>& edge_feature)
{
	if(AABB_Segment_Tree) {delete AABB_Segment_Tree; AABB_Segment_Tree = NULL;}
	if(mesh_->n_edges() == 0) return;

	std::vector<CGAL_3_Segment> segment_vectors;
	for(unsigned i=0;i<edge_feature.size();++i)
	{
		if(edge_feature[i] == 1)
		{
			OpenVolumeMesh::EdgeHandle eh(i);
			OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge(eh);
			OpenVolumeMesh::VertexHandle vh_from = edge.from_vertex();
			OpenVolumeMesh::VertexHandle vh_to = edge.to_vertex();
			OpenVolumeMesh::Geometry::Vec3d p_from = mesh_->vertex(vh_from);
			OpenVolumeMesh::Geometry::Vec3d p_to = mesh_->vertex(vh_to);
			CGAL_double_3_Point pf(p_from[0], p_from[1], p_from[2]);
			CGAL_double_3_Point pt(p_to[0], p_to[1], p_to[2]);
			segment_vectors.push_back( CGAL_3_Segment(pf, pt) );
		}
	}
	AABB_Segment_Tree = new CGAL_AABB_Segment_Tree(segment_vectors.begin(), segment_vectors.end());
	AABB_Segment_Tree->accelerate_distance_queries();
	printf("Finish Constructing AABB Segment Tree.\n");
}

void defined_tensor::project_on_ref_mesh(OpenVolumeMesh::Geometry::Vec3d& p)
{
	CGAL_double_3_Point pos = AABB_Tree->closest_point( CGAL_double_3_Point(p[0], p[1], p[2]) );
	p = OpenVolumeMesh::Geometry::Vec3d( pos.x(), pos.y(), pos.z() );
}

//if input face id = -1, will change face_id , p
void defined_tensor::project_on_ref_mesh_with_guess_face(OpenVolumeMesh::Geometry::Vec3d& p, int& guess_face_id, const double& radius)
{
	if (guess_face_id == -1)
	{
		CGAL_AABB_Tree::Point_and_primitive_id point_primitive = AABB_Tree->closest_point_and_primitive(CGAL_double_3_Point(p[0], p[1], p[2]));
		CGAL_double_3_Point pos = point_primitive.first;
		CGAL_Triangle_Iterator it = point_primitive.second;
		guess_face_id = std::distance(triangle_vectors.begin(), it);
		p = OpenVolumeMesh::Geometry::Vec3d(pos.x(), pos.y(), pos.z());
	}
	else
	{
		int visited_face_count = 0; double* p_data = p.data();
		visited_ref_face_id[0] = guess_face_id; visited_ref_face[guess_face_id] = 1; ++visited_face_count;
		OpenVolumeMesh::Geometry::Vec3d np; OpenVolumeMesh::Geometry::Vec3d np_; double min_d = 1.0e30; int min_face_id = guess_face_id;
		double distance_th = 25.0*radius; int current_face_count = 0;
		while (current_face_count < visited_face_count)
		{
			//int face_id = Q.front(); Q.pop();
			int face_id = visited_ref_face_id[current_face_count]; ++current_face_count;
			std::vector<OpenVolumeMesh::Geometry::Vec3d>& one_face_pos = face_pos[face_id];
			//double d = distPointTriangleSquared(p, one_face_pos[0], one_face_pos[1], one_face_pos[2], np);
			double d = distPointTriangleSquared2(p_data, one_face_pos[0].data(), one_face_pos[1].data(), one_face_pos[2].data(), np.data());
			if (d < min_d) { min_d = d; min_face_id = face_id; np_ = np; }

			std::vector<int>& one_face_opp_face_id = opp_face_id[face_id];
			for (unsigned i = 0; i < 3; ++i)
			{
				if (one_face_opp_face_id[i] >= 0 && visited_ref_face[one_face_opp_face_id[i]] == -1)
				{
					std::vector<OpenVolumeMesh::Geometry::Vec3d>& opp_face_pos = face_pos[one_face_opp_face_id[i]];
					OpenVolumeMesh::Geometry::Vec3d& opp_face_pos1 = opp_face_pos[0];
					double d1 = (p[0] - opp_face_pos1[0])*(p[0] - opp_face_pos1[0]) + (p[1] - opp_face_pos1[1])*(p[1] - opp_face_pos1[1]) + (p[2] - opp_face_pos1[2])*(p[2] - opp_face_pos1[2]);
					if (d1 < distance_th)
					{
						visited_ref_face_id[visited_face_count] = one_face_opp_face_id[i];
						visited_ref_face[one_face_opp_face_id[i]] = 1;
						++visited_face_count;
					}
					else
					{
						OpenVolumeMesh::Geometry::Vec3d& opp_face_pos2 = opp_face_pos[1];
						double d2 = (p[0] - opp_face_pos2[0])*(p[0] - opp_face_pos2[0]) + (p[1] - opp_face_pos2[1])*(p[1] - opp_face_pos2[1]) + (p[2] - opp_face_pos2[2])*(p[2] - opp_face_pos2[2]);
						if (d2 < distance_th)
						{
							visited_ref_face_id[visited_face_count] = one_face_opp_face_id[i];
							visited_ref_face[one_face_opp_face_id[i]] = 1;
							++visited_face_count;
						}
						else
						{
							OpenVolumeMesh::Geometry::Vec3d& opp_face_pos3 = opp_face_pos[2];
							double d3 = (p[0] - opp_face_pos3[0])*(p[0] - opp_face_pos3[0]) + (p[1] - opp_face_pos3[1])*(p[1] - opp_face_pos3[1]) + (p[2] - opp_face_pos3[2])*(p[2] - opp_face_pos3[2]);
							if (d3 < distance_th)
							{
								visited_ref_face_id[visited_face_count] = one_face_opp_face_id[i];
								visited_ref_face[one_face_opp_face_id[i]] = 1;
								++visited_face_count;
							}
						}
					}
				}
			}
		}

		for (unsigned i = 0; i < visited_face_count; ++i)
		{
			visited_ref_face[visited_ref_face_id[i]] = -1;
		}

		guess_face_id = min_face_id;
		p = np_;
	}

}

void defined_tensor::project_on_ref_mesh_with_metric(OpenVolumeMesh::Geometry::Vec3d& p, OpenVolumeMesh::Geometry::Vec6d& M)
{
	CGAL_AABB_Tree::Point_and_primitive_id point_primitive = AABB_Tree->closest_point_and_primitive( CGAL_double_3_Point(p[0], p[1], p[2]) );
	CGAL_double_3_Point pos = point_primitive.first;
	CGAL_Triangle_Iterator it = point_primitive.second;
	unsigned face_id = std::distance( triangle_vectors.begin(), it);
	p = OpenVolumeMesh::Geometry::Vec3d( pos.x(), pos.y(), pos.z() );

	//printf("F : %d\n", face_id);

	OpenMesh::Vec3d p_(p[0], p[1], p[2]);
	SurfaceMesh::ConstFaceVertexIter fv_it = bm->cfv_iter( bm->face_handle(face_id) );
	OpenMesh::Vec3d p0 = bm->point(fv_it.handle()); int v0_id = fv_it.handle().idx();
	++fv_it; OpenMesh::Vec3d p1 = bm->point(fv_it.handle()); int v1_id = fv_it.handle().idx();
	++fv_it; OpenMesh::Vec3d p2 = bm->point(fv_it.handle()); int v2_id = fv_it.handle().idx();

	OpenMesh::Vec3d bc;
	if (!baryCoord(p_, p0, p1, p2, bc))
		bc[0] = bc[1] = bc[2] = 1.0/3.0;

	M = bc[0] *bm_metric[v0_id] + bc[1] *bm_metric[v1_id] + bc[2] *bm_metric[v2_id];
}

void defined_tensor::project_on_ref_edge(OpenVolumeMesh::Geometry::Vec3d& p)
{
	CGAL_double_3_Point pos = AABB_Segment_Tree->closest_point( CGAL_double_3_Point(p[0], p[1], p[2]) );
	p = OpenVolumeMesh::Geometry::Vec3d( pos.x(), pos.y(), pos.z() );
}

void defined_tensor::construct_boundary_mesh()
{
	OpenVolumeMesh::BoundaryFaceIter bf_it(ref_mesh_->bf_iter());
	OpenVolumeMesh::HalfFaceVertexIter hfv_it = ref_mesh_->hfv_iter( ref_mesh_->halfface_handle(*bf_it,0) );

	std::vector<SurfaceMesh::VertexHandle> vertexHandleVec;
	std::vector<SurfaceMesh::VertexHandle> faceVertexHandle;
	std::map<int,int> VolumeSurfaceVertex;
	std::map<int,int>::iterator map_it;
	int indexOnSurface = 0;
	int InvalidHF =0;
	bm->clear();
	for (; bf_it.valid(); ++bf_it)
	{
		OpenVolumeMesh::HalfFaceHandle hfHandle = ref_mesh_->halfface_handle(*bf_it,0);
		OpenVolumeMesh::HalfFaceHandle hfHandle2 = ref_mesh_->halfface_handle(*bf_it,1);
		int cell_id = ref_mesh_->incident_cell(hfHandle2).idx();
		if( cell_id >= 0 )
		{
			hfHandle2 = hfHandle;
			cell_id = ref_mesh_->incident_cell(hfHandle2).idx();
			hfHandle = ref_mesh_->halfface_handle(*bf_it,1);
		}
		++InvalidHF;
		hfv_it = ref_mesh_->hfv_iter( hfHandle );
		faceVertexHandle.clear();
		for( hfv_it; hfv_it.valid(); ++hfv_it)
		{
			map_it = VolumeSurfaceVertex.find(hfv_it->idx()) ;
			if( map_it == VolumeSurfaceVertex.end() )
			{
				VolumeSurfaceVertex.insert(std::pair<int,int>( hfv_it->idx(), indexOnSurface ));
				OpenVolumeMesh::Geometry::Vec3d v1 = ref_mesh_->vertex(*hfv_it);
				SurfaceMesh::Point v2( v1[0], v1[1], v1[2] );
				vertexHandleVec.push_back( bm->add_vertex( v2 ) );
				bm->data(vertexHandleVec.back()).set_tet_vertex_id(hfv_it->idx());
				faceVertexHandle.push_back( vertexHandleVec.back() );
				++indexOnSurface;
			}
			else
			{
				faceVertexHandle.push_back(vertexHandleVec[map_it->second]);
			}
		}

		std::reverse(faceVertexHandle.begin(), faceVertexHandle.end());
		OpenMesh::FaceHandle fh = bm->add_face(faceVertexHandle);
		bm->data(fh).set_cell_id( cell_id );
	}

	bm->update_normals();
}

void defined_tensor::save_boundary_mesh_metric()
{
	FILE* f_b_m = fopen("A:\\Code\\VolumeMeshProcessing\\Models\\Tetrahedrization\\elphant\\boundary.off", "w");
	unsigned nv = bm->n_vertices(); unsigned nf = bm->n_faces();
	fprintf(f_b_m, "OFF");
	fprintf(f_b_m, "\n%d %d 0", nv, nf);
	for(SurfaceMesh::VertexIter v_it = bm->vertices_begin(); v_it != bm->vertices_end(); ++v_it)
	{
		OpenMesh::Vec3d p = bm->point(v_it);
		fprintf(f_b_m, "\n%20.19f %20.19f %20.19f", p[0], p[1], p[2]);
	}
	for(SurfaceMesh::FaceIter f_it = bm->faces_begin(); f_it != bm->faces_end(); ++f_it)
	{
		fprintf(f_b_m, "\n3");
		for( SurfaceMesh::FaceVertexIter fv_it = bm->fv_iter(f_it); fv_it; ++fv_it )
		{
			fprintf(f_b_m, " %d", fv_it.handle().idx());
		}
	}
	for(unsigned i=0;i<bm_metric.size();++i)
	{
		fprintf(f_b_m, "\n%20.19f %20.19f %20.19f %20.19f %20.19f %20.19f", bm_metric[i][0],bm_metric[i][1],bm_metric[i][2],bm_metric[i][3],bm_metric[i][4],bm_metric[i][5]);
	}

	fclose(f_b_m);
}

void defined_tensor::smmoth_metric_to_interior(VolumeMesh* mesh_)
{
	if(ref_mesh_) delete ref_mesh_;
	ref_mesh_ = mesh_;

	if(bm) delete bm;
	bm = new SurfaceMesh();
	bm->request_face_status();
	bm->request_vertex_status();
	bm->request_edge_status();
	bm->request_face_normals();
	bm->request_vertex_normals();

	construct_boundary_mesh();
	//printf("1\n");

	std::vector<double> k1, k2; std::vector<OpenMesh::Vec3d> d1, d2;
	compute_principal_curvature(bm,k1,k2,d1,d2); bm_metric.resize(k1.size());
	tet_vertex_metric.clear(); unsigned tet_nv = mesh_->n_vertices(); tet_vertex_metric.resize(tet_nv);
	std::vector<int> tet_vertex_flag(tet_nv, -1); std::vector<int> new_one_ring_vertex(tet_nv, -1); int new_one_ring_vertex_count = 0;
	Eigen::Matrix3d R, D, M; D.setZero();
	//printf("2 %d %d %d\n", k1.size(), bm->n_vertices(), tet_nv);
	for(unsigned i=0;i<k1.size();++i)
	{
		SurfaceMesh::VertexHandle vh = bm->vertex_handle(i);
		int tet_v_id = bm->data(vh).get_tet_vertex_id();
		//printf("%d\n", tet_v_id);
		//use curvature to construct metric
		OpenMesh::Vec3d n = OpenMesh::cross(d1[i], d2[i]); n.normalize();
		R(0,0) = d1[i][0]; R(1,0) = d1[i][1]; R(2,0) = d1[i][2];
		R(0,1) = d2[i][0]; R(1,1) = d2[i][1]; R(2,1) = d2[i][2];
		R(0,2) =     n[0]; R(1,2) =     n[1]; R(2,2) =     n[2];
		D(0,0) = std::abs(k1[i]) < 1.0e-4 ? 1.0e-4 : std::abs(k1[i]);
		D(1,1) = std::abs(k2[i]) < 1.0e-4 ? 1.0e-4 : std::abs(k2[i]);
		double max_k = std::abs(k1[i]) > std::abs(k2[i]) ? std::abs(k1[i]) : std::abs(k2[i]);
		D(2,2) = max_k < 1e-4 ? 1e-4 : max_k;
		//D(2,2) = 0.0;
		M = R*D*R.transpose();
		tet_vertex_metric[tet_v_id] = OpenVolumeMesh::Geometry::Vec6d(M(0,0), M(0,1), M(0,2), M(1,1), M(1,2), M(2,2));
		bm_metric[i] = tet_vertex_metric[tet_v_id];
		tet_vertex_flag[tet_v_id] = 1; OpenVolumeMesh::VertexHandle tet_vh(tet_v_id);
		for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(tet_vh); voh_it; ++voh_it)
		{
			OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
			OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
			OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
			if(f_vh != tet_vh && !mesh_->is_boundary(f_vh))
			{
				new_one_ring_vertex[f_vh.idx()] = 1; ++new_one_ring_vertex_count;
			}
			else if(t_vh != tet_vh && !mesh_->is_boundary(t_vh))
			{
				new_one_ring_vertex[t_vh.idx()] = 1; ++new_one_ring_vertex_count;
			}
		}
	}

	//printf("3,1\n");

	save_boundary_mesh_metric();

	//printf("3,2\n");

	//init value
	while(new_one_ring_vertex_count > 0)
	{
		std::vector<int> old_v; old_v.reserve(new_one_ring_vertex_count);
		for(unsigned i=0;i<new_one_ring_vertex.size();++i)
		{
			if(new_one_ring_vertex[i] == 1)
			{
				old_v.push_back(i); OpenVolumeMesh::VertexHandle tet_vh(i);
				OpenVolumeMesh::Geometry::Vec6d ave_M(0,0,0,0,0,0); double sum_count = 0.0;
				for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(tet_vh); voh_it; ++voh_it)
				{
					OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
					OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
					OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
					if(f_vh != tet_vh)
					{
						if(tet_vertex_flag[f_vh.idx()] == 1)
						{
							ave_M += tet_vertex_metric[f_vh.idx()]; sum_count += 1.0;
						}
					}
					else if(t_vh != tet_vh)
					{
						if(tet_vertex_flag[t_vh.idx()] == 1)
						{
							ave_M += tet_vertex_metric[t_vh.idx()]; sum_count += 1.0;
						}
					}
				}

				ave_M /= sum_count;
				tet_vertex_metric[i] = ave_M;
			}
		}

		for(unsigned i=0;i<old_v.size();++i)
		{
			tet_vertex_flag[old_v[i]] = 1; new_one_ring_vertex[old_v[i]] = -1;
		}

		new_one_ring_vertex_count = 0;
		for(unsigned i=0;i<old_v.size();++i)
		{
			OpenVolumeMesh::VertexHandle tet_vh(old_v[i]);
			for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(tet_vh); voh_it; ++voh_it)
			{
				OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
				OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
				OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
				if(f_vh != tet_vh && tet_vertex_flag[f_vh.idx()] == -1)
				{
					new_one_ring_vertex[f_vh.idx()] = 1; ++new_one_ring_vertex_count;
				}
				else if(t_vh != tet_vh && tet_vertex_flag[t_vh.idx()] == -1)
				{
					new_one_ring_vertex[t_vh.idx()] = 1; ++new_one_ring_vertex_count;
				}
			}
		}
	}

	//printf("4\n");
	//smooth
	for(unsigned i=0;i<50;++i)
	{
		printf("%d ", i);
		for(OpenVolumeMesh::VertexIter v_it = mesh_->vertices_begin(); v_it != mesh_->vertices_end(); ++v_it)
		{
			if(mesh_->is_boundary(*v_it)) continue;
			OpenVolumeMesh::Geometry::Vec6d ave_M(0,0,0,0,0,0); double sum_count = 0.0;
			for(OpenVolumeMesh::VertexOHalfEdgeIter voh_it = mesh_->voh_iter(*v_it); voh_it; ++voh_it)
			{
				OpenVolumeMesh::OpenVolumeMeshEdge edge = mesh_->edge( mesh_->edge_handle(*voh_it) );
				OpenVolumeMesh::VertexHandle f_vh = edge.from_vertex();
				OpenVolumeMesh::VertexHandle t_vh = edge.to_vertex();
				if( f_vh != *v_it )
				{
					ave_M += tet_vertex_metric[f_vh.idx()]; sum_count += 1.0;
				}
				else if( t_vh != *v_it )
				{
					ave_M += tet_vertex_metric[t_vh.idx()]; sum_count += 1.0;
				}
			}
			ave_M /= sum_count;
			tet_vertex_metric[v_it->idx()] = ave_M;
		}
	}
	printf("\n");
}

void defined_tensor::save_tet_vertex_metric(const char* filename)
{
	FILE* f_v_m = fopen(filename, "w");

	fprintf(f_v_m, "%d", tet_vertex_metric.size());
	for(unsigned i=0;i<tet_vertex_metric.size();++i)
	{
		fprintf(f_v_m, "\n%20.19f %20.19f %20.19f %20.19f %20.19f %20.19f", tet_vertex_metric[i][0],tet_vertex_metric[i][1],tet_vertex_metric[i][2],tet_vertex_metric[i][3],tet_vertex_metric[i][4],tet_vertex_metric[i][5] );
	}

	fclose(f_v_m);
}

void defined_tensor::load_tet_vertex_metric(const char* filename)
{
	FILE* f_v_m = fopen(filename, "r");
	int nv = 0;
	char buf[4096];  fgets(buf, 4096, f_v_m);
	sscanf(buf, "%d", &nv); tet_vertex_metric.clear(); tet_vertex_metric.resize(nv); int v_count = 0;
	char h0[128]; char h1[128]; char h2[128]; char h4[128]; char h5[128]; char h8[128];
	while ( !feof(f_v_m) )
	{
		fgets(buf, 4096, f_v_m);
		sscanf(buf, "%s %s %s %s %s %s", h0, h1, h2, h4, h5, h8);
		tet_vertex_metric[v_count] = OpenVolumeMesh::Geometry::Vec6d(atof(h0), atof(h1),atof(h2), atof(h4), atof(h5), atof(h8));
		++v_count;
	}

	fclose(f_v_m);
}

void defined_tensor::load_ref_tet_mesh(VolumeMesh* mesh_)
{
	if(tet_vertex_metric.size() == 0)
	{
		printf("Please Load Metric First!!!\n");
		return;
	}
	if(ref_mesh_) delete ref_mesh_;
	ref_mesh_ = mesh_;
	if(bm) delete bm;
	bm = new SurfaceMesh();
	construct_boundary_mesh();
	bm_metric.resize(bm->n_vertices());
	for(unsigned i=0;i<bm->n_vertices(); ++i)
	{
		SurfaceMesh::VertexHandle vh = bm->vertex_handle(i);
		int tet_v_id = bm->data(vh).get_tet_vertex_id();
		bm_metric[i] = tet_vertex_metric[tet_v_id];
	}
	build_AABB_Tree_using_ref_mesh(bm);
	build_ANN_KD_tree();
}

void defined_tensor::build_ANN_KD_tree()
{
	int nc = ref_mesh_->n_cells();
	ANNpointArray dataPts = annAllocPts(nc, 3);
	for(OpenVolumeMesh::CellIter c_it = ref_mesh_->cells_begin(); c_it != ref_mesh_->cells_end();++c_it)
	{
		OpenVolumeMesh::Geometry::Vec3d c_c(0,0,0); double count = 0.0;
		for(OpenVolumeMesh::CellVertexIter cv_it = ref_mesh_->cv_iter(*c_it); cv_it; ++cv_it )
		{
			c_c += ref_mesh_->vertex(*cv_it); count += 1.0;
		}
		c_c /= count;
		int c_id = c_it->idx();
		dataPts[c_id][0] = c_c[0]; dataPts[c_id][1] = c_c[1]; dataPts[c_id][2] = c_c[2];
	}
	if(kdTree) delete kdTree;
	kdTree = new ANNkd_tree(dataPts, nc, 3);
}

bool defined_tensor::check_in_tet(const OpenVolumeMesh::Geometry::Vec3d& p, const int& cel_id)
{
	if(cel_id < 0) return false;

	OpenVolumeMesh::CellHandle ch(cel_id);
	OpenVolumeMesh::OpenVolumeMeshCell cell = ref_mesh_->cell(ch);
	std::vector<OpenVolumeMesh::HalfFaceHandle> hfh_vec = cell.halffaces();
	for(unsigned i=0;i<hfh_vec.size();++i)
	{
		OpenVolumeMesh::HalfFaceVertexIter hfv_it = ref_mesh_->hfv_iter(hfh_vec[i]);
		OpenVolumeMesh::Geometry::Vec3d p0 = ref_mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p1 = ref_mesh_->vertex(*hfv_it);
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p2 = ref_mesh_->vertex(*hfv_it);

		double v = OpenVolumeMesh::Geometry::dot(p-p0, OpenVolumeMesh::Geometry::cross(p1-p0,p2-p0));
		if(v < 0) return false;
	}
	return true;
}

int defined_tensor::find_nearest_cell_id(const OpenVolumeMesh::Geometry::Vec3d& p)
{
	ANNpoint tp = annAllocPt(3); tp[0] = p[0]; tp[1] = p[1]; tp[2] = p[2];
	ANNidxArray nnIdx = new ANNidx[1]; ANNdistArray dists = new ANNdist[1];
	kdTree->annkSearch(tp, 1, nnIdx, dists);
	int cell_id = nnIdx[0];
	delete [] nnIdx;

	std::vector<int> visited_cell_id(ref_mesh_->n_cells(), -1);
	visited_cell_id[cell_id] = 1;
	std::queue<int> Q; Q.push(cell_id);
	while (Q.size() > 0)
	{
		cell_id = Q.front(); Q.pop();
		if( check_in_tet(p, cell_id) ) return cell_id;
		OpenVolumeMesh::CellHandle ch(cell_id);
		OpenVolumeMesh::OpenVolumeMeshCell cell = ref_mesh_->cell(ch);
		std::vector<OpenVolumeMesh::HalfFaceHandle> hfh_vec = cell.halffaces();
		for(unsigned i=0;i<hfh_vec.size();++i)
		{
			OpenVolumeMesh::HalfFaceHandle hfh = ref_mesh_->opposite_halfface_handle(hfh_vec[i]);
			OpenVolumeMesh::CellHandle t_ch = ref_mesh_->incident_cell(hfh);
			if(t_ch != VolumeMesh::InvalidCellHandle && visited_cell_id[t_ch.idx()] == -1)
			{
				Q.push( t_ch.idx() ); visited_cell_id[t_ch.idx()] = 1;
			}
		}
	}

	return -1;
}

void defined_tensor::find_cell_bay_cor(const OpenVolumeMesh::Geometry::Vec3d& p,const int& cell_id,
									   OpenVolumeMesh::Geometry::Vec4i& v_id, OpenVolumeMesh::Geometry::Vec4d& b)
{
	OpenVolumeMesh::CellHandle ch(cell_id);
	OpenVolumeMesh::OpenVolumeMeshCell cell = ref_mesh_->cell(ch);
	std::vector<OpenVolumeMesh::HalfFaceHandle> hfh_vec = cell.halffaces(); double sum_b= 0.0;
	for(unsigned i=0;i<hfh_vec.size();++i)
	{
		OpenVolumeMesh::HalfFaceVertexIter hfv_it = ref_mesh_->hfv_iter(hfh_vec[i]);
		OpenVolumeMesh::Geometry::Vec3d p0 = ref_mesh_->vertex(*hfv_it); int v0 = hfv_it->idx();
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p1 = ref_mesh_->vertex(*hfv_it); int v1 = hfv_it->idx();
		++hfv_it; OpenVolumeMesh::Geometry::Vec3d p2 = ref_mesh_->vertex(*hfv_it); int v2 = hfv_it->idx();

		b[i] = OpenVolumeMesh::Geometry::dot(p - p0, OpenVolumeMesh::Geometry::cross(p1-p0,p2-p0));
		sum_b += b[i];
		unsigned j = (i+1) % hfh_vec.size();
		for( hfv_it = ref_mesh_->hfv_iter(hfh_vec[j]); hfv_it; ++hfv_it)
		{
			if(hfv_it->idx() != v0 &&hfv_it->idx() != v1 &&hfv_it->idx() != v2 )
			{
				v_id[i] = hfv_it->idx();
				break;
			}
		}
	}
	b /= sum_b;
}

void defined_tensor::project_for_interior(OpenVolumeMesh::Geometry::Vec3d& p, OpenVolumeMesh::Geometry::Vec6d& M)
{
	//printf("...........\n");
	int cell_id = find_nearest_cell_id(p);
	if(cell_id < 0) printf("%d\n", cell_id);

	OpenVolumeMesh::Geometry::Vec4i v_id; OpenVolumeMesh::Geometry::Vec4d b;
	find_cell_bay_cor(p, cell_id, v_id, b);
	M = tet_vertex_metric[v_id[0]] * b[0] + tet_vertex_metric[v_id[1]] * b[1] + tet_vertex_metric[v_id[2]] * b[2] + tet_vertex_metric[v_id[3]] * b[3];
}


#include "DataStructures.h"
#include "IncompressibleFluid.h"
#include "math.h"
#include "MatrixMath.h"
#include "PolyMath.h"
#include <Eigen/Core>

namespace CoolProp {

/// A thermophysical property provider for all properties
/**
This fluid instance is populated using an entry from a JSON file and uses
simplified polynomial and exponential functions to calculate thermophysical
and transport properties.
*/
//IncompressibleFluid::IncompressibleFluid();

void IncompressibleFluid::validate(){
    return;
    // TODO: Implement validation function

    // u and s have to be of the polynomial type!
    //throw NotImplementedError("TODO");
}

bool IncompressibleFluid::is_pure() {
    if (density.coeffs.cols()==1) return true;
    return false;
}

/// Base exponential function
double IncompressibleFluid::baseExponential(IncompressibleData data, double y, double ybase){
    Eigen::VectorXd coeffs = makeVector(data.coeffs);
    size_t r=coeffs.rows(),c=coeffs.cols();
    if (strict && (r!=3 || c!=1) ) throw ValueError(format("%s (%d): You have to provide a 3,1 matrix of coefficients, not  (%d,%d).",__FILE__,__LINE__,r,c));
    return exp( (double) (coeffs[0] / ( (y-ybase)+coeffs[1] ) - coeffs[2] ) );
}
/// Base exponential function with logarithmic term
double IncompressibleFluid::baseLogexponential(IncompressibleData data, double y, double ybase){
    Eigen::VectorXd coeffs = makeVector(data.coeffs);
    size_t r=coeffs.rows(),c=coeffs.cols();
    if (strict && (r!=3 || c!=1) ) throw ValueError(format("%s (%d): You have to provide a 3,1 matrix of coefficients, not  (%d,%d).",__FILE__,__LINE__,r,c));
    return exp( (double) ( log( (double) (1.0/((y-ybase)+coeffs[0]) + 1.0/((y-ybase)+coeffs[0])/((y-ybase)+coeffs[0]) ) ) *coeffs[1]+coeffs[2] ) );
}

double IncompressibleFluid::basePolyOffset(IncompressibleData data, double y, double z){
    size_t r=data.coeffs.rows(),c=data.coeffs.cols();
    double offset = 0.0;
    double in     = 0.0;
    Eigen::MatrixXd coeffs;
    if (r>0 && c>0) {
        offset = data.coeffs(0,0);
        if (r==1 && c>1) { // row vector -> function of z
            coeffs = Eigen::MatrixXd(data.coeffs.block(0,1,r,c-1));
            in = z;
        } else if (r>1 && c==1) { // column vector -> function of y
            coeffs = Eigen::MatrixXd(data.coeffs.block(1,0,r-1,c));
            in = y;
        } else {
            throw ValueError(format("%s (%d): You have to provide a vector (1D matrix) of coefficients, not  (%d,%d).",__FILE__,__LINE__,r,c));
        }
        return poly.evaluate(coeffs, in, 0, offset);
    }
    throw ValueError(format("%s (%d): You have to provide a vector (1D matrix) of coefficients, not  (%d,%d).",__FILE__,__LINE__,r,c));
}

/// Density as a function of temperature, pressure and composition.
double IncompressibleFluid::rho (double T, double p, double x){
    switch (density.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(density.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(density, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(density, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(density.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(density, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,density.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,density.type));
    }
}

/// Heat capacities as a function of temperature, pressure and composition.
double IncompressibleFluid::c   (double T, double p, double x){
    switch (specific_heat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            //throw NotImplementedError("Here you should implement the polynomial.");
            return poly.evaluate(specific_heat.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,specific_heat.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for specific heat.",__FILE__,__LINE__,specific_heat.type));
    }
}

/// Viscosity as a function of temperature, pressure and composition.
double IncompressibleFluid::visc(double T, double p, double x){
    switch (viscosity.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(viscosity.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(viscosity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(viscosity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(viscosity.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(viscosity, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,viscosity.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,viscosity.type));
    }
}
/// Thermal conductivity as a function of temperature, pressure and composition.
double IncompressibleFluid::cond(double T, double p, double x){
    switch (conductivity.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(conductivity.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(conductivity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(conductivity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(conductivity.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(conductivity, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,conductivity.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,conductivity.type));
    }
}
/// Saturation pressure as a function of temperature and composition.
double IncompressibleFluid::psat(double T,           double x){
    if (T<=this->TminPsat) return 0.0;
    switch (p_sat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(p_sat.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(p_sat, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(p_sat, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(p_sat.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(p_sat, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,p_sat.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,p_sat.type));
    }
}
/// Freezing temperature as a function of pressure and composition.
double IncompressibleFluid::Tfreeze(       double p, double x){
    switch (T_freeze.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(T_freeze.coeffs, p, x, 0, 0, 0.0, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(T_freeze, x, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(T_freeze, x, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(T_freeze.coeffs, p, x, 0, 0, 0.0, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(T_freeze, p, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,T_freeze.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,T_freeze.type));
    }
}

/* Below are direct calculations of the derivatives. Nothing
 * special is going on, we simply use the polynomial class to
 * derive the different functions with respect to temperature.
 */
/// Partial derivative of density with respect to temperature at constant pressure and composition
double IncompressibleFluid::drhodTatPx (double T, double p, double x){
    switch (density.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
        	return poly.derivative(density.coeffs, T, x, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,density.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for density.",__FILE__,__LINE__,density.type));
    }
}
/// Partial derivative of entropy
//  with respect to temperature at constant pressure and composition
//  integrated in temperature
double IncompressibleFluid::dsdTatPxdT(double T, double p, double x){
	switch (specific_heat.type) {
		case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
			return poly.integral(specific_heat.coeffs, T, x, 0, -1, 0, Tbase, xbase);
		case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
			throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,specific_heat.type));
		default:
			throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for entropy.",__FILE__,__LINE__,specific_heat.type));
	}
}
/// Partial derivative of enthalpy
//  with respect to temperature at constant pressure and composition
//  integrated in temperature
double IncompressibleFluid::dhdTatPxdT(double T, double p, double x){
	switch (specific_heat.type) {
		case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
			return poly.integral(specific_heat.coeffs, T, x, 0, 0, 0, Tbase, xbase);
		case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
			throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,specific_heat.type));
		default:
			throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for entropy.",__FILE__,__LINE__,specific_heat.type));
	}
}

/// Mass fraction conversion function
/** If the fluid type is mass-based, it does not do anything. Otherwise,
 *  it converts the mass fraction to the required input. */
double IncompressibleFluid::inputFromMass (double T,     double x){
    if (this->xid==IFRAC_PURE) {
            return _HUGE;
    } else if (this->xid==IFRAC_MASS) {
        return x;
    } else {
        throw NotImplementedError("Mass composition conversion has not been implemented.");
        //switch (mass2input.type) {
        //    case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
        //        return poly.evaluate(mass2input.coeffs, T, x, 0, 0, 0.0, 0.0); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
        //        return baseExponential(mass2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
        //        return baseLogexponential(mass2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
        //        return exp(poly.evaluate(mass2input.coeffs, T, x, 0, 0, 0.0, 0.0)); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
        //        return basePolyOffset(mass2input, T, x);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
        //        throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,mass2input.type));
        //        break;
        //    default:
        //        throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,mass2input.type));
        //        break;
        //}
        //return _HUGE;
    }
}

/// Volume fraction conversion function
/** If the fluid type is volume-based, it does not do anything. Otherwise,
 *  it converts the volume fraction to the required input. */
double IncompressibleFluid::inputFromVolume (double T,   double x){
    if (this->xid==IFRAC_PURE) {
            return _HUGE;
    } else if (this->xid==IFRAC_VOLUME) {
        return x;
    } else {
        throw NotImplementedError("Volume composition conversion has not been implemented.");
        //switch (volume2input.type) {
        //    case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
        //        return poly.evaluate(volume2input.coeffs, T, x, 0, 0, 0.0, 0.0); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
        //        return baseExponential(volume2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
        //        return baseLogexponential(volume2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
        //        return exp(poly.evaluate(volume2input.coeffs, T, x, 0, 0, 0.0, 0.0)); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
        //        return basePolyOffset(volume2input, T, x);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
        //        throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,volume2input.type));
        //        break;
        //    default:
        //        throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,volume2input.type));
        //        break;
        //}
        //return _HUGE;
    }
}

/// Mole fraction conversion function
/** If the fluid type is mole-based, it does not do anything. Otherwise,
 *  it converts the mole fraction to the required input. */
double IncompressibleFluid::inputFromMole (double T,     double x){
    if (this->xid==IFRAC_PURE) {
            return _HUGE;
    } else if (this->xid==IFRAC_MOLE) {
        return x;
    } else {
        throw NotImplementedError("Mole composition conversion has not been implemented.");
        /*
        switch (mole2input.type) {
            case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
                return poly.evaluate(mole2input.coeffs, T, x, 0, 0, 0.0, 0.0); // TODO: make sure Tbase and xbase are defined in the correct way
                break;
            case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
                return baseExponential(mole2input, x, 0.0);
                break;
            case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
                return baseLogexponential(mole2input, x, 0.0);
                break;
            case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
                return exp(poly.evaluate(mole2input.coeffs, T, x, 0, 0, 0.0, 0.0)); // TODO: make sure Tbase and xbase are defined in the correct way
                break;
            case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
                return basePolyOffset(mole2input, T, x);
                break;
            case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
                throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,mole2input.type));
                break;
            default:
                throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,mole2input.type));
                break;
        }
        return _HUGE;
        */
    }
}

/* Some functions can be inverted directly, those are listed
 * here. It is also possible to solve for other quantities, but
 * that involves some more sophisticated processing and is not
 * done here, but in the backend, T(h,p) for example.
 */
/// Temperature as a function of density, pressure and composition.
double IncompressibleFluid::T_rho (double Dmass, double p, double x){
    double d_raw = Dmass; // No changes needed, no reference values...
    switch (density.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.solve_limits(density.coeffs, x, d_raw, Tmin, Tmax, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,specific_heat.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for inverse density.",__FILE__,__LINE__,specific_heat.type));
    }
}
/// Temperature as a function of heat capacities as a function of temperature, pressure and composition.
double IncompressibleFluid::T_c   (double Cmass, double p, double x){
    double c_raw = Cmass; // No changes needed, no reference values...
    switch (specific_heat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.solve_limits(specific_heat.coeffs, x, c_raw, Tmin, Tmax, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,specific_heat.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for inverse specific heat.",__FILE__,__LINE__,specific_heat.type));
    }
}

/*
 * Some more functions to provide a single implementation
 * of important routines.
 * We start with the check functions that can validate input
 * in terms of pressure p, temperature T and composition x.
 */
/// Check validity of temperature input.
/** Compares the given temperature T to the result of a
 *  freezing point calculation. This is not necessarily
 *  defined for all fluids, default values do not cause errors. */
bool IncompressibleFluid::checkT(double T, double p, double x) {
    if (Tmin <= 0.) throw ValueError("Please specify the minimum temperature.");
    if (Tmax <= 0.) throw ValueError("Please specify the maximum temperature.");
    if ((Tmin > T) || (T > Tmax)) throw ValueError(format("Your temperature %f is not between %f and %f.", T, Tmin, Tmax));
    double TF = 0.0;
    if (T_freeze.type!=IncompressibleData::INCOMPRESSIBLE_NOT_SET) TF = Tfreeze(p, x);
    if ( T<TF) throw ValueError(format("Your temperature %f is below the freezing point of %f.", T, TF));
    return true;
}

/// Check validity of pressure input.
/** Compares the given pressure p to the saturation pressure at
 *  temperature T and throws and exception if p is lower than
 *  the saturation conditions.
 *  The default value for psat is -1 yielding true if psat
 *  is not redefined in the subclass.
 *  */
bool IncompressibleFluid::checkP(double T, double p, double x) {
    double ps = 0.0;
    if (p_sat.type!=IncompressibleData::INCOMPRESSIBLE_NOT_SET) ps = psat(T, x);
    if (p < 0.0) throw ValueError(format("You cannot use negative pressures: %f < %f. ", p, 0.0));
    if (ps> 0.0 && p < ps)  throw ValueError(format("Equations are valid for liquid phase only: %f < %f (psat). ", p, ps));
    return true;
}

/// Check validity of composition input.
/** Compares the given composition x to a stored minimum and
 *  maximum value. Enforces the redefinition of xmin and
 *  xmax since the default values cause an error. */
bool IncompressibleFluid::checkX(double x){
    if (xmin < 0.0 || xmin > 1.0) throw ValueError("Please specify the minimum concentration between 0 and 1.");
    if (xmax < 0.0 || xmax > 1.0) throw ValueError("Please specify the maximum concentration between 0 and 1.");
    if ((xmin > x) || (x > xmax)) throw ValueError(format("Your composition %f is not between %f and %f.", x, xmin, xmax));
    return true;
}

} /* namespace CoolProp */

// Testing still needs to be enhanced.
/* Below, I try to carry out some basic tests for both 2D and 1D
 * polynomials as well as the exponential functions for vapour
 * pressure etc.
 */
#ifdef ENABLE_CATCH
#include <math.h>
#include <iostream>
#include "catch.hpp"
#include "TestObjects.h"

Eigen::MatrixXd makeMatrix(const std::vector<double> &coefficients){
    //IncompressibleClass::checkCoefficients(coefficients,18);
    std::vector< std::vector<double> > matrix;
    std::vector<double> tmpVector;

    tmpVector.clear();
    tmpVector.push_back(coefficients[0]);
    tmpVector.push_back(coefficients[6]);
    tmpVector.push_back(coefficients[11]);
    tmpVector.push_back(coefficients[15]);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[1]*100.0);
    tmpVector.push_back(coefficients[7]*100.0);
    tmpVector.push_back(coefficients[12]*100.0);
    tmpVector.push_back(coefficients[16]*100.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[2]*100.0*100.0);
    tmpVector.push_back(coefficients[8]*100.0*100.0);
    tmpVector.push_back(coefficients[13]*100.0*100.0);
    tmpVector.push_back(coefficients[17]*100.0*100.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[3]*100.0*100.0*100.0);
    tmpVector.push_back(coefficients[9]*100.0*100.0*100.0);
    tmpVector.push_back(coefficients[14]*100.0*100.0*100.0);
    tmpVector.push_back(0.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[4]*100.0*100.0*100.0*100.0);
    tmpVector.push_back(coefficients[10]*100.0*100.0*100.0*100.0);
    tmpVector.push_back(0.0);
    tmpVector.push_back(0.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[5]*100.0*100.0*100.0*100.0*100.0);
    tmpVector.push_back(0.0);
    tmpVector.push_back(0.0);
    tmpVector.push_back(0.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    return CoolProp::vec_to_eigen(matrix).transpose();
}

TEST_CASE("Internal consistency checks and example use cases for the incompressible fluids","[IncompressibleFluids]")
{
    bool PRINT = false;
    std::string tmpStr;
    std::vector<double> tmpVector;
    std::vector< std::vector<double> > tmpMatrix;

    SECTION("Test case for \"SylthermXLT\" by Dow Chemicals") {

        std::vector<double> cRho;
        cRho.push_back(+1.1563685145E+03);
        cRho.push_back(-1.0269048032E+00);
        cRho.push_back(-9.3506079577E-07);
        cRho.push_back(+1.0368116627E-09);
        CoolProp::IncompressibleData density;
        density.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL;
        density.coeffs = CoolProp::vec_to_eigen(cRho);

        std::vector<double> cHeat;
        cHeat.push_back(+1.1562261074E+03);
        cHeat.push_back(+2.0994549103E+00);
        cHeat.push_back(+7.7175381057E-07);
        cHeat.push_back(-3.7008444051E-20);
        CoolProp::IncompressibleData specific_heat;
        specific_heat.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL;
        specific_heat.coeffs = CoolProp::vec_to_eigen(cHeat);

        std::vector<double> cCond;
        cCond.push_back(+1.6121957379E-01);
        cCond.push_back(-1.3023781944E-04);
        cCond.push_back(-1.4395238766E-07);
        CoolProp::IncompressibleData conductivity;
        conductivity.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL;
        conductivity.coeffs = CoolProp::vec_to_eigen(cCond);

        std::vector<double> cVisc;
        cVisc.push_back(+1.0337654989E+03);
        cVisc.push_back(-4.3322764383E+01);
        cVisc.push_back(+1.0715062356E+01);
        CoolProp::IncompressibleData viscosity;
        viscosity.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL;
        viscosity.coeffs = CoolProp::vec_to_eigen(cVisc);

        CoolProp::IncompressibleFluid XLT;
        XLT.setName("XLT");
        XLT.setDescription("SylthermXLT");
        XLT.setReference("Dow Chemicals data sheet");
        XLT.setTmax(533.15);
        XLT.setTmin(173.15);
        XLT.setxmax(0.0);
        XLT.setxmin(0.0);
        XLT.setTminPsat(533.15);

        XLT.setTbase(0.0);
        XLT.setxbase(0.0);

        /// Setters for the coefficients
        XLT.setDensity(density);
        XLT.setSpecificHeat(specific_heat);
        XLT.setViscosity(viscosity);
        XLT.setConductivity(conductivity);
        //XLT.setPsat(parse_coefficients(fluid_json, "saturation_pressure", false));
        //XLT.setTfreeze(parse_coefficients(fluid_json, "T_freeze", false));
        //XLT.setVolToMass(parse_coefficients(fluid_json, "volume2mass", false));
        //XLT.setMassToMole(parse_coefficients(fluid_json, "mass2mole", false));

        /// A function to check coefficients and equation types.
        //XLT.validate();
        double acc = 0.0001;
        double val = 0;
        double res = 0;

        // Prepare the results and compare them to the calculated values
        double T = 273.15+50;
        double p = 10e5;
        double x = 0.0;

        // Compare density
        val = 824.4615702148608;
        res = XLT.rho(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(val);
        CAPTURE(res);
        CHECK( check_abs(val,res,acc) );
        }

        // Compare cp
        val = 1834.7455527670554;
        res = XLT.c(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(val);
        CAPTURE(res);
        CHECK( check_abs(val,res,acc) );
        }

        // Check property functions
        CHECK_THROWS(XLT.s(T,p,x));
        CHECK_THROWS(XLT.h(T,p,x));
        CHECK_THROWS(XLT.u(T,p,x));

        // Compare v
        val = 0.0008931435169681835;
        res = XLT.visc(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(val);
        CAPTURE(res);
        CHECK( check_abs(val,res,acc) );
        }

        // Compare l
        val = 0.10410086156049088;
        res = XLT.cond(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(val);
        CAPTURE(res);
        CHECK( check_abs(val,res,acc) );
        }
    }

    SECTION("Test case for Methanol from SecCool") {

        CoolProp::IncompressibleFluid CH3OH = CoolPropTesting::incompressibleFluidObject();

        // Prepare the results and compare them to the calculated values
        double acc = 0.0001;
        double T   = 273.15+10;
        double p   = 10e5;
        double x   = 0.25;
        double expected = 0;
        double actual = 0;

        // Compare density
        expected = 963.2886528091547;
        actual = CH3OH.rho(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(p);
        CAPTURE(x);
        CAPTURE(expected);
        CAPTURE(actual);
        CHECK( check_abs(expected,actual,acc) );
        }

        // Compare cp
        expected = 3993.9748117022423;
        actual = CH3OH.c(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(p);
        CAPTURE(x);
        CAPTURE(expected);
        CAPTURE(actual);
        CHECK( check_abs(expected,actual,acc) );
        }

        // Check property functions
        CHECK_THROWS(CH3OH.s(T,p,x));
        CHECK_THROWS(CH3OH.h(T,p,x));
        CHECK_THROWS(CH3OH.u(T,p,x));

        // Compare v
        expected = 0.0023970245009602097;
        actual = CH3OH.visc(T,p,x)/1e3;
        {
        CAPTURE(T);
        CAPTURE(p);
        CAPTURE(x);
        CAPTURE(expected);
        CAPTURE(actual);
        std::string errmsg = CoolProp::get_global_param_string("errstring");
        CAPTURE(errmsg);
        CHECK( check_abs(expected,actual,acc) );
        }

        // Compare conductivity
        expected = 0.44791148414693727;
        actual = CH3OH.cond(T,p,x);
        {
        CAPTURE(T);
        CAPTURE(p);
        CAPTURE(x);
        CAPTURE(expected);
        CAPTURE(actual);
        std::string errmsg = CoolProp::get_global_param_string("errstring");
        CAPTURE(errmsg);
        CHECK( check_abs(expected,actual,acc) );
        }

        // Compare Tfreeze
        expected = -20.02+273.15;// 253.1293105454671;
        actual = CH3OH.Tfreeze(p,x);
        {
        CAPTURE(T);
        CAPTURE(p);
        CAPTURE(x);
        CAPTURE(expected);
        CAPTURE(actual);
        std::string errmsg = CoolProp::get_global_param_string("errstring");
        CAPTURE(errmsg);
        CHECK( check_abs(expected,actual,acc) );
        }

    }

}

#endif /* ENABLE_CATCH */


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_ITERATIVE_CLOSEST_POINT_2D_HPP
#define PIC_COMPUTER_VISION_ITERATIVE_CLOSEST_POINT_2D_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../util/math.hpp"

#include "../features_matching/brief_descriptor.hpp"

#include "../util/eigen_util.hpp"

#ifndef PIC_DISABLE_EIGEN
    #ifndef PIC_EIGEN_NOT_BUNDLED
        #include "../externals/Eigen/Dense"
        #include "../externals/Eigen/SVD"
        #include "../externals/Eigen/Geometry"
    #else
        #include <Eigen/Dense>
        #include <Eigen/SVD>
        #include <Eigen/Geometry>
    #endif
#endif

namespace pic {
#ifndef PIC_DISABLE_EIGEN

/**
 * @brief getMean
 * @param p
 * @return
 */
PIC_INLINE Eigen::Vector2f getMeanVector2f(std::vector< Eigen::Vector2f > &p)
{
    auto c = p[0];
    for(unsigned int i = 1; i < p.size(); i++) {
        c += p[i];
    }
    c /= float(p.size());

    return c;
}

/**
 * @brief getMedianVector2f
 * @param p
 * @return
 */
PIC_INLINE Eigen::Vector2f getMedianVector2f(std::vector< Eigen::Vector2f > &p)
{
    auto n = p.size();
    float *x = new float[n];
    float *y = new float[n];

    for(unsigned int i = 0; i < n; i++) {
        x[i] = p[i][0];
        y[i] = p[i][1];
    }

    std::sort(x, x + n);
    std::sort(y, y + n);

    Eigen::Vector2f med;

    med[0] = x[n >> 1];
    med[1] = y[n >> 1];

#ifdef PIC_DEBUG
    printf("%f %f\n", med[0], med[1]);
#endif

    delete[] x;
    delete[] y;
    return med;
}

class ICP2DTransform
{
public:
    Eigen::Matrix2f R;
    Eigen::Vector2f t;
    float scale;

    ICP2DTransform()
    {
        R.setIdentity();

        t.setZero();

        scale = 1.0f;
    }

    ICP2DTransform(float tx, float ty, float angle, float scale)
    {
        this->scale = scale;
        t[0] = tx;
        t[1] = ty;

        float cos_a = cosf(angle);
        float sin_a = sinf(angle);

        R(0, 0) =  cos_a;
        R(0, 1) = -sin_a;
        R(1, 0) =  sin_a;
        R(1, 1) =  cos_a;
    }

    void print()
    {
        printf("R:\n %f %f\n %f %f\n", R(0,0), R(0,1), R(1,0), R(1,1));

        printf("T: %f %f\n", t[0], t[1]);

        printf("S: %f\n\n", scale);
    }

    void apply(std::vector< Eigen::Vector2f > &points) {
        //apply transform
        for(unsigned int i  = 0; i < points.size(); i++) {
            Eigen::Vector2f tmp = points[i];
            points[i] = ((R * tmp) + t) * scale;
        }
    }

    void apply(std::vector< Eigen::Vector2f > &points,
               std::vector< Eigen::Vector2f > &out) {
        //apply transform
        for(unsigned int i  = 0; i < points.size(); i++) {
            Eigen::Vector2f tmp = ((R * points[i]) + t) * scale;
            out.push_back(tmp);
        }
    }

    //
    //
    //

    void applyC(std::vector< Eigen::Vector2f > &points) {

        //compute centroid to points
        Eigen::Vector2f c = getMeanVector2f(points);
        auto shift = c + t;

        //apply transform
        for(unsigned int i  = 0; i < points.size(); i++) {
            Eigen::Vector2f tmp = points[i] - c;
            points[i] = (R * tmp) * scale + shift;
        }
    }

    void applyC(std::vector< Eigen::Vector2f > &points,
               std::vector< Eigen::Vector2f > &out) {

        //compute centroid to points
        Eigen::Vector2f c = getMeanVector2f(points);
        auto shift = c + t;

        //apply transform
        for(unsigned int i  = 0; i < points.size(); i++) {
            Eigen::Vector2f tmp = points[i] - c;
            Eigen::Vector2f tmp2 = (R * tmp) * scale + shift;
            out.push_back(tmp2);
        }
    }
};

/**
 * @brief estimateRotatioMatrixAndTranslation
 * @param p0
 * @param p1
 * @param p0_descs
 * @param p1_descs
 * @param ind
 * @return
 */
PIC_INLINE ICP2DTransform estimateRotatioMatrixAndTranslation(std::vector< Eigen::Vector2f > &p0,
                                                   std::vector< Eigen::Vector2f > &p1,
                                                   std::vector< unsigned int *> &p0_descs,
                                                   std::vector< unsigned int *> &p1_descs,
                                                   int size_descs,
                                                   int *ind = NULL)
{
    ICP2DTransform ret;

    if(p0.size() < 2 || p1.size() < 2) {
        return ret;
    }

    bool bFlag = false;
    if(ind == NULL) {
        ind = new int[p1.size()];
        bFlag = true;
    }

    //compute c0
    Eigen::Vector2f c1 = getMeanVector2f(p1);

    //compute c1
    Eigen::Vector2f c0;
    c0.setZero();
    int n = 0;

#ifdef PIC_DEBUG
    printf("Size: %d\n", size_descs);
#endif

    for(int i = 0; i < p1.size(); i++) {
        auto p_i = p1[i];

        float d_min = FLT_MAX;
        int index = -1;
        for(int j = 0; j < p0.size(); j++) {
            auto delta_ij = p_i - p0[j];
            float d_tmp = delta_ij.norm();

            int value = BRIEFDescriptor::match(p0_descs[j], p1_descs[i], size_descs);
            d_tmp += float(size_descs * 32) - float(value);

            if(d_tmp < d_min) {
                d_min = d_tmp;
                index = j;
            }

        }

        if(index > -1) {
            ind[i] = index;
            c0 += p0[index];
            n++;
        }
    }
    c0 /= float(n);

    //compute R
    Eigen::Matrix2f H;
    H.setZero();

    for(unsigned int i = 0; i < p1.size(); i++) {
        int j = ind[i];

        auto t0 = p0[j] - c0;
        auto t1 = p1[i] - c1;

        Eigen::RowVector2f t1r = t1;
        Eigen::Matrix2f tmp = t0 * t1r;

/*      tmp(0, 0) = t0(0) * t1(0);
        tmp(0, 1) = t0(0) * t1(1);
        tmp(1, 0) = t0(1) * t1(0);
        tmp(1, 1) = t0(1) * t1(1);*/
        H += tmp;
    }

    //SVD decomposition
    Eigen::JacobiSVD< Eigen::Matrix2f > svd(H, Eigen::ComputeFullV | Eigen::ComputeFullU);
    Eigen::Matrix2f U = svd.matrixU();
    Eigen::Matrix2f V = svd.matrixV();

    Eigen::Matrix2f U_t = U.transpose();
    Eigen::Matrix2f R = V * U_t;

    if(R.determinant() < 0.0f) {
        for(unsigned int i = 0; i < V.rows(); i++) {
            V(i, 1) = -V(i, 1);
        }

        R = V * U_t;
    }

    ret.R = R;
    ret.t = c0 - (ret.R * c1);

    if(bFlag) {
        delete[] ind;
    }

    return ret;
}

/**
 * @brief getErrorPointsList
 * @param p0
 * @param p1
 * @return
 */
PIC_INLINE float getErrorPointsList(std::vector< Eigen::Vector2f > &p0,
                         std::vector< Eigen::Vector2f > &p1)
{
    float err = 0.0f;
    for(unsigned int i = 0; i < p0.size(); i++) {
        auto p_i = p0[i];

        float tmp_err = FLT_MAX;
        for(unsigned int j = 0; j < p1.size(); j++) {
            auto delta_ij = p_i - p1[j];
            float dist = delta_ij.norm();

            if(dist < tmp_err) {
                tmp_err = dist;
            }
        }

        err += tmp_err;
    }

    return err / float(p0.size());
}

/**
 * @brief iterativeClosestPoints2D
 * @param points_pattern
 * @param points
 * @param points_pattern_descs
 * @param points_descs
 * @param thresholdErr
 * @param maxIterations
 */
PIC_INLINE void iterativeClosestPoints2D(std::vector<Eigen::Vector2f> &points_pattern,
                              std::vector<Eigen::Vector2f> &points,
                              std::vector< unsigned int *> &points_pattern_descs,
                              std::vector< unsigned int *> &points_descs,
                              int size_descs,
                              int maxIterations = 1000)
{
    ICP2DTransform t_init;
    t_init.t = getMedianVector2f(points) - getMeanVector2f(points_pattern);
    t_init.apply(points_pattern);

    float err = getErrorPointsList(points_pattern, points);;
    float prev_err = 1e32f;
    int iter = 0;
    while(iter < maxIterations) {
        prev_err = err;
        ICP2DTransform t = estimateRotatioMatrixAndTranslation(points, points_pattern,
                                                               points_descs, points_pattern_descs,
                                                               size_descs);

#ifdef PIC_DEBUG
        t.print();
#endif

//        std::vector< Eigen::Vector2f > points_pattern_tmp;
        t.apply(points_pattern);

        err = getErrorPointsList(points_pattern, points);

        /*
        if(err < prev_err) {
            points_pattern.clear();
            std::copy(points_pattern_tmp.begin(), points_pattern_tmp.end(),
                      std::back_inserter(points_pattern));
        } else {
            iter = maxIterations;
        }
        */

        #ifdef PIC_DEBUG
            printf("Error: %f %f\n", err, prev_err);
        #endif

        iter++;
    }
}

#endif
} // end namespace pic

#endif // PIC_COMPUTER_VISION_ITERATIVE_CLOSEST_POINT_2D_HPP


/**
 * Kalman filter implementation using Eigen. Based on the following
 * introductory paper:
 *
 *     http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
 *
 * @author: Hayk Martirosyan
 * @date: 2014.11.15
 */

#include <Eigen/Dense>
#include <mutex>

using namespace std;

#pragma once

class KalmanFilter {

public:

    /**
     * Create a Kalman filter with the specified matrices.
     *   A - System dynamics matrix
     *   C - Output matrix
     *   Q - Process noise covariance -> W
     *   R - Measurement noise covariance -> V (symétrique)
     *   P - Estimate error covariance
     */
    KalmanFilter(
                 double dt,
                 double Fobj,
                 double L,
                 double k,
                 double v_robot,
                 const Eigen::VectorXd& u,
                 const Eigen::MatrixXd& A, // matrice de doubles de taille non définie
                 const Eigen::MatrixXd& C,
                 const Eigen::MatrixXd& Q,
                 const Eigen::MatrixXd& R,
                 const Eigen::MatrixXd& P
                 );

    // Create a blank estimator.
    KalmanFilter();

    // Create a Kalman filter adapted for the specific robot case
    KalmanFilter setRobotKalman(double stepTime, double ForceObjective);

    // Initialize the filter with initial states as zero.
    void init();

    // Initialize the filter with a guess for initial states.
    void init(double t0, const Eigen::VectorXd& x0);

    /* Update the estimated state based on measured values. The
     time step is assumed to remain constant.*/
    double update(double a, double b);

    // Return the current state or time.
    Eigen::VectorXd getState() { return x_hat; };
    double getTime() { return t; };

private:

    // Matrices for computation
    Eigen::MatrixXd A, C, W, V, P, K, P0;

    // System dimensions
    int m, n, c;

    // Initial and current time
    double t0, t;

    // Discrete time step
    double dt;

    // Force objective
    double Fobj;

    // Lenght of tool (specific to the robot)
    double L;

    //stiffness of the tool
    double k;

    // Command vector
    Eigen::VectorXd u;

    // Speed of the robot
    double v_robot;

    // Is the filter initialized?
    bool initialized;

    // n-size identity
    Eigen::MatrixXd I;

    // Estimated states
    Eigen::VectorXd x_hat, x_hat_new;
};