#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;

  }


// 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));
          });
}


//==============================================================================
//         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


#pragma once
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>

#include <Eigen/Core>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/srs/spheroid.hpp>
#if BOOST_VERSION >= 107500
#include <boost/geometry/strategy/area.hpp>
#include <boost/geometry/strategy/geographic/area.hpp>
#else
#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/strategies/geographic/area.hpp>
#endif
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>
#include <optional>

#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp::geodetic {

/// Distance calculation strategy.
enum DistanceStrategy { kAndoyer = 0x0, kThomas = 0x1, kVincenty = 0x2 };

using Andoyer = boost::geometry::strategy::distance::andoyer<
    boost::geometry::srs::spheroid<double>>;
using Thomas = boost::geometry::strategy::distance::thomas<
    boost::geometry::srs::spheroid<double>>;
using Vincenty = boost::geometry::strategy::distance::vincenty<
    boost::geometry::srs::spheroid<double>>;

/// Calculate the area
template <typename Geometry>
[[nodiscard]] inline auto area(const Geometry &geometry,
                               const std::optional<System> &wgs) -> double {
  auto spheroid = wgs.has_value()
                      ? boost::geometry::srs::spheroid(wgs->semi_major_axis(),
                                                       wgs->semi_minor_axis())
                      : boost::geometry::srs::spheroid<double>();
  auto strategy = boost::geometry::strategy::area::geographic<
      boost::geometry::strategy::vincenty, 5>(spheroid);
  return boost::geometry::area(geometry, strategy);
}

/// Checks if the first geometry is inside or on border the second geometry
/// using the specified strategy.
template <typename Geometry1, typename Geometry2>
[[nodiscard]] inline auto covered_by(
    const Geometry2 &geometry2, const Eigen::Ref<const Eigen::VectorXd> &lon,
    const Eigen::Ref<const Eigen::VectorXd> &lat, const size_t num_threads)
    -> pybind11::array_t<int8_t> {
  detail::check_eigen_shape("lon", lon, "lat", lat);
  auto size = lon.size();
  auto result =
      pybind11::array_t<int8_t>(pybind11::array::ShapeContainer{{size}});
  auto _result = result.template mutable_unchecked<1>();

  {
    pybind11::gil_scoped_release release;

    // Captures the detected exceptions in the calculation function
    // (only the last exception captured is kept)
    auto except = std::exception_ptr(nullptr);

    detail::dispatch(
        [&](size_t start, size_t end) {
          try {
            for (auto ix = static_cast<int64_t>(start);
                 ix < static_cast<int64_t>(end); ++ix) {
              _result(ix) = static_cast<int8_t>(boost::geometry::covered_by(
                  Geometry1(lon(ix), lat(ix)), geometry2));
            }
          } catch (...) {
            except = std::current_exception();
          }
        },
        size, num_threads);

    if (except != nullptr) {
      std::rethrow_exception(except);
    }
  }
  return result;
}

/// Calculate the distance between two geometries.
template <typename Geometry1, typename Geometry2>
[[nodiscard]] inline auto distance(const Geometry1 &geometry1,
                                   const Geometry2 &geometry2,
                                   const DistanceStrategy strategy,
                                   const std::optional<System> &wgs) -> double {
  auto spheroid = wgs.has_value()
                      ? boost::geometry::srs::spheroid(wgs->semi_major_axis(),
                                                       wgs->semi_minor_axis())
                      : boost::geometry::srs::spheroid<double>();
  switch (strategy) {
    case kAndoyer:
      return boost::geometry::distance(geometry1, geometry2, Andoyer(spheroid));
      break;
    case kThomas:
      return boost::geometry::distance(geometry1, geometry2, Thomas(spheroid));
      break;
    case kVincenty:
      return boost::geometry::distance(geometry1, geometry2,
                                       Vincenty(spheroid));
      break;
  }
  throw std::invalid_argument("unknown strategy: " +
                              std::to_string(static_cast<int>(strategy)));
}

/// Calculate the distance between two geometries.
template <typename Geometry1, typename Geometry2>
[[nodiscard]] inline auto distance(const Geometry1 &geometry1,
                                   const Geometry2 &geometry2) -> double {
  return boost::geometry::distance(geometry1, geometry2);
}

/// Calculate the distance between coordinates.
template <typename Geometry, typename Strategy>
[[nodiscard]] inline auto coordinate_distances(
    const Eigen::Ref<const Eigen::VectorXd> &lon1,
    const Eigen::Ref<const Eigen::VectorXd> &lat1,
    const Eigen::Ref<const Eigen::VectorXd> &lon2,
    const Eigen::Ref<const Eigen::VectorXd> &lat2, const Strategy &strategy,
    const size_t num_threads) -> pybind11::array_t<double> {
  auto size = lon1.size();
  auto result =
      pybind11::array_t<double>(pybind11::array::ShapeContainer{{size}});
  auto _result = result.template mutable_unchecked<1>();

  {
    pybind11::gil_scoped_release release;

    // Captures the detected exceptions in the calculation function
    // (only the last exception captured is kept)
    auto except = std::exception_ptr(nullptr);

    detail::dispatch(
        [&](size_t start, size_t end) {
          try {
            for (auto ix = static_cast<int64_t>(start);
                 ix < static_cast<int64_t>(end); ++ix) {
              _result(ix) = boost::geometry::distance(
                  Geometry(lon1(ix), lat1(ix)), Geometry(lon2(ix), lat2(ix)),
                  strategy);
            }
          } catch (...) {
            except = std::current_exception();
          }
        },
        size, num_threads);

    if (except != nullptr) {
      std::rethrow_exception(except);
    }
  }
  return result;
}

