#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/ZLineShape.h"
#include "PhysicsTools/Utilities/interface/Gaussian.h"
#include "PhysicsTools/Utilities/interface/Numerical.h"
#include "PhysicsTools/Utilities/interface/Exponential.h"
#include "PhysicsTools/Utilities/interface/Polynomial.h"
#include "PhysicsTools/Utilities/interface/Constant.h"
#include "PhysicsTools/Utilities/interface/Convolution.h"
#include "PhysicsTools/Utilities/interface/Operations.h"
#include "PhysicsTools/Utilities/interface/Integral.h"
#include "PhysicsTools/Utilities/interface/MultiHistoChiSquare.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/FunctClone.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TROOT.h"
#include "TH1.h"
#include "TFile.h"
#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include <iostream>
#include <algorithm>
#include <exception>
#include <iterator>
#include <string>
#include <vector>
using namespace std;

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

//A function that sets istogram contents to 0
//if they are too small
void fix(TH1* histo) {
  for(int i = 1; i <= histo->GetNbinsX(); ++i) {
    if(histo->GetBinContent(i) < 0.1) {
      histo->SetBinContent(i, 0.0);
      histo->SetBinError(i, 0.0);
    }
  }
}

typedef funct::GaussIntegrator IntegratorConv;
typedef funct::GaussIntegrator IntegratorNorm;

typedef funct::Product<funct::Exponential,
		       funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type>::type ZPeak;

typedef funct::Master<ZPeak> SigPeak;
typedef funct::Slave<ZPeak> SigPeakClone;
typedef funct::Product<funct::Parameter, SigPeak>::type Sig1;
typedef funct::Product<funct::Parameter, SigPeakClone>::type Sig2;

typedef funct::Product<
            funct::Exponential,
            funct::Polynomial<2> >::type ExpPoly;

NUMERICAL_FUNCT_INTEGRAL(ExpPoly, GaussIntegrator);

typedef funct::DefIntegral<ExpPoly, funct::Constant, funct::Constant, IntegratorNorm> ExpPolyNormFactor;
typedef funct::Ratio<ExpPoly, ExpPolyNormFactor>::type ExpPolyNorm;

typedef funct::Product<
          funct::Parameter,
          funct::Difference<
            funct::Ratio<
              funct::Numerical<2>,
              funct::Parameter>::type,
            funct::Numerical<2> >::type>::type Coeff1;

typedef funct::Product<Coeff1, ExpPolyNorm>::type Bkg1;

typedef funct::Product<
          funct::Parameter,
          funct::Square<
            funct::Difference<
              funct::Ratio<
                funct::Numerical<1>,
                funct::Parameter>::type,
              funct::Numerical<1> >::type>::type>::type Coeff2;

typedef funct::Product<Coeff2, ExpPolyNorm>::type Bkg2;

NUMERICAL_FUNCT_INTEGRAL(Bkg1, GaussIntegrator);
NUMERICAL_FUNCT_INTEGRAL(Bkg2, GaussIntegrator);

typedef funct::Sum<Sig1, Bkg1>::type Fun1;
typedef funct::Sum<Sig2, Bkg2>::type Fun2;
typedef fit::MultiHistoChiSquare<Fun1, Fun2> ChiSquared;

int main(int ac, char *av[]) {
  gROOT->SetStyle("Plain");
  try {
    double fMin, fMax;
    string ext;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("input-file,i", po::value< vector<string> >(), "input file")
      ("min,m", po::value<double>(&fMin)->default_value(60), "minimum value for fit range")
      ("max,M", po::value<double>(&fMax)->default_value(120), "maximum value for fit range")
      ("plot-format,p", po::value<string>(&ext)->default_value("ps"),
       "output plot format")
      ;

    po::positional_options_description p;
    p.add("input-file", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	    options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
      }

    fit::RootMinuitCommands<ChiSquared> commands("csa08IsoBkg.txt");

    if (vm.count("input-file")) {
      cout << "Input files are: "
	   << vm["input-file"].as< vector<string> >() << "\n";
      vector<string> v_file = vm["input-file"].as< vector<string> >();
      for(vector<string>::const_iterator it = v_file.begin();
	  it != v_file.end(); ++it) {
	TFile * root_file = new TFile(it->c_str(),"read");

	TH1D * histo1 = (TH1D*) root_file->Get("oneNonIsolatedZToMuMuPlots/zMass");
	fix(histo1);
	TH1D * histo2 = (TH1D*) root_file->Get("twoNonIsolatedZToMuMuPlots/zMass");
	fix(histo2);

	cout << ">>> histogram loaded\n";
	string f_string = *it;
	replace(f_string.begin(), f_string.end(), '.', '_');
	replace(f_string.begin(), f_string.end(), '/', '_');
	string plot_string = f_string + "." + ext;
	cout << ">>> Input files loaded\n";

	const char * kYieldZMuMu1 = "YieldZMuMu1";
	const char * kYieldZMuMu2 = "YieldZMuMu2";
	const char * kYieldBkg = "YieldBkg";
	const char * kEffBkg = "EffBkg";
	const char * kLambdaZMuMu = "LambdaZMuMu";
	const char * kMass = "Mass";
	const char * kGamma = "Gamma";
	const char * kPhotonFactorZMuMu = "PhotonFactorZMuMu";
	const char * kInterferenceFactorZMuMu = "InterferenceFactorZMuMu";
	const char * kMeanZMuMu = "MeanZMuMu";
	const char * kSigmaZMuMu = "SigmaZMuMu";
	const char * kAlpha = "Alpha";
	const char * kA0 = "A0";
	const char * kA1 = "A1";
	const char * kA2 = "A2";

	funct::Parameter lambdaZMuMu(kLambdaZMuMu, commands.par(kLambdaZMuMu));
	funct::Parameter mass(kMass, commands.par(kMass));
	funct::Parameter gamma(kGamma, commands.par(kGamma));
	funct::Parameter photonFactorZMuMu(kPhotonFactorZMuMu, commands.par(kPhotonFactorZMuMu));
	funct::Parameter interferenceFactorZMuMu(kInterferenceFactorZMuMu, commands.par(kInterferenceFactorZMuMu));
	funct::Parameter yieldZMuMu1(kYieldZMuMu1, commands.par(kYieldZMuMu1));
	funct::Parameter yieldZMuMu2(kYieldZMuMu2, commands.par(kYieldZMuMu2));
	funct::Parameter yieldBkg(kYieldBkg, commands.par(kYieldBkg));
	funct::Parameter effBkg(kEffBkg, commands.par(kEffBkg));
	funct::Parameter meanZMuMu(kMeanZMuMu, commands.par(kMeanZMuMu));
	funct::Parameter sigmaZMuMu(kSigmaZMuMu, commands.par(kSigmaZMuMu));
	funct::Parameter alpha(kAlpha, commands.par(kAlpha));
	funct::Parameter a0(kA0, commands.par(kA0));
	funct::Parameter a1(kA1, commands.par(kA1));
	funct::Parameter a2(kA2, commands.par(kA2));
	funct::Constant cFMin(fMin), cFMax(fMax);

	IntegratorConv integratorConv(1.e-4);
	IntegratorNorm integratorNorm(1.e-4);

	ZPeak zPeak = funct::Exponential(lambdaZMuMu) *
	  funct::conv(funct::ZLineShape(mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu),
		      funct::Gaussian(meanZMuMu, sigmaZMuMu),
		      -3*sigmaZMuMu.value(), 3*sigmaZMuMu.value(), integratorConv);
	SigPeak sp = funct::master(zPeak);
	SigPeakClone spc = funct::slave(sp);
	Sig1 sig1 = yieldZMuMu1 * sp;
	Sig2 sig2 = yieldZMuMu2 * spc;
	funct::Numerical<1> _1;
	funct::Numerical<2> _2;
	ExpPoly ep = funct::Exponential(alpha) * funct::Polynomial<2>(a0, a1, a2);
	ExpPolyNorm epn = ep / ExpPolyNormFactor(ep, cFMin, cFMax, integratorNorm);
	Coeff1 c1 = yieldBkg * (_2 / effBkg - _2);
	Bkg1 bkg1 = c1 * epn;
	Coeff2 c2 = yieldBkg * ((_1 /effBkg - _1) ^ _2);
	Bkg2 bkg2 = c2 * epn;
	Fun1 f1 = sig1 + bkg1;
	Fun2 f2 = sig2 + bkg2;

	ChiSquared chi2(f1, histo1, f2, histo2, fMin, fMax);
	cout << "N. deg. of freedom: " << chi2.degreesOfFreedom() << endl;
	fit::RootMinuit<ChiSquared> minuit(chi2, true);
	commands.add(minuit, yieldZMuMu1);
	commands.add(minuit, yieldZMuMu2);
	commands.add(minuit, yieldBkg);
	commands.add(minuit, effBkg);
	commands.add(minuit, lambdaZMuMu);
	commands.add(minuit, mass);
	commands.add(minuit, gamma);
	commands.add(minuit, photonFactorZMuMu);
	commands.add(minuit, interferenceFactorZMuMu);
	commands.add(minuit, meanZMuMu);
	commands.add(minuit, sigmaZMuMu);
	commands.add(minuit, alpha);
	commands.add(minuit, a0);
	commands.add(minuit, a1);
	commands.add(minuit, a2);
	commands.run(minuit);
	const unsigned int nPar = 15;//WARNIG: this must be updated manually for now
	ROOT::Math::SMatrix<double, nPar, nPar, ROOT::Math::MatRepSym<double, nPar> > err;
	minuit.getErrorMatrix(err);
	std::cout << "error matrix:" << std::endl;
	for(unsigned int i = 0; i < nPar; ++i) {
	  for(unsigned int j = 0; j < nPar; ++j) {
	    std::cout << err(i, j) << "\t";
	  }
	  std::cout << std::endl;
	}
	minuit.printFitResults();

	funct::GaussIntegrator integrator(1.e-6);
	double nbkg1 = funct::integral_f(bkg1, fMin, fMax, integrator);
	double nbkg2 = funct::integral_f(bkg2, fMin, fMax, integrator);
	std::cout << "Background yields in [" << fMin <<", " << fMax << "]: "
		  << nbkg1 <<", " <<nbkg2 << std::endl;

	// binning is at 1 GeV, fortunately
	double i1 = histo1->Integral(int(fMin), int(fMax));
	double i2 = histo2->Integral(int(fMin), int(fMax));
	std::cout << "Histogram integrals in [" << fMin <<", " << fMax << "]: "
		  << i1 <<", " << i2 << std::endl;
	double s;
	s = 0;
	for(int i = 1; i <= histo1->GetNbinsX(); ++i)
	  s += histo1->GetBinContent(i);
	histo1->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histo2->GetNbinsX(); ++i)
	  s += histo2->GetBinContent(i);
	histo2->SetEntries(s);

	double extrap = nbkg1*nbkg1 / nbkg2 /4;
	cout << "extrapolated background with no isolated muons:" << extrap << endl;

	string Plot1 = "OneIsolated_" + plot_string;
	root::plot<Fun1>(Plot1.c_str(), *histo1, f1, fMin, fMax,
			 yieldZMuMu1, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
			 meanZMuMu, sigmaZMuMu, yieldBkg, effBkg, alpha, a0, a1, a2,
			 kRed, 2, kDashed, 100,
			 "Z -> #mu #mu mass", "#mu #mu invariant mass (GeV/c^{2})",
			 "Events");
	string Plot2 = "TwoIsolated_" + plot_string;
	root::plot<Fun2>(Plot2.c_str(), *histo2, f2, fMin, fMax,
			 yieldZMuMu2, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
			 meanZMuMu, sigmaZMuMu, yieldBkg, effBkg, alpha, a0, a1, a2,
			 kRed, 2, kDashed, 100,
			 "Z -> #mu #mu mass", "#mu #mu invariant mass (GeV/c^{2})",
			 "Events");
      }
    }

  }
  catch(exception& e) {
    cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    cerr << "Exception of unknown type!\n";
  }
  return 0;
}


#pragma once

/*
struct Coord {
  double x;
  double y;
  Coord(){}
  Coord(int x, int y) : x(x), y(y) {}
};
*/

#include <boost/geometry/geometries/geometries.hpp>
typedef boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian> point3;

class Coord3 : public point3
{
public:
  Coord3(){}
  Coord3(double x, double y, double z) : point3(x, y, z){}

  double x()
  {
    return this->get<0>();
  }

