#include "SchaeferNewtonParametrizer.h"

#include "eigen_stl_utils.h"
#include <Eigen/SparseCholesky>
#include <igl/cotmatrix_entries.h>
#include <igl/doublearea.h>
#include <iostream>

#include "../ScafData.h"

//#undef NDEBUG
#include <assert.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
//#define NDEBUG

DECLARE_DIFFSCALAR_BASE();

using namespace std;
SchaeferNewtonParametrizer::SchaeferNewtonParametrizer(ScafData &_sd)
		: has_precomputed(false), d_(_sd) {
	// empty
}

void SchaeferNewtonParametrizer::newton_iteration(const Eigen::MatrixXi &F,
												  Eigen::MatrixXd &uv) {
	// flat uv (x1,y1,x2,y2,...)
	cout << "computing grad and hessian" << endl;
	Eigen::VectorXd x; mat2_to_vec(uv,x);
	// compute energy for hessian grad calculation
	//DScalar Fx;// = 0;
	Eigen::VectorXd grad; Eigen::SparseMatrix<double> hessian;
	double energy = compute_energy_gradient_hessian(F,
													uv,
													grad,
													hessian);

	// Grad and hessian finite-diff check (super slow, only perform on very small meshes)
	/*
	bool grad_ok = check_gradient(V,F,x,grad);
	cout << "grad ok = " << grad_ok << endl;
	assert(grad_ok);
	Eigen::MatrixXd dense_hessian(hessian);
	bool hessian_ok = checkHessian(V,F,x,dense_hessian,0);
	cout << "hessian ok = " << hessian_ok << endl;
	assert(hessian_ok);
	*/

	// perform newton iteration
	cout << "performing newton iteration" << endl;

// Modified by Zhongshi at May 11, 2017 to keep boundary
	// The flattening order is different from the rest!!!
#define KEEP_BND
#ifdef KEEP_BND
	const auto& bnd_ids = d_.frame_ids;
	using namespace Eigen;

	auto bnd_n = bnd_ids.size(); assert(bnd_n > 0);
	MatrixXd bnd_pos;
	int dim = 2;
	int v_n = d_.w_uv.rows();
	igl::slice(d_.w_uv, bnd_ids, 1, bnd_pos);

	VectorXi known_ids(bnd_n * dim);
	VectorXi unknown_ids((v_n - bnd_n) * dim);

	{ // get the complement of bnd_ids.
		int assign = 0, i = 0;
		for (int get = 0; i < v_n && get < bnd_ids.size(); i++) {
			if (bnd_ids(get) == i) get++;
			else unknown_ids(2*(assign++)) = 2*i;
		}
		while (i < v_n) unknown_ids(2*(assign++)) = 2*(i++);
		assert(assign + bnd_ids.size() == v_n);
	}

	VectorXd known_pos(bnd_n * dim);
	  for(int i=0; i<bnd_n; i++) {
		  known_ids(i * 2) = 2*bnd_ids(i);
		  known_ids(i * 2 + 1) = 2*bnd_ids(i) + 1;
	  }
	for(int i=0; i<v_n - bnd_n; i++) {
		unknown_ids(i*2 + 1) = unknown_ids(i*2) + 1;
	}

	Eigen::SparseMatrix<double> hessian_unknown;
  	igl::slice(hessian, unknown_ids, unknown_ids, hessian_unknown);

	Eigen::VectorXd grad_unknown;
	igl::slice(grad, unknown_ids, 1, grad_unknown);

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
	solver.compute(hessian_unknown);
	if(solver.info()!=Eigen::Success) {
		cout << "Eigen Failure!" << endl;
		exit(1);
	}
	Eigen::VectorXd res = solver.solve(grad_unknown);
	VectorXd Uc = VectorXd::Zero(2* v_n);
	igl::slice_into(res, unknown_ids.matrix(), 1, Uc);
	x -= Uc;
#else

	Eigen::SparseLU<Eigen::SparseMatrix<double> > solver;
  int n = hessian.rows();
  Eigen::SparseMatrix<double> id(n,n); id.setIdentity();
//	solver.compute(hessian + (1e-5) * id);
  solver.compute(hessian);
	if(solver.info()!=Eigen::Success) {
		cout << "Eigen Failure!" << endl;
        exit(1);
	}
	Eigen::VectorXd res = solver.solve(grad);
	x -= res;
#endif
	// unflatten uv
	vec_to_mat2(x,uv);
}

double SchaeferNewtonParametrizer::evaluate_energy(const Eigen::MatrixXi &F,
												   Eigen::MatrixXd &uv) {
	precompute(F);
	double energy = 0;

	for (int f_idx = 0; f_idx < F.rows(); f_idx++) {
		int v_1 = F(f_idx, 0);
		int v_2 = F(f_idx, 1);
		int v_3 = F(f_idx, 2);

		// compute current triangle squared area
		auto x1 = uv(v_1, 0);
		auto y1 = uv(v_1, 1);
		auto x2 = uv(v_2, 0);
		auto y2 = uv(v_2, 1); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);
		auto x3 = uv(v_3, 0);
		auto y3 = uv(v_3, 1); //DScalar x0(F(f,0),0); DScalar y0(F(f,
		// 0),1);

		auto rx = x1 - x3;//uv(F(f,0),0)-uv(F(f,2),0);
		auto sx = x2 - x3;//uv(F(f,1),0)-uv(F(f,2),0);
		auto ry = y1 - y3;//uv(F(f,0),1)-uv(F(f,2),1);
		auto sy = y2 - y3;//uv(F(f,1),1)-uv(F(f,2),1);
		auto dblAd = rx * sy - ry * sx;
		auto uv_sqrt_dbl_area = dblAd * dblAd;

		auto l_part = (1 / (m_dblArea_orig(f_idx)) + (m_dblArea_orig(f_idx)
				/ uv_sqrt_dbl_area)) *
				m_dbl_area_weight(f_idx);

		//DScalar part_1 = (uv.row(v_3)-uv.row(v_1)).squaredNorm() * m_cached_edges_1[f_idx];
		auto part_1 =
				(pow(x3 - x1, 2) + pow(y3 - y1, 2)) * m_cached_edges_1[f_idx];
		//part_1 += (uv.row(v_2)-uv.row(v_1)).squaredNorm()* m_cached_edges_2[f_idx];
		part_1 += (pow(x2 - x1, 2) + pow(y2 - y1, 2)) * m_cached_edges_2[f_idx];
		part_1 /= (2 * m_dblArea_orig(f_idx));

		//DScalar part_2_1 = (uv.row(v_3)-uv.row(v_1)).dot(uv.row(v_2)-uv.row(v_1));
		auto part_2_1 = (x3 - x1) * (x2 - x1) + (y3 - y1) * (y2 - y1);
		double part_2_2 = m_cached_dot_prod[f_idx];
		auto part_2 = -(part_2_1 * part_2_2) / (m_dblArea_orig(f_idx));

		auto r_part = part_1 + part_2;

		energy += l_part * r_part;
	}
	return energy;
}

double SchaeferNewtonParametrizer::compute_energy_gradient_hessian(const Eigen::MatrixXi &F,
																   Eigen::MatrixXd &uv,
																   Eigen::VectorXd &grad,
																   Eigen::SparseMatrix<double> &hessian) {

	// can save some computation time
	hessian.resize(2*d_.v_num,2*d_.v_num);
	hessian.reserve(10*2*d_.v_num);
	std::vector<Eigen::Triplet<double> > IJV;//(10*2*6*d_.v_num);
	IJV.reserve(36*F.rows());
	grad.resize(2*d_.v_num); grad.setZero();

	precompute(F); // precompute if needed
	// uv is arranged by (x1,y1,x2,y2,...)
	double energy = 0;
	for (int i = 0; i < F.rows(); i++) {
		DiffScalarBase::setVariableCount(6); // 3 vertices with 2 rows for each
		auto l_part = compute_face_energy_left_part(F, uv, i);
		auto r_part =
				compute_face_energy_right_part(F, uv, i);

		auto temp = l_part * r_part;
		energy += temp.getValue();

		Eigen::VectorXd local_grad = temp.getGradient();
		for (int v_i = 0; v_i < 3; v_i++) {
			int v_global = F(i,v_i);

			grad(v_global*2) = grad(v_global*2) + local_grad(v_i*2); // x
			grad(v_global*2+1) = grad(v_global*2+1) + local_grad(v_i*2+1); // y
		}

		Eigen::MatrixXd local_hessian = temp.getHessian();
//              Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> es(local_hessian);
//              Eigen::MatrixXd D = es.eigenvalues();
//              Eigen::MatrixXd U = es.eigenvectors();
//              for (int i = 0; i < 6; i++)
//                      D(i) = (D(i) < 0) ? 0 : D(i);
//              local_hessian = U * D.asDiagonal()* U.inverse();
		for (int v1 = 0; v1 < 6; v1++) {
			for (int v2 = 0; v2 < 6; v2++) {
				int v1_global = F(i,v1/2)*2 + v1%2;
				int v2_global = F(i,v2/2)*2 + v2%2;

				IJV.push_back(Eigen::Triplet<double>(v1_global,v2_global, local_hessian(v1,v2)));
			}
		}
	}
	hessian.setFromTriplets(IJV.begin(),IJV.end());
	return energy;
}

DScalar SchaeferNewtonParametrizer::compute_face_energy_left_part(const
																  Eigen::MatrixXi &F,
																  const Eigen::MatrixXd &uv,
																  int f_idx) {

	int v_1 = F(f_idx,0); int v_2 = F(f_idx,1); int v_3 = F(f_idx,2);

	// compute current triangle squared area
	DScalar x1(0*2,uv(v_1,0)); DScalar y1(0*2+1,uv(v_1,1)); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);
	DScalar x2(1*2,uv(v_2,0)); DScalar y2(1*2+1,uv(v_2,1)); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);
	DScalar x3(2*2,uv(v_3,0)); DScalar y3(2*2+1,uv(v_3,1)); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);

    auto rx = x1-x3;//uv(F(f,0),0)-uv(F(f,2),0);
    auto sx = x2-x3;//uv(F(f,1),0)-uv(F(f,2),0);
    auto ry = y1-y3;//uv(F(f,0),1)-uv(F(f,2),1);
    auto sy = y2-y3;//uv(F(f,1),1)-uv(F(f,2),1);
    auto dblAd = rx*sy - ry*sx;
	auto uv_sqrt_dbl_area = dblAd*dblAd;

    return (1/(m_dblArea_orig(f_idx)) + (m_dblArea_orig(f_idx)
			/uv_sqrt_dbl_area)) *
			m_dbl_area_weight(f_idx);
}