/// Calculate the distance between coordinates.
template <typename Geometry>
[[nodiscard]] inline auto coordinate_distances(
    const Eigen::Ref<const Eigen::VectorXd> &lon1,
    const Eigen::Ref<const Eigen::VectorXd> &lat1,
    const Eigen::Ref<const Eigen::VectorXd> &lon2,
    const Eigen::Ref<const Eigen::VectorXd> &lat2,
    const DistanceStrategy strategy, const std::optional<System> &wgs,
    const size_t num_threads) -> pybind11::array_t<double> {
  detail::check_eigen_shape("lon1", lon1, "lat1", lat1, "lon2", lon2, "lat2",
                            lat2);
  auto spheroid = wgs.has_value()
                      ? boost::geometry::srs::spheroid(wgs->semi_major_axis(),
                                                       wgs->semi_minor_axis())
                      : boost::geometry::srs::spheroid<double>();
  switch (strategy) {
    case kAndoyer:
      return coordinate_distances<Geometry, Andoyer>(
          lon1, lat1, lon2, lat2, Andoyer(spheroid), num_threads);
      break;
    case kThomas:
      return coordinate_distances<Geometry, Thomas>(
          lon1, lat1, lon2, lat2, Thomas(spheroid), num_threads);
      break;
    case kVincenty:
      return coordinate_distances<Geometry, Vincenty>(
          lon1, lat1, lon2, lat2, Vincenty(spheroid), num_threads);
      break;
  }
  throw std::invalid_argument("unknown strategy: " +
                              std::to_string(static_cast<int>(strategy)));
}

}  // namespace pyinterp::geodetic