  double y()
  {
    return this->get<1>();
  }

  double z()
  {
    return this->get<2>();
  }

  void setx(double x)
  {
    this->set<0>(x);
  }

  void sety(double y)
  {
    this->set<1>(y);
  }

  void setz(double z)
  {
    this->set<2>(z);
  }
};


#include <cmath>
#include <vector>
#include <string>
#include "multi_lidar_calib/common.h"
#include "multi_lidar_calib/tic_toc.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/passthrough.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
 #include <ceres/ceres.h>
 #include "lidarFactor.hpp"
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include<vector>

#include<mutex>

#define PI 3.1415926

using std::atan2;
using std::cos;
using std::sin;

double lidar_z_height = -1;

ros::Publisher  pubFittedPlane;

double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; // 激光雷达间相对位姿关系

Eigen::Map<Eigen::Quaterniond> q_to_be_optimized(parameters);
Eigen::Map<Eigen::Vector3d> t_to_be_optimized(parameters + 4);

std::string extracted_path = " ";
int pcd_num_per_lidar = 0;
void planeFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr extracted_cloud, pcl::ModelCoefficients::Ptr & coefficients)
{

    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
     seg.setInputCloud(extracted_cloud);
    seg.segment (*inliers, *coefficients);

    // // ax + by + cz + d = 0，其中法向量为(a, b, c)
    // std::cout<<"平面参数："<<std::endl;
    // std::cout<<"a："<<coefficients->values[0]<<std::endl;
    // std::cout<<"b："<<coefficients->values[1]<<std::endl;
    // std::cout<<"c："<<coefficients->values[2]<<std::endl;
    // std::cout<<"d："<<coefficients->values[3]<<std::endl;
    double a,b,c,d;
    a =   coefficients->values[0];
    b =  coefficients->values[1];
    c =  coefficients->values[2];
    d =  coefficients->values[3];
    // double numerator = fabs(  a*x_c+b*y_c+c*z_c+d  );
    // double denominator = std::sqrt(  a*a+b*b+c*c );
    // double distanceToArea = numerator/ denominator;
    pcl::PointCloud<pcl::PointXYZ>::Ptr fittedPlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i=0; i<inliers->indices.size();i++)
    {
        int ind = inliers->indices[i];
         fittedPlaneCloud->points.push_back(  extracted_cloud->points[ind]  );
    }

    sensor_msgs::PointCloud2 fittedPlaneCloudMsg;
    pcl::toROSMsg(*fittedPlaneCloud, fittedPlaneCloudMsg);
    fittedPlaneCloudMsg.header.stamp = ros::Time::now();
    fittedPlaneCloudMsg.header.frame_id = "/rslidar";
    pubFittedPlane.publish(fittedPlaneCloudMsg);

}

pcl::PointCloud<pcl::PointXYZ>::Ptr planeFittingReturn(pcl::PointCloud<pcl::PointXYZ>::Ptr extracted_cloud, pcl::ModelCoefficients::Ptr & coefficients)
{

    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
     seg.setInputCloud(extracted_cloud);
    seg.segment (*inliers, *coefficients);

    // // ax + by + cz + d = 0，其中法向量为(a, b, c)
    // std::cout<<"平面参数："<<std::endl;
    // std::cout<<"a："<<coefficients->values[0]<<std::endl;
    // std::cout<<"b："<<coefficients->values[1]<<std::endl;
    // std::cout<<"c："<<coefficients->values[2]<<std::endl;
    // std::cout<<"d："<<coefficients->values[3]<<std::endl;
    double a,b,c,d;
    a =   coefficients->values[0];
    b =  coefficients->values[1];
    c =  coefficients->values[2];
    d =  coefficients->values[3];
    // double numerator = fabs(  a*x_c+b*y_c+c*z_c+d  );
    // double denominator = std::sqrt(  a*a+b*b+c*c );
    // double distanceToArea = numerator/ denominator;
    pcl::PointCloud<pcl::PointXYZ>::Ptr fittedPlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i=0; i<inliers->indices.size();i++)
    {
        int ind = inliers->indices[i];
         fittedPlaneCloud->points.push_back(  extracted_cloud->points[ind]  );
    }

    sensor_msgs::PointCloud2 fittedPlaneCloudMsg;
    pcl::toROSMsg(*fittedPlaneCloud, fittedPlaneCloudMsg);
    fittedPlaneCloudMsg.header.stamp = ros::Time::now();
    fittedPlaneCloudMsg.header.frame_id = "/rslidar";
    pubFittedPlane.publish(fittedPlaneCloudMsg);

    return fittedPlaneCloud;

}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "multi_lidar_calibration");
	ros::NodeHandle nh;

    ros::param::get("~extracted_path", extracted_path);
    ros::param::get("~pcd_num_per_lidar", pcd_num_per_lidar);

    pubFittedPlane = nh.advertise<sensor_msgs::PointCloud2>("/fitted_plane", 100);

    for (int iterCount = 0; iterCount < 10; iterCount++)
    {

            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::LocalParameterization *q_parameterization =
                new ceres::EigenQuaternionParameterization();
            ceres::Problem::Options problem_options;

            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters, 4, q_parameterization);
            problem.AddParameterBlock(parameters + 4, 3);

            for (int k=0; k<pcd_num_per_lidar; k++)
            {

                std::stringstream ss;
                std::string filename_pt0 = extracted_path;
                std::string filename_pt1 = extracted_path;
                ss << k+1;
                std::string num = ss.str();
                filename_pt0.append(num);
                filename_pt0.append("_0.pcd");
                filename_pt1.append(num);
                filename_pt1.append("_1.pcd");

                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt0<<std::endl;
                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt1<<std::endl;
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);

                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt0, *cloud0) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }
                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt1, *cloud1) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }

                pcl::ModelCoefficients::Ptr coefficients0 (new pcl::ModelCoefficients);
                planeFitting(cloud0, coefficients0);
                pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
                planeFitting(cloud1,coefficients1);

                // 两平面法线的夹角作为一个损失值
                Eigen::Vector3d norm0(coefficients0->values[0], coefficients0->values[1], coefficients0->values[2]);
                Eigen::Vector3d norm1(coefficients1->values[0], coefficients1->values[1], coefficients1->values[2]);
                ceres::CostFunction *cost_function;
                cost_function = LidarNormFactor::Create(norm0, norm1);
                problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

                TicToc t_solver;
                ceres::Solver::Options options;
                options.linear_solver_type = ceres::DENSE_QR;
                options.max_num_iterations = 10;
                options.minimizer_progress_to_stdout = false;
                options.check_gradients = false;
                options.gradient_check_relative_precision = 1e-4;
                ceres::Solver::Summary summary;
                ceres::Solve(options, &problem, &summary);

                sleep(0.2);

            }

    }
    printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],parameters[4], parameters[5], parameters[6]);
  // ====================================================== //=========================================================
  // 接下来标平移：（之所以分开标定是因为如果平移不准，起码保证旋转是准的，平移不会影响旋转。实验发现，这样标定的话平移也挺准）
    for (int iterCount = 0; iterCount < 10; iterCount++)
    {

            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::Problem::Options problem_options;
            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters + 4, 3);

            for (int k=0; k<pcd_num_per_lidar; k++)
            {

                std::stringstream ss;
                std::string filename_pt0 = extracted_path;
                std::string filename_pt1 = extracted_path;
                ss << k+1;
                std::string num = ss.str();
                filename_pt0.append(num);
                filename_pt0.append("_0.pcd");
                filename_pt1.append(num);
                filename_pt1.append("_1.pcd");

                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt0<<std::endl;
                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt1<<std::endl;
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0_plane(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1_plane(new pcl::PointCloud<pcl::PointXYZ>);

                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt0, *cloud0) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }
                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt1, *cloud1) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }

                pcl::ModelCoefficients::Ptr coefficients0 (new pcl::ModelCoefficients);
                cloud0_plane = planeFittingReturn(cloud0, coefficients0);
                pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
                cloud1_plane = planeFittingReturn(cloud1,coefficients1);

                // 一共有三维损失值，分别代表：。。。。。。先加一维
                // 0：质心距离
                // 计算质心
                double center_x0 = 0;
                double center_y0 = 0;
                double center_z0 = 0;
                double center_x1 = 0;
                double center_y1 = 0;
                double center_z1 = 0;
                for (int i=0; i<cloud0_plane->points.size(); i++)
                {
                    center_x0 += cloud0_plane->points[i].x;
                    center_y0 += cloud0_plane->points[i].y;
                    center_z0 += cloud0_plane->points[i].z;
                }
                center_x0 /= cloud0_plane->points.size();
                center_y0 /= cloud0_plane->points.size();
                center_z0 /= cloud0_plane->points.size();

                for (int i=0; i<cloud1_plane->points.size(); i++)
                {
                    // 先将点用旋转转过去：
                    Eigen::Vector3d tmppoint(cloud1_plane->points[i].x,  cloud1_plane->points[i].y, cloud1_plane->points[i].z);
                    Eigen::Vector3d tmppoint_roted =  q_to_be_optimized.matrix() * tmppoint;
                    center_x1 += tmppoint_roted.x();
                    center_y1 +=tmppoint_roted.y();
                    center_z1 += tmppoint_roted.z();
                }
                center_x1 /= cloud1_plane->points.size();
                center_y1 /= cloud1_plane->points.size();
                center_z1 /= cloud1_plane->points.size();

                Eigen::Vector3d plane_center0(center_x0, center_y0, center_z0) ;
                Eigen::Vector3d plane_center1(center_x1, center_y1, center_z1) ;

                ceres::CostFunction *cost_function;
                cost_function = LidarCenterFactor::Create(plane_center0, plane_center1);
                problem.AddResidualBlock(cost_function, loss_function, parameters + 4);

                TicToc t_solver;
                ceres::Solver::Options options;
                options.linear_solver_type = ceres::DENSE_QR;
                options.max_num_iterations = 10;
                options.minimizer_progress_to_stdout = false;
                options.check_gradients = false;
                options.gradient_check_relative_precision = 1e-4;
                ceres::Solver::Summary summary;
                ceres::Solve(options, &problem, &summary);

                sleep(0.2);

            }

    }

    std::cout<<"======================最终标定结果======================"<<std::endl;
    printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],parameters[4], parameters[5], parameters[6]);
    std::cout<<q_to_be_optimized.matrix()<<std::endl;

    return 0;
}


#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <cmath>
#include <string>
#include <chrono>
#include <exception>
#include <queue>
#include <stack>
#include <bitset>
#include <random>
#include <set>
#include <boost/math/distributions/normal.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <sys/time.h>
#include "DataTypes.hpp"
#include "ATG.hpp"

std::ostream& operator<<(std::ostream &os, const NodeType  &et) {
  std::string state;
  switch(et.status) {
      case NotReady : state = "NotReady"; break;
      case Ready    : state = "Ready"; break;
      case Finished : state = "Finished"; break;
      case Active   : state = "Active"; break;
      default  : state = "None"; break;
  }
  os << "Task{id:"<<et.id<<",start:" << et.start_time << ",C:" <<  et.C << ",stop:"<<et.stop_time<<",alloc:"<<et.alloc<<",status:"<<state<<"}";
  return os;
}

/* Execution time characteristics of jth MT-task */
double AppTaskGraph::tau_j(int jid, int l) {
  NodeType *nd = &(this->gSS[this->vdMap[jid]]);
  double dj    = nd->dj;
  double tau1  = nd->tau1;
  double ret   = (tau1)*(pow(l,-dj));
  return ret;
}

/* Generate a random task graph */
AppTaskGraph::AppTaskGraph(int N, double p) : vdMap(N) {
    if (p >= 1 && p < 0) {
        throw std::invalid_argument("Wrong value of parameter p");
    }
    // initExecTimeChar(int N)
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis2(0.01, 0.99);
    std::uniform_real_distribution<> dis1(80,400);
    std::bernoulli_distribution d(p);

    int i, j, cnt = 0;

    this->wC = 0.0;
    /* Create the nodes */
    /*
     * -1 reflects unintialized values
     */
    for (i = 0; i < N; i++) {
        NodeType vi2       = {.status = NotReady, \
                              .estart = -1, \
                              .dj = dis2(gen), .tau1 = dis1(gen), .id = i, \
                              .alloc = -1, .start_time = -1, .stop_time = -1, .C = -1};
        this->vdMap[i]     = add_vertex(vi2,this->gSS);

        this->wC += this->tau_j(vi2.id,1);
    }
    this->deadline = 0.4*(this->wC);

    /* Connect the nodes */
    for (i = 0; i < N; i++) {
        for (j = i+1; j < N; j++) {
            if (d(gen)) {
                EdgeType edg = {.edgeNum = cnt++};
                add_edge(this->vdMap[i],this->vdMap[j],edg,this->gSS);
            }
        }
    }

    /* Initialize the ready set and schedule set to null */
    this->t = 0;
    this->computeNbdMap();
    this->initialized = true;
}