DScalar SchaeferNewtonParametrizer::compute_face_energy_right_part
		(const Eigen::MatrixXi &F, const Eigen::MatrixXd &uv, int f_idx) {
	int v_1 = F(f_idx,0); int v_2 = F(f_idx,1); int v_3 = F(f_idx,2);

	DScalar x1(0*2,uv(v_1,0)); DScalar y1(0*2+1,uv(v_1,1)); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);
	DScalar x2(1*2,uv(v_2,0)); DScalar y2(1*2+1,uv(v_2,1)); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);
	DScalar x3(2*2,uv(v_3,0)); DScalar y3(2*2+1,uv(v_3,1)); //DScalar x0(F(f,0),0); DScalar y0(F(f,0),1);

	//DScalar part_1 = (uv.row(v_3)-uv.row(v_1)).squaredNorm() * m_cached_edges_1[f_idx];
	auto part_1 =  ( pow(x3-x1,2) + pow(y3-y1,2) ) * m_cached_edges_1[f_idx];
	//part_1 += (uv.row(v_2)-uv.row(v_1)).squaredNorm()* m_cached_edges_2[f_idx];
	part_1 += ( pow(x2-x1,2) + pow(y2-y1,2) ) * m_cached_edges_2[f_idx];
	part_1 /= (2*m_dblArea_orig(f_idx));

	//DScalar part_2_1 = (uv.row(v_3)-uv.row(v_1)).dot(uv.row(v_2)-uv.row(v_1));
	auto part_2_1 = (x3-x1) * (x2-x1) + (y3-y1) * (y2-y1);
	double part_2_2 = m_cached_dot_prod[f_idx];
	auto part_2 = -(part_2_1 * part_2_2)/ (m_dblArea_orig(f_idx));

	return part_1+part_2;
}

void SchaeferNewtonParametrizer::precompute(const Eigen::MatrixXi &F) {
	using namespace Eigen;
	if (!has_precomputed) {

//    	igl::doublearea(V,F, m_dblArea_orig);
		m_dblArea_orig.resize(d_.f_num);
		m_dblArea_orig.head(d_.mf_num) = d_.m_M*2;

		VectorXd scaf_area;
		igl::doublearea(d_.w_uv, d_.s_T, scaf_area);
		m_dblArea_orig.tail(d_.sf_num) = scaf_area;

		m_dbl_area_weight = m_dblArea_orig;
		m_dbl_area_weight.tail(d_.sf_num) = d_.s_M*2;

		//m_cached_l_energy_per_face.resize(F.rows());
		//m_cached_r_energy_per_face.resize(F.rows());
		assert(F.rows() == d_.f_num);

		m_cached_edges_1.resize(F.rows());
		m_cached_edges_2.resize(F.rows());
		m_cached_dot_prod.resize(F.rows());

      	auto V = d_.m_V;
		for (int f = 0; f < d_.mf_num; f++) {
			int v_1 = F(f, 0);
			int v_2 = F(f, 1);
			int v_3 = F(f, 2);

			m_cached_edges_1[f] = (V.row(v_2) - V.row(v_1)).squaredNorm();
			m_cached_edges_2[f] = (V.row(v_3) - V.row(v_1)).squaredNorm();
			m_cached_dot_prod[f] =
					(V.row(v_3) - V.row(v_1)).dot(V.row(v_2) - V.row(v_1));
		}

		V = d_.w_uv;

      double min_bnd_edge_len = INFINITY;
      int acc_bnd = 0;
      for(int i=0; i<d_.bnd_sizes.size(); i++) {
        int current_size = d_.bnd_sizes[i];

        for(int e=acc_bnd; e<acc_bnd + current_size - 1; e++) {
          min_bnd_edge_len = (std::min)(min_bnd_edge_len,
                                      (d_.w_uv.row(d_.internal_bnd(e)) -
                                          d_.w_uv.row(d_.internal_bnd(e+1)))
                                          .squaredNorm());
        }
        min_bnd_edge_len = (std::min)(min_bnd_edge_len,
                                    (d_.w_uv.row(d_.internal_bnd(acc_bnd)) -
                                        d_.w_uv.row(d_.internal_bnd(acc_bnd +current_size -
                                            1))).squaredNorm());
        acc_bnd += current_size;
      }

      std::cout<<"MinBndEdge"<<min_bnd_edge_len<<std::endl;
      double area_threshold = min_bnd_edge_len/4.0;

		for(int f=d_.mf_num; f< d_.f_num; f++) {
			int v_1 = F(f, 0);
			int v_2 = F(f, 1);
			int v_3 = F(f, 2);

			if(m_dblArea_orig(f) <= area_threshold)
			{
				m_dblArea_orig(f) = area_threshold;
				auto dblA = m_dblArea_orig(f);
				double h = sqrt((dblA) / sin(
						M_PI / 3.0));
				Eigen::Vector3d v1, v2, v3;
				v1 << 0, 0, 0;
				v2 << h, 0, 0;
				v3 << h / 2., (sqrt(3) / 2.) * h, 0;

				m_cached_edges_1[f] = (v2 - v1).squaredNorm();
				m_cached_edges_2[f] = (v3 - v1).squaredNorm();
				m_cached_dot_prod[f] =(v3 - v1).dot(v2-v1);
			} else {
				m_cached_edges_1[f] = (V.row(v_2) - V.row(v_1)).squaredNorm();
				m_cached_edges_2[f] = (V.row(v_3) - V.row(v_1)).squaredNorm();
				m_cached_dot_prod[f] =
						(V.row(v_3) - V.row(v_1)).dot(V.row(v_2) - V.row(v_1));
			}
		}

		has_precomputed = true;
	}
}

double energy_value(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const Eigen::VectorXd& xx) {
	Eigen::MatrixXd mat; vec_to_mat2(xx,mat);
	assert(false && "Seems to be only used in finite verification");
	return 0;
}

void SchaeferNewtonParametrizer::finiteGradient(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
				const Eigen::VectorXd &x, Eigen::VectorXd &grad, int accuracy) {
    // accuracy can be 0, 1, 2, 3

    const double eps = 2.2204e-8;
    const size_t D = x.rows();
    const int idx = (accuracy-3)/2;
    const std::vector< std::vector <double>> coeff =
    { {1, -1}, {1, -8, 8, -1}, {-1, 9, -45, 45, -9, 1}, {3, -32, 168, -672, 672, -168, 32, -3} };
    const std::vector< std::vector <double>> coeff2 =
    { {1, -1}, {-2, -1, 1, 2}, {-3, -2, -1, 1, 2, 3}, {-4, -3, -2, -1, 1, 2, 3, 4} };
    const std::vector<double> dd = {2, 12, 60, 840};

    Eigen::VectorXd finiteDiff(D);
    for (size_t d = 0; d < D; d++) {
      finiteDiff[d] = 0;
      for (int s = 0; s < 2*(accuracy+1); ++s)
      {
        Eigen::VectorXd xx = x.eval();
        xx[d] += coeff2[accuracy][s]*eps;

        finiteDiff[d] += coeff[accuracy][s]*energy_value(V,F,xx);
      }
      finiteDiff[d] /= (dd[accuracy]* eps);
    }
    grad = finiteDiff;
  }

bool SchaeferNewtonParametrizer::check_gradient(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
				const Eigen::VectorXd& x, const Eigen::VectorXd& actual_grad, int accuracy) {
   	const int D = x.rows();
    Eigen::VectorXd expected_grad(D);
    cout << "computing finite gradient" << endl;
    finiteGradient(V,F,x, expected_grad, accuracy);
    cout << "done computing finite gradient" << endl;

    bool correct = true;

    for (int d = 0; d < D; ++d) {
      double scale = (std::max)((std::max)(fabs(actual_grad[d]), fabs(expected_grad[d])), 1.);
      if(fabs(actual_grad[d]-expected_grad[d])>1e-2 * scale)
        correct = false;
    	break;
    }
    return correct;
}

 void SchaeferNewtonParametrizer::finiteHessian(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
 			const Eigen::VectorXd & x, Eigen::MatrixXd & hessian, int accuracy) {
    const double eps = 2.2204e-08;
    const size_t DIM = x.rows();

    if(accuracy == 0) {
      for (size_t i = 0; i < DIM; i++) {
        for (size_t j = 0; j < DIM; j++) {

          Eigen::VectorXd xx = x;

          xx[i] += eps; xx[j] += eps;
          double f1 = energy_value(V,F,xx);
          xx[i] -= eps; xx[j] -= eps;

          xx[i] += eps;
          double f2 = energy_value(V,F,xx);
          xx[i] -= eps;

          xx[j] += eps;
          double f3 = energy_value(V,F,xx);
          xx[j] -= eps;

          double f4 = energy_value(V,F,xx);

          hessian(i, j) = (f1 - f2 - f3 + f4) / (eps * eps);
        }
      }
    } else {
      Eigen::VectorXd xx;
      for (size_t i = 0; i < DIM; i++) {
        for (size_t j = 0; j < DIM; j++) {

          double term_1 = 0;
          xx = x.eval(); xx[i] += 1*eps;  xx[j] += -2*eps;  term_1 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += 2*eps;  xx[j] += -1*eps;  term_1 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += -2*eps; xx[j] += 1*eps;   term_1 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += -1*eps; xx[j] += 2*eps;   term_1 += energy_value(V,F,xx);

          double term_2 = 0;
          xx = x.eval(); xx[i] += -1*eps; xx[j] += -2*eps;  term_2 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += -2*eps; xx[j] += -1*eps;  term_2 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += 1*eps;  xx[j] += 2*eps;   term_2 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += 2*eps;  xx[j] += 1*eps;   term_2 += energy_value(V,F,xx);

          double term_3 = 0;
          xx = x.eval(); xx[i] += 2*eps;  xx[j] += -2*eps;  term_3 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += -2*eps; xx[j] += 2*eps;   term_3 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += -2*eps; xx[j] += -2*eps;  term_3 -= energy_value(V,F,xx);
          xx = x.eval(); xx[i] += 2*eps;  xx[j] += 2*eps;   term_3 -= energy_value(V,F,xx);

          double term_4 = 0;
          xx = x.eval(); xx[i] += -1*eps; xx[j] += -1*eps;  term_4 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += 1*eps;  xx[j] += 1*eps;   term_4 += energy_value(V,F,xx);
          xx = x.eval(); xx[i] += 1*eps;  xx[j] += -1*eps;  term_4 -= energy_value(V,F,xx);
          xx = x.eval(); xx[i] += -1*eps; xx[j] += 1*eps;   term_4 -= energy_value(V,F,xx);

          hessian(i, j) = (-63 * term_1+63 * term_2+44 * term_3+74 * term_4)/(600.0 * eps * eps);

        }
      }
    }

  }

  void SchaeferNewtonParametrizer::get_gradient(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
                  Eigen::VectorXd& uv, Eigen::VectorXd& grad) {
  	 Eigen::SparseMatrix<double> hessian;
  	 Eigen::MatrixXd uv_mat; vec_to_mat2(uv,uv_mat);
	  compute_energy_gradient_hessian(F,
									  uv_mat,
									  grad,
									  hessian);
  }

  void SchaeferNewtonParametrizer::finiteHessian_with_grad(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
  					const Eigen::VectorXd& x,
 					Eigen::MatrixXd & hessian, int accuracy) {
  	int var_num = V.rows()*2;
  	const double eps = 2.2204e-08;
  	hessian.resize(var_num,var_num);

  	for (int i = 0; i < var_num; i++) {
  		for (int j = 0; j < var_num; j++) {
  			Eigen::VectorXd new_x = x;

  			Eigen::VectorXd grad_cur; get_gradient(V,F,new_x, grad_cur);
  			double gi_x = grad_cur(i); double gj_x = grad_cur(j);

  			new_x(j) = new_x(j) + eps;
  			Eigen::VectorXd grad_i;  get_gradient(V,F,new_x, grad_i);
  			double gradj_plus_i = grad_i(i);
  			new_x(j) = new_x(j) - eps;

  			new_x(i) = new_x(i) + eps;
  			Eigen::VectorXd grad_j;  get_gradient(V,F,new_x, grad_j);
  			double gradi_plus_j = grad_j(j);
  			new_x(j) = new_x(j) - eps;

  			hessian(i,j) =  (gradj_plus_i - gj_x)/(2*eps) + (gradi_plus_j - gi_x)/(2*eps);
  		}
  	}
  }