/*
 * BSD 3-Clause License
 * Copyright (c) 2020, Levente Kiss
 * All rights reserved.
 *
 * You may obtain a copy of the License at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#ifndef OLP_LOAMSLAMCALCULATOR_HPP
#define OLP_LOAMSLAMCALCULATOR_HPP

#include <thread>

#include <boost/thread/barrier.hpp>

#include <pcl/common/io.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>

#include "../../helpers/ViewerHelper.h"
#include "Calculator.h"

namespace olp
{
namespace compute
{
template<typename PointType>
class LOAMSLAMCalculator : public Calculator<PointType>
{
public:
    LOAMSLAMCalculator(TransformData data) : Calculator<PointType>(data), BEAMCOUNT_(16), _barrier(BEAMCOUNT_ + 1)
    {
        _previousCloud = pcl::PointCloud<PointType>().makeShared();
    }

    TransformData calculate(typename pcl::PointCloud<PointType>::ConstPtr input) override;

    const std::string stringId() const override
    {
        return "LOAM";
    }

protected:
    unsigned const BEAMCOUNT_;
    const double SCAN_PERIOD_ = 0.1;
    const float THRESHOLD_ = 0.1;
    const double NEARBY_SCAN_ = 2.5;
private:
    boost::barrier _barrier;
    typename pcl::PointCloud<PointType>::Ptr _previousCloud;

    inline int _pointID(const PointType& point);

    void _calculateDistortion(typename pcl::PointCloud<PointType>::Ptr points);

    void _transformToStart(PointType const* const pi,
                           PointType* const po,
                           const Eigen::Vector3d& translation,
                           const Eigen::Vector3d& rotation);

    void _findPlanarAndEdgePoint(const typename pcl::PointCloud<PointType>::ConstPtr input,
                                 std::vector<int>& planarPointIndexes,
                                 std::vector<int>& edgePointIndexes,
                                 int maxEdgePoints, int maxPlanarPoints,
                                 float threshold);

    void _findCorrespondances(const typename pcl::PointCloud<PointType>::ConstPtr previousCloud,
                              const typename pcl::PointCloud<PointType>::ConstPtr currentCloud,
                              const std::vector<int>& planarPointIndexes,
                              const std::vector<int>& edgePointIndexes,
                              Eigen::Vector3d& rotation,
                              Eigen::Vector3d& translation,
                              float threshold);
};

template<typename PointType>
TransformData LOAMSLAMCalculator<PointType>::calculate(typename pcl::PointCloud<PointType>::ConstPtr input)
{
    typename pcl::PointCloud<PointType>::Ptr correctedIn(new pcl::PointCloud<PointType>);
    pcl::copyPointCloud(*input, *correctedIn);
    std::vector<int> planarPoints;
    std::vector<int> edgePoints;
    std::shared_ptr<std::map<int, float>> cValues(new std::map<int, float>);

    _calculateDistortion(correctedIn);
    _findPlanarAndEdgePoint(correctedIn, planarPoints, edgePoints, 2, 4, THRESHOLD_);

    if (_previousCloud->size() > 0)
    {
        Eigen::Vector3d rotation, translation;
        _findCorrespondances(_previousCloud, correctedIn, planarPoints, edgePoints, rotation, translation, 25);
        TransformData result = TransformData(70);
        result.transform *= Eigen::AngleAxisd(rotation(2), Eigen::Vector3d::UnitZ());
        result.transform.translation() = translation;
        return result;
    }
    pcl::copyPointCloud(*input, *_previousCloud);
    return TransformData();
}

template<typename PointType>
inline int LOAMSLAMCalculator<PointType>::_pointID(const PointType& point)
{
    float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
    int scanID = 0;
    scanID = int((angle + 15) / 2 + 0.5);
    if (scanID > (BEAMCOUNT_ - 1) || scanID < 0)
    {
        return -1;
    }
    return scanID;
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_calculateDistortion(typename pcl::PointCloud<PointType>::Ptr points)
{
    bool halfPassed = false;
    float startOri = -atan2(points->points[0].y, points->points[0].x);
    float endOri = -atan2(points->points[points->points.size() - 1].y,
                          points->points[points->points.size() - 1].x) +
                   2 * M_PI;
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }
    int cloudSize = points->points.size();
    for (int i = 0; i < cloudSize; i++)
    {
        int scanID = _pointID(points->points[i]);

        float ori = -atan2(points->points[i].y, points->points[i].x);
        if (!halfPassed)
        {
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }

        float relTime = (ori - startOri) / (endOri - startOri);
        points->points[i].intensity = scanID + SCAN_PERIOD_ * relTime;
    }
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_transformToStart(
    PointType const* const pi,
    PointType* const po,
    const Eigen::Vector3d& translation,
    const Eigen::Vector3d& rotation)
{
    //interpolation ratio
    double s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD_;

    auto scaledT = translation * s;
    auto scaledR = rotation * s;

    float rx = s * scaledR[0];
    float ry = s * scaledR[1];
    float rz = s * scaledR[2];
    float tx = s * scaledT[0];
    float ty = s * scaledT[1];
    float tz = s * scaledT[2];

    float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
    float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
    float z1 = (pi->z - tz);

    float x2 = x1;
    float y2 = cos(rx) * y1 + sin(rx) * z1;
    float z2 = -sin(rx) * y1 + cos(rx) * z1;

    po->x = cos(ry) * x2 - sin(ry) * z2;
    po->y = y2;
    po->z = sin(ry) * x2 + cos(ry) * z2;
    po->intensity = pi->intensity;
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_findPlanarAndEdgePoint(
    const typename pcl::PointCloud<PointType>::ConstPtr input,
    std::vector<int>& planarPointIndexes,
    std::vector<int>& edgePointIndexes,
    int maxEdgePoints, int maxPlanarPoints,
    float threshold)
{
    const float MAXDISTANCE = 0.05;
    std::vector<std::shared_ptr<std::vector<std::pair<int, Eigen::Vector3f>>>> pointsPerBeam;
    for (auto i = 0; i < BEAMCOUNT_; ++i)
    {
        pointsPerBeam.push_back(std::shared_ptr<std::vector<std::pair<int, Eigen::Vector3f>>>(
            new std::vector<std::pair<int, Eigen::Vector3f>>()));
    }
    for (auto i = 0; i < input->size(); ++i)
    {
        int beamIndex = _pointID(input->points[i]);
        Eigen::Vector3f tmpPt(input->points[i].x, input->points[i].y, input->points[i].z);
        pointsPerBeam[beamIndex]->push_back(std::pair<int, Eigen::Vector3f>(i, tmpPt));

    }
    std::vector<std::shared_ptr<std::vector<int>>> beamEdgesPoints;
    std::vector<std::shared_ptr<std::vector<int>>> beamPlanarPoints;

    auto f = [this, &threshold, &maxEdgePoints, &maxPlanarPoints, &MAXDISTANCE](
        const std::vector<std::pair<int, Eigen::Vector3f>>& points,
        std::shared_ptr<std::vector<int>> selectedEdgePoints,
        std::shared_ptr<std::vector<int>> selectedPlanarPoints)
    {
        std::vector<float> cloudCurvature(points.size());
        std::vector<int> cloudSortInd(points.size());
        std::vector<int> cloudNeighborPicked(points.size());
        std::vector<int> cloudLabel(points.size());
        if (points.size() > 11)
        {
            for (int i = 5; i < points.size() - 6; i++)
            {
                float diffX =
                    points.at(i - 5).second.x() + points.at(i - 4).second.x() + points.at(i - 3).second.x() +
                    points.at(i - 2).second.x() + points.at(i - 1).second.x() - 10 * points.at(i).second.x() +
                    points.at(i + 1).second.x() + points.at(i + 2).second.x() + points.at(i + 3).second.x() +
                    points.at(i + 4).second.x() + points.at(i + 5).second.x();
                float diffY =
                    points.at(i - 5).second.y() + points.at(i - 4).second.y() + points.at(i - 3).second.y() +
                    points.at(i - 2).second.y() + points.at(i - 1).second.y() - 10 * points.at(i).second.y() +
                    points.at(i + 1).second.y() + points.at(i + 2).second.y() + points.at(i + 3).second.y() +
                    points.at(i + 4).second.y() + points.at(i + 5).second.y();
                float diffZ =
                    points.at(i - 5).second.z() + points.at(i - 4).second.z() + points.at(i - 3).second.z() +
                    points.at(i - 2).second.z() + points.at(i - 1).second.z() - 10 * points.at(i).second.z() +
                    points.at(i + 1).second.z() + points.at(i + 2).second.z() + points.at(i + 3).second.z() +
                    points.at(i + 4).second.z() + points.at(i + 5).second.z();
                float c = diffX * diffX + diffY * diffY + diffZ * diffZ;
                cloudCurvature[i] = c;
                cloudSortInd[i] = i;
                cloudNeighborPicked[i] = 0;
                cloudLabel[i] = 0;
            }

            for (int i = 0; i < 6; ++i)
            {
                int startIndex = 5 + (points.size() - 10) * i / 6;
                int endIndex = 5 + (points.size() - 10) * (i + 1) / 6 - 1;
                std::sort(cloudSortInd.begin() + startIndex, cloudSortInd.begin() + endIndex + 1,
                          [&cloudCurvature](int i, int j)
                          {
                              return cloudCurvature[i] < cloudCurvature[j];
                          });
                int largestPickedNum = 0;
                for (int k = endIndex; k >= startIndex; k--)
                {
                    int ind = cloudSortInd[k];

                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] > threshold)
                    {

                        largestPickedNum++;

                        if (largestPickedNum <= maxEdgePoints)
                        {
                            cloudLabel[ind] = 1;
                            selectedEdgePoints->push_back(points[ind].first);
                        }
                        else
                        {
                            break;
                        }

                        cloudNeighborPicked[ind] = 1;
                        //not selecting close points
                        for (int l = 1; l <= 5; l++)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l - 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l - 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l - 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l + 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l + 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l + 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }

                }

                int smallestPickedNum = 0;
                for (int k = startIndex; k <= endIndex; k++)
                {
                    int ind = cloudSortInd[k];

                    if (ind < 1)
                    {
                        continue;
                    }

                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] < threshold)
                    {

                        cloudLabel[ind] = -1;
                        selectedPlanarPoints->push_back(points.at(ind).first);

                        smallestPickedNum++;
                        if (smallestPickedNum >= maxPlanarPoints)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind] = 1;
                        for (int l = 1; l <= 5; l++)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l - 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l - 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l - 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l + 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l + 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l + 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
            }
        }
        _barrier.wait();
    };
    int i = 0;
    for (auto beamPoints : pointsPerBeam)
    {
        auto edgeTmp = std::shared_ptr<std::vector<int>>(new std::vector<int>);
        beamEdgesPoints.push_back(edgeTmp);
        auto planeTmp = std::shared_ptr<std::vector<int>>(new std::vector<int>);
        beamPlanarPoints.push_back(planeTmp);
        std::thread(f, *beamPoints, edgeTmp, planeTmp).detach();
        ++i;
    }
    _barrier.wait();
    for (auto edgePoints : beamEdgesPoints)
    {
        edgePointIndexes.insert(edgePointIndexes.end(), edgePoints->begin(), edgePoints->end());
    }
    for (auto planarPoints : beamPlanarPoints)
    {
        planarPointIndexes.insert(planarPointIndexes.end(), planarPoints->begin(), planarPoints->end());
    }
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_findCorrespondances(
    const typename pcl::PointCloud<PointType>::ConstPtr previousCloud,
    const typename pcl::PointCloud<PointType>::ConstPtr currentCloud,
    const std::vector<int>& planarPointIndexes,
    const std::vector<int>& edgePointIndexes,
    Eigen::Vector3d& rotation,
    Eigen::Vector3d& translation,
    float threshold)
{
    Eigen::Vector3d tr = Eigen::Vector3d::Zero();
    Eigen::Vector3d rt = Eigen::Vector3d::Zero();

    typename pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
    typename pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());
    kdtreeCornerLast->setInputCloud(previousCloud);
    kdtreeSurfLast->setInputCloud(previousCloud);

    int cornerPointsSharpNum = edgePointIndexes.size();
    int surfPointsFlatNum = planarPointIndexes.size();

    // neighbour edgePoint indices (J,K)
    std::map<int, int> edgeJPointIndices;
    std::map<int, int> edgeLPointIndices;

    // neighbour planarPoint indices (J,K,L)
    std::map<int, int> planarJPointIndices;
    std::map<int, int> planarLPointIndices;
    std::map<int, int> planarMPointIndices;

    PointType coeff;
    typename pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
    typename pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());

    if (planarPointIndexes.size() > 100 && edgePointIndexes.size() > 10)
    {
        for (int iterationCount = 0; iterationCount < 25; ++iterationCount)
        {
            std::vector<int> pointSearchInd(1);
            std::vector<float> pointSearchSqDis(1);

            for (auto idx : planarPointIndexes)
            {
                PointType currentPt = currentCloud->points[idx];
                _transformToStart(&currentCloud->points[idx], &currentPt, tr, rt);
                if (iterationCount % 5 == 0)
                {
                    kdtreeSurfLast->nearestKSearch(currentPt, 1, pointSearchInd, pointSearchSqDis);
                    int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                    if (pointSearchSqDis[0] < threshold)
                    {
                        closestPointInd = pointSearchInd[0];
                        int closestPointScan = _pointID(previousCloud->points[closestPointInd]);

                        float pointSqDis, minPointSqDis2 = threshold, minPointSqDis3 = threshold;
                        for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++)
                        {
                            int currentPointID = _pointID(previousCloud->points[closestPointInd]);
                            if (currentPointID > closestPointScan + NEARBY_SCAN_)
                            {
                                break;
                            }
                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);
                            if (currentPointID <= closestPointScan && pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                            else if (currentPointID > closestPointScan && pointSqDis < minPointSqDis3)
                            {
                                minPointSqDis3 = pointSqDis;
                                minPointInd3 = j;
                            }
                        }
                        for (int j = closestPointInd - 1; j >= 0; j--)
                        {
                            int currentPointID = _pointID(previousCloud->points[closestPointInd]);
                            if (currentPointID < closestPointScan - NEARBY_SCAN_)
                            {
                                break;
                            }
                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);
                            if (currentPointID >= closestPointScan && pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                            else if (_pointID(previousCloud->points[j]) < closestPointScan &&
                                     pointSqDis < minPointSqDis3)
                            {
                                minPointSqDis3 = pointSqDis;
                                minPointInd3 = j;
                            }
                        }
                    }

                    planarJPointIndices[idx] = closestPointInd;
                    planarLPointIndices[idx] = minPointInd2;
                    planarMPointIndices[idx] = minPointInd3;
                    pointSearchInd.clear();
                    pointSearchSqDis.clear();
                }

                if (planarMPointIndices[idx] >= 0 && planarLPointIndices[idx] >= 0)
                {
                    Eigen::Vector3f J = previousCloud->points[planarJPointIndices[idx]].getVector3fMap();
                    Eigen::Vector3f L = previousCloud->points[planarLPointIndices[idx]].getVector3fMap();
                    Eigen::Vector3f M = previousCloud->points[planarMPointIndices[idx]].getVector3fMap();

                    float pa = (L[1] - J[1]) * (M[2] - J[2])
                               - (M[1] - J[1]) * (L[2] - L[2]);

                    float pb = (L[2] - J[2]) * (M[0] - J[0])
                               - (M[2] - J[2]) * (L[0] - J[0]);

                    float pc = (L[0] - J[0]) * (M[1] - J[1])
                               - (M[0] - J[0]) * (L[1] - J[1]);
                    float pd = -(pa * J[0] + pb * J[1] + pc * J[2]);

                    float ps = sqrt(pa * pa + pb * pb + pc * pc);

                    pa /= ps;
                    pb /= ps;
                    pc /= ps;
                    pd /= ps;

                    float pd2 = pa * currentPt.x + pb * currentPt.y + pc * currentPt.z + pd;

                    float s = 1;
                    if (iterationCount >= 5)
                    {
                        s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(currentPt.x * currentPt.x
                                                            + currentPt.y * currentPt.y +
                                                            currentPt.z * currentPt.z));
                    }

                    coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;

                    if (s > 0.1 && pd2 != 0)
                    {

                        laserCloudOri->push_back(currentCloud->points[idx]);
                        coeffSel->push_back(coeff);
                    }
                }
            }

            for (auto idx : edgePointIndexes)
            {
                PointType currentPt = currentCloud->points[idx];
                _transformToStart(&currentCloud->points[idx], &currentPt, tr, rt);
                if (iterationCount % 5 == 0)
                {
                    kdtreeCornerLast->nearestKSearch(currentPt, 1, pointSearchInd, pointSearchSqDis);
                    int closestPointInd = -1, minPointInd2 = -1;
                    if (pointSearchSqDis[0] < threshold)
                    {
                        closestPointInd = pointSearchInd[0];
                        int closestPointScan = _pointID(previousCloud->points[closestPointInd]);

                        float pointSqDis, minPointSqDis2 = threshold;
                        for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
                        {
                            int currentPointID = _pointID(previousCloud->points[j]);
                            if (currentPointID > closestPointScan + NEARBY_SCAN_)
                            {
                                break;
                            }

                            if (currentPointID <= closestPointScan)
                            {
                                continue;
                            }
                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);

                            if (pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }

                        for (int j = closestPointInd - 1; j >= 0; j--)
                        {
                            int currentPointID = _pointID(previousCloud->points[j]);
                            if (currentPointID < closestPointScan - NEARBY_SCAN_)
                            {
                                break;
                            }
                            if (currentPointID >= closestPointScan)
                            {
                                continue;
                            }

                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);

                            if (pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }
                    }
                    /*********************************************/

                    edgeJPointIndices[idx] = closestPointInd;
                    edgeLPointIndices[idx] = minPointInd2;
                    pointSearchInd.clear();
                    pointSearchSqDis.clear();
                }

                if (edgeLPointIndices[idx] >= 0)
                {
                    Eigen::Vector3f I = currentPt.getVector3fMap();
                    Eigen::Vector3f J = previousCloud->points[edgeJPointIndices[idx]].getVector3fMap();
                    Eigen::Vector3f L = previousCloud->points[edgeLPointIndices[idx]].getVector3fMap();

                    float num = (I - J).cross(I - L).norm();

                    float deNum = (J - L).norm();

                    float d = num / deNum;

                    float la = ((J[1] - L[1]) * ((I[0] - J[0]) * (I[1] - L[1]) - (I[0] - L[0]) * (I[1] - J[1]))
                                + (J[2] - L[2]) * ((I[0] - J[0]) * (I[2] - L[2]) - (I[0] - L[0]) * (I[2] - J[2]))) /
                               num / deNum;

                    float lb = -((J[0] - L[0]) * ((I[0] - J[0]) * (I[1] - L[1]) - (I[0] - L[0]) * (I[1] - J[1]))
                                 -
                                 (J[2] - L[2]) * ((I[1] - J[1]) * (I[2] - L[2]) - (I[1] - L[1]) * (I[2] - J[2]))) /
                               num / deNum;

                    float lc = -((J[0] - L[0]) * ((I[0] - J[0]) * (I[2] - L[2]) - (I[0] - L[0]) * (I[2] - J[2]))
                                 +
                                 (J[1] - L[1]) * ((I[1] - J[1]) * (I[2] - L[2]) - (I[1] - L[1]) * (I[2] - J[2]))) /
                               num / deNum;

                    float s = 1;
                    if (iterationCount >= 5)
                    {
                        s = 1 - 1.8 * fabs(d);
                    }

                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * d;

                    if (s > 0.1 && d != 0)
                    {
                        laserCloudOri->push_back(currentCloud->points[idx]);
                        coeffSel->push_back(coeff);
                    }
                }
            }
            int pointSelNum = laserCloudOri->points.size();

            if (pointSelNum < 10)
            {
                continue;
            }
            Eigen::MatrixXf matA = Eigen::MatrixXf::Zero(pointSelNum, 6);
            Eigen::MatrixXf matAt = Eigen::MatrixXf::Zero(6, pointSelNum);
            Eigen::MatrixXf matAtA = Eigen::MatrixXf::Zero(6, 6);
            Eigen::VectorXf matB = Eigen::VectorXf::Zero(pointSelNum);
            Eigen::VectorXf matAtB = Eigen::VectorXf::Zero(6);
            Eigen::VectorXf matX = Eigen::VectorXf::Zero(6);

            Eigen::MatrixXf matP = Eigen::MatrixXf::Zero(6, 6);
            bool isDegenerate = false;

            for (int i = 0; i < pointSelNum; i++)
            {
                PointType pointOri = laserCloudOri->points[i];
                coeff = coeffSel->points[i];

                float s = 1;

                float srx = sin(s * rt[0]);
                float crx = cos(s * rt[0]);
                float sry = sin(s * rt[1]);
                float cry = cos(s * rt[1]);
                float srz = sin(s * rt[2]);
                float crz = cos(s * rt[2]);
                float tx = s * tr[0];
                float ty = s * tr[1];
                float tz = s * tr[2];

                float arx = (-s * crx * sry * srz * pointOri.x + s * crx * crz * sry * pointOri.y +
                             s * srx * sry * pointOri.z
                             + s * tx * crx * sry * srz - s * ty * crx * crz * sry - s * tz * srx * sry) * coeff.x
                            + (s * srx * srz * pointOri.x - s * crz * srx * pointOri.y + s * crx * pointOri.z
                               + s * ty * crz * srx - s * tz * crx - s * tx * srx * srz) * coeff.y
                            + (s * crx * cry * srz * pointOri.x - s * crx * cry * crz * pointOri.y -
                               s * cry * srx * pointOri.z
                               + s * tz * cry * srx + s * ty * crx * cry * crz - s * tx * crx * cry * srz) *
                              coeff.z;

                float ary = ((-s * crz * sry - s * cry * srx * srz) * pointOri.x
                             + (s * cry * crz * srx - s * sry * srz) * pointOri.y - s * crx * cry * pointOri.z
                             + tx * (s * crz * sry + s * cry * srx * srz) +
                             ty * (s * sry * srz - s * cry * crz * srx)
                             + s * tz * crx * cry) * coeff.x
                            + ((s * cry * crz - s * srx * sry * srz) * pointOri.x
                               + (s * cry * srz + s * crz * srx * sry) * pointOri.y - s * crx * sry * pointOri.z
                               + s * tz * crx * sry - ty * (s * cry * srz + s * crz * srx * sry)
                               - tx * (s * cry * crz - s * srx * sry * srz)) * coeff.z;

                float arz = ((-s * cry * srz - s * crz * srx * sry) * pointOri.x +
                             (s * cry * crz - s * srx * sry * srz) * pointOri.y
                             + tx * (s * cry * srz + s * crz * srx * sry) -
                             ty * (s * cry * crz - s * srx * sry * srz)) * coeff.x
                            + (-s * crx * crz * pointOri.x - s * crx * srz * pointOri.y
                               + s * ty * crx * srz + s * tx * crx * crz) * coeff.y
                            + ((s * cry * crz * srx - s * sry * srz) * pointOri.x +
                               (s * crz * sry + s * cry * srx * srz) * pointOri.y
                               + tx * (s * sry * srz - s * cry * crz * srx) -
                               ty * (s * crz * sry + s * cry * srx * srz)) * coeff.z;

                float atx = -s * (cry * crz - srx * sry * srz) * coeff.x + s * crx * srz * coeff.y
                            - s * (crz * sry + cry * srx * srz) * coeff.z;

                float aty = -s * (cry * srz + crz * srx * sry) * coeff.x - s * crx * crz * coeff.y
                            - s * (sry * srz - cry * crz * srx) * coeff.z;

                float atz = s * crx * sry * coeff.x - s * srx * coeff.y - s * crx * cry * coeff.z;

                float d2 = coeff.intensity;

                matA(i, 0) = arx;
                matA(i, 1) = ary;
                matA(i, 2) = arz;
                matA(i, 3) = atx;
                matA(i, 4) = aty;
                matA(i, 5) = atz;
                matB(i) = -0.05 * d2;
            }
            matAt = matA.transpose();

            matAtA = matAt * matA;
            matAtB = matAt * matB;
            matX = matAtA.householderQr().solve(matAtB);

            if (iterationCount == 0)
            {

                Eigen::VectorXf matE = Eigen::VectorXf::Zero(6);
                Eigen::MatrixXf matV = Eigen::MatrixXf::Zero(6, 6);
                Eigen::MatrixXf matV2 = Eigen::MatrixXf::Zero(6, 6);

                Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(matAtA);
                matV = solver.eigenvectors();
                matE = solver.eigenvalues();
                matV2 = matV;

                isDegenerate = false;
                float eignThre[6] = {10, 10, 10, 10, 10, 10};
                for (int i = 5; i >= 0; i--)
                {
                    if (matE.transpose()(i) < eignThre[i])
                    {
                        for (int j = 0; j < 6; j++)
                        {
                            matV2(i, j) = 0;
                        }
                        isDegenerate = true;
                    }
                    else
                    {
                        break;
                    }
                }

                matP = matV.inverse() * matV2;
            }

            if (isDegenerate)
            {
                Eigen::VectorXf matX2(matX);
                matX = matP * matX2;
            }

            rt[0] += matX[0];
            rt[1] += matX[1];
            rt[2] += matX[2];
            tr[0] += matX[3];
            tr[1] += matX[4];
            tr[2] += matX[5];

            for (int i = 0; i < 3; i++)
            {
                if (isnan(tr[i]))
                    tr[i] = 0;
                if (isnan(rt[i]))
                    rt[i] = 0;
            }

            float deltaR = sqrt(
                pow((matX[0] / M_PI * 180.0), 2) +
                pow((matX[1] / M_PI * 180.0), 2) +
                pow((matX[2] / M_PI * 180.0), 2));
            float deltaT = sqrt(
                pow(matX[3] * 100, 2) +
                pow(matX[4] * 100, 2) +
                pow(matX[5] * 100, 2));

            //  std::cout << "delta R: " << deltaR << " delta T: " << deltaT << std::endl;

            if (deltaR < 0.1 && deltaT < 0.1)
            {
                break;
            }
        }
    }
    else
    {
        std::cerr << "Not enough features." << std::endl;
    }

    rotation = rt;
    translation = tr;
}

} // compute
} // olp