AppTaskGraph::AppTaskGraph() : vdMap(6) {
    /* Create a DAG as shown in buggy.dot */
    int N = 6;
    int i;

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis2(0.01, 0.99);
    std::uniform_real_distribution<> dis1(80,400);
    // std::bernoulli_distribution d(p);

    for (i = 0; i < N; i++) {
        NodeType vi2       = {.status = NotReady, \
                              .estart = -1, \
                              .dj = dis2(gen), .tau1 = dis1(gen), .id = i, \
                              .alloc = -1, .start_time = -1, .stop_time = -1, .C = -1};
        this->vdMap[i]     = add_vertex(vi2,this->gSS);
    }

    EdgeType edg = {.edgeNum = 8};
    add_edge(this->vdMap[0],this->vdMap[2],edg,this->gSS);
    add_edge(this->vdMap[0],this->vdMap[4],edg,this->gSS);
    add_edge(this->vdMap[0],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[1],this->vdMap[2],edg,this->gSS);
    add_edge(this->vdMap[1],this->vdMap[4],edg,this->gSS);
    add_edge(this->vdMap[1],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[2],this->vdMap[3],edg,this->gSS);
    add_edge(this->vdMap[2],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[3],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[4],this->vdMap[5],edg,this->gSS);

}

void AppTaskGraph::writeDot(int iter) {
    std::cout << "\n-- graphviz output START --" << std::endl;
    std::ofstream dotf;
    std::string dotFilename = "demo"+std::to_string(iter)+".dot";
    dotf.open(dotFilename,std::ios::out | std::ios::trunc);
    write_graphviz(dotf,this->gSS,DAGVertexPW(this->gSS),DAGEdgePW(this->gSS));
    dotf.close();
    std::cout << "\n-- graphviz output END --" << std::endl;
}

/* Compute the longest path to each node in DAG from source node */
void AppTaskGraph::computeHeight(bool schedule) {
    DAGInEdgeIterator  eiIS, eiIE;
    int N = this->vdMap.size();
    for (int i = 0; i < N; i++) {
        NodeType *nd = &(this->gSS[this->vdMap[i]]);
        nd->estart   = 0;

        int max = 0;
        for (boost::tie(eiIS,eiIE) = boost::in_edges(this->vdMap[i],this->gSS); eiIS != eiIE; eiIS++) {
            auto ndPrevId    = boost::source(*eiIS, this->gSS);
            NodeType *ndPrev = &(this->gSS[ndPrevId]);
            if (max < (ndPrev->estart + ndPrev->C))
                max = ndPrev->estart + ndPrev->C;
        }
        nd->estart = max;

        if (schedule) {
            nd->start_time = nd->estart;
            nd->stop_time  = nd->start_time + nd->C;
        }
    }
}

void AppTaskGraph::reset() {
    for (auto it = this->vdMap.begin(); it != this->vdMap.end(); it++) {
        NodeType *nd = &(this->gSS[*it]);

        nd->status = NotReady;
        nd->estart = -1;
        nd->start_time = -1;
        nd->stop_time = -1;
        nd->alloc  = -1;
    }
}

void AppTaskGraph::computeNbdMap() {
    int N = this->vdMap.size();
    DAGInEdgeIterator  eiIS, eiIE;
    DAGOutEdgeIterator  eiOS, eiOE;

    for (int i = 0; i < N; i++) {
        /* Insert predecessors */
        std::vector<DAGVertexType> preds;
        std::vector<DAGVertexType> succs;

        for (boost::tie(eiIS,eiIE) = boost::in_edges(this->vdMap[i],this->gSS); eiIS != eiIE; eiIS++) {
            auto ndPrevId    = boost::source(*eiIS, this->gSS);
            preds.push_back(ndPrevId);
        }

        /* Insert successors */
        for (boost::tie(eiOS,eiOE) = boost::out_edges(this->vdMap[i],this->gSS); eiOS != eiOE; eiOS++) {
            auto ndSuccId    = boost::target(*eiOS, this->gSS);
            succs.push_back(ndSuccId);
        }

        /* */
        this->predVdMap.insert({this->vdMap[i],preds});
        this->succVdMap.insert({this->vdMap[i],succs});
    }
}

/* Construct an ATG from a dot file */
AppTaskGraph::AppTaskGraph(std::string dotFile) {
    boost::dynamic_properties dp;
    std::ifstream dotF(dotFile);

    dp.property("dj", boost::get(&NodeType::dj,this->gSS));
    dp.property("tau1", boost::get(&NodeType::tau1,this->gSS));
    dp.property("node_id", boost::get(&NodeType::id,this->gSS));

    read_graphviz(dotF,this->gSS,dp);
}


/*
 * Copyright 2017-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <gram_savitzky_golay/spatial_filters.h>

#include <boost/circular_buffer.hpp>
#include <Eigen/SVD>

namespace gram_sg
{
RotationFilter::RotationFilter(const gram_sg::SavitzkyGolayFilterConfig & conf)
: sg_conf(conf), sg_filter(conf), buffer(2 * sg_filter.config().m + 1)

{
  reset(Eigen::Matrix3d::Zero());
}

void RotationFilter::reset(const Eigen::Matrix3d & r)
{
  buffer.clear();
  // Initialize to data
  for(size_t i = 0; i < buffer.capacity(); i++)
  {
    buffer.push_back(r);
  }
}

void RotationFilter::reset()
{
  RotationFilter::reset(Eigen::Matrix3d::Zero());
}

void RotationFilter::clear()
{
  buffer.clear();
}

void RotationFilter::add(const Eigen::Matrix3d & r)
{
  buffer.push_back(r);
}
Eigen::Matrix3d RotationFilter::filter() const
{
  // Apply a temporal (savitzky-golay) convolution,
  // followed by an orthogonalization
  const Eigen::Matrix3d & result = sg_filter.filter(buffer);
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(result, Eigen::ComputeFullV | Eigen::ComputeFullU);
  Eigen::Matrix3d res = svd.matrixU() * svd.matrixV().transpose();
  return res;
}

TransformFilter::TransformFilter(const gram_sg::SavitzkyGolayFilterConfig & conf) : trans_filter(conf), rot_filter(conf)
{
}

void TransformFilter::reset(const Eigen::Affine3d & T)
{
  trans_filter.reset(T.translation());
  rot_filter.reset(T.rotation());
}

void TransformFilter::reset()
{
  trans_filter.reset();
  rot_filter.reset();
}

void TransformFilter::clear()
{
  trans_filter.clear();
  rot_filter.clear();
}

void TransformFilter::add(const Eigen::Affine3d & T)
{
  trans_filter.add(T.translation());
  rot_filter.add(T.rotation());
}

Eigen::Affine3d TransformFilter::filter() const
{
  const Eigen::Vector3d & trans_res = trans_filter.filter();
  const Eigen::Matrix3d & rot_res = rot_filter.filter();
  Eigen::Matrix4d rot = Eigen::Matrix4d::Identity();
  rot.block<3, 3>(0, 0) = rot_res;
  rot.block<3, 1>(0, 3) = trans_res;
  return Eigen::Affine3d(rot);
}

} // namespace gram_sg


#include <stdio.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <cassert>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/filesystem.hpp>
#include <yaml-cpp/yaml.h>

/**
 * @brief Stereo camera parameters
 *
 */
struct StereoParam
{

    // extrinsic matrix, transform right camera frame into left camera frame
    Eigen::Matrix4d T_rl;
    // left camera intrinsic
    Eigen::Matrix3d cam0_intrinsic;
    // left camera distortion_coeffs
    double cam0_distortions[5];

    // right camera intrinsic
    Eigen::Matrix3d cam1_intrinsic;
    // right camera distortion_coeffs
    double cam1_distortions[5];

    int img_width;
    int img_height;
    // assume cam0 and cam1 use identical distortion type
    std::string distortion_type;

    StereoParam() : T_rl(Eigen::Matrix4d::Identity()),
                    cam0_intrinsic(Eigen::Matrix3d::Identity()),
                    cam1_intrinsic(Eigen::Matrix3d::Identity()),
                    img_width(2592),
                    img_height(2048)
    {
        memset(cam0_distortions, 0, 5 * sizeof(double));
        memset(cam1_distortions, 0, 5 * sizeof(double));
        distortion_type = "equidistant";
    }

    void getExtrinsic(cv::Mat &R_rl, cv::Mat &t_rl)
    {
        Eigen::Matrix3d R = T_rl.block<3, 3>(0, 0);
        Eigen::Vector3d t = T_rl.block<3, 1>(0, 3);

        cv::eigen2cv(R, R_rl);
        cv::eigen2cv(t, t_rl);
    }

    void getCam0Param(cv::Mat &K, cv::Mat &D)
    {
        cv::eigen2cv(cam0_intrinsic, K);
//        cv::Mat distortion = (cv::Mat_<double>(1, 5) << cam0_distortions[0], cam0_distortions[1],
//                cam0_distortions[2], cam0_distortions[3], cam0_distortions[4]);
        cv::Mat distortion = (cv::Mat_<double>(1, 4) << cam0_distortions[0], cam0_distortions[1],
                cam0_distortions[2], cam0_distortions[3]);
        D = distortion.clone();
    }

    void getCam1Param(cv::Mat &K, cv::Mat &D)
    {
        cv::eigen2cv(cam1_intrinsic, K);
//        cv::Mat distortion = (cv::Mat_<double>(1, 5) << cam1_distortions[0], cam1_distortions[1],
//                cam1_distortions[2], cam1_distortions[3], cam1_distortions[4]);
        cv::Mat distortion = (cv::Mat_<double>(1, 4) << cam1_distortions[0], cam1_distortions[1],
                cam1_distortions[2], cam1_distortions[3]);
        D = distortion.clone();
    }
};

/**
 * @brief Monocular camera parameters
 *
 */
struct MonocularParam{
    // camera intrinsic
    Eigen::Matrix3d cam_intrinsic;
    // camera distortion_coeffs
    double cam_distortions[5];

    int img_width;
    int img_height;

    std::string distortion_type;

    MonocularParam() :cam_intrinsic(Eigen::Matrix3d::Identity()),
                    img_width(2592),
                    img_height(2048)
    {
        memset(cam_distortions, 0, 5 * sizeof(double));
        distortion_type = "equidistant";
    }

    void getCamParam(cv::Mat &K, cv::Mat &D)
    {
        cv::eigen2cv(cam_intrinsic, K);
//        cv::Mat distortion = (cv::Mat_<double>(1, 5) << cam_distortions[0], cam_distortions[1],
//                cam_distortions[2], cam_distortions[3], cam_distortions[4]);
        cv::Mat distortion = (cv::Mat_<double>(1, 4) << cam_distortions[0], cam_distortions[1],
                cam_distortions[2], cam_distortions[3]);
        D = distortion.clone();
    }

};

/**
 * @brief parse kalibr yaml file support: pinhole + equidistant
 *
 * @param kalib_intrin_file
 * @param camera Camera Model param
 */