bool SchaeferNewtonParametrizer::checkHessian(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
					const Eigen::VectorXd & x, const Eigen::MatrixXd& actual_hessian, int accuracy) {
    // TODO: check if derived class exists:
    // int(typeid(&Rosenbrock<double>::gradient) == typeid(&Problem<double>::gradient)) == 1 --> overwritten
    const int D = x.rows();
    bool correct = true;

    Eigen::MatrixXd expected_hessian = Eigen::MatrixXd::Zero(D, D);
    //finiteHessian(V,F,x, expected_hessian, accuracy);
    finiteHessian_with_grad(V,F,x, expected_hessian, accuracy);

    for (int d = 0; d < D; ++d) {
      for (int e = 0; e < D; ++e) {
        double scale = (std::max)((std::max)(fabs(actual_hessian(d, e)), fabs(expected_hessian(d, e))), 1.);
        if(fabs(actual_hessian(d, e)- expected_hessian(d, e))>1e-1 * scale) {
        		cout << "not correct for d = " << d << " and e = " << e << endl;
        		correct = false;
        	}
      }
    }
    return correct;

  }


#include <efanna.hpp>
#include <iostream>
#include <fstream>
#include <ctime>
#include <malloc.h>
#include <boost/timer/timer.hpp>

using namespace boost;
using namespace efanna;
using namespace std;
void load_data(char* filename, float*& data, size_t& num,int& dim){// load data with sift10K pattern
  ifstream in(filename, ios::binary);
  if(!in.is_open()){cout<<"open file error"<<endl;exit(-1);}
  in.read((char*)&dim,4);
  cout<<"data dimension: "<<dim<<endl;
  in.seekg(0,ios::end);
  ios::pos_type ss = in.tellg();
  size_t fsize = (size_t)ss;
  num = fsize / (dim+1) / 4;
  int cols = (dim + 7)/8*8;
  data = (float*)memalign(KGRAPH_MATRIX_ALIGN, num * cols * sizeof(float));
if(dim!=cols)cout<<"data align to dimension "<<cols<<" for avx2 inst"<<endl;

  in.seekg(0,ios::beg);
  for(size_t i = 0; i < num; i++){
    in.seekg(4,ios::cur);
    in.read((char*)(data+i*cols),dim*4);
  }
  in.close();
}
int main(int argc, char** argv){
  if(argc!=11 && argc!=12){cout<< argv[0] << " data_file tree_index graph_index query_file result_file tree epoch initsz extend querNN search_method(optional)" <<endl; exit(-1);}

  float* data_load = NULL;
  float* query_load = NULL;
  size_t points_num;
  int dim;
  load_data(argv[1], data_load, points_num,dim);
  size_t q_num;
  int qdim;
  load_data(argv[4], query_load, q_num,qdim);
  Matrix<float> dataset(points_num,dim,data_load);
  Matrix<float> query(q_num,qdim,query_load);

  FIndex<float> index(dataset, new L2DistanceAVX<float>(), efanna::KDTreeUbIndexParams(true, 8 ,8 ,10,25,30,10));
  index.loadTrees(argv[2]);
  index.loadGraph(argv[3]);

  int search_trees = atoi(argv[6]);
  int search_epoc = atoi(argv[7]);
  int poolsz = atoi(argv[8]);
  int search_extend = atoi(argv[9]);
  int search_method = argc == 12 ? atoi(argv[11]) : 0;
  index.setSearchParams(search_epoc, poolsz, search_extend, search_trees, search_method);

//clock_t s,f;
boost::timer::auto_cpu_timer timer;
//s=clock();
  index.knnSearch(atoi(argv[10])/*query nn*/,query);
//f=clock();

//cout<<(f-s)*1.0/CLOCKS_PER_SEC/8<<endl;
cout<<timer.elapsed().wall / 1e9<<endl;
  index.saveResults(argv[5]);
  return 0;
}


// Copyright (C) 2016 Vicente J. Botet Escriba
//
//  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)

// <experimental/ordinal.hpp>

#include <experimental/ordinal.hpp>
#include "Bool.hpp"
#include "Bounded.hpp"

#include <boost/detail/lightweight_test.hpp>

int main()
{
  namespace stde = std::experimental;

  {
    static_assert(stde::ordinal::meta::size<Bool>::value == 2, "");
    static_assert(stde::ordinal::meta::pos<Bool, Bool::False>::value == 0, "");
    static_assert(stde::ordinal::meta::pos<Bool, Bool::True>::value == 1, "");
    static_assert(stde::ordinal::meta::val<Bool, 0>::value == Bool::False, "");
    static_assert(stde::ordinal::meta::val<Bool, 1>::value == Bool::True, "");
  }
  {
    BOOST_TEST(stde::ordinal::size<Bool>() == 2);
    BOOST_TEST(stde::ordinal::traits<Bool>::pos(Bool::False) == 0);
    BOOST_TEST(stde::ordinal::pos(Bool::False) == 0);
    BOOST_TEST(stde::ordinal::pos(Bool::True) == 1);
    BOOST_TEST(stde::ordinal::val<Bool>(0) == Bool::False);
    BOOST_TEST(stde::ordinal::val<Bool>(1) == Bool::True);
    BOOST_TEST(stde::ordinal::first<Bool>() == Bool::False);
    BOOST_TEST(stde::ordinal::last<Bool>() == Bool::True);
    BOOST_TEST(stde::ordinal::succ(Bool::False) == Bool::True);
    BOOST_TEST(stde::ordinal::pred(Bool::True) == Bool::False);
  }
  {
    static_assert(stde::ordinal::meta::size<Bounded<0,3,int>>::value == 4, "");
  }
  {
    using T = Bounded<1,4,unsigned char>;
    BOOST_TEST(stde::ordinal::size<T>() == 4);
    BOOST_TEST(stde::ordinal::pos(T{1}) == 0);
    BOOST_TEST(stde::ordinal::pos(T{2}) == 1);
    BOOST_TEST(stde::ordinal::pos(T{3}) == 2);
    BOOST_TEST(stde::ordinal::pos(T{4}) == 3);
    BOOST_TEST(stde::ordinal::val<T>(0).value == 1);
    BOOST_TEST(stde::ordinal::val<T>(1).value == 2);
    BOOST_TEST(stde::ordinal::first<T>().value == 1);
    BOOST_TEST(stde::ordinal::last<T>().value == 4);
    BOOST_TEST(stde::ordinal::succ(T{1}).value == 2);
    BOOST_TEST(stde::ordinal::succ(T{2}).value == 3);
    BOOST_TEST(stde::ordinal::succ(T{3}).value == 4);
    BOOST_TEST(stde::ordinal::pred(T{4}).value == 3);
    BOOST_TEST(stde::ordinal::pred(T{3}).value == 2);
    BOOST_TEST(stde::ordinal::pred(T{2}).value == 1);

  }
  return ::boost::report_errors();
}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <Eigen/Dense>
#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <functional>

namespace vinecopulib {

//! Tools for working with Eigen types
namespace tools_eigen {
//! An `Eigen::Matrix` containing `bool`s (similar to `Eigen::MatrixXd`).
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> MatrixXb;

template<typename T>
Eigen::MatrixXd
unaryExpr_or_nan(const Eigen::MatrixXd& x, const T& func)
{
  return x.unaryExpr([&func](const double& y) {
    if ((boost::math::isnan)(y)) {
      return std::numeric_limits<double>::quiet_NaN();
    } else {
      return func(y);
    }
  });
}

template<typename T>
Eigen::VectorXd
binaryExpr_or_nan(const Eigen::MatrixXd& u, const T& func)
{
  auto func_or_nan = [&func](const double& u1, const double& u2) {
    if ((boost::math::isnan)(u1) | (boost::math::isnan)(u2)) {
      return std::numeric_limits<double>::quiet_NaN();
    } else {
      return func(u1, u2);
    }
  };
  return u.col(0).binaryExpr(u.col(1), func_or_nan);
}

void
remove_nans(Eigen::MatrixXd& x);

void
remove_nans(Eigen::MatrixXd& x, Eigen::VectorXd& weights);

void
trim(Eigen::MatrixXd& x,
     const double& lower = 1e-10,
     const double& upper = 1 - 1e-10);

void
trim(Eigen::VectorXd& x,
     const double& lower = 1e-10,
     const double& upper = 1 - 1e-10);

bool
check_if_in_unit_cube(const Eigen::MatrixXd& u);

Eigen::MatrixXd
swap_cols(Eigen::MatrixXd u);

Eigen::VectorXd
unique(const Eigen::VectorXd& x);

Eigen::VectorXd
invert_f(const Eigen::VectorXd& x,
         std::function<Eigen::VectorXd(const Eigen::VectorXd&)> f,
         const double lb = 1e-20,
         const double ub = 1 - 1e-20,
         int n_iter = 35);

Eigen::MatrixXd
expand_grid(const Eigen::VectorXd& grid_points);

Eigen::MatrixXd
read_matxd(const char* filename, int max_buffer_size = static_cast<int>(1e6));

Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic>
read_matxs(const char* filename, int max_buffer_size = static_cast<int>(1e6));
}
}

#include <vinecopulib/misc/implementation/tools_eigen.ipp>


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to 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)

#include <algorithms/test_area.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