#endif //OLP_LOAMSLAMCALCULATOR_HPP


#include <iostream>
#include <vector>

//#define NUMBER_DOUBLE 1
#define NUMBER_DOUBLE_DOUBLE 1

#include "Nbodies/summation.h"
#include "Integration/methods.h"
#include "Writer/writer.h"
#include "Nbodies/energy.h"

#include <boost/multiprecision/float128.hpp>

#ifdef NUMBER_DOUBLE_DOUBLE
#include <qd/dd_real.h>
#include <qd/fpu.h>
#endif

#include <string>

using namespace std;
using namespace boost::multiprecision;

#include <chrono>
int main() {

#ifdef NUMBER_DOUBLE_DOUBLE
    unsigned int oldcw;
    fpu_fix_start(&oldcw);
#endif

    using current_type = dd_real;

    std::vector<Body<current_type>> bodies;

    //make_universe(bodies, 40, current_type(0.0), current_type(0.0), current_type(0.0));
/*
    bodies.push_back(Body<current_type>({current_type(0),current_type(0),current_type(0)},{current_type(0),current_type(0),current_type(0)},current_type(2e14)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.4e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.3e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.2e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.1e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.56e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.5e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.51e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
*/

    current_type a("0");
    current_type b("3");
    current_type c("6");
    current_type mm("2e14");
    current_type diss("1.3e3");

    bodies.push_back(Body<current_type>({a,a,a},{a,a,a},mm));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.3e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.2e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.1e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.56e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.5e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.51e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.4e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));

    current_type init_energy = summation<current_type, kinetic_energy_proxy<current_type>>(kinetic_energy_proxy(bodies), bodies.size()) / current_type(2) +
                         summation<current_type, potential_energy_proxy<current_type>>(potential_energy_proxy(bodies), bodies.size() * bodies.size()) / current_type(2) ;

    vec<current_type> init_impulse_moment = summation<vec<current_type>, impulse_moment_proxy<vec<current_type>,current_type>>(impulse_moment_proxy<vec<current_type>,current_type>(bodies), bodies.size());

    current_type total_mass = summation<current_type, total_mass_proxy<current_type>>(total_mass_proxy<current_type>(bodies), bodies.size());
    vec<current_type> init_center_mass = summation<vec<current_type>, mass_center_proxy<vec<current_type>, current_type>>(mass_center_proxy<vec<current_type>, current_type>(bodies), bodies.size());
    init_center_mass = init_center_mass / total_mass;
    vec<current_type> init_vel_mass = summation<vec<current_type>, mass_vel_proxy<vec<current_type>, current_type>>(mass_vel_proxy<vec<current_type>, current_type>(bodies), bodies.size());
    init_vel_mass = init_vel_mass / total_mass;

    for( int i = 0; i < bodies.size(); i++){
       // bodies[i].r -= init_center_mass;
       // bodies[i].v -= init_vel_mass;
    }

    json data_energy, data_impulse_moment, data_center, data_bodies;
    current_type h("0.1");
    int iterations = 100000;

    std::vector<current_type> coefs = initDDCoef<current_type>();
    auto start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < iterations; i++) {
         dormanPrince8(bodies, h, coefs);
         //RungeKutta4(bodies, h);
        if(true ) {
            for(int j = 0; j < bodies.size(); j++){

#ifdef NUMBER_DOUBLE_DOUBLE
                    data_bodies[j]["X"][i] = bodies[j].r.X._hi();
                    data_bodies[j]["Y"][i] = bodies[j].r.Y._hi();
                    data_bodies[j]["Z"][i] = bodies[j].r.Z._hi();
#endif
#ifdef NUMBER_DOUBLE

              data_bodies[j]["X"][i] = bodies[j].r.X;
              data_bodies[j]["Y"][i] = bodies[j].r.Y;
              data_bodies[j]["Z"][i] = bodies[j].r.Z;
                    /*
                    data_bodies[j]["X"][i] = bodies[j].r.X.str(32);
                    data_bodies[j]["Y"][i] = bodies[j].r.Y.str(32);
                    data_bodies[j]["Z"][i] = bodies[j].r.Z.str(32);
                     */
#endif
            }

            vec<current_type> center_mass = summation<vec<current_type>, mass_center_proxy<vec<current_type>, current_type>>(mass_center_proxy<vec<current_type>, current_type>(bodies), bodies.size());
            center_mass = center_mass / total_mass;

            current_type energy = summation<current_type, kinetic_energy_proxy<current_type>>(kinetic_energy_proxy(bodies), bodies.size()) / current_type(2) +
                            summation<current_type, potential_energy_proxy<current_type>>(potential_energy_proxy(bodies),bodies.size() * bodies.size()) / current_type(2);

            vec<current_type> impulse_moment = summation<vec<current_type>, impulse_moment_proxy<vec<current_type>,current_type>>(impulse_moment_proxy<vec<current_type>,current_type>(bodies), bodies.size());

            data_energy["n"].push_back(i);
#ifdef NUMBER_DOUBLE_DOUBLE
            data_energy["energy"].push_back((abs((energy - init_energy)/init_energy)).to_string());
#endif
#ifdef NUMBER_DOUBLE
            //data_energy["energy"].push_back(abs((energy - init_energy)/init_energy).str(32));
          data_energy["energy"].push_back(abs((energy - init_energy)/init_energy));
#endif

            data_impulse_moment["n"].push_back(i);
#ifdef NUMBER_DOUBLE_DOUBLE
            data_impulse_moment["moment"].push_back(abs((impulse_moment - init_impulse_moment).Len() / init_impulse_moment.Len()).to_string());
#endif
#ifdef NUMBER_DOUBLE
            //data_impulse_moment["moment"].push_back(abs((impulse_moment - init_impulse_moment).Len() / init_impulse_moment.Len()).str(32));
          data_impulse_moment["moment"].push_back(abs((impulse_moment - init_impulse_moment).Len() / init_impulse_moment.Len()));
#endif

                data_center["n"].push_back(i);
#ifdef NUMBER_DOUBLE_DOUBLE
                data_center["center"].push_back(abs((init_center_mass - center_mass).Len()).to_string());
#endif
#ifdef NUMBER_DOUBLE
                //data_center["center"].push_back(abs((init_center_mass - center_mass).Len()).str(32));
                data_center["center"].push_back(abs((init_center_mass - center_mass).Len()));
#endif

        }
    }
    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = duration_cast<std::chrono::microseconds>(stop - start);
    printf("time: %lld microseconds \n", duration.count());

    writer<current_type> w;
    w.writeRes("../log.json", data_energy);
    w.writeRes("../bodies.json", data_bodies);
    w.writeRes("../moment.json", data_impulse_moment);
    w.writeRes("../center.json", data_center);