bool loadKalibrResult(const std::string &kalib_result_file, StereoParam &stereo_param)
{
    if (kalib_result_file.empty())
    {
        std::cout << "[loadKalibrResult] Empty input file name!\n";
        return false;
    }

    std::ifstream fs(kalib_result_file);
    if (!fs.is_open())
    {
        std::cout << "[loadKalibrResult] Error read " << kalib_result_file << "\n";
        return false;
    }

    YAML::Node root_node;
    YAML::Node cam0_node;
    YAML::Node cam1_node;

    try
    {
        root_node = YAML::LoadFile(kalib_result_file);
    }
    catch (YAML::BadFile &e)
    {
        std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file << "\n";
        return false;
    }
    catch (YAML::ParserException &e)
    {
        std::cout << "[loadKalibrResult] Invalid file format: " << kalib_result_file << "\n";
        return false;
    }
    if (root_node.IsNull())
    {
        std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file << "\n";
        return false;
    }

    int img_width = 0, img_height = 0;
    std::string distortion_type = "equidistant";

    cam0_node = root_node["cam0"];
    double cam0_distortion[5] = {0.0};
    cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
    cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
    cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
    cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();
    // cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
    Eigen::Matrix3d cam0_intrinsic = Eigen::Matrix3d::Identity();
    cam0_intrinsic(0, 0) = cam0_node["intrinsics"][0].as<double>();
    cam0_intrinsic(1, 1) = cam0_node["intrinsics"][1].as<double>();
    cam0_intrinsic(0, 2) = cam0_node["intrinsics"][2].as<double>();
    cam0_intrinsic(1, 2) = cam0_node["intrinsics"][3].as<double>();

    cam1_node = root_node["cam1"];
    double cam1_distortion[5] = {0.0};
    cam1_distortion[0] = cam1_node["distortion_coeffs"][0].as<double>();
    cam1_distortion[1] = cam1_node["distortion_coeffs"][1].as<double>();
    cam1_distortion[2] = cam1_node["distortion_coeffs"][2].as<double>();
    cam1_distortion[3] = cam1_node["distortion_coeffs"][3].as<double>();
    // cam1_distortion[0] = cam1_node["distortion_coeffs"][0].as<double>();
    Eigen::Matrix3d cam1_intrinsic = Eigen::Matrix3d::Identity();
    cam1_intrinsic(0, 0) = cam1_node["intrinsics"][0].as<double>();
    cam1_intrinsic(1, 1) = cam1_node["intrinsics"][1].as<double>();
    cam1_intrinsic(0, 2) = cam1_node["intrinsics"][2].as<double>();
    cam1_intrinsic(1, 2) = cam1_node["intrinsics"][3].as<double>();

    img_width = cam1_node["resolution"][0].as<int>();
    img_height = cam1_node["resolution"][1].as<int>();

    distortion_type = cam1_node["distortion_model"].as<std::string>();

    Eigen::Matrix4d extrinsic = Eigen::Matrix4d::Identity();
    for (size_t i = 0; i < cam1_node["T_cn_cnm1"].size(); ++i)
    {
        extrinsic(i, 0) = cam1_node["T_cn_cnm1"][i][0].as<double>();
        extrinsic(i, 1) = cam1_node["T_cn_cnm1"][i][1].as<double>();
        extrinsic(i, 2) = cam1_node["T_cn_cnm1"][i][2].as<double>();
        extrinsic(i, 3) = cam1_node["T_cn_cnm1"][i][3].as<double>();
    }

    stereo_param.T_rl = extrinsic;
    std::cout << "[loadKalibrResult] T_rl : \n"
              << stereo_param.T_rl << "\n";
    Eigen::Vector3d baseline = stereo_param.T_rl.block<3,1>(0,3);
    std::cout << "[loadKalibrResult] baseline norm : \n"
              << baseline.norm() << "\n";

    std::cout << "[loadKalibrResult] baseline * fx = \n" <<
                 (baseline.norm() * cam0_intrinsic(0, 0)) << "\n";

    stereo_param.cam0_intrinsic = cam0_intrinsic;
    std::cout << "[loadKalibrResult] cam0 intrinsic : \n"
              << stereo_param.cam0_intrinsic << "\n";
    memcpy(stereo_param.cam0_distortions, cam0_distortion, 5 * sizeof(double));
    std::cout << "[loadKalibrResult] cam0 distrotion : " << stereo_param.cam0_distortions[0] << ", " << stereo_param.cam0_distortions[3] << "\n";
    stereo_param.cam1_intrinsic = cam1_intrinsic;
    std::cout << "[loadKalibrResult] cam1 intrinsic : \n"
              << stereo_param.cam1_intrinsic << "\n";
    memcpy(stereo_param.cam1_distortions, cam1_distortion, 5 * sizeof(double));
    std::cout << "[loadKalibrResult] cam1 distrotion : " << stereo_param.cam1_distortions[0] << ", " << stereo_param.cam1_distortions[3] << "\n";
    stereo_param.img_width = img_width;
    stereo_param.img_height = img_height;
    stereo_param.distortion_type = distortion_type;

    return true;
}

/**
 * @brief parse kalibr yaml file support: pinhole + equidistant
 *
 * @param kalib_intrin_file
 * @param camera Camera Model param
 */
bool loadKalibrResult(const std::string &kalib_result_file, MonocularParam &mono_param)
{
    if (kalib_result_file.empty())
    {
        std::cout << "[loadKalibrResult] Empty input file name!\n";
        return false;
    }

    std::ifstream fs(kalib_result_file);
    if (!fs.is_open())
    {
        std::cout << "[loadKalibrResult] Error read " << kalib_result_file << "\n";
        return false;
    }

    YAML::Node root_node;
    YAML::Node cam0_node;
    try
    {
        root_node = YAML::LoadFile(kalib_result_file);
    }
    catch (YAML::BadFile &e)
    {
        std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file << "\n";
        return false;
    }
    catch (YAML::ParserException &e)
    {
        std::cout << "[loadKalibrResult] Invalid file format: " << kalib_result_file << "\n";
        return false;
    }
    if (root_node.IsNull())
    {
        std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file << "\n";
        return false;
    }

    int img_width = 0, img_height = 0;
    std::string distortion_type = "equi-distant";

    cam0_node = root_node["cam0"];
    double cam0_distortion[5] = {0.0};
    cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
    cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
    cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
    cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();
    // cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
    Eigen::Matrix3d cam0_intrinsic = Eigen::Matrix3d::Identity();
    cam0_intrinsic(0, 0) = cam0_node["intrinsics"][0].as<double>();
    cam0_intrinsic(1, 1) = cam0_node["intrinsics"][1].as<double>();
    cam0_intrinsic(0, 2) = cam0_node["intrinsics"][2].as<double>();
    cam0_intrinsic(1, 2) = cam0_node["intrinsics"][3].as<double>();

    img_width = cam0_node["resolution"][0].as<int>();
    img_height = cam0_node["resolution"][1].as<int>();

    distortion_type = cam0_node["distortion_model"].as<std::string>();

    mono_param.cam_intrinsic = cam0_intrinsic;
    std::cout << "[loadKalibrResult] cam0 intrinsic : \n"
              << mono_param.cam_intrinsic << "\n";
    memcpy(mono_param.cam_distortions, cam0_distortion, 5 * sizeof(double));
    std::cout << "[loadKalibrResult] cam0 distrotion : " << mono_param.cam_distortions[0] << ", " << mono_param.cam_distortions[3] << "\n";

    mono_param.img_width = img_width;
    mono_param.img_height = img_height;
    mono_param.distortion_type = distortion_type;

    return true;
}

/**
 * @brief Write mono-camera parameters
 *
 * @param file_name is the output yaml file name.
 * @param mono_param is the monocular camera parameters.
 */
bool writeCamParam(const std::string &file_name, MonocularParam &mono_param)
{
    if (file_name.empty())
    {
        std::cout << "[writeCamParam] Empty file name!\n";
        return false;
    }

    cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        std::cout << "[writeCamParam] Fail to open " << file_name << "\n";
        return false;
    }

    fs << "camera_model" << mono_param.distortion_type;

    cv::Mat K, D;
    //
    {
        mono_param.getCamParam(K, D);
        fs << "camera_matrix" << K;
        fs << "distortion_coefficients" << D;
    }

    fs << "avg_reprojection_error" << 1.2799437834241592e-01;
    fs << "image_width" << mono_param.img_width;
    fs << "image_height" << mono_param.img_height;
    fs << "serial_num" <<  1;
    fs.release();
    return true;
}

/**
 * @brief Write stereo-camera parameters
 *
 * @param file_name is the output yaml file name.
 * @param cam_index is .
 * @param mono_param is the monocular camera parameters.
 */
bool writeCamParam(const std::string &file_name, const int cam_index, StereoParam &stereo_param)
{
    if (file_name.empty())
    {
        std::cout << "[writeCamParam] Empty file name!\n";
        return false;
    }

    cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        std::cout << "[writeCamParam] Fail to open " << file_name << "\n";
        return false;
    }

    fs << "image_width" << stereo_param.img_width;
    fs << "image_height" << stereo_param.img_height;
    fs << "distortion_type" << stereo_param.distortion_type;

    cv::Mat K, D;
    //
    if (0 == cam_index)
    {
        stereo_param.getCam0Param(K, D);
        fs << "camera_matrix" << K;
        fs << "distortion_coefficients" << D;
    }
    if (1 == cam_index)
    {
        stereo_param.getCam1Param(K, D);
        fs << "camera_matrix" << K;
        fs << "distortion_coefficients" << D;
    }

    fs.release();
    return true;
}
bool writeExtrinsicParam(const std::string &file_name, StereoParam &stereo_param)
{
    if (file_name.empty())
    {
        std::cout << "[writeExtrinsicParam] Empty file name!\n";
        return false;
    }

    cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        std::cout << "[writeExtrinsicParam] Fail to open " << file_name << "\n";
        return false;
    }

    cv::Mat T, R, t;
    stereo_param.getExtrinsic(R, t);
    cv::eigen2cv(stereo_param.T_rl, T);

    fs << "T_rl" << T;
    fs << "R" << R;
    fs << "T" << t.t();
    fs.release();
    return true;
}

#define MONOCULAR_RESULT 0
#define STEREO_RESULT 1

int main(int argc, char *argv[])
{
    if (argc < 3)
    {
        std::cout << "[convert_Kalibr_output] Usage : convert_Kalibr_output kalibr_output.yaml output_dir  model \n"
                  << "This executable is used to convert multicamera kalibr calibration file into 3 files seperately!\n"
                  << "model : 0--convert monocular kalibr output yaml file \n"
                  << "model : 1--convert stereo kalibr output yaml file \n";
        return -1;
    }

    std::string input_fn(argv[1]);
    std::string output_dir(argv[2]);
    int model = std::stoi (argv[3]);

    if (output_dir.back() != '/')
        output_dir += "/";

    if(model == MONOCULAR_RESULT)
    {
        std::string output_cam0_intrinsic_fn = output_dir + "equi_intrinsic.yml";
        MonocularParam mono_param;

        bool sts = loadKalibrResult (input_fn, mono_param);
        if (!sts)
        {
            std::cout << "[convert_kalibr_result] Fail to load monocular kalibr result!\n";
            return -1;
        }

        sts = writeCamParam(output_cam0_intrinsic_fn, mono_param);
        if (!sts)
        {
            std::cout << "[convert_kalibr_result] Fail to write camera0 intrinsic!" << output_cam0_intrinsic_fn << "\n";
            return -1;
        }

        return 0;
    }

    if(model == STEREO_RESULT)
    {
        std::string output_cam0_intrinsic_fn = output_dir + "left_intrinsic.yml";
        std::string output_cam1_intrinsic_fn = output_dir + "right_intrinsic.yml";
        std::string output_stereo_fn = output_dir + "extrinsic.yml";

        StereoParam stereo_param;
        bool sts = loadKalibrResult(input_fn, stereo_param);
        if (!sts)
        {
            std::cout << "[convert_kalibr_result] Fail to load stereo kalibr result!\n";
            return -1;
        }

        sts = writeCamParam(output_cam0_intrinsic_fn, 0, stereo_param);
        if (!sts)
        {
            std::cout << "[convert_kalibr_result] Fail to write camera0 intrinsic!" << output_cam0_intrinsic_fn << "\n";
            return -1;
        }
        sts = writeCamParam(output_cam1_intrinsic_fn, 1, stereo_param);
        if (!sts)
        {
            std::cout << "[convert_kalibr_result] Fail to write camera1 intrisnic!" << output_cam1_intrinsic_fn << "\n";
            return -1;
        }

        sts = writeExtrinsicParam(output_stereo_fn, stereo_param);
        if (!sts)
        {
            std::cout << "[convert_kalibr_result] Fail to write extrinsic file " << output_stereo_fn << "\n";
            return -1;
        }
    }
    return 0;
}


#ifndef INCLUDE_SWIFT_VIO_POINT_LANDMARK_MODELS_HPP_
#define INCLUDE_SWIFT_VIO_POINT_LANDMARK_MODELS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <okvis/ceres/LocalParamizationAdditionalInterfaces.hpp>

namespace swift_vio {
// Expressed in an anchor camera frame [\alpha, \beta, 1, \rho] = [x, y, z, w]/z.
class InverseDepthParameterization final: public okvis::ceres::LocalParamizationAdditionalInterfaces
{
public:
  static const int kModelId = 1;
  static const int kGlobalDim = 4;
  static const int kLocalDim = 3;