template <typename P>
void test_all()
{
    typedef bg::model::multi_polygon<bg::model::polygon<P> > mp;
    test_geometry<mp>("MULTIPOLYGON(((0 0,0 7,4 2,2 0,0 0)))", 16.0);
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


// Copyright (c) 2003   INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s)     : Julia Floetotto
//                 Mael Rouxel-Labbé

#include <CGAL/Interpolation/internal/helpers.h>
#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/regular_neighbor_coordinates_2.h>

#include <CGAL/Interpolation_traits_2.h>
#include <CGAL/interpolation_functions.h>
#include <CGAL/Interpolation_gradient_fitting_traits_2.h>
#include <CGAL/sibson_gradient_fitting.h>

#include <CGAL/algorithm.h>
#include <CGAL/double.h>
#include <CGAL/function_objects.h>
#include <functional>
#include <CGAL/Origin.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Random.h>
#include <CGAL/squared_distance_2.h>

#include <boost/utility/result_of.hpp>

#include <iostream>
#include <cassert>
#include <utility>

template < typename Traits_ >
struct Extract_point
{
  typedef Traits_                               Traits;
  typedef typename Traits::Point_2              Point_2;
  typedef typename Traits::Weighted_point_2     Weighted_point_2;

  typedef typename Traits::Construct_point_2    Construct_point_2;

  Extract_point(const Traits& traits = Traits()) : traits(traits) {}

  const Point_2& operator()(const Point_2& p) const { return p; }

  Point_2 operator()(const Weighted_point_2& wp) const {
    return traits.construct_point_2_object()(wp);
  }

  template <typename VH>
  typename boost::result_of<const Construct_point_2(const Point_2&)>::type
  operator()(const VH& vh) const {
    return traits.construct_point_2_object()(vh->point());
  }

private:
  Traits traits;
};

template <typename V, typename T>
struct Value_function
{
  typedef V                                                 argument_type;
  typedef std::pair<T, bool>                                result_type;

  Value_function(std::size_t i) : index(i) { }

  result_type operator()(const argument_type& a) const {
    return result_type(a->info()[index].value, true);
  }

private:
  std::size_t index;
};

template <typename V, typename G>
struct Gradient_function
  : public CGAL::cpp98::iterator<std::output_iterator_tag, void, void, void, void>
{

  typedef V                                                 argument_type;
  typedef std::pair<G, bool>                                result_type;

  Gradient_function(std::size_t i) : index(i) { }

  result_type operator()(const argument_type& a) const {
    return std::make_pair(a->info()[index].gradient,
                          a->info()[index].gradient != CGAL::NULL_VECTOR);
  }

private:
  std::size_t index;
};

template < class ForwardIterator >
bool test_norm(ForwardIterator first, ForwardIterator beyond,
               typename std::iterator_traits<ForwardIterator>::value_type::second_type norm)
{
  typename std::iterator_traits<ForwardIterator>::value_type::second_type sum(0);
  for(; first!=beyond; ++first)
    sum += first->second;

  if(norm != sum)
  {
    std::cerr << "norm / sum differs: " << norm << " " << sum << std::endl;
    return false;
  }
  else
  {
    return true;
  }
}

template < class Tr, class ForwardIterator, class Point >
bool test_barycenter(ForwardIterator first, ForwardIterator beyond,
                     typename std::iterator_traits<ForwardIterator>::value_type::second_type norm,
                     const Point& p,
                     const typename std::iterator_traits<ForwardIterator>::value_type::second_type& tolerance)
{
  typedef typename Tr::Geom_traits                                               Gt;
  typedef typename Gt::Point_2                                                   Bare_point;
  Extract_point<Gt> cp;

  Bare_point b(CGAL::ORIGIN);
  for(; first!=beyond; ++first)
    b = b + (first->second / norm) * (cp(first->first) - CGAL::ORIGIN);

  return CGAL::squared_distance(cp(p), b) <= tolerance;
}

/////////////////////////////////////////////////////////////////
// Accessory function testing functions that require sqrt().
// Doesn't instantiate anything if RT doesn't support sqrt().
template < class ForwardIterator, class ValueFunctor, class GradFunctor, class Gt, class Point>
bool
_test_sibson_c1_interpolation_sqrt(ForwardIterator , ForwardIterator ,
                                   const typename std::iterator_traits<ForwardIterator>::value_type::second_type& /* norm */,
                                   const Point& /* p */,
                                   ValueFunctor /* f */,
                                   GradFunctor /* grad_f */,
                                   const Gt& /*geom_traits */,
                                   const typename std::iterator_traits<ForwardIterator>::value_type::second_type& /* tolerance */,
                                   const typename std::iterator_traits<ForwardIterator>::value_type::second_type& /* exact_value */,
                                   CGAL::Integral_domain_without_division_tag t)
{
  std::cout << std::endl
            << "NOTE: "
            << typeid(t).name()
            << " doesn't support sqrt() and sibson_c1_interpolation is thus not tested." << std::endl;
  return true;
}

template < class ForwardIterator, class ValueFunctor, class GradFunctor, class Gt, class Point>
bool _test_sibson_c1_interpolation_sqrt(ForwardIterator first, ForwardIterator beyond,
                                        const typename std::iterator_traits<ForwardIterator>::value_type::second_type& norm,
                                        const Point& p,
                                        ValueFunctor f,
                                        GradFunctor grad_f,
                                        const Gt& geom_traits,
                                        const typename std::iterator_traits<ForwardIterator>::value_type::second_type& tolerance,
                                        const typename std::iterator_traits<ForwardIterator>::value_type::second_type& exact_value,
                                        CGAL::Field_with_sqrt_tag)
{
  typedef typename std::iterator_traits<ForwardIterator>::value_type::first_type  arg_type;
  typedef typename boost::result_of<ValueFunctor(arg_type)>::type                 value_functor_result_type;

  value_functor_result_type res = CGAL::sibson_c1_interpolation(first, beyond,
                                                                norm, p, f,
                                                                grad_f, geom_traits);

  return res.second && (CGAL_NTS abs(res.first-exact_value) <= tolerance);
}

template < class ForwardIterator, class ValueFunctor, class GradFunctor, class Gt, class Point>
bool test_interpolation_with_value(ForwardIterator first, ForwardIterator beyond,
                                   const typename std::iterator_traits<ForwardIterator>::value_type::second_type& norm,
                                   const Point& p,
                                   const typename boost::result_of<
                                                    ValueFunctor(typename std::iterator_traits<ForwardIterator>::value_type::first_type)>
                                                    ::type::first_type exact_value,
                                   ValueFunctor f,
                                   GradFunctor grad_f,
                                   const Gt& geom_traits,
                                   const int& i,
                                   const typename std::iterator_traits<ForwardIterator>::value_type::second_type& tolerance)
{
  typedef typename std::iterator_traits<ForwardIterator>::value_type::first_type  arg_type;
  typedef typename boost::result_of<ValueFunctor(arg_type)>::type                 value_functor_result_type;
  typedef typename value_functor_result_type::first_type                          Value_type;

  if(i == 0)
  {
    Value_type val = CGAL::linear_interpolation(first, beyond, norm, f);
    assert(CGAL_NTS abs(val - exact_value) <= tolerance);
  }

  value_functor_result_type res = CGAL::quadratic_interpolation(first, beyond, norm, p, f,
                                                                grad_f, geom_traits);
  assert(res.second && (CGAL_NTS abs(res.first - exact_value) <= tolerance));

  if(i<2)
  {
    //without sqrt:
    res = CGAL::sibson_c1_interpolation_square(first, beyond,
                                               norm, p, f,
                                               grad_f, geom_traits);
    assert(res.second && (CGAL_NTS abs(res.first - exact_value) <= tolerance));

    //with sqrt:
    typedef CGAL::Algebraic_structure_traits<Value_type> AST;

    assert(_test_sibson_c1_interpolation_sqrt(first, beyond, norm, p, f, grad_f,
                                              geom_traits, tolerance, exact_value,
                                              typename AST::Algebraic_category()));
  }

  res = CGAL::farin_c1_interpolation(first, beyond, norm, p, f, grad_f, geom_traits);
  assert(res.second);
  assert(CGAL_NTS abs(res.first - exact_value) <= tolerance);

  return true;
}

template < class ForwardIterator, class ValueFunctor, class GradFunctor, class Gt, class Point, class Value_type>
bool test_interpolation(ForwardIterator first, ForwardIterator beyond,
                        const typename std::iterator_traits<ForwardIterator>::value_type::second_type& norm,
                        const Point& p,
                        const Value_type exact_value,
                        ValueFunctor f,
                        GradFunctor grad_f,
                        const Gt& geom_traits,
                        const int& i,
                        const typename std::iterator_traits<ForwardIterator>::value_type::second_type& tolerance)
{
  return test_interpolation_with_value(first, beyond, norm, p, exact_value, f, grad_f, geom_traits, i, tolerance);
}

template <class Dt>
void _test_interpolation_functions_2_Delaunay_without_OutputFunctor(const Dt&, const typename Dt::Geom_traits::FT& tolerance)
{
  std::cout << "Testing backward compatibility..." << std::endl;

  CGAL::Set_ieee_double_precision pfr;
  Dt T;

  int n=20, m=20;
  double r = 3;
  double max_value = 5;

  typedef typename Dt::Geom_traits                             Gt;
  typedef CGAL::Interpolation_traits_2<Gt>                     Traits;

  typedef typename Dt::Point                                   Point;

  typedef typename Gt::FT                                      Coord_type;
  typedef typename Gt::Vector_2                                Vector;

  typedef std::map<Point, Coord_type, typename Gt::Less_xy_2>  Point_value_map;
  typedef std::map<Point, Vector, typename Gt::Less_xy_2>      Point_vector_map;

  typedef std::vector<std::pair<Point, Coord_type> >           Point_coordinate_vector;

  std::cout << "NN2: Testing random points." << std::endl;

  //test random points in a square of length r:
  std::vector<Point> points;
  points.reserve(n+m);

  //put four bounding box points:
  points.push_back(Point(-r, -r));
  points.push_back(Point(r, -r));
  points.push_back(Point(-r, r));
  points.push_back(Point(r, r));

  // Create n+m-4 points within a disc of radius 2
  CGAL::Random_points_in_square_2<Point> g(r);
  std::copy_n(g, n+m, std::back_inserter(points));

  CGAL::Random random;

  Point_value_map values[3];
  Point_vector_map gradients[3];

  Coord_type alpha = Coord_type(random.get_double(-max_value, max_value)),
             beta1 = Coord_type(random.get_double(-max_value, max_value)),
             beta2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma1 = Coord_type(random.get_double(-max_value, max_value)),
             gamma2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma3 = Coord_type(random.get_double(-max_value, max_value));

  //INSERTION + DET. of GRADIENT for n DATA POINTS :
  for(int j=0; j<n; ++j)
  {
    T.insert(points[j]);

    gradients[0].insert(std::make_pair(points[j], Vector(beta1, beta2)));

    gradients[1].insert(std::make_pair(points[j],
                                       Vector(beta1 + Coord_type(2)*gamma1*points[j].x(),
                                              beta2 + Coord_type(2)*gamma1*points[j].y())));
    gradients[2].insert(std::make_pair(points[j],
                                       Vector(beta1 + Coord_type(2)*gamma1*points[j].x() + gamma3*points[j].y(),
                                              beta2 + Coord_type(2)*gamma2*points[j].y() + gamma3*points[j].x())));
  }

  //DETERMINE VALUES FOR n DATA POINTS AND m RANDOM TEST POINTS:
  for(int j=0; j<n+m; j++)
  {
    //linear function
    values[0].insert(std::make_pair(points[j], alpha + beta1*points[j].x() + beta2*points[j].y()));

    //spherical function:
    values[1].insert(std::make_pair(points[j], alpha + beta1*points[j].x() +
                                                       beta2*points[j].y() +
                                                       gamma1*points[j].x()*points[j].x()+
                                                       gamma1*points[j].y()*points[j].y()));

    //quadratic function
    values[2].insert(std::make_pair(points[j], alpha + beta1*points[j].x() +
                                                       beta2*points[j].y() +
                                                       gamma1*points[j].x()*points[j].x() +
                                                       gamma2*points[j].y()*points[j].y() +
                                                       gamma3*points[j].x()*points[j].y()));
  }

  //INTERPOLATION OF RANDOM POINTS:
  Coord_type norm;
  Point_coordinate_vector coords;
  for(int j=n; j<n+m; ++j)
  {
    CGAL::Triple<std::back_insert_iterator<Point_coordinate_vector>, Coord_type, bool> coordinate_result =
        CGAL::natural_neighbor_coordinates_2(T, points[j], std::back_inserter(coords));
    assert(coordinate_result.third);
    norm = coordinate_result.second;

    bool is_equal = test_norm(coords.begin(), coords.end(), norm);
    assert(norm > 0);
    assert(is_equal);
    is_equal = test_barycenter<Dt>(coords.begin(), coords.end(), norm, points[j], tolerance);
    assert(is_equal);

    for(int i=0; i<3; ++i)
    {
      assert(test_interpolation(coords.begin(), coords.end(), norm,
                                points[j], values[i][points[j]],
                                CGAL::Data_access< Point_value_map >(values[i]),
                                CGAL::Data_access< Point_vector_map >(gradients[i]),
                                Traits(), i, tolerance));
    }
    coords.clear();
  }

  //TESTING THE GRADIENT APPRXIMATION METHOD:
    // sibson_gradient_fitting is tested when calling nn_2
  std::cout << "Testing gradient estimation method on random points." << std::endl;

  typedef CGAL::Interpolation_gradient_fitting_traits_2<Gt> GradTraits;
  Point_vector_map approx_gradients[2];
  CGAL::sibson_gradient_fitting_nn_2(T,
                                     std::inserter(approx_gradients[0], approx_gradients[0].begin()),
                                     CGAL::Data_access<Point_value_map>(values[0]), GradTraits());

  CGAL::sibson_gradient_fitting_nn_2(T,
                                     std::inserter(approx_gradients[1], approx_gradients[1].begin()),
                                     CGAL::Data_access<Point_value_map>(values[1]), GradTraits());

  for(int j=0; j<n; ++j)
  {
    std::pair<Vector, bool> res = CGAL::Data_access<Point_vector_map>(approx_gradients[0])(points[j]);

    if(res.second)
    {
      // if it is the exact computation kernel: test the equality:
      assert(tolerance > Coord_type(0) ||
             res.first == CGAL::Data_access<Point_vector_map>(gradients[0])(points[j]).first);
      res = CGAL::Data_access<Point_vector_map>(approx_gradients[1])(points[j]);

      // if one exists->the other must also exist
      assert(res.second);

      assert(tolerance > Coord_type(0) ||
             res.first == CGAL::Data_access<Point_vector_map>(gradients[1])(points[j]).first);
    }
    else
    {
      assert(!CGAL::Data_access<Point_vector_map>(approx_gradients[1])(points[j]).second);
    }
  }

  //TESTING A POINT == A DATA POINT:
  CGAL::Triple<std::back_insert_iterator<Point_coordinate_vector>, Coord_type, bool> coordinate_result =
      CGAL::natural_neighbor_coordinates_2(T, points[n/2], std::back_inserter(coords));
  assert(coordinate_result.third);
  norm = coordinate_result.second;
  assert(norm == Coord_type(1));

  typename std::vector<std::pair<Point, Coord_type> >::iterator ci = coords.begin();
  assert(ci->first == points[n/2]);
  assert(ci->second == Coord_type(1));
  ci++;
  assert(ci==coords.end());

  for(int j=0; j<3; ++j)
  {
    assert(test_interpolation(coords.begin(), coords.end(), norm,
                              points[n/2], values[j][points[n/2]],
                              CGAL::Data_access<Point_value_map>(values[j]),
                              CGAL::Data_access<Point_vector_map>(gradients[j]),
                              Traits(), j, tolerance));
  }
  coords.clear();
}

template <class Dt>
void _test_interpolation_functions_2_Delaunay_with_OutputFunctor(const Dt&, const typename Dt::Geom_traits::FT& tolerance)
{
  std::cout << "Testing with OutputFunctor..." << std::endl;

  CGAL::Set_ieee_double_precision pfr;
  Dt T;

  int n=20, m=20;
  double r = 3;
  double max_value = 5;

  typedef typename Dt::Geom_traits                               Gt;
  typedef CGAL::Interpolation_traits_2<Gt>                       Traits;

  typedef typename Dt::Vertex_handle                             Vertex_handle;

  typedef typename Gt::FT                                        Coord_type;
  typedef typename Dt::Point                                     Point;
  typedef typename Gt::Vector_2                                  Vector;

  typedef std::vector<std::pair<Vertex_handle, Coord_type> >     Coordinate_vector;
  typedef typename Coordinate_vector::const_iterator             CV_cit;
  typedef CGAL::Identity<std::pair<Vertex_handle, Coord_type> >  Output_functor;

  typedef std::map<Point, Coord_type>                            Point_value_map;
  typedef std::map<Point, Vector>                                Point_vector_map;

  std::cout << "NN2: Testing random points." << std::endl;

  //test random points in a square of length r:
  std::vector<Point> points;
  points.reserve(n+m);

  //put four bounding box points:
  points.push_back(Point(-r, -r));
  points.push_back(Point(r, -r));
  points.push_back(Point(-r, r));
  points.push_back(Point(r, r));

  // Create n+m-4 points within a disc of radius 2
  CGAL::Random_points_in_square_2<Point> g(r);
  std::copy_n(g, n+m, std::back_inserter(points));

  CGAL::Random random;

  Coord_type alpha = Coord_type(random.get_double(-max_value, max_value)),
             beta1 = Coord_type(random.get_double(-max_value, max_value)),
             beta2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma1 = Coord_type(random.get_double(-max_value, max_value)),
             gamma2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma3 = Coord_type(random.get_double(-max_value, max_value));

  //INSERTION + DET. of GRADIENT for n DATA POINTS :
  //DETERMINE VALUES FOR n DATA POINTS AND m RANDOM TEST POINTS:
  Point_value_map exact_values[3];

  std::map<Point, Vertex_handle> p_to_vh;

  for(int j=0; j<n+m; ++j)
  {
    Vector gradient0(beta1, beta2);
    Vector gradient1(beta1 + Coord_type(2)*gamma1*points[j].x(),
                     beta2 + Coord_type(2)*gamma1*points[j].y());
    Vector gradient2(beta1 + Coord_type(2)*gamma1*points[j].x() + gamma3*points[j].y(),
                     beta2 + Coord_type(2)*gamma2*points[j].y() + gamma3*points[j].x());

    Coord_type value0 = alpha + beta1*points[j].x() + beta2*points[j].y();
    Coord_type value1 = alpha + beta1*points[j].x()
                              + beta2*points[j].y()
                              + gamma1*points[j].x()*points[j].x()
                              + gamma1*points[j].y()*points[j].y();
    Coord_type value2 = alpha + beta1*points[j].x()
                              + beta2*points[j].y()
                              + gamma1*points[j].x()*points[j].x()
                              + gamma2*points[j].y()*points[j].y()
                              + gamma3*points[j].x()*points[j].y();

    if(j<n) // only insert n points
    {
      Vertex_handle vh = T.insert(points[j]);
      p_to_vh[points[j]] = vh;

      vh->info()[0].gradient = gradient0;
      vh->info()[1].gradient = gradient1;
      vh->info()[2].gradient = gradient2;

      vh->info()[0].value = value0;
      vh->info()[1].value = value1;
      vh->info()[2].value = value2;
    }
    else
    {
      exact_values[0][points[j]] = value0;
      exact_values[1][points[j]] = value1;
      exact_values[2][points[j]] = value2;
    }
  }

  //INTERPOLATION OF RANDOM POINTS:
  Coord_type norm;

  Coordinate_vector coords;
  Output_functor out_fct;

  for(int j=n; j<n+m; ++j)
  {
    CGAL::Triple<std::back_insert_iterator<Coordinate_vector>, Coord_type, bool> coordinate_result =
        CGAL::natural_neighbor_coordinates_2(T, points[j], std::back_inserter(coords), out_fct);
    assert(coordinate_result.third);
    norm = coordinate_result.second;

    bool is_equal = test_norm(coords.begin(), coords.end(), norm);
    assert(norm > 0);
    assert(is_equal);
    is_equal = test_barycenter<Dt>(coords.begin(), coords.end(), norm, points[j], tolerance);
    assert(is_equal);

    assert(test_interpolation(coords.begin(), coords.end(), norm,
                              points[j], exact_values[0][points[j]],
                              [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[0].value, true); },
                              [](const Vertex_handle vh) -> std::pair<Vector, bool> { return std::make_pair(vh->info()[0].gradient, true); },
                              Traits(), 0, tolerance));

    // wrapping the lambda in a std function
    std::function<std::pair<Coord_type, bool>(const Vertex_handle)> value_function_1 =
      [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[1].value, true); };

    std::function<std::pair<Vector, bool>(const Vertex_handle)> gradient_function_1 =
      [](const Vertex_handle vh) -> std::pair<Vector, bool> { return std::make_pair(vh->info()[1].gradient, true); };

    assert(test_interpolation(coords.begin(), coords.end(), norm,
                              points[j], exact_values[1][points[j]],
                              value_function_1, gradient_function_1,
                              Traits(), 1, tolerance));

    assert(test_interpolation(coords.begin(), coords.end(), norm,
                              points[j], exact_values[2][points[j]],
                              [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[2].value, true); },
                              [](const Vertex_handle vh) -> std::pair<Vector, bool> { return std::make_pair(vh->info()[2].gradient, true); },
                              Traits(), 2, tolerance));

    coords.clear();
  }

  //TESTING THE GRADIENT APPRXIMATION METHOD:
    // sibson_gradient_fitting is tested when calling nn_2
  std::cout << "Testing gradient estimation method on random points." << std::endl;

  typedef CGAL::Interpolation_gradient_fitting_traits_2<Gt> GradTraits;

  Point_vector_map approx_gradients[2];

  {
    CGAL::sibson_gradient_fitting_nn_2(T,
                                       std::inserter(approx_gradients[0], approx_gradients[0].begin()), // OutputIterator
                                       CGAL::Interpolation::internal::Extract_point_in_pair<Dt, Vector>(), // OutputFunctor
                                       [](const Vertex_handle vh)
                                          -> std::pair<Coord_type, bool>
                                         { return std::make_pair(vh->info()[0].value, true); },
                                       GradTraits());

    std::function<std::pair<Coord_type, bool>(const Vertex_handle)> value_function_1 =
      [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[1].value, true); };

    CGAL::sibson_gradient_fitting_nn_2(T,
                                       std::inserter(approx_gradients[1], approx_gradients[1].begin()),
                                       CGAL::Interpolation::internal::Extract_point_in_pair<Dt, Vector>(),
                                       value_function_1,
                                       GradTraits());
  }

  for(int j=0; j<n; ++j)
  {
    std::pair<Vector, bool> res = CGAL::Data_access<Point_vector_map>(approx_gradients[0])(points[j]);

    if(res.second)
    {
      Gradient_function<Vertex_handle, Vector> gradient_function_0(0);
      Gradient_function<Vertex_handle, Vector> gradient_function_1(1);

      // if it is the exact computation kernel: test the equality:
      assert(tolerance > Coord_type(0) ||
             res.first == (gradient_function_0(p_to_vh[points[j]])).first);
      res = CGAL::Data_access<Point_vector_map>(approx_gradients[1])(points[j]);

      // if one exists->the other must also exist
      assert(res.second);

      assert(tolerance > Coord_type(0) ||
             res.first == gradient_function_1(p_to_vh[points[j]]).first);
    }
    else
    {
      assert(!CGAL::Data_access<Point_vector_map>(approx_gradients[1])(points[j]).second);
    }
  }

  //TESTING A POINT == A DATA POINT:
  CGAL::Triple<std::back_insert_iterator<Coordinate_vector>, Coord_type, bool> coordinate_result =
      CGAL::natural_neighbor_coordinates_2(T, points[n/2], std::back_inserter(coords), out_fct);
  assert(coordinate_result.third);
  norm = coordinate_result.second;
  assert(norm == Coord_type(1));

  CV_cit ci = coords.begin();
  assert(ci->first == p_to_vh[points[n/2]]);
  assert(ci->second == Coord_type(1));
  ci++;
  assert(ci == coords.end());

  Value_function<Vertex_handle, Coord_type> value_function_0(0);
  Value_function<Vertex_handle, Coord_type> value_function_2(2);

  assert(test_interpolation(coords.begin(), coords.end(), norm,
                            points[n/2], value_function_0(p_to_vh[points[n/2]]).first,
                            [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[0].value, true); },
                            [](const Vertex_handle vh) -> std::pair<Vector, bool> { return std::make_pair(vh->info()[0].gradient, true); },
                            Traits(), 0, tolerance));

  // wrapping the lambda in a std function
  std::function<std::pair<Coord_type, bool>(const Vertex_handle)> value_function_1 =
      [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[1].value, true); };

  std::function<std::pair<Vector, bool>(const Vertex_handle)> gradient_function_1 =
      [](const Vertex_handle vh) -> std::pair<Vector, bool> { return std::make_pair(vh->info()[1].gradient, true); };

  assert(test_interpolation(coords.begin(), coords.end(), norm,
                            points[n/2], value_function_1(p_to_vh[points[n/2]]).first,
                            value_function_1, gradient_function_1,
                            Traits(), 1, tolerance));

  assert(test_interpolation(coords.begin(), coords.end(), norm,
                            points[n/2], value_function_2(p_to_vh[points[n/2]]).first,
                            [](const Vertex_handle vh) -> std::pair<Coord_type, bool> { return std::make_pair(vh->info()[2].value, true); },
                            [](const Vertex_handle vh) -> std::pair<Vector, bool> { return std::make_pair(vh->info()[2].gradient, true); },
                            Traits(), 2, tolerance));

  coords.clear();
}