#ifdef NUMBER_DOUBLE_DOUBLE
    fpu_fix_end(&oldcw);
#endif
    return 0;
}


/**
 * Copyright (C) 2016, Wu Tao. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <string>
#include <boost/functional/hash.hpp>
#include "FilterStrategy.h"
#include "Slice.h"

namespace lessdb {

class BloomFilterStrategy : public FilterStrategy {
  //
  // m: the total number of bits our bloom filter has.
  // n: the number of elements.
  // k: the number of hash functions.
  // For a given m and n, the value of k that minimizes the false positive
  // probability is ln2*(m/n).
  //
  // Use double-hashing to generate a sequence of hash values.
  // See analysis in [Kirsch,Mitzenmacher 2006].
  // The double-hashing functions is of the form gi(x) = h1(x) + i*h2(x).
  //

  const size_t kMinBloomFilterLength = 64;

 public:
  BloomFilterStrategy(size_t bits_per_key) : bits_per_byte_(bits_per_key) {
    k_ = static_cast<size_t>(bits_per_key * 0.69);  // ln2 ~= 0.69
  }

  inline void Put(const Slice &key, Slice &bits) override {
    size_t h1 = boost::hash_range(key.RawData(), key.RawData() + key.Len());
    size_t h2 = (h1 >> 17) | (h1 << 15);  // Rotate right 17 bits
    for (size_t i = 0; i < k_; ++i) {
      size_t g = (h1 + i * h2) % bits.Len();
      bits[g / 8] |= (1 << (g % 8));
    }
  }

  inline bool MightContain(const Slice &key, const Slice &bits) const override {
    size_t h1 = boost::hash_range(key.RawData(), key.RawData() + key.Len());
    size_t h2 = (h1 >> 17) | (h1 << 15);  // Rotate right 17 bits
    for (size_t i = 0; i < k_; ++i) {
      size_t g = (h1 + i * h2) % bits.Len();
      if (!(bits[g / 8] & (1 << (g % 8))))
        return false;
    }
    return true;
  }

 private:
  size_t bits_per_byte_;
  size_t k_;
};

FilterStrategy *FilterStrategy::Default(size_t bits_per_bytes) {
  return new BloomFilterStrategy(bits_per_bytes);
}

}  // namespace lessdb


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
// Copyright (c) 2022 Ekaterina Chukavina <kate@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ZK_COMMITMENTS_KZG_HPP
#define CRYPTO3_ZK_COMMITMENTS_KZG_HPP

#include <tuple>
#include <vector>
#include <type_traits>

#include <boost/assert.hpp>
#include <boost/iterator/zip_iterator.hpp>
#include <boost/accumulators/accumulators.hpp>

#include <nil/crypto3/math/polynomial/polynomial.hpp>
#include <nil/crypto3/algebra/type_traits.hpp>
#include <nil/crypto3/algebra/algorithms/pair.hpp>
#include <nil/crypto3/algebra/pairing/pairing_policy.hpp>

using namespace nil::crypto3::math;

#include <nil/crypto3/math/polynomial/polynomial.hpp>

namespace nil {
    namespace crypto3 {
        namespace zk {
            namespace commitments {
                template<typename CurveType>
                struct kzg {

                    typedef CurveType curve_type;
                    typedef algebra::pairing::pairing_policy<curve_type> pairing;
                    typedef typename curve_type::gt_type::value_type gt_value_type;

                    using base_field_value_type = typename curve_type::base_field_type::value_type;
                    using commitment_key_type = std::vector<typename curve_type::template g1_type<>::value_type>;
                    using verification_key_type = typename curve_type::template g2_type<>::value_type;
                    using commitment_type = typename curve_type::template g1_type<>::value_type;
                    using proof_type = commitment_type;

                    struct params_type {
                        std::size_t a;
                    };

                    static std::pair<commitment_key_type, verification_key_type> setup(const std::size_t n,
                                                                                params_type params) {

                        size_t a_scaled = params.a;
                        commitment_key_type commitment_key = {curve_type::template g1_type<>::value_type::one()};
                        verification_key_type verification_key =
                            curve_type::template g2_type<>::value_type::one() * params.a;

                        for (std::size_t i = 0; i < n; i++) {
                            commitment_key.emplace_back(a_scaled * (curve_type::template g1_type<>::value_type::one()));
                            a_scaled = a_scaled * params.a;
                        }

                        return std::make_pair(commitment_key, verification_key);
                    }

                    static commitment_type commit(const commitment_key_type &commitment_key,
                                                  const polynomial<base_field_value_type> &f) {

                        commitment_type commitment = f[0] * commitment_key[0];

                        for (std::size_t i = 0; i < f.size(); i++) {
                            commitment = commitment + commitment_key[i] * f[i];
                        }

                        return commitment;
                    }

                    static proof_type proof_eval(commitment_key_type commitment_key,
                                                 typename curve_type::base_field_type::value_type x,
                                                 typename curve_type::base_field_type::value_type y,
                                                 const polynomial<base_field_value_type> &f) {

                        const polynomial<base_field_value_type> denominator_polynom = {-x, 1};

                        const polynomial<base_field_value_type> q =
                            (f + polynomial<base_field_value_type> {-y}) / denominator_polynom;

                        proof_type p = kzg_commitment::commit(commitment_key, q);
                        return p;
                    }

                    static bool verify_eval(verification_key_type verification_key,
                                            commitment_type C_f,
                                            base_field_value_type x,
                                            base_field_value_type y,
                                            proof_type p) {

                        typename curve_type::gt_type::value_type gt1 =
                            algebra::pair<curve_type>(C_f - curve_type::template g1_type<>::value_type::one() * y,
                                                      curve_type::template g2_type<>::value_type::one());

                        typename curve_type::gt_type::value_type gt2 = algebra::pair<curve_type>(
                            p, verification_key - curve_type::template g2_type<>::value_type::one() * x);

                        return gt1 == gt2;
                    }
                };
            };    // namespace commitments
        }         // namespace zk
    }             // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ZK_COMMITMENTS_KZG_HPP


#pragma once

#include <Eigen/Dense>
#include <vector>

// quadrature rules
#include "quadrature/qhermite.hpp"
#include "quadrature/qmaxwell.hpp"
#include "spectral/lagrange_polynomial.hpp"

namespace boltzmann {
namespace impl {

/**
 * @brief stores integrals over Lagrange polynomials
 *        on the Hermite Quadrature nodes
 */