  // Generalization of the addition operation,
  //
  //   x_plus_delta = Plus(x, delta)
  //
  // with the condition that Plus(x, 0) = x.
  bool Plus(const double* x, const double* delta, double* x_plus_delta) const final {
    return plus(x, delta, x_plus_delta);
  }

  // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  //
  // jacobian is a row-major GlobalSize() x LocalSize() matrix.
  bool ComputeJacobian(const double* x, double* jacobian) const final {
    return plusJacobian(x, jacobian);
  }

  static bool plusJacobian(const double*, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, kLocalDim, Eigen::RowMajor>> j(jacobian);
    j.setZero();
    j(0, 0) = 1.0;
    j(1, 1) = 1.0;
    j(3, 2) = 1.0;
    return true;
  }

  // Size of x.
  int GlobalSize() const final {
    return kGlobalDim;
  }

  // Size of delta.
  int LocalSize() const final {
    return kLocalDim;
  }

  static bool plus(const double* x, const double* delta, double* x_plus_delta);

  static bool minus(const double* x, const double* x_plus_delta, double* delta);

  bool Minus(const double *x, const double *x_plus_delta,
             double *delta) const final {
    return minus(x, x_plus_delta, delta);
  }

  static bool liftJacobian(const double* /*x*/, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kLocalDim, kGlobalDim, Eigen::RowMajor>> j(jacobian);
    j.setZero();
    j(0, 0) = 1.0;
    j(1, 1) = 1.0;
    j(2, 3) = 1.0;
    return true;
  }

  /// \brief Computes the Jacobian from minimal space to naively
  /// overparameterised space as used by ceres.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  bool ComputeLiftJacobian(const double *x, double *jacobian) const final {
    return liftJacobian(x, jacobian);
  }

  static void toHomogeneousPoint(const double *x, double *y, double *jacobian) {
    Eigen::Map<const Eigen::Matrix<double, kGlobalDim, 1>> inversepoint(x);
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, 1>> homogpoint(y);
    double z = 1.0 / inversepoint[3];
    homogpoint = inversepoint * z;
    if (jacobian) {
      Eigen::Map<Eigen::Matrix<double, kGlobalDim, kGlobalDim, Eigen::RowMajor>>
          j(jacobian);
      double z2 = z * z;
      j << z, 0, 0, -inversepoint[0] * z2,
          0, z, 0, -inversepoint[1] * z2,
          0, 0, z, -inversepoint[2] * z2,
          0, 0, 0, 0;
    }
  }
};

// [x, y, z, w, c, s]
// [x, y, z, w] is the quaternion underlying unit bearing vector n such that
// n = q(w, x, y, z) * [0, 0, 1]'.
// c, s are cos(theta) and sin(theta) where \theta is the parallax angle.
class ParallaxAngleParameterization final: public okvis::ceres::LocalParamizationAdditionalInterfaces {
public:
  static const int kModelId = 2;
  static const int kGlobalDim = 6;
  static const int kLocalDim = 3;

  // Generalization of the addition operation,
  //
  //   x_plus_delta = Plus(x, delta)
  //
  // with the condition that Plus(x, 0) = x.
  bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    return plus(x, delta, x_plus_delta);
  }

  // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  // jacobian is a row-major GlobalSize() x LocalSize() matrix.
  bool ComputeJacobian(const double* /*x*/, double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, kLocalDim, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }

  static bool plusJacobian(const double*, double* jacobian);

  // Size of x.
  int GlobalSize() const final {
    return kGlobalDim;
  }

  // Size of delta.
  int LocalSize() const final {
    return kLocalDim;
  }

  /// \brief Generalization of the addition operation,
  ///        x_plus_delta = Plus(x, delta)
  ///        with the condition that Plus(x, 0) = x.
  /// @param[in] x Variable.
  /// @param[in] delta Perturbation.
  /// @param[out] x_plus_delta Perturbed x.
  static bool plus(const double* x, const double* delta, double* x_plus_delta);

  static bool minus(const double* x, const double* x_plus_delta, double* delta);

  bool Minus(const double *x, const double *x_plus_delta,
             double *delta) const final {
    return minus(x, x_plus_delta, delta);
  }

  /// \brief Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  static bool liftJacobian(const double* /*x*/, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kLocalDim, kGlobalDim, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }

  /// \brief Computes the Jacobian from minimal space to naively
  /// overparameterised space as used by ceres.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  bool ComputeLiftJacobian(const double *x, double *jacobian) const final {
    return liftJacobian(x, jacobian);
  }
};

std::shared_ptr<okvis::ceres::LocalParamizationAdditionalInterfaces> createLandmarkLocalParameterization(int modelId);
} // namespace swift_vio

#endif // INCLUDE_SWIFT_VIO_POINT_LANDMARK_MODELS_HPP_


#pragma once
#include <boost/variant.hpp>
#include <vector>
#include "export.hpp"

namespace ear {

  struct EAR_EXPORT CartesianPosition {
    CartesianPosition(double X = 0.0, double Y = 0.0, double Z = 0.0)
        : X(X), Y(Y), Z(Z){};
    double X;
    double Y;
    double Z;
  };

  struct EAR_EXPORT PolarPosition {
    PolarPosition(double azimuth = 0.0, double elevation = 0.0,
                  double distance = 1.0)
        : azimuth(azimuth), elevation(elevation), distance(distance){};
    double azimuth;
    double elevation;
    double distance;
  };

  using Position = boost::variant<CartesianPosition, PolarPosition>;

}  // namespace ear


#pragma once
#include <memory>
#include <vector>
#include <boost/serialization/access.hpp>

namespace snn
{
    enum class activation
    {
        sigmoid = 0,
        iSigmoid,
        tanh,
        ReLU,
        gaussian,
        identity
    };
}
namespace snn::internal
{
    class ActivationFunction
    {
    private:

        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

    public:
        static std::vector<std::shared_ptr<ActivationFunction>> activationFunctions;

        const float min;
        const float max;

        ActivationFunction(float min, float max);
        virtual ~ActivationFunction() = default;
        static void initialize();
        static std::shared_ptr<ActivationFunction> get(activation type);

        [[nodiscard]] virtual float function(const float) const = 0;
        [[nodiscard]] virtual float derivative(const float) const = 0;

        [[nodiscard]] virtual activation getType() const = 0;

        virtual bool operator==(const ActivationFunction& activationFunction) const;
        virtual bool operator!=(const ActivationFunction& activationFunction) const;
    };
}


#ifndef OSRM_UTIL_MSB_HPP
#define OSRM_UTIL_MSB_HPP

#include <boost/assert.hpp>

#include <cstdint>
#include <utility>

namespace osrm
{
namespace util
{

// get the msb of an integer
// return 0 for integers without msb
template <typename T> std::size_t msb(T value)
{
    static_assert(std::is_integral<T>::value && !std::is_signed<T>::value, "Integer required.");
    std::size_t msb = 0;
    while (value > 0)
    {
        value >>= 1u;
        msb++;
    }
    BOOST_ASSERT(msb > 0);
    return msb - 1;
}

#if (defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)) && __x86_64__
inline std::size_t msb(std::uint64_t v)
{
    BOOST_ASSERT(v > 0);
    return 63UL - __builtin_clzl(v);
}
inline std::size_t msb(std::uint32_t v)
{
    BOOST_ASSERT(v > 0);
    return 31UL - __builtin_clz(v);
}
#endif
}
}

#endif


/*
 * Copyright (C) 2019 by AutoSense Organization. All rights reserved.
 * Gary Chan <chenshj35@mail2.sysu.edu.cn>
 */

#ifndef COMMON_INCLUDE_COMMON_COMMON_HPP_
#define COMMON_INCLUDE_COMMON_COMMON_HPP_

#include <pcl/common/centroid.h>    // pcl::compute3DCentroid
#include <pcl/common/transforms.h>  // pcl::transformPointCloud
#include <pcl/io/pcd_io.h>          // pcl::io::savePCDFileASCII
#include <Eigen/Core>
#include <cmath>  // sqrt, pow
#include <string>
#include <utility>
#include <vector>

#include "common/types/type.h"

namespace autosense {
namespace common {
// float precision
const float EPSILON = 1e-9;

//----------------------------------- sort compare function
template <typename PointType>
bool sortByAxisXAsc(PointType p1, PointType p2) {
    return p1.x < p2.x;
}

template <typename PointType>
bool sortByAxisZAsc(PointType p1, PointType p2) {
    return p1.z < p2.z;
}

template <typename ObjType>
bool sortByObjSizeDesc(ObjType obj1, ObjType obj2) {
    return obj1->cloud->size() > obj2->cloud->size();
}

/// \brief Utility function for swapping two values.
template <typename T>
bool swap_if_gt(T& a, T& b) {  // NOLINT
    if (a > b) {
        std::swap(a, b);
        return true;
    }
    return false;
}

//----------------------------------- *.pcd
static void savePCDModel(PointICloudConstPtr pc,
                         const std::string& model_name) {
    // std::string pcd_model_file = "model.pcd";
    pcl::io::savePCDFileASCII(model_name, *pc);
}

static void loadPCDModel(PointICloudPtr pc, const std::string& model_name) {
    pcl::io::loadPCDFile<PointI>(model_name, *pc);
}

static bool loadPCDModel(PointCloudPtr pc,
                         Eigen::Affine3f& model2world) {  // NOLINT
    std::string pcd_model_file =
        "/home/gary/Workspace/intern_ws/pcl_learning/model.pcd";
    if (pcl::io::loadPCDFile<Point>(pcd_model_file, *pc) == -1) {
        return false;
    } else {

        model2world = Eigen::Affine3f::Identity();
        Eigen::Vector4f model_centroid;
        pcl::compute3DCentroid<Point>(*pc, model_centroid);
        model2world.translation().matrix() = Eigen::Vector3f(
            model_centroid[0], model_centroid[1], model_centroid[2]);
        pcl::transformPointCloud(*pc, *pc, model2world.inverse());

        return true;
    }
}

/**
 * @brief convert PointI cloud in indices to PointD cloud
 * @param cloud
 * @param indices
 * @param trans_cloud
 */
static void convertPointCloud(PointICloudPtr icloud,
                              const std::vector<int>& indices,
                              PointDCloud* dcloud) {
    if (dcloud->size() != indices.size()) {
        dcloud->resize(indices.size());
    }
    for (size_t i = 0u; i < indices.size(); ++i) {
        const PointI& p = icloud->at(indices[i]);
        Eigen::Vector3d v(p.x, p.y, p.z);
        PointD& tp = dcloud->at(i);
        tp.x = v.x();
        tp.y = v.y();
        tp.z = v.z();
        tp.intensity = p.intensity;
    }
}

//----------------------------------- math utils
static float toRad(float degree) { return degree * (M_PI / 180.f); }

}  // namespace common
}  // namespace autosense