template <class Rt>
void _test_interpolation_functions_2_regular_without_OutputFunctor(const Rt&, const typename Rt::Geom_traits::FT& tolerance)
{
  std::cout << "Testing backward compatibility..." << std::endl;

  CGAL::Set_ieee_double_precision pfr;
  Rt T;

  std::size_t n=20, m=20;
  double r = 3.;
  double max_value = 5.;

  typedef typename Rt::Geom_traits                             Gt;
  typedef CGAL::Interpolation_traits_2<Gt>                     Traits;

  typedef typename Rt::Bare_point                              Bare_point;
  typedef typename Rt::Weighted_point                          Weighted_point;

  typedef typename Gt::FT                                      Coord_type;
  typedef typename Gt::Vector_2                                Vector;

  typedef std::map<Weighted_point, Coord_type>                 Point_value_map;
  typedef std::map<Weighted_point, Vector>                     Point_vector_map;

  typedef std::vector<std::pair<Weighted_point, Coord_type> >  Point_coordinate_vector;

  std::cout << "RN2: Testing random points." << std::endl;

  // test random points in a square of length r:
  std::vector<Weighted_point> points;
  points.reserve(n+m);

  // put four bounding box points to never have infinite vertices
  // move them away from the cube of side 'r' to be sure they are not hidden
  double rr = 10 * r;
  points.push_back(Weighted_point(Bare_point(-rr, -rr), 0));
  points.push_back(Weighted_point(Bare_point(rr, -rr), 0));
  points.push_back(Weighted_point(Bare_point(-rr, rr), 0));
  points.push_back(Weighted_point(Bare_point(rr, rr), 0));

  // Create n+m-4 points within a disc of radius 2, with random weights
  CGAL::Random random;
  CGAL::Random_points_in_square_2<Bare_point> g(r);

  while(points.size() != n+m)
  {
    Weighted_point p(*g++, random.get_double(0., 0.5));
    points.push_back(p);
  }

  Point_value_map values[3];
  Point_vector_map gradients[3];

  Coord_type alpha = Coord_type(random.get_double(-max_value, max_value)),
             beta1 = Coord_type(random.get_double(-max_value, max_value)),
             beta2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma1 = Coord_type(random.get_double(-max_value, max_value)),
             gamma2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma3 = Coord_type(random.get_double(-max_value, max_value));

  //INSERTION + DET. of GRADIENT for n DATA POINTS :
  for(std::size_t j=0; j<n; ++j)
  {
    T.insert(points[j]);

    gradients[0].insert(std::make_pair(points[j], Vector(beta1, beta2)));

    gradients[1].insert(std::make_pair(points[j],
                                       Vector(beta1 + Coord_type(2)*gamma1*points[j].x(),
                                              beta2 + Coord_type(2)*gamma1*points[j].y())));
    gradients[2].insert(std::make_pair(points[j],
                                       Vector(beta1 + Coord_type(2)*gamma1*points[j].x() + gamma3*points[j].y(),
                                              beta2 + Coord_type(2)*gamma2*points[j].y() + gamma3*points[j].x())));
  }

  //DETERMINE VALUES FOR n DATA POINTS AND m RANDOM TEST POINTS:
  for(std::size_t j=0; j<n+m; j++)
  {
    //linear function
    values[0].insert(std::make_pair(points[j], alpha + beta1*points[j].x() + beta2*points[j].y()));

    //spherical function:
    values[1].insert(std::make_pair(points[j], alpha + beta1*points[j].x() +
                                                       beta2*points[j].y() +
                                                       gamma1*points[j].x()*points[j].x()+
                                                       gamma1*points[j].y()*points[j].y()));

    //quadratic function
    values[2].insert(std::make_pair(points[j], alpha + beta1*points[j].x() +
                                                       beta2*points[j].y() +
                                                       gamma1*points[j].x()*points[j].x() +
                                                       gamma2*points[j].y()*points[j].y() +
                                                       gamma3*points[j].x()*points[j].y()));
  }

  //INTERPOLATION OF RANDOM POINTS:
  Coord_type norm;
  Point_coordinate_vector coords;
  for(std::size_t j=n; j<n+m; ++j)
  {
    CGAL::Triple<std::back_insert_iterator<Point_coordinate_vector>, Coord_type, bool> coordinate_result =
        CGAL::regular_neighbor_coordinates_2(T, points[j], std::back_inserter(coords));
    assert(coordinate_result.third);

    norm = coordinate_result.second;
    assert(norm >= 0); // can be null if the weight was too small
    bool is_equal = test_norm(coords.begin(), coords.end(), norm);
    assert(is_equal);

    // further tests are pointless if there are no coordinates
    if(norm == 0)
      continue;

    is_equal = test_barycenter<Rt>(coords.begin(), coords.end(), norm, points[j], tolerance);
    assert(is_equal);

    for(int i=0; i<3; ++i)
    {
      assert(test_interpolation(coords.begin(), coords.end(), norm,
                                points[j], values[i][points[j]],
                                CGAL::Data_access< Point_value_map >(values[i]),
                                CGAL::Data_access< Point_vector_map >(gradients[i]),
                                Traits(), i, tolerance));
    }
    coords.clear();
  }

  //TESTING THE GRADIENT APPRXIMATION METHOD:
    // sibson_gradient_fitting is tested when calling rn_2
  std::cout << "Testing gradient estimation method on random points." << std::endl;
  typedef CGAL::Interpolation_gradient_fitting_traits_2<Gt> GradTraits;
  Point_vector_map approx_gradients[2];
  CGAL::sibson_gradient_fitting_rn_2(T,
                                     std::inserter(approx_gradients[0], approx_gradients[0].begin()),
                                     CGAL::Data_access<Point_value_map>(values[0]), GradTraits());

  CGAL::sibson_gradient_fitting_rn_2(T,
                                     std::inserter(approx_gradients[1], approx_gradients[1].begin()),
                                     CGAL::Data_access<Point_value_map>(values[1]), GradTraits());

  for(std::size_t j=0; j<n; ++j)
  {
    std::pair<Vector, bool> res = CGAL::Data_access<Point_vector_map>(approx_gradients[0])(points[j]);

    if(res.second)
    {
      // if it is the exact computation kernel: test the equality:
      assert(tolerance > Coord_type(0) ||
             res.first == CGAL::Data_access<Point_vector_map>(gradients[0])(points[j]).first);
      res = CGAL::Data_access<Point_vector_map>(approx_gradients[1])(points[j]);

      // if one exists->the other must also exist
      assert(res.second);
      assert(tolerance > Coord_type(0) ||
             res.first == CGAL::Data_access<Point_vector_map>(gradients[1])(points[j]).first);
    }
    else
    {
      assert(!CGAL::Data_access<Point_vector_map>(approx_gradients[1])(points[j]).second);
    }
  }

  //TESTING A POINT == A DATA POINT:
  CGAL::Triple<std::back_insert_iterator<Point_coordinate_vector>, Coord_type, bool> coordinate_result =
      CGAL::regular_neighbor_coordinates_2(T, points[n/2], std::back_inserter(coords));
  assert(coordinate_result.third);
  norm = coordinate_result.second;
  // if the point is hidden, the norm will be 0
  assert(norm == Coord_type(0) || norm == Coord_type(1));

  if(norm == Coord_type(1)) // nothing to do if the point is hidden
  {
    typename std::vector<std::pair<Weighted_point, Coord_type> >::iterator ci = coords.begin();
    assert(ci->first == points[n/2]);
    assert(ci->second == Coord_type(1));
    ci++;
    assert(ci == coords.end());

    for(int j=0; j<3; ++j)
    {
      assert(test_interpolation(coords.begin(), coords.end(), norm,
                                points[n/2], values[j][points[n/2]],
                                CGAL::Data_access<Point_value_map>(values[j]),
                                CGAL::Data_access<Point_vector_map>(gradients[j]),
                                Traits(), j, tolerance));
    }
    coords.clear();
  }
  else
  {
    assert(coords.empty()); // point was hidden
  }
}