class outflow_helper
{
 private:
  typedef Eigen::VectorXd vec_t;

 public:
  outflow_helper(int K);

  /**
   *
   *
   * @param hw  Hermite weights
   * @param hx  Hermite nodes
   *
   * Note: both weights and nodes are wrt weight \f$ e^{-x^2} \f$
   */
  outflow_helper(const vec_t& hw, const vec_t& hx);

  /**
   * @brief returns \f$ \int_{\mathbb{R}^+} y l_i(y) e^{-y^2/2} dy\f$
   *
   * @param i the i-th lagrange polynomial
   *
   * @return
   */
  inline double get_y(unsigned int i) const
  {
    assert(i < ly_.size());
    return ly_[i];
  }

  /**
   * @brief returns \f$ \int_{\mathbb{R}} l_i(x) e^{-x^2/2} dx\f$
   *
   * @param i
   *
   * @return
   */
  inline double get_x(unsigned int i) const
  {
    assert(i < lx_.size());
    return lx_[i];
  }

  inline const vec_t& lx() const { return lx_; }

  inline const vec_t& ly() const { return ly_; }

  template <typename DERIVED>
  inline double compute(const Eigen::DenseBase<DERIVED>& src) const;

 private:
  vec_t ly_;
  vec_t lx_;
};

template <typename DERIVED>
inline double
outflow_helper::compute(const Eigen::DenseBase<DERIVED>& src) const
{
  assert(src.rows() == ly_.size());
  assert(src.cols() == lx_.size());

  int K = ly_.size();

  // compute outflow
  double rho_p = 0;
  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      rho_p += src(i, j) * get_x(j) * get_y(i);
    }
  }
  return rho_p;
}

}  // end namespace impl
}  // end namespace boltzmann