#endif  // COMMON_INCLUDE_COMMON_COMMON_HPP_


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[assign_2d_point
//` Shows the usage of assign to set point coordinates, and, besides that, shows how you can initialize ttmath points with high precision

#include <iostream>
#include <iomanip>

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

#if defined(HAVE_TTMATH)
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

int main()
{
    using boost::geometry::assign_values;

    boost::geometry::model::d2::point_xy<double> p1;
    assign_values(p1, 1.2345, 2.3456);

#if defined(HAVE_TTMATH)
    boost::geometry::model::d2::point_xy<ttmath::Big<1,4> > p2;
    assign_values(p2, "1.2345", "2.3456"); /*< It is possible to assign coordinates with other types than the coordinate type.
        For ttmath, you can e.g. conveniently use strings. The advantage is that it then has higher precision, because
        if doubles are used for assignments the double-precision is used.
        >*/
#endif

    std::cout
        << std::setprecision(20)
        << boost::geometry::dsv(p1) << std::endl
#if defined(HAVE_TTMATH)
        << boost::geometry::dsv(p2) << std::endl
#endif
        ;

    return 0;
}

//]

//[assign_2d_point_output
/*`
Output:
[pre
(1.2344999999999999, 2.3456000000000001)
(1.2345, 2.3456)
]
*/
//]


/*
 correlation.hxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#pragma once

#include <boost/numeric/ublas/vector.hpp>
#include <vector>

#include "neighbour.hxx"
#include "supercell.hxx"

namespace ublas = boost::numeric::ublas;

class Correlation {

public:

  Correlation();

  void Calculate(const Neighbour& neighbour_table,
                 Supercell& supercell);

  double ErrorFunction(double x);

  long number;
  std::vector<double> pair_clusters;
  std::vector<long> pair_count;
  std::vector<double> pair_correlations;
  ublas::vector<double> errors;

};


//
// Created by rgrandia on 14.02.20.
//

#include "ocs2_core/loopshaping/LoopshapingPropertyTree.h"

#include <boost/property_tree/info_parser.hpp>

namespace ocs2 {
namespace loopshaping_property_tree {
Filter readSISOFilter(const boost::property_tree::ptree& pt, std::string filterName, bool invert) {
  // Get Sizes
  auto numRepeats = pt.get<size_t>(filterName + ".numRepeats");
  auto numPoles = pt.get<size_t>(filterName + ".numPoles");
  auto numZeros = pt.get<size_t>(filterName + ".numZeros");
  auto DCGain = pt.get<scalar_t>(filterName + ".DCGain");
  size_t numStates = numRepeats * numPoles;
  size_t numInputs = numRepeats;
  size_t numOutputs = numRepeats;

  // Setup Filter, convention a0*s^n + a1*s^(n-1) + ... + an
  vector_t numerator(numZeros + 1);
  numerator.setZero();
  numerator(0) = 1.0;
  for (size_t z = 0; z < numZeros; z++) {
    auto zero = pt.get<scalar_t>(filterName + ".zeros." + "(" + std::to_string(z) + ")");
    numerator.segment(1, z + 1) -= zero * numerator.segment(0, z + 1).eval();
  }

  vector_t denominator(numPoles + 1);
  denominator.setZero();
  denominator(0) = 1.0;
  for (size_t p = 0; p < numPoles; p++) {
    auto pole = pt.get<scalar_t>(filterName + ".poles." + "(" + std::to_string(p) + ")");
    denominator.segment(1, p + 1) -= pole * denominator.segment(0, p + 1).eval();
  }

  // Scale
  if (DCGain > 0) {
    scalar_t currentDCGain = numerator(numZeros) / denominator(numPoles);
    if (currentDCGain < 1e-6 || currentDCGain > 1e6) {
      throw std::runtime_error("Trouble rescaling transfer function, current DCGain: " + std::to_string(currentDCGain));
    }
    scalar_t scaling = DCGain / currentDCGain;
    numerator *= scaling;
  }

  if (invert) {
    vector_t temp;
    temp = numerator;
    numerator = denominator;
    denominator = temp;
  }

  // Convert to state space
  matrix_t a, b, c, d;
  ocs2::tf2ss(numerator, denominator, a, b, c, d);

  matrix_t A = matrix_t::Zero(numStates, numStates);
  matrix_t B = matrix_t::Zero(numStates, numInputs);
  matrix_t C = matrix_t::Zero(numInputs, numStates);
  matrix_t D = matrix_t::Zero(numInputs, numInputs);
  size_t statecount = 0;
  for (size_t r = 0; r < numRepeats; r++) {
    A.block(statecount, statecount, numPoles, numPoles) = a;
    B.block(statecount, r, numPoles, 1) = b;
    C.block(r, statecount, 1, numPoles) = c;
    D.block(r, r, 1, 1) = d;
    statecount += numPoles;
  }

  return Filter(A, B, C, D);
}

Filter readMIMOFilter(const boost::property_tree::ptree& pt, std::string filterName, bool invert) {
  auto numFilters = pt.get<size_t>(filterName + ".numFilters");
  matrix_t A(0, 0), B(0, 0), C(0, 0), D(0, 0);
  if (numFilters > 0) {
    // Read the sisoFilters
    std::vector<Filter> sisoFilters;
    size_t numStates(0), numInputs(0), numOutputs(0);
    for (size_t i = 0; i < numFilters; ++i) {
      // Read filter
      std::string sisoFilterName = filterName + ".Filter" + std::to_string(i);
      sisoFilters.emplace_back(readSISOFilter(pt, sisoFilterName, invert));

      // Track sizes
      numStates += sisoFilters.back().getNumStates();
      numInputs += sisoFilters.back().getNumInputs();
      numOutputs += sisoFilters.back().getNumOutputs();
    }

    // Concatenate siso matrices into one MIMO filter
    A = matrix_t::Zero(numStates, numStates);
    B = matrix_t::Zero(numStates, numInputs);
    C = matrix_t::Zero(numOutputs, numStates);
    D = matrix_t::Zero(numOutputs, numInputs);
    size_t statecount(0), inputcount(0), outputcount(0);
    for (const auto& filt : sisoFilters) {
      A.block(statecount, statecount, filt.getNumStates(), filt.getNumStates()) = filt.getA();
      B.block(statecount, inputcount, filt.getNumStates(), filt.getNumInputs()) = filt.getB();
      C.block(outputcount, statecount, filt.getNumOutputs(), filt.getNumStates()) = filt.getC();
      D.block(outputcount, inputcount, filt.getNumOutputs(), filt.getNumInputs()) = filt.getD();
      statecount += filt.getNumStates();
      inputcount += filt.getNumInputs();
      outputcount += filt.getNumOutputs();
    }
  }
  return Filter(A, B, C, D);
}

std::shared_ptr<LoopshapingDefinition> load(const std::string& settingsFile) {
  // Read from settings File
  boost::property_tree::ptree pt;
  boost::property_tree::read_info(settingsFile, pt);
  Filter r_filter = loopshaping_property_tree::readMIMOFilter(pt, "r_filter");
  Filter s_filter = loopshaping_property_tree::readMIMOFilter(pt, "s_inv_filter", /*invert=*/true);
  auto gamma = pt.get<scalar_t>("gamma");

  if (r_filter.getNumOutputs() > 0 && s_filter.getNumOutputs() > 0) {
    throw std::runtime_error("[LoopshapingDefinition] using both r and s filter not implemented");
  }

  if (r_filter.getNumOutputs() > 0) {
    return std::make_shared<LoopshapingDefinition>(LoopshapingType::outputpattern, r_filter, gamma);
  }
  if (s_filter.getNumOutputs() > 0) {
    auto eliminateInputs = pt.get<bool>("eliminateInputs");
    if (eliminateInputs) {
      return std::make_shared<LoopshapingDefinition>(LoopshapingType::eliminatepattern, s_filter, gamma);
    } else {
      return std::make_shared<LoopshapingDefinition>(LoopshapingType::inputpattern, s_filter, gamma);
    }
  }

  throw std::runtime_error("[LoopshapingDefinition] error loading loopshaping definition, no valid filter found");
}

}  // namespace loopshaping_property_tree
}  // namespace ocs2


//
// HOGpp - Fast histogram of oriented gradients computation using integral
// histograms
//
// Copyright 2021 Sergiu Deitsch <sergiu.deitsch@gmail.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#define BOOST_TEST_MODULE hogpp

#include <cmath>

#include <hogpp/signedgradient.hpp>
#include <hogpp/unsignedgradient.hpp>

#include <boost/mpl/list.hpp>
#include <boost/test/included/unit_test.hpp>

using Scalars = boost::mpl::list<float, double, long double>;

// clang-format off
BOOST_TEST_DECORATOR
(
    *boost::unit_test::tolerance(5.96048e-08f)
    *boost::unit_test::tolerance(3.90314e-17l)
)
// clang-format on
BOOST_AUTO_TEST_CASE_TEMPLATE(signed_gradient, Scalar, Scalars)
{
    using std::fpclassify;
    using std::nextafter;

    hogpp::SignedGradient<Scalar> binning;

    BOOST_TEST(fpclassify(binning(+1, 0)) == FP_ZERO);
    BOOST_TEST(binning(-1, 0) == Scalar(0.5));
    BOOST_TEST(fpclassify(binning(0, 0)) == FP_ZERO);
    BOOST_TEST(binning(+1, nextafter(Scalar{0}, Scalar{-1})) == Scalar{1});
    BOOST_TEST(binning(-1, nextafter(Scalar{0}, Scalar{+1})) == Scalar(0.5));
}

// clang-format off
BOOST_TEST_DECORATOR
(
    *boost::unit_test::tolerance(5.96048e-08f)
    *boost::unit_test::tolerance(3.90314e-17l)
)
// clang-format on
BOOST_AUTO_TEST_CASE_TEMPLATE(unsigned_gradient, Scalar, Scalars)
{
    using std::fpclassify;
    using std::nextafter;

    hogpp::UnsignedGradient<Scalar> binning;

    BOOST_TEST(fpclassify(binning(+1, 0)) == FP_ZERO);
    BOOST_TEST(fpclassify(binning(-1, 0)) == FP_ZERO);
    BOOST_TEST(binning(0, +1) == Scalar(0.5));
    BOOST_TEST(binning(0, -1) == Scalar(0.5));
    BOOST_TEST(fpclassify(binning(0, 0)) == FP_ZERO);
    BOOST_TEST(binning(+1, nextafter(Scalar{0}, Scalar{-1})) == Scalar{1});
    BOOST_TEST(binning(-1, nextafter(Scalar{0}, Scalar{+1})) == Scalar{1});
}


#define NDEBUG
#include "hlpc_ls.h"
#include "invert_matrix.hpp"
//#include "invert_matrix_gj.hpp"
#include "stopwatch.hpp"
#include <cmath>
#include <limits>
#include <fstream>

#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#if HLPC_DO_TIMING == 1
#define TIME_THIS(x, y) cxxutil::stopwatch x(y)
#else
#define TIME_THIS(x, y)
#endif

typedef ublas::matrix<double> dmatrix;
typedef ublas::vector<double> dvector;

void toeplitz(const dvector& c, const dvector& r, dmatrix& result) {
    TIME_THIS(watch, "toeplitz");
    result.resize(c.size(), r.size(), false);

    for (int i = 0; i < result.size1(); i++) {
	for (int j = 0; j < result.size2(); j++) {
	    if (j <= i) {
		result(i, j) = c(i-j);
	    } else {
		result(i, j) = r(j-i);
	    }
	}
    }
}

void geninv(const dmatrix& mat, dmatrix& inverse) {
    TIME_THIS(watch, "geninv");
    int m = mat.size1();
    int n = mat.size2();
    dmatrix A;
    bool transpose = false;
    {
	TIME_THIS(getA, "get A");
	if (m < n) {
	    transpose = true;
	    A = prod(mat, ublas::trans(mat));
	    n = m;
	} else {
	    A = prod(ublas::trans(mat), mat);
	}
    }

    typedef ublas::banded_matrix<double> diag_matrix;
    diag_matrix dA(A.size1(), A.size2(), 0, 0);
    ublas::matrix_vector_slice<dmatrix> A_diag(A, ublas::slice(0, 1, A.size1()), ublas::slice(0, 1, A.size2()));
    { TIME_THIS(getdA, "get dA");
	for (int i = 0; i < dA.size1(); i++) {
	    if (i < dA.size2()) {
		dA(i, i) = A_diag(i);
	    }
	}
    }
    double minimum = std::numeric_limits<double>::max();
    {
	TIME_THIS(getMin,"get minimum");
	for (int i = 0; i < dA.size1(); i++) {
	    if (i < dA.size2()) {
		if (dA(i, i) > 0.) {
		    if (minimum > dA(i, i)) {
			minimum = dA(i, i);
		    }
		}
	    }
	}
    }
    double tol = minimum * 1e-9;
    dmatrix L(A.size1(), A.size2());
    {
	TIME_THIS(getL, "get L");
	for (int i = 0; i < L.size1(); i++) {
	    for (int j = 0; j < L.size2(); j++) {
		L(i, j) = 0.;
	    }
	}
    }
    int r = 0;

    {
	TIME_THIS(chol, "cholesky_decomposition");
	for (int k = 0; k < n; k++) {
	    r++;
	    ublas::vector<double> nullvec(n-k);
	    if (r > 1) {
		ublas::project(L, ublas::range(k, n), ublas::range(r-1, r))
		    = ublas::project(A, ublas::range(k, n), ublas::range(k, k+1))
		    - ublas::prod(
			    ublas::project(L, ublas::range(k, n), ublas::range(0, r)),
			    ublas::trans(ublas::project(L, ublas::range(k, k+1), ublas::range(0, r)))
			    );
		dmatrix projection(ublas::prod(ublas::project(L, ublas::range(k, n), ublas::range(0, 3)),
			    ublas::trans(ublas::project(L, ublas::range(k, k+1), ublas::range(0, 3)))));
	    } else {
		ublas::project(L, ublas::range(k, n), ublas::range(r-1, r))
		    = ublas::project(A, ublas::range(k, n), ublas::range(k, k+1));
		dmatrix projection(ublas::project(A, ublas::range(k, n), ublas::range(k, k+1)));
	    }
	    if (L(k, r-1) > tol) {
		L(k, r-1) = std::sqrt(L(k, r-1));
		if (k < n-1) {
		    ublas::project(L, ublas::range(k+1, n), ublas::range(r-1, r))
			= ublas::project(L, ublas::range(k+1, n), ublas::range(r-1, r)) / L(k, r-1);
		}
	    } else {
		r--;
	    }
	}
    }
    { TIME_THIS(projecting, "projecting L");
	L = project(L, ublas::range(0, L.size1()), ublas::range(0, r));
    }
    dmatrix p(ublas::prod(ublas::trans(L), L));
    dmatrix M(p.size1(), p.size2());
    {
	TIME_THIS(inverting, "inverting matrix");
	invert_matrix(p, M);
    }
//    {
//	TIME_THIS(inverting, "inverting matrix via gauss-jordan");
//	bool b = invert_matrix_gj(p, M);
//	if (!b) {
//	    std::cerr << "matrix inversion via gj failed." << std::endl;
//	}
//    }

    {
	TIME_THIS(matmult, "final multiplications");
	if (transpose) {
	    inverse = ublas::prod(ublas::trans(mat), L);
	    inverse = ublas::prod(inverse, M);
	    inverse = ublas::prod(inverse, M);
	    inverse = ublas::prod(inverse, ublas::trans(L));
	} else {
	    inverse = ublas::prod(L, M);
	    inverse = ublas::prod(inverse, M);
	    inverse = ublas::prod(inverse, ublas::trans(L));
	    inverse = ublas::prod(inverse, ublas::trans(mat));
	}
    }
}