template <class Rt>
void _test_interpolation_functions_2_regular_with_OutputFunctor(const Rt&, const typename Rt::Geom_traits::FT& tolerance)
{
  CGAL::Set_ieee_double_precision pfr;
  Rt T;

  std::size_t n=20, m=20;
  double r = 3.;
  double max_value = 5.;

  typedef typename Rt::Geom_traits                               Gt;
  typedef CGAL::Interpolation_traits_2<Gt>                       Traits;

  typedef typename Rt::Bare_point                                Bare_point;
  typedef typename Rt::Weighted_point                            Weighted_point;

  typedef typename Gt::FT                                        FT;
  typedef typename Gt::FT                                        Coord_type;
  typedef typename Gt::Vector_2                                  Vector;

  typedef typename Rt::Vertex_handle                             Vertex_handle;

  // These are the values at points which won't be inserted in the triangulation
  typedef std::map<Weighted_point, Coord_type>                   Point_value_map;

  typedef std::map<Vertex_handle, Coord_type>                    Vertex_value_map;
  typedef std::map<Vertex_handle, Vector>                        Vertex_vector_map;

  typedef std::vector<std::pair<Vertex_handle, Coord_type> >     Vertex_coordinate_vector;
  typedef typename Vertex_coordinate_vector::const_iterator      VCV_cit;
  typedef CGAL::Identity<std::pair<Vertex_handle, Coord_type> >  Identity_output_functor;

  Identity_output_functor vh_fct;

  std::cout << "RN2: Testing random points." << std::endl;

  // test random points in a square of length r:
  std::vector<Weighted_point> points;
  points.reserve(n+m);

  // put four bounding box points to never have infinite vertices
  // move them away from the cube of side 'r' to be sure they are not hidden
  double rr = 10 * r;
  points.push_back(Weighted_point(Bare_point(-rr, -rr), 0));
  points.push_back(Weighted_point(Bare_point(rr, -rr), 0));
  points.push_back(Weighted_point(Bare_point(-rr, rr), 0));
  points.push_back(Weighted_point(Bare_point(rr, rr), 0));

  // Create n+m-4 points within a disc of radius 2, with random weights
  CGAL::Random random;
  CGAL::Random_points_in_square_2<Bare_point> g(r);

  while(points.size() != n+m)
  {
    Weighted_point p(*g++, FT(random.get_double(0., 0.5)));
    points.push_back(p);
  }

  Vertex_value_map values[3];
  Vertex_vector_map gradients[3];
  Point_value_map pt_values[3];

  Coord_type alpha = Coord_type(random.get_double(-max_value, max_value)),
             beta1 = Coord_type(random.get_double(-max_value, max_value)),
             beta2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma1 = Coord_type(random.get_double(-max_value, max_value)),
             gamma2 = Coord_type(random.get_double(-max_value, max_value)),
             gamma3 = Coord_type(random.get_double(-max_value, max_value));

  //INSERTION + DET. of GRADIENT for n DATA POINTS :
  for(std::size_t j=0; j<n; ++j)
  {
    Vertex_handle vh = T.insert(points[j]);

    if(vh != Vertex_handle())
    {
      gradients[0].insert(std::make_pair(vh, Vector(beta1, beta2)));

      gradients[1].insert(std::make_pair(vh,
                                         Vector(beta1 + Coord_type(2)*gamma1*points[j].x(),
                                                beta2 + Coord_type(2)*gamma1*points[j].y())));
      gradients[2].insert(std::make_pair(vh,
                                         Vector(beta1 + Coord_type(2)*gamma1*points[j].x() + gamma3*points[j].y(),
                                                beta2 + Coord_type(2)*gamma2*points[j].y() + gamma3*points[j].x())));

      //linear function
      values[0].insert(std::make_pair(vh, alpha + beta1*points[j].x() + beta2*points[j].y()));

      //spherical function:
      values[1].insert(std::make_pair(vh, alpha + beta1*points[j].x() +
                                                  beta2*points[j].y() +
                                                  gamma1*points[j].x()*points[j].x()+
                                                  gamma1*points[j].y()*points[j].y()));

      //quadratic function
      values[2].insert(std::make_pair(vh, alpha + beta1*points[j].x() +
                                                  beta2*points[j].y() +
                                                  gamma1*points[j].x()*points[j].x() +
                                                  gamma2*points[j].y()*points[j].y() +
                                                  gamma3*points[j].x()*points[j].y()));
    }

    // Fill the values of points that are not in the triangulation (these are exact
    // values that are used to compare with approximations obtained through interpolation functions)
    for(std::size_t j=0; j<n+m; j++)
    {
      // linear function
      pt_values[0].insert(std::make_pair(points[j], alpha + beta1*points[j].x() + beta2*points[j].y()));

      // spherical function:
      pt_values[1].insert(std::make_pair(points[j], alpha + beta1*points[j].x() +
                                                            beta2*points[j].y() +
                                                            gamma1*points[j].x()*points[j].x()+
                                                            gamma1*points[j].y()*points[j].y()));

      // quadratic function
      pt_values[2].insert(std::make_pair(points[j], alpha + beta1*points[j].x() +
                                                            beta2*points[j].y() +
                                                            gamma1*points[j].x()*points[j].x() +
                                                            gamma2*points[j].y()*points[j].y() +
                                                            gamma3*points[j].x()*points[j].y()));
    }
  }

  //INTERPOLATION OF RANDOM POINTS:
  Coord_type norm;
  Vertex_coordinate_vector vh_coords;
  for(std::size_t j=n; j<n+m; ++j)
  {
    CGAL::Triple<std::back_insert_iterator<Vertex_coordinate_vector>, Coord_type, bool> coordinate_result =
        CGAL::regular_neighbor_coordinates_2(T, points[j], std::back_inserter(vh_coords), vh_fct);
    assert(coordinate_result.third);

    norm = coordinate_result.second;
    assert(norm >= 0); // can be null if the weight was too small
    bool is_equal = test_norm(vh_coords.begin(), vh_coords.end(), norm);
    assert(is_equal);

    // further tests are pointless if there are no coordinates
    if(norm == 0)
      continue;

    // equivalent to a call to 'test_barycenter'
    Extract_point<Gt> cp;
    Bare_point b(CGAL::ORIGIN);
    VCV_cit ci = vh_coords.begin();
    for(; ci!=vh_coords.end(); ++ci)
    {
      Vertex_handle vh = ci->first;
      b = b + (ci->second / norm) * (cp(vh->point()) - CGAL::ORIGIN);
    }
    assert(CGAL::squared_distance(cp(points[j]), b) <= tolerance);

    for(int i=0; i<3; ++i)
    {
      std::pair<FT, bool> ev = CGAL::Data_access<Point_value_map>(pt_values[i])(points[j]);
      assert(ev.second);

      assert(test_interpolation_with_value(vh_coords.begin(), vh_coords.end(), norm, points[j],
                                           ev.first /*exact value*/,
                                           CGAL::Data_access<Vertex_value_map>(values[i]),
                                           CGAL::Data_access<Vertex_vector_map>(gradients[i]),
                                           Traits(), i, tolerance));
    }
    vh_coords.clear();
  }

  //TESTING THE GRADIENT APPRXIMATION METHOD:
    // sibson_gradient_fitting is tested when calling rn_2
  std::cout << "Testing gradient estimation method on random points." << std::endl;
  typedef CGAL::Interpolation_gradient_fitting_traits_2<Gt> GradTraits;
  Vertex_vector_map approx_gradients[2];
  CGAL::sibson_gradient_fitting_rn_2(T,
                                     std::inserter(approx_gradients[0], approx_gradients[0].begin()),
                                     CGAL::Identity<std::pair<Vertex_handle, Vector> >(),
                                     CGAL::Data_access<Vertex_value_map>(values[0]), GradTraits());

  CGAL::sibson_gradient_fitting_rn_2(T,
                                     std::inserter(approx_gradients[1], approx_gradients[1].begin()),
                                     CGAL::Identity<std::pair<Vertex_handle, Vector> >(),
                                     CGAL::Data_access<Vertex_value_map>(values[1]), GradTraits());

  typename Rt::Finite_vertices_iterator vit = T.finite_vertices_begin();
  for(; vit!=T.finite_vertices_end(); ++vit)
  {
    Vertex_handle vh = vit;
    assert(vh != Vertex_handle());
    std::pair<Vector, bool> res = CGAL::Data_access<Vertex_vector_map>(approx_gradients[0])(vh);

    if(res.second)
    {
      // if it is the exact computation kernel: test the equality:
      assert(tolerance > Coord_type(0) ||
             res.first == CGAL::Data_access<Vertex_vector_map>(gradients[0])(vh).first);
      res = CGAL::Data_access<Vertex_vector_map>(approx_gradients[1])(vh);

      // if one exists->the other must also exist
      assert(res.second);
      assert(tolerance > Coord_type(0) ||
             res.first == CGAL::Data_access<Vertex_vector_map>(gradients[1])(vh).first);
    }
    else
    {
      assert(!CGAL::Data_access<Vertex_vector_map>(approx_gradients[1])(vh).second);
    }
  }

  //TESTING A POINT == A DATA POINT:
  Vertex_handle vh = T.finite_vertices_begin();
  std::cout << "testing at data point: " << vh->point() << std::endl;
  CGAL::Triple<std::back_insert_iterator<Vertex_coordinate_vector>, Coord_type, bool> coordinate_result =
      CGAL::regular_neighbor_coordinates_2(T, vh->point(), std::back_inserter(vh_coords), vh_fct);
  assert(coordinate_result.third);
  norm = coordinate_result.second;

  // if the point is hidden, the norm will be 0
  assert(norm == Coord_type(0) || norm == Coord_type(1));

  if(norm == Coord_type(1)) // nothing to do if the point is hidden
  {
    VCV_cit ci = vh_coords.begin();
    assert(ci->first == vh);
    assert(ci->second == Coord_type(1));
    ci++;
    assert(ci == vh_coords.end());

    for(int j=0; j<3; ++j)
    {
      std::pair<FT, bool> ev = CGAL::Data_access<Vertex_value_map>(values[j])(vh);
      assert(ev.second);

      assert(test_interpolation_with_value(vh_coords.begin(), vh_coords.end(), norm,
                                           vh->point(), ev.first /*exact value*/,
                                           CGAL::Data_access<Vertex_value_map>(values[j]),
                                           CGAL::Data_access<Vertex_vector_map>(gradients[j]),
                                           Traits(), j, tolerance));
    }
    vh_coords.clear();
  }
  else
  {
    assert(vh_coords.empty()); // point was hidden
  }
}