void hlpc_ls(double* y, int len, int order, int compr, float* poles)
{
    TIME_THIS(hlpc_watch, "hlpc_ls");
    int col_length = len - order;
    int row_length = order;

    dvector column(col_length);
    dvector row(row_length);
    dvector a_multiplier(col_length);

    for (int i = 0; i < col_length; i++) {
	column(i) = y[order - 1 + i];
	a_multiplier(i) = y[order + i];
    }
    for (int i = 0; i < row_length; i++) {
	row(i) = y[order - 1 - i];
    }

    dmatrix R;
    toeplitz(column, row, R);
    dmatrix inv;
    geninv(R, inv);
    TIME_THIS(watch, "hlpc_finalize");
    dvector tmp_a = prod(inv, a_multiplier);
    double err = ublas::norm_2(a_multiplier - prod(R, tmp_a));
    if (isnan(err)) {
	std::cerr << "err is nan!" << std::endl;
	std::ofstream out("broken_inv.txt", std::ios::out);
	out << inv << std::endl;
	out.close();
	out.open("broken_R.txt", std::ios::out);
	out << R << std::endl;
	out.close();
	out.open("broken_column.txt", std::ios::out);
	out << column << std::endl;
	out.close();
	out.open("broken_row.txt", std::ios::out);
	out << row << std::endl;
	out.close();
	out.open("broken_y.txt", std::ios::out);
	for (int k = 0; k < len; k++) {
	    out << y[k] << std::endl;
	}
	out.close();
	std::cerr << "y_len=" << len << ", order=" << order << std::endl;
	std::exit(1);
    }
    dvector a(1 + tmp_a.size());
    a(0) = 1.;
    for (int i = 1; i < a.size(); i++) {
	a(i) = -tmp_a(i-1);
    }
    a = a / sqrt(err);

    // copy over to output
    for (int i = 0; i < a.size(); i++) {
	poles[i] = a(i);
	if (isnan(poles[i])) {
	    std::cerr << "poles[" << i << "] is nan!" << std::endl;
	    std::ofstream out("broken_inv.txt", std::ios::out);
	    out << inv << std::endl;
	    out.close();
	    out.open("broken_R.txt", std::ios::out);
	    out << R << std::endl;
	    out.close();
	    out.open("broken_column.txt", std::ios::out);
	    out << column << std::endl;
	    out.close();
	    out.open("broken_row.txt", std::ios::out);
	    out << row << std::endl;
	    out.close();
	    out.open("broken_y.txt", std::ios::out);
	    for (int k = 0; k < len; k++) {
		out << y[k] << std::endl;
	    }
	    out.close();
	    out.open("broken_a.txt", std::ios::out);
	    out << a << std::endl;
	    out.close();
	    out.open("broken_tmp_a.txt", std::ios::out);
	    out << tmp_a << std::endl;
	    out.close();
	    std::cerr << "y_len=" << len << ", order=" << order << ", err=" << err << std::endl;
	    std::exit(1);
	}
    }
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "FockDiis.h"
#include <Eigen/QR>
#include <algorithm>

namespace Scine {
namespace Utils {

FockDiis::FockDiis() {
  setSubspaceSize(5);
}

void FockDiis::setUnrestricted(bool b) {
  unrestricted_ = b;
  diisError_.setUnrestricted(b);
  restart();
}

void FockDiis::setSubspaceSize(int n) {
  bool resizeNeeded = n != subspaceSize_;

  subspaceSize_ = n;

  if (resizeNeeded) {
    resizeMembers();
  }
}

void FockDiis::setNAOs(int n) {
  bool resizeNeeded = n != nAOs_;

  nAOs_ = n;

  if (resizeNeeded) {
    resizeMembers();
  }
}

void FockDiis::resizeMembers() {
  fockMatrices.resize(subspaceSize_);
  diisError_.resize(subspaceSize_);
  diisStepErrors_.resize(subspaceSize_);

  overlap = Eigen::MatrixXd::Zero(nAOs_, nAOs_);

  B = Eigen::MatrixXd::Ones(subspaceSize_ + 1, subspaceSize_ + 1) * (-1);
  B(0, 0) = 0;

  rhs = Eigen::VectorXd::Zero(subspaceSize_ + 1);
  rhs(0) = -1;

  restart();
}

void FockDiis::setOverlapMatrix(const Eigen::MatrixXd& S) {
  overlap = S.selfadjointView<Eigen::Lower>();
  restart();
}

void FockDiis::restart() {
  C = Eigen::VectorXd::Zero(subspaceSize_ + 1);
  iterationNo_ = 0;
  index_ = 0;
}

void FockDiis::addMatrices(const SpinAdaptedMatrix& F, const DensityMatrix& P) {
  iterationNo_++;
  lastAdded_ = index_;

  fockMatrices[index_] = F;

  diisError_.setErrorFromMatrices(index_, F, P, overlap);
  diisStepErrors_[index_] = std::sqrt(diisError_.getError(index_, index_)) / nAOs_;

  updateBMatrix();

  index_ = (index_ + 1) % subspaceSize_;
}

void FockDiis::updateBMatrix() {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;

  // Bii element
  B(lastAdded_ + 1, lastAdded_ + 1) = diisError_.getError(lastAdded_, lastAdded_);

  // Bij elements
  for (int i = 1; i < activeSize + 1; i++) {
    if (i == lastAdded_ + 1) {
      continue;
    }
    B(lastAdded_ + 1, i) = diisError_.getError(lastAdded_, i - 1);
    B(i, lastAdded_ + 1) = B(lastAdded_ + 1, i);
  }
}

SpinAdaptedMatrix FockDiis::getMixedFockMatrix() {
  if (iterationNo_ > subspaceSize_) {
    iterationNo_ = subspaceSize_;
  }

  // If we have only one Fock matrix
  if (iterationNo_ < 2) {
    return fockMatrices[0];
  }

  C.head(iterationNo_ + 1) =
      B.block(0, 0, iterationNo_ + 1, iterationNo_ + 1).colPivHouseholderQr().solve(rhs.head(iterationNo_ + 1));

  return calculateLinearCombination();
}

SpinAdaptedMatrix FockDiis::calculateLinearCombination() {
  if (unrestricted_) {
    Eigen::MatrixXd FAlpha = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    Eigen::MatrixXd FBeta = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    for (int i = 0; i < iterationNo_; i++) {
      FAlpha += C(i + 1) * fockMatrices[i].alphaMatrix();
      FBeta += C(i + 1) * fockMatrices[i].betaMatrix();
    }
    return SpinAdaptedMatrix::createUnrestricted(std::move(FAlpha), std::move(FBeta));
  }
  Eigen::MatrixXd Fsol = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
  for (int i = 0; i < iterationNo_; i++) {
    Fsol += C(i + 1) * fockMatrices[i].restrictedMatrix();
  }
  return SpinAdaptedMatrix::createRestricted(std::move(Fsol));
}

double FockDiis::getMaxError() const {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;
  auto maxIter = std::max_element(diisStepErrors_.begin(), diisStepErrors_.begin() + activeSize);
  return *maxIter;
}

double FockDiis::getMinError() const {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;
  auto minIter = std::min_element(diisStepErrors_.begin(), diisStepErrors_.begin() + activeSize);
  return *minIter;
}

double FockDiis::getLastError() const {
  return diisStepErrors_[lastAdded_];
}
} // namespace Utils
} // namespace Scine


// Boost.Geometry

// Copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>
#include <boost/geometry/algorithms/detail/result_inverse.hpp>

namespace boost { namespace geometry { namespace detail
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with first order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/

template <typename CT, bool EnableDistance, bool EnableAzimuth>
struct andoyer_inverse
{
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        result_type result;

        // coordinates in radians

        if ( math::equals(lon1, lon2)
          && math::equals(lat1, lat2) )
        {
            result.set(CT(0), CT(0));
            return result;
        }

        CT const pi_half = math::pi<CT>() / CT(2);

        if ( math::equals(math::abs(lat1), pi_half)
          && math::equals(math::abs(lat2), pi_half) )
        {
            result.set(CT(0), CT(0));
            return result;
        }

        CT const dlon = lon2 - lon1;
        CT const sin_dlon = sin(dlon);
        CT const cos_dlon = cos(dlon);
        CT const sin_lat1 = sin(lat1);
        CT const cos_lat1 = cos(lat1);
        CT const sin_lat2 = sin(lat2);
        CT const cos_lat2 = cos(lat2);

        // H,G,T = infinity if cos_d = 1 or cos_d = -1
        // lat1 == +-90 && lat2 == +-90
        // lat1 == lat2 && lon1 == lon2
        CT const cos_d = sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon;
        CT const d = acos(cos_d);
        CT const sin_d = sin(d);

        // just in case since above lat1 and lat2 is checked
        // the check below is equal to cos_d == 1 || cos_d == -1 || d == 0
        if ( math::equals(sin_d, CT(0)) )
        {
            result.set(CT(0), CT(0));
            return result;
        }

        // if the function returned before this place
        // and endpoints were on the poles +-90 deg
        // in this case the azimuth could either be 0 or +-pi

        CT const f = detail::flattening<CT>(spheroid);

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            CT const K = math::sqr(sin_lat1-sin_lat2);
            CT const L = math::sqr(sin_lat1+sin_lat2);
            CT const three_sin_d = CT(3) * sin_d;
            // H or G = infinity if cos_d = 1 or cos_d = -1
            CT const H = (d+three_sin_d)/(CT(1)-cos_d);
            CT const G = (d-three_sin_d)/(CT(1)+cos_d);

            // for e.g. lat1=-90 && lat2=90 here we have G*L=INF*0
            CT const dd = -(f/CT(4))*(H*K+G*L);

            CT const a = get_radius<0>(spheroid);

            result.distance = a * (d + dd);
        }
        else
        {
            result.distance = CT(0);
        }

        if ( BOOST_GEOMETRY_CONDITION(EnableAzimuth) )
        {
            CT A = CT(0);
            CT U = CT(0);
            if ( ! math::equals(cos_lat2, CT(0)) )
            {
                CT const tan_lat2 = sin_lat2/cos_lat2;
                CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon;
                A = atan2(sin_dlon, M);
                CT const sin_2A = sin(CT(2)*A);
                U = (f/CT(2))*math::sqr(cos_lat1)*sin_2A;
            }

            CT V = CT(0);
            if ( ! math::equals(cos_lat1, CT(0)) )
            {
                CT const tan_lat1 = sin_lat1/cos_lat1;
                CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon;
                CT const B = atan2(sin_dlon, N);
                CT const sin_2B = sin(CT(2)*B);
                V = (f/CT(2))*math::sqr(cos_lat2)*sin_2B;
            }

            // infinity if sin_d = 0, so cos_d = 1 or cos_d = -1
            CT const T = d / sin_d;
            CT const dA = V*T-U;

            result.azimuth = A - dA;
        }
        else
        {
            result.azimuth = CT(0);
        }

        return result;
    }
};

}}} // namespace boost::geometry::detail

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP


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