template <class Dt>
void _test_interpolation_functions_2_Delaunay(const Dt& dt, const typename Dt::Geom_traits::FT& tolerance)
{
  _test_interpolation_functions_2_Delaunay_without_OutputFunctor(dt, tolerance);
  _test_interpolation_functions_2_Delaunay_with_OutputFunctor(dt, tolerance);
}

template <class Rt>
void _test_interpolation_functions_2_regular(const Rt& rt, const typename Rt::Geom_traits::FT& tolerance)
{
  _test_interpolation_functions_2_regular_without_OutputFunctor(rt, tolerance);
  _test_interpolation_functions_2_regular_with_OutputFunctor(rt, tolerance);
}


#ifndef PYTHONIC_INCLUDE_NUMPY_ARCTAN_HPP
#define PYTHONIC_INCLUDE_NUMPY_ARCTAN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/atan.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    auto atan(T const &v) -> decltype(boost::simd::atan(v))
    {
      return boost::simd::atan(v);
    }
    template <class T>
    std::complex<T> atan(std::complex<T> const &v)
    {
      return std::atan(v);
    }
  }
#define NUMPY_NARY_FUNC_NAME arctan
#define NUMPY_NARY_FUNC_SYM wrapper::atan
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.
#include <fstream>
#include <iostream>
#include <string>

#include "dtt.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <torch/script.h>
#include <torch/torch.h>

#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/parsers/urdf.hpp"

torch::Tensor validTensor(torch::Tensor x) {
  if (x.dim() < 2) {
    x = x.unsqueeze(1);
  }
  return x.to(torch::kDouble);
}

Eigen::VectorXd matrixToVector(Eigen::MatrixXd A) {
  return Eigen::VectorXd(
      Eigen::Map<Eigen::VectorXd>(A.data(), A.cols() * A.rows()));
}

struct RobotModelPinocchio : torch::CustomClassHolder {
  pinocchio::Model model_;
  pinocchio::Data model_data_;
  pinocchio::FrameIndex ee_idx_;

  std::string xml_buffer_;
  std::string ee_joint_name_;

  RobotModelPinocchio(std::string urdf_filename, std::string ee_joint_name) {
    ee_joint_name_ = ee_joint_name;

    std::ifstream stream(urdf_filename);
    xml_buffer_ = std::string((std::istreambuf_iterator<char>(stream)),
                              std::istreambuf_iterator<char>());

    initialize();
  }

  RobotModelPinocchio(std::vector<std::string> serialized_state) {
    ee_joint_name_ = serialized_state[0];
    xml_buffer_ = serialized_state[1];
    initialize();
  }

  void initialize() {
    pinocchio::urdf::buildModelFromXML(xml_buffer_, model_);
    model_data_ = pinocchio::Data(model_);
    ee_idx_ = model_.getFrameId(ee_joint_name_);
  }

  c10::List<torch::Tensor> get_joint_angle_limits(void) {
    c10::List<torch::Tensor> result;
    torch::Tensor l_result = torch::zeros(model_.nq, torch::kFloat32);
    torch::Tensor u_result = torch::zeros(model_.nq, torch::kFloat32);

    for (int i = 0; i < model_.nq; i++) {
      l_result[i] = model_.lowerPositionLimit[i];
      u_result[i] = model_.upperPositionLimit[i];
    }
    result.push_back(l_result);
    result.push_back(u_result);

    return result;
  }