#ifndef HOMENCIPHER_HPP
#define HOMENCIPHER_HPP

#include <NTL/ZZ.h>
#include "Encipher.hpp"
#include "Random.h"

template <typename C, typename P>
class HOMEncipher : public Encipher<C,P>
{
protected:
	NTL::ZZ modulus;
	NTL::ZZ p;
	NTL::ZZ q;
	Random* rng;

	void generateModulus(int lambda, int eta){
		p = NTL::RandomPrime_ZZ(lambda,20);
		q = NTL::RandomPrime_ZZ(eta,20);
		modulus = p*q;
	}

public:
	void init(){
		this->rng = new Random();
		NTL::ZZ_p::init(modulus);
	};
	NTL::ZZ getModulus(){
		return modulus;
	};
	virtual std::string writeParametersToJSON()=0;

	HOMEncipher(){
		rng=nullptr;
	};
	virtual ~HOMEncipher(){
		delete rng;
	};
};

#endif


#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/random.hpp>
#include <ctime>
#include <chrono>

#include <kv/matrix-inversion.hpp>

int main()
{
	boost::numeric::ublas::matrix<double> a(2, 2);
	boost::numeric::ublas::matrix<double> b;

	std::chrono::system_clock::time_point t;

	a(0, 0) = 1.; a(0, 1) = 2.;
	a(1, 0) = 3.; a(1, 1) = 4.;

	kv::invert(a, b);

	std::cout << b << "\n";

	int i, j, n;

	n = 2000;

	boost::numeric::ublas::matrix<double> c(n, n);
	using namespace boost;
	variate_generator< mt19937, uniform_real<double> > rand (mt19937(time(0)), uniform_real<double>(-1., 1.));

	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			c(i, j) = rand();
		}
	}

	t = std::chrono::system_clock::now();
	kv::invert(c, b);
	std::cout << std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - t).count() / 1e9 << " sec\n";

	boost::numeric::ublas::vector<double> d(n);

	t = std::chrono::system_clock::now();
	kv::linear_equation(c, d, d);
	std::cout << std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - t).count() / 1e9 << " sec\n";

	t = std::chrono::system_clock::now();
	kv::mm_mult(c, b, b);
	std::cout << std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - t).count() / 1e9 << " sec\n";
}


#include "get_nn.h"
#include "geometry.h"

#include <cmath>
#include <algorithm>
#include <iostream>
#include <unordered_map>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;

namespace marvel {

spatial_hash::spatial_hash(const MatrixXd& points_, const size_t& nn_num_)
    : points(points_), points_num(points_.cols()), nn_num(nn_num_)
{
    //set hash parameter
    size_t table_size = size_t(floor(pow(points.cols(), 0.5)));
    //build hash_map
    points_hash = unordered_multimap<Vector3i, size_t>(table_size);
    hash_NNN();
}

int spatial_hash::get_shell(const Eigen::Vector3i& query, const int& radi, std::vector<Vector3i>& shell) const
{
    assert(radi > -1);
    //init
    shell.clear();
    int res = 0;

    auto loop = [&](const int& face, const int& face_axis, const int& radi_1, const int& radi_2) {
        if (face <= max_id(face_axis) && face >= min_id(face_axis))
        {
            int axis_1 = (face_axis + 1) % 3, axis_2 = (face_axis + 2) % 3;
            for (int j = query(axis_1) - radi_1; j < query(axis_1) + radi_1 + 1; ++j)
            {
                for (int k = query(axis_2) - radi_2; k < query(axis_2) + radi_2 + 1; ++k)
                {
                    Vector3i one_grid;
                    one_grid[face_axis] = face;
                    one_grid[axis_1]    = j;
                    one_grid[axis_2]    = k;
                    shell.push_back(one_grid);
                }
            }
            return 0;
        }
        else
        {
            return 1;
        }
    };
    if (radi > 0)
    {
        int touch_time = 0;
        for (size_t i = 0; i < 6; ++i)
        {

            touch_time += loop(i % 2 == 0 ? query(i / 2) - radi : query(i / 2) + radi, i / 2, i < 4 ? radi : radi - 1, i < 2 ? radi : radi - 1);
        }
        if (touch_time == 6)
            res = -1;
    }
    else
    {
        shell.push_back(query);
    }
    return res;
}

int spatial_hash::find_NN(const size_t& point_id, vector<pair_dis>& NN_cand)
{
    return std::move(find_NN(point_id, NN_cand, nn_num));
}

int spatial_hash::find_NN(const size_t& point_id, vector<pair_dis>& NN_cand, const size_t& nn_num_)
{

    size_t cand_num  = 0;
    int    grid_delt = 0;
    // bool once_more = false;
    int once_more = 0;
    int touch_bd  = 0;
    //count for grid_delt;
    do
    {
        vector<Vector3i> shell;
        touch_bd = get_shell(points_dis.col(point_id), grid_delt, shell);
        for (auto& grid : shell)
        {
            auto range = points_hash.equal_range(grid);
            if (range.first != range.second)
            {
                for_each(range.first, range.second, [&](decltype(points_hash)::value_type& one_point) {
                    double dis = (points.col(point_id) - points.col(one_point.second)).norm();
                    NN_cand.push_back({ one_point.second, dis });
                });
            }
        }
        ++grid_delt;
        if (NN_cand.size() > nn_num_ + 2)
            // once_more = !once_more;
            once_more++;
        if (touch_bd)
            break;

    } while (NN_cand.size() < nn_num_ + 2 || once_more < 2);
    return 0;
}

int spatial_hash::hash_NNN()
{
    //init
    NN.setZero(nn_num, points_num);
    points_hash.clear();
    //set parameter

    double   cell_num = pow(points.cols() / float(nn_num), 1.0 / 3);
    MatrixXd bdbox(3, 2);
    build_bdbox(points, bdbox);
    cell_size = (bdbox.col(1) - bdbox.col(0)) / cell_num;
    //generate discretized 3D position
    points_dis = MatrixXi(3, points_num);
    for (size_t i = 0; i < points.rows(); ++i)
    {
        points_dis.row(i) = floor(points.row(i).array() / cell_size(i)).cast<int>();
    }

    max_id = { points_dis.row(0).maxCoeff(), points_dis.row(1).maxCoeff(), points_dis.row(2).maxCoeff() };
    min_id = { points_dis.row(0).minCoeff(), points_dis.row(1).minCoeff(), points_dis.row(2).minCoeff() };

    //insert elements

    for (size_t i = 0; i < points_num; ++i)
    {
        points_hash.insert({ points_dis.col(i), i });
    }

    return 0;
}

const MatrixXi& spatial_hash::get_NN(const size_t& nn_num_)
{
    auto useless_sup_radi = get_sup_radi(nn_num_);

    return NN;
}
const MatrixXi& spatial_hash::get_NN()
{
    return std::move(get_NN(nn_num));
}

const VectorXi spatial_hash::get_NN(const Vector3d& query, const size_t& nn_num_)
{
    //init data
    assert(nn_num_ < points_num);
    VectorXi         near_nei = VectorXi::Zero(nn_num_);
    vector<pair_dis> NN_cand;

    size_t         cand_num    = 0;
    int            grid_delt   = 0;
    int            once_more   = 0;
    const Vector3i center_grid = floor(query.array() / cell_size.array()).cast<int>();
    do
    {
        vector<Vector3i> shell;
        get_shell(center_grid, grid_delt, shell);
        for (auto& grid : shell)
        {
            auto range = points_hash.equal_range(grid);
            if (range.first != range.second)
            {
                for_each(range.first, range.second, [&](decltype(points_hash)::value_type& one_point) {
                    double dis = (query - points.col(one_point.second)).norm();
                    NN_cand.push_back({ one_point.second, dis });
                });
            }
        }
        ++grid_delt;
        if (NN_cand.size() > nn_num_ + 2)
            once_more++;
    } while (NN_cand.size() < nn_num_ + 2 || once_more < 2);

    sort(NN_cand.begin(), NN_cand.end(), [](const pair_dis& a, const pair_dis& b) { return a.dis < b.dis; });

    for (size_t j = 0; j < nn_num_; ++j)
    {
        near_nei(j) = NN_cand[j].n;
    }
    return near_nei;
}

const Eigen::MatrixXi spatial_hash::get_four_noncoplanar_NN(const Eigen::MatrixXd& nods)
{
    MatrixXi near_nei = MatrixXi::Zero(4, nods.cols());
    // #pragma omp parallel for
    for (size_t i = 0; i < nods.cols(); ++i)
    {

        auto     ver_nn_num = 4;
        Vector2i two_index;
        two_index << 2, 3;
        bool is_co_line = true, is_co_plane = true;
        do
        {
            auto     NN_index = get_NN(nods.col(i), ver_nn_num);
            Vector3d V1       = (points.col(NN_index(1)) - points.col(NN_index(0))).normalized();
            Vector3d V2       = (points.col(NN_index(two_index(0))) - points.col(NN_index(0))).normalized();
            Vector3d cross_   = V1.cross(V2);
            if (cross_.squaredNorm() < 1e-5)
            {
                two_index(0)++;
                two_index(1)++;
                ver_nn_num++;
                is_co_line = true;
            }
            else
            {
                is_co_line = false;

                do
                {
                    Vector3d V3 = (points.col(NN_index(two_index(1))) - points.col(NN_index(0))).normalized();
                    if (fabs(cross_.dot(V3)) < 1e-5)
                    {
                        two_index(1)++;
                        ver_nn_num++;
                        NN_index = get_NN(nods.col(i), ver_nn_num);
                        // assert(ver_nn_num < 10);
                        is_co_plane = true;
                    }
                    else
                    {
                        is_co_plane    = false;
                        near_nei(0, i) = NN_index(0);
                        near_nei(1, i) = NN_index(1);
                        near_nei(2, i) = NN_index(two_index(0));
                        near_nei(3, i) = NN_index(two_index(1));
                        break;
                    }
                } while (is_co_plane);
            }
        } while (is_co_line);
    }
    return near_nei;
}

const VectorXd& spatial_hash::get_sup_radi()
{
    return std::move(get_sup_radi(nn_num));
}

const VectorXd& spatial_hash::get_sup_radi(const size_t& nn_num_)
{
    assert(points.cols() >= nn_num_);

    //init data

    sup_radi.setZero(points_num);

#pragma omp parallel for
    for (size_t i = 0; i < points_num; ++i)
    {
        vector<pair_dis> NN_cand;
        find_NN(i, NN_cand, nn_num_ + 2);
        sort(NN_cand.begin(), NN_cand.end(), [](const pair_dis& a, const pair_dis& b) { return a.dis < b.dis; });
        for (size_t j = 0; j < nn_num_; ++j)
        {
            NN(j, i) = NN_cand[j].n;
            sup_radi[i] += NN_cand[j].dis;
        }
    }
    sup_radi *= 3.0 / nn_num_;
    return sup_radi;
}
int spatial_hash::get_friends(const Vector3d& query, const double& sup_radi, vector<size_t>& friends, bool is_sample) const
{

    friends.clear();
    int            grid_delt   = static_cast<int>(ceil(sup_radi / cell_size.col(0).minCoeff())) + 1;
    const Vector3i center_grid = floor(query.array() / cell_size.array()).cast<int>();
    for (size_t i = 0; i < grid_delt; ++i)
    {
        vector<Vector3i> shell;
        get_shell(center_grid, i, shell);
        for (auto& one_grid : shell)
        {
            auto range = points_hash.equal_range(one_grid);
            if (range.first != range.second)
            {
                for_each(range.first, range.second, [&](const decltype(points_hash)::value_type& one_point) {
                    double dis = (points.col(one_point.second) - query).norm();
                    if (dis < sup_radi)
                        friends.push_back(one_point.second);
                });
            }
        }
    }
    assert(friends.size() > nn_num);
    return 0;
}
// int spatial_hash::update_points(const Eigen::MatrixXd &points_){
//   points = points_;
//   hash_NNN();
//   return 0;
// }

}  // namespace marvel