  torch::Tensor get_joint_velocity_limits(void) {
    torch::Tensor result = torch::zeros(model_.nq, torch::kFloat32);

    for (int i = 0; i < model_.nq; i++) {
      result[i] = model_.velocityLimit[i];
    }

    return result;
  }

  c10::List<torch::Tensor> forward_kinematics(torch::Tensor joint_positions) {
    c10::List<torch::Tensor> result;
    torch::Tensor pos_result = torch::zeros(3, torch::kFloat32);
    torch::Tensor quat_result = torch::zeros(4, torch::kFloat32);

    joint_positions = validTensor(joint_positions);
    pinocchio::forwardKinematics(
        model_, model_data_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)));
    pinocchio::updateFramePlacement(model_, model_data_, ee_idx_);

    auto pos_data = model_data_.oMf[ee_idx_].translation().transpose();
    auto quat_data = Eigen::Quaterniond(model_data_.oMf[ee_idx_].rotation());

    for (int i = 0; i < 3; i++) {
      pos_result[i] = pos_data[i];
    }
    quat_result[0] = quat_data.x();
    quat_result[1] = quat_data.y();
    quat_result[2] = quat_data.z();
    quat_result[3] = quat_data.w();

    result.push_back(pos_result);
    result.push_back(quat_result);

    return result;
  }

  torch::Tensor compute_jacobian(torch::Tensor joint_positions) {
    joint_positions = validTensor(joint_positions);

    torch::Tensor result = torch::zeros({6, model_.nq}, torch::kFloat64);
    Eigen::Map<dtt::MatrixXrm<double>> J(result.data_ptr<double>(),
                                         result.size(0), result.size(1));
    pinocchio::computeFrameJacobian(
        model_, model_data_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)), ee_idx_,
        pinocchio::LOCAL_WORLD_ALIGNED, J);

    return result;
  }

  torch::Tensor inverse_dynamics(torch::Tensor joint_positions,
                                 torch::Tensor joint_velocities,
                                 torch::Tensor joint_accelerations) {
    joint_positions = validTensor(joint_positions);
    joint_velocities = validTensor(joint_velocities);
    joint_accelerations = validTensor(joint_accelerations);
    auto q = matrixToVector(dtt::libtorch2eigen<double>(joint_positions));
    auto v = matrixToVector(dtt::libtorch2eigen<double>(joint_velocities));
    auto a = matrixToVector(dtt::libtorch2eigen<double>(joint_accelerations));

    Eigen::Matrix<double, Eigen::Dynamic, 1> tau =
        pinocchio::rnea(model_, model_data_, q, v, a);
    std::vector<int64_t> dims = {tau.rows()};
    return torch::from_blob(tau.data(), dims, torch::kFloat64).clone();
  }
};

TORCH_LIBRARY(torchscript_pinocchio, m) {
  m.class_<RobotModelPinocchio>("RobotModelPinocchio")
      .def(torch::init<std::string, std::string>())
      .def("get_joint_angle_limits",
           &RobotModelPinocchio::get_joint_angle_limits)
      .def("get_joint_velocity_limits",
           &RobotModelPinocchio::get_joint_velocity_limits)
      .def("forward_kinematics", &RobotModelPinocchio::forward_kinematics)
      .def("compute_jacobian", &RobotModelPinocchio::compute_jacobian)
      .def("inverse_dynamics", &RobotModelPinocchio::inverse_dynamics)
      .def_pickle(
          // __getstate__
          [](const c10::intrusive_ptr<RobotModelPinocchio> &self)
              -> std::vector<std::string> {
            return std::vector<std::string>{self->ee_joint_name_,
                                            self->xml_buffer_};
          },
          // __setstate__
          [](std::vector<std::string> state)
              -> c10::intrusive_ptr<RobotModelPinocchio> {
            return c10::make_intrusive<RobotModelPinocchio>(std::move(state));
          });
}


#ifndef GREENCHEETAH
#define GREENCHEETAH

#include <armadillo>

using namespace std;
using namespace arma;

const float hbar = 1.054571817E-34;  // J.s
const float Planck = 6.62607015E-34; // J.s
const float mass = 9.1093837015E-31; // kg
const float exrg = 1.602176634E-19;  // C
const float eta = 1E-15;

class GreensFunction {
    private:
        float   hex, hey;
        float   dx, dy;
        int     nr, nc;
        mat     Potential;

        cx_fmat leadGreensFunction(float);
        cx_fmat Hamiltonian(int);

    public:
        float   Transmission(float, float);
        vec     EnergySweep(float, float, int,float);
        mat     DOS(float, float);

        GreensFunction(mat, float, float, float);
        GreensFunction(void);
};

#endif


#include "EncVector.hpp"
#include "EncMatrix.hpp"
#include "fhe/replicate.h"
#include <NTL/ZZX.h>
#include <vector>
#include <atomic>
#include <thread>
namespace MDL {
#ifdef FHE_THREAD
const long WORKER_NR = 8;
#else
const long WORKER_NR = 1;
#endif
EncVector& EncVector::pack(const Vector<long>  & vec,
                           const EncryptedArray& ea)
{
    assert(vec.size() <= ea.size());
    if (vec.size() < ea.size()) {
        auto tmp(vec);
        tmp.resize(ea.size(), 0);
        ea.encrypt(*this, getPubKey(), tmp);
    } else {
        ea.encrypt(*this, getPubKey(), vec);
    }
    return *this;
}

std::vector<EncVector>EncVector::partition_pack(const Vector<long>  & vec,
                                                const FHEPubKey     & pk,
                                                const EncryptedArray& ea)
{
    auto parts_nr = (vec.size() + ea.size() - 1) / ea.size();
    std::vector<EncVector> ctxts(parts_nr, pk);

    return ctxts;
}

template<>
bool EncVector::unpack(Vector<long>        & result,
                       const FHESecKey     & sk,
                       const EncryptedArray& ea,
                       bool                  negate) const
{
    ea.decrypt(*this, sk, result);
    if (negate) {
        auto plainSpace = ea.getContext().alMod.getPPowR();
        auto half = plainSpace >> 1;
        for (auto &e : result) {
            if (e > half) e -= plainSpace;
        }
    }
    return this->isCorrect();
}

template<>
bool EncVector::unpack(Vector<NTL::ZZX>    & result,
                       const FHESecKey     & sk,
                       const EncryptedArray& ea,
                       bool                  negate) const
{
    ea.decrypt(*this, sk, result);
    if (negate) {
        auto plainSpace = ea.getContext().alMod.getPPowR();
        auto half = plainSpace >> 1;
        for (auto &e : result) {
            // if e is 0, the length of NTL::ZZX is zero
            if (e.rep.length() > 0 && e[0] > half) {
                e[0] -= plainSpace;
            }
        }
    }
    return this->isCorrect();
}

EncVector& EncVector::dot(const EncVector     & oth,
                          const EncryptedArray& ea)
{
    this->multiplyBy(oth);
    totalSums(ea, *this);
    return *this;
}

EncMatrix EncVector::covariance(const EncryptedArray& ea, long actualDimension)
{
    actualDimension = actualDimension == 0 ? ea.size() : actualDimension;
    EncMatrix mat(getPubKey());
    mat.resize(actualDimension, *this);
    std::atomic<long> counter(0);
    std::vector<std::thread> workers;
    for (long wr = 0; wr < WORKER_NR; wr++) {
        workers.push_back(std::move(std::thread([&counter, &actualDimension,
                                                &mat, &ea, this]() {
            long next;
            while((next = counter.fetch_add(1)) < actualDimension) {
                auto tmp(*this);
                replicate(ea, tmp, next);
                mat[next].multiplyBy(tmp);
            }
            })));
    }

    for (auto &&wr : workers) wr.join();
    return mat;
}
} // namespace MDL


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEBAL_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEBAL_HPP_INCLUDED

#include <nt2/linalg/functions/gebal.hpp>
#include <nt2/linalg/details/lapack/declare/gebal.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/linalg/details/utility/workspace.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2 { namespace ext
{

//---------------------------------------------Real-double------------------------------------------------//

  BOOST_DISPATCH_IMPLEMENT  ( gebal_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(A3)(C0)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >)) //a
                              ((container_<nt2::tag::table_,  double_<A1>, S1 >)) //scale
                              (scalar_< integer_<A2> >)                           //ilo
                              (scalar_< integer_<A3> >)                           //ihi
                              (scalar_< ints8_<C0> >)                             //job
                            )
  {
    typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& scale
                                             , A2& ilo, A3& ihi, C0 job) const
     {
        result_type info;
        nt2_la_int  n   = nt2::width(a);
        BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
        nt2_la_int  lda = nt2::max(a.leading_size(), One<size_t>());
        NT2_F77NAME(dgebal) (&job, &n
                            , a.data(), &lda
                            , &ilo, &ihi
                            , scale.data(), &info);
        return info;
     }
  };

//---------------------------------------------Real-single------------------------------------------------//

  BOOST_DISPATCH_IMPLEMENT  ( gebal_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(A3)(C0)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >)) //a
                              ((container_<nt2::tag::table_,  single_<A1>, S1 >)) //scale
                              (scalar_< integer_<A2> >)                           //ilo
                              (scalar_< integer_<A3> >)                           //ihi
                              (scalar_< ints8_<C0> >)                             //job
                            )
  {
    typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& scale
                                             , A2& ilo, A3& ihi, C0 job) const
     {
        result_type info;
        nt2_la_int  n   = nt2::width(a);
        BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
        nt2_la_int  lda = nt2::max(a.leading_size(), One<size_t>());
        NT2_F77NAME(sgebal) (&job, &n
                            , a.data(), &lda
                            , &ilo, &ihi
                            , scale.data(), &info);
        return info;
     }
  };

//---------------------------------------------Complex-single------------------------------------------------//

  BOOST_DISPATCH_IMPLEMENT  ( gebal_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(A3)(C0)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> > , S0 >)) //a
                              ((container_<nt2::tag::table_,  single_<A1>, S1 >)) //scale
                              (scalar_< integer_<A2> >)                           //ilo
                              (scalar_< integer_<A3> >)                           //ihi
                              (scalar_< ints8_<C0> >)                             //job
                            )
  {
    typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& scale
                                             , A2& ilo, A3& ihi, C0 job) const
     {
        result_type info;
        nt2_la_int  n   = nt2::width(a);
        BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
        nt2_la_int  lda = nt2::max(a.leading_size(), One<size_t>());
        NT2_F77NAME(cgebal) (&job, &n
                            , a.data(), &lda
                            , &ilo, &ihi
                            , scale.data(), &info);
        return info;
     }
  };

//---------------------------------------------Complex-double------------------------------------------------//

  BOOST_DISPATCH_IMPLEMENT  ( gebal_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(A3)(C0)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> > , S0 >)) //a
                              ((container_<nt2::tag::table_,  double_<A1>, S1 >)) //scale
                              (scalar_< integer_<A2> >)                           //ilo
                              (scalar_< integer_<A3> >)                           //ihi
                              (scalar_< ints8_<C0> >)                             //job
                            )
  {
    typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& scale
                                             , A2& ilo, A3& ihi, C0 job) const
     {
        result_type info = 0;
        nt2_la_int  n   = nt2::width(a);
        BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
        nt2_la_int  lda = nt2::max(a.leading_size(), One<size_t>());
        NT2_F77NAME(zgebal) (&job, &n
                            , a.data(), &lda
                            , &ilo, &ihi
                            , scale.data(), &info);
        return info;
     }
  };

} }

#endif