// C++ standard library headers
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <limits>
#include <numeric>
#include <vector>

// Boost headers
#include <boost/math/interpolators/pchip.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

// triumf++ headers
#include <triumf/bnmr/nuclei.hpp>
#include <triumf/nmr/dipole_dipole.hpp>
#include <triumf/nmr/nuclei.hpp>
#include <triumf/numpy.hpp>
#include <triumf/srim/pdf.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/pippard.hpp>

// ROOT headers
#include <ROOT/RCsvDS.hxx>
#include <ROOT/RDF/RInterface.hxx>
#include <ROOT/RDataFrame.hxx>
#include <TCanvas.h>
#include <TGraph.h>

//
double slr_fcn(double d, double R_0, double d_0) {
  // return d < d_0 ? 1.5 * R_0 : R_0 * std::exp((d - d_0) / (10.0 * d_0));
  return d < d_0 ? 1.0 * R_0 : 0.5 * R_0;
}

/// model slr rate
template <typename T = double>
T slr_rate_z(T z, T temperature, T critical_temperature, T gap_meV, T xi_0,
             T mean_free_path, T lambda_0, T exponent, T applied_field,
             T dipole_field, T correlation_rate, T slr_constant, T slr_exponent,
             T surface_thickness, T surface_rate) {
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  if (_z_ < 0.0) {
    return surface_rate;
  } else {
    // calculate the local field from the screening profile
    T screened_field =
        temperature > critical_temperature
            ? applied_field
            : triumf::superconductivity::pippard::field_penetration<T>(
                  _z_, temperature, critical_temperature, gap_meV, xi_0,
                  mean_free_path, lambda_0, exponent, applied_field);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<double>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

const double TEMPERATURE = 2.5;
const double T_C = 9.25;
const double D_0 = triumf::superconductivity::bcs::gap_meV<double>(TEMPERATURE);
const double XI_0 = 38.0;
const double ELL = 1e4;
const double LAMBDA_0 = 40.0;
const double EXPONENT = 4.0;
const double B_0 = 0.02;
const double B_D = 5e-5;
const double NU_C = 0.1 / 23.8e-6;
const double SLR_C = 0.75;
const double SLR_N = 1.0;
const double SURFACE_THICKNESS = 5.0;
const double SURFACE_RATE = 10.0;

constexpr double GLOBAL_R_0 = 1.0;
constexpr double GLOBAL_D_0 = 15.0;

//
template <typename T = double> class DepthAverage {
public:
  // constructor
  DepthAverage(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha = df.Take<T>("Alpha").GetValue();
    _alpha_error = df.Take<T>("Alpha Error").GetValue();
    _beta = df.Take<T>("Beta").GetValue();
    _beta_error = df.Take<T>("Beta Error").GetValue();
    _z_max = df.Take<T>("Max (nm)").GetValue();
    _z_max_error = df.Take<T>("Max Error (nm)").GetValue();
  };

  //
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  //
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  //
  T alpha(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> alpha_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_alpha)));
    return alpha_interpolator(energy_keV);
  };

  //
  T beta(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> beta_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_beta)));
    return beta_interpolator(energy_keV);
  };

  //
  T z_max(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> z_max_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_z_max)));
    return z_max_interpolator(energy_keV);
  };

  //
  T z_average(T energy_keV) {
    T a = alpha(energy_keV);
    T b = beta(energy_keV);
    T zm = z_max(energy_keV);
    return zm * a / (a + b);
  };

  /*
  // depth-averaging using numeric integration
  T operator()(T energy_keV) {
    // T a = alpha(energy_keV);
    // T b = beta(energy_keV);
    // T zm = z_max(energy_keV);
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      // return slr_fcn(z, GLOBAL_R_0, GLOBAL_D_0) *
      return slr_rate_z<T>(z, TEMPERATURE, T_C, D_0, XI_0, ELL, LAMBDA_0,
                           EXPONENT, B_0, B_D, NU_C, SLR_C, SLR_N,
                           SURFACE_THICKNESS, SURFACE_RATE) *
             triumf::srim::pdf::modified_beta<T>(
                 z, alpha(energy_keV), beta(energy_keV), z_max(energy_keV));
    };
    T Q =
        integrator.integrate(integrand, 0.0, z_max(energy_keV),
                             std::cbrt(std::numeric_limits<double>::epsilon()));
    return Q;
  };
  */

  // depth-averaging using "histogram" summation
  T operator()(T energy_keV) {
    constexpr std::size_t n = 201;
    // bin edges
    std::vector<T> z_edge =
        triumf::numpy::linspace<T>(0.0, z_max(energy_keV), n);
    // bin widths - adjust ranges by one for "correct" size
    std::vector<T> dz(n - 1);
    std::adjacent_difference(std::begin(z_edge) + 1, std::end(z_edge),
                             std::begin(dz));
    // bin centres
    std::vector<T> z(n - 1);
    // probabilities
    std::vector<T> p_z(n - 1);
    std::vector<T> weights(n - 1);
    std::vector<T> slr_rates(n - 1);

    for (std::size_t i = 0; i < z.size(); ++i) {
      z.at(i) = z_edge.at(i) + 0.5 * dz.at(i);
      p_z.at(i) = triumf::srim::pdf::modified_beta<T>(
          z.at(i), alpha(energy_keV), beta(energy_keV), z_max(energy_keV));
      // std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";
      weights.at(i) = dz.at(i) * p_z.at(i);
      // slr_rates.at(i) = slr_fcn(z.at(i), GLOBAL_R_0, GLOBAL_D_0);
      slr_rates.at(i) = slr_rate_z<T>(z.at(i), TEMPERATURE, T_C, D_0, XI_0, ELL,
                                      LAMBDA_0, EXPONENT, B_0, B_D, NU_C, SLR_C,
                                      SLR_N, SURFACE_THICKNESS, SURFACE_RATE);
    }

    T sum_weights = std::reduce(std::begin(weights), std::end(weights));
    // std::cout << "sum_weights = " << sum_weights << "\n";
    T sum_weights_slr = std::transform_reduce(
        std::begin(weights), std::end(weights), std::begin(slr_rates), 0.0);
    // std::cout << "sum_weights_slr = " << sum_weights_slr << "\n";
    T weighted_average = sum_weights_slr / sum_weights;
    return weighted_average;
  };

private:
  // vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha;
  std::vector<T> _alpha_error;
  std::vector<T> _beta;
  std::vector<T> _beta_error;
  std::vector<T> _z_max;
  std::vector<T> _z_max_error;
};

void test_depth_averaging() {
  const double alpha = 2.5;
  const double beta = 4.5;
  const double z_max = 250.0;

  //
  // const double R_0 = 10.0;
  // const double d_0 = 25.0; // (1.0 / 6.0) * z_max;

  // bin edges
  std::vector<double> z_edge = triumf::numpy::linspace<double>(0.0, z_max, 201);
  // bin widths - adjust ranges by one for "correct" size
  std::vector<double> dz(z_edge.size() - 1);
  std::adjacent_difference(std::begin(z_edge) + 1, std::end(z_edge),
                           std::begin(dz));
  // bin centres
  std::vector<double> z(z_edge.size() - 1);
  // probabilities
  std::vector<double> p_z(z_edge.size() - 1);

  std::vector<double> weights(z_edge.size() - 1);

  std::vector<double> slr_rates(z_edge.size() - 1);

  std::cout << std::setprecision(16);
  std::cout << std::fixed;

  for (std::size_t i = 0; i < z.size(); ++i) {
    z.at(i) = z_edge.at(i) + 0.5 * dz.at(i);
    p_z.at(i) = triumf::srim::pdf::modified_beta(z.at(i), alpha, beta, z_max);
    // std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";

    weights.at(i) = dz.at(i) * p_z.at(i);
    // slr_rates.at(i) = slr_fcn(z.at(i), GLOBAL_R_0, GLOBAL_D_0);
    slr_rates.at(i) = slr_rate_z<double>(
        z.at(i), TEMPERATURE, T_C, D_0, XI_0, ELL, LAMBDA_0, EXPONENT, B_0, B_D,
        NU_C, SLR_C, SLR_N, SURFACE_THICKNESS, SURFACE_RATE);
  }

  double sum_weights = std::reduce(std::begin(weights), std::end(weights));
  std::cout << "sum_weights = " << sum_weights << "\n";

  double sum_weights_slr = std::transform_reduce(
      std::begin(weights), std::end(weights), std::begin(slr_rates), 0.0);
  std::cout << "sum_weights_slr = " << sum_weights_slr << "\n";

  double weighted_average = sum_weights_slr / sum_weights;
  // std::cout << "weighted_average = " << weighted_average << "\n";

  /*
  for (std::size_t i = 0; i < z.size(); ++i) {
    p_z.at(i) = triumf::srim::pdf::modified_beta(z.at(i), alpha, beta, z_max);
    std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";
  }
  */

  // double sum = std::reduce(std::begin(p_z), std::end(p_z));
  // std::cout << "sum = " << sum << "\n";

  //

  /*
  std::cout << dz.size() << "\n";
  for (auto &bw : dz) {
    std::cout << bw << "\n";
  }
  */

  /*
  std::transform(std::begin(z), std::end(z), std::begin(p_z), [&](double d) {
    return triumf::srim::pdf::modified_beta(z.at(i), alpha, beta, z_max);
  });
  */

  TCanvas *c_p = new TCanvas();

  TGraph *g_p = new TGraph(z.size(), z.data(), p_z.data());
  g_p->SetTitle(";Depth (nm);Stopping probability (nm^{-1})");
  g_p->SetMarkerStyle(kFullCircle);
  g_p->SetMarkerColor(kBlack);
  g_p->Draw("AP");

  TCanvas *c_slr = new TCanvas();

  TGraph *g_slr = new TGraph(z.size(), z.data(), slr_rates.data());
  g_slr->SetTitle(";Depth (nm);Model SLR Rate (s^{-1})");
  g_slr->SetMarkerStyle(kFullCircle);
  g_slr->SetMarkerColor(kBlack);
  g_slr->Draw("AP");
  // g_slr->GetYaxis()->SetRangeUser(0, 10);

  // logarithmic scale
  // gPad->SetLogx();
  gPad->SetLogy();

  // tick marks on all sides of the plot
  gPad->SetTickx();
  gPad->SetTicky();

  // grid lines
  gPad->SetGridx();
  gPad->SetGridy();

  c_slr->Print("toy_depth_model.pdf", "EmbedFonts");

  // use numeric integration!
  // the "summation" approach (which is error-prone) converges to this answer
  // when the number of "bins" becomes large (e.g., ~1000).
  static boost::math::quadrature::tanh_sinh<double> slr_integrator;
  auto slr_integrand = [&](double d) {
    return triumf::srim::pdf::modified_beta(d, alpha, beta, z_max) *
           slr_fcn(d, GLOBAL_R_0, GLOBAL_D_0);
    /*
    slr_rate_z<double>(d, TEMPERATURE, T_C, D_0, XI_0, ELL, LAMBDA_0,
                       EXPONENT, B_0, B_D, NU_C, SLR_C, SLR_N,
                       SURFACE_THICKNESS, SURFACE_RATE);
    */
  };
  double Q = slr_integrator.integrate(slr_integrand, 0.0, z_max);
  std::cout << "weighted_average    = " << weighted_average << "\n";
  std::cout << "numeric_integration = " << Q << "\n";
  double tolerance = std::sqrt(std::numeric_limits<double>::epsilon());
  std::cout << "tolerance           = " << tolerance << "\n";

  DepthAverage<double> da("srim_profile_8li_nb_fitpar.csv");
  // double delta = 0.001;
  std::vector<double> energies =
      triumf::numpy::linspace<double>(da.energy_min(), da.energy_max(), 100);
  std::vector<double> rates;
  std::vector<double> mean_depths;

  auto time_start = std::chrono::steady_clock::now();
  for (auto &e : energies) {
    rates.push_back(da(e));
    mean_depths.push_back(da.z_average(e));
  }
  auto time_stop = std::chrono::steady_clock::now();

  TCanvas *c_r = new TCanvas();

  TGraph *g_r = new TGraph(energies.size(), energies.data(), rates.data());
  g_r->SetMarkerStyle(kFullCircle);
  g_r->SetMarkerColor(kRed);
  g_r->SetTitle(";Energy (keV);Depth averaged SLR rate 1/T_{1} (s^{-1})");

  g_r->Draw("AP");
  // g_r->GetYaxis()->SetRangeUser(0, 10);

  // logarithmic scale
  // gPad->SetLogx();
  gPad->SetLogy();

  // tick marks on all sides of the plot
  gPad->SetTickx();
  gPad->SetTicky();

  // grid lines
  gPad->SetGridx();
  gPad->SetGridy();

  c_r->Print("toy_depth_average.pdf", "EmbedFonts");

  TCanvas *c_rz = new TCanvas();

  TGraph *g_rz =
      new TGraph(mean_depths.size(), mean_depths.data(), rates.data());
  g_rz->SetMarkerStyle(kFullCircle);
  g_rz->SetMarkerColor(kRed);
  g_rz->SetTitle(";z_{average} (nm);Depth averaged SLR rate 1/T_{1} (s^{-1})");

  g_rz->Draw("AP");
  // g_rz->GetYaxis()->SetRangeUser(0, 10);

  c_rz->Print("toy_depth_average2.pdf", "EmbedFonts");

  std::chrono::duration<double> elapsed_seconds = time_stop - time_start;
  std::cout << "Elapsed time: " << elapsed_seconds.count() << " (s)\n";
}

#ifndef __CLING__
int main() {
  test_depth_averaging();
  return EXIT_SUCCESS;
}
#endif


#include <iostream>

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

#include <vector>
#include <math.h>

#include "NRGclasses.hpp"

void OneChQS_UpdateQm1fQ(CNRGmatrix &Qm1fNQ,CNRGarray Aeig,
			 CNRGbasisarray Abasis){

  boost::numeric::ublas::matrix<double> Mtemp;
  boost::numeric::ublas::matrix<double> fnw;

  int icount=0;

  Qm1fNQ.ClearAll();

  Qm1fNQ.SyncNRGarray(Aeig);

  // Find blocks such that Q'=Q+1, S'=S+-1/2

  for (int ii=0;ii<Aeig.NumBlocks();ii++)
    {
      double Qi=Aeig.GetQNumber(ii,0);
      double Si=Aeig.GetQNumber(ii,1);

      int Nstibl=Aeig.GetBlockSize(ii);

      for (int jj=ii+1;jj<Aeig.NumBlocks();jj++)
	{

	  double Qj=Aeig.GetQNumber(jj,0);
	  double Sj=Aeig.GetQNumber(jj,1);

	  int Nstjbl=Aeig.GetBlockSize(jj);

	  int kkp=0;
	  //if (Sj==Si+0.5) kkp=23;
	  //if (Sj==Si-0.5) kkp=32;
	  if (fabs(Sj-(Si+0.5))<1.0E-10) kkp=23;
	  if (fabs(Sj-(Si-0.5))<1.0E-10) kkp=32;

	  //	  if ( (Qj==Qi+1.0)&&( kkp!=0 ) )
	  if ( ( fabs(Qj-(Qi+1.0))<1.0E-10 )&&( kkp!=0 ) )
	    {

	      cout << "  Setting up Block i : " << ii
		   << " (size " <<  Nstibl
		   << ") x Block j " << jj
		   << " (size " << Nstjbl << ")" << endl;

 	      Qm1fNQ.MatBlockMap.push_back(ii);
 	      Qm1fNQ.MatBlockMap.push_back(jj);
	      Qm1fNQ.MatBlockBegEnd.push_back(icount);
	      icount+=Nstibl*Nstjbl;
	      Qm1fNQ.MatBlockBegEnd.push_back(icount-1);

	      // Sets the uBLAS matrix for Z_QS(iblock)
	      // Sets the uBLAS matrix for Z_QS(jblock)

	      cout << "    Setting up BLAS matrices..." << endl;
// 	      boost::numeric::ublas::matrix<double> Zibl=Aeig.EigVec2BLAS(ii);
// 	      boost::numeric::ublas::matrix<double> Zjbl=Aeig.EigVec2BLAS(jj);

	      boost::numeric::ublas::matrix<double> Zibl(Nstibl,Nstibl);
	      boost::numeric::ublas::matrix<double> Zjbl(Nstjbl,Nstjbl);
	      Zibl=Aeig.EigVec2BLAS(ii);
	      Zjbl=Aeig.EigVec2BLAS(jj);

	      cout << "    ...Zs done ..." << endl;
	      // Sets the uBLAS matrix <type|fn|type^'> with a bunch of zeroes

	      boost::numeric::ublas::matrix<double> fnbasis (Nstibl,Nstjbl);

	      // Check time
	      boost::timer t;
	      double time_elapsed;

	      // Loop on each block
	      int istbl=0;
	      for (int ist=Abasis.GetBlockLimit(ii,0);
		   ist<=Abasis.GetBlockLimit(ii,1);ist++)
		{
		  int typei=Abasis.iType[ist];
		  int stcfi=Abasis.StCameFrom[ist];
		  //cout << "Type i : " << typei << endl;
		  int jstbl=0;
		  for (int jst=Abasis.GetBlockLimit(jj,0);
		       jst<=Abasis.GetBlockLimit(jj,1);jst++)
		    {
		      int typej=Abasis.iType[jst];
		      int stcfj=Abasis.StCameFrom[jst];
		      //cout << "Type j : " << typej << endl;
		      // setting MatBlockMap
//  		      Qm1fNQ.MatBlockMap.push_back(ii);
//  		      Qm1fNQ.MatBlockMap.push_back(jj);

		      fnbasis(istbl,jstbl)=0.0;

		      // Both basis sates have to originate from the same
		      // previous state! Otherwise, zero.

		      if (stcfi==stcfj)
			{
			  if (
			      ( (typei==1) && (typej==2) &&(kkp==23) )||
			      ( (typei==1) && (typej==3) &&(kkp==32) )
			      )
			    fnbasis(istbl,jstbl)=1.0;
			  if ( (typej==4)&&(typei==2)&&(kkp==32) )
			    fnbasis(istbl,jstbl)=-sqrt((2.0*Si+1.0)/(2.0*Si));
			  if ( (typej==4)&&(typei==3)&&(kkp==23) )
			    fnbasis(istbl,jstbl)=sqrt((2.0*Si+1.0)/(2.0*Si+2.0));
			}
		      //cout << "fn(" << istbl << "," << jstbl << ") = ";
		      //cout << fnbasis(istbl,jstbl) << endl;

		      jstbl++;
		    }
		  istbl++;

		}

	      cout << "    ...fnbasis done." << endl;

	      // Multiply the 3 of them. Zi has eigvecs in ROWS!

	      cout << "    Multiplying BLAS matrices... " << endl;

//  	      boost::numeric::ublas::matrix<double> Mtemp=prod(fnbasis,trans(Zjbl));
//  	      boost::numeric::ublas::matrix<double> fnw=prod (Zibl,Mtemp);
// 	      fnw=prod (Zibl,Mtemp);

	      //Mtemp.resize(Nstibl,Nstjbl);
	      fnw.resize(Nstibl,Nstjbl);
 	      //noalias(Mtemp)=prod(fnbasis,trans(Zjbl));
	      //cout << "    ...one done..." << endl;
 	      //noalias(fnw)=prod (Zibl, Mtemp );

	      t.restart();
 	      noalias(fnw)=prod (Zibl,
			boost::numeric::ublas::matrix<double>(prod(fnbasis,trans(Zjbl))) );
	      time_elapsed=t.elapsed();
	      cout << "    ...done. Elapsed time:" << time_elapsed << endl;

//   	      cout << "Z(i="<<ii<<")  : " <<  Zibl << endl;
//   	      cout << "Z(j="<<jj<<")  : " <<  Zjbl << endl;
//    	      cout << "fbasis   :" <<  fnbasis << endl;
//    	      cout << "Zi.fbasis.ZjT : " <<  fnw << endl;

	      // Add to Qm1fNQ
	      for (int ii=0;ii<fnw.size1();ii++)
		{
		  for (int jj=0;jj<fnw.size2();jj++)
		    {
		      Qm1fNQ.MatEl.push_back(fnw(ii,jj));
		    }
		}

	    }
	}
      //Loop in jblock
    }
  // Loop in iblock

//    cout << "Qm1fNQ MatBlockMap : ";
//    for (int ii=0;ii<Qm1fNQ.MatBlockMap.size();ii++)
//      cout << Qm1fNQ.MatBlockMap[ii] << " ";
//    cout << endl;

// Too slow.
//     Qm1fNQ.FilterMap_SetBegEnd();

//    cout << "Filtered Qm1fNQ MatBlockMap : ";
//    for (int ii=0;ii<Qm1fNQ.MatBlockMap.size();ii++)
//      cout << Qm1fNQ.MatBlockMap[ii] << " ";
//    cout << endl;
//    cout << "Qm1fNQ MatBlockBegEnd : ";
//    for (int ii=0;ii<Qm1fNQ.MatBlockBegEnd.size();ii++)
//      cout << Qm1fNQ.MatBlockBegEnd[ii] << " ";
//    cout << endl;

//   for (int ibl=0;ibl<Qm1fNQ.NumMatBlocks();ibl++)
//     {
//       Qm1fNQ.PrintMatBlock(ibl);
//     }

}


#include "sweepline_state.hpp"

#include "geometry.hpp"
#include "util.hpp"

#include <boost/assert.hpp>

#include <algorithm>
#include <functional>
#include <iostream>

std::ostream& operator<<(std::ostream& rhs, const sweepline_state::edge& lhs)
{
    rhs << lhs.first << "->" << lhs.second;

    return rhs;
}

bool sweepline_state::edge_comparator(const sweepline_state::edge& lhs,
                                      const sweepline_state::edge& rhs) const
{
    const auto& lhs_start = coordinates[lhs.first];
    const auto& lhs_end = coordinates[lhs.second];
    const auto& rhs_start = coordinates[rhs.first];
    const auto& rhs_end = coordinates[rhs.second];
    auto lhs_params = geometry::segment_intersection(sweepline_start, sweepline_end, lhs_start, lhs_end);
    auto rhs_params = geometry::segment_intersection(sweepline_start, sweepline_end, rhs_start, rhs_end);

    auto lhs_x = std::max(lhs_start.x, lhs_end.x);
    auto rhs_x = std::max(rhs_start.x, rhs_end.x);

    // compare the intersection points on the sweepline
    bool result = util::epsilon_compare(1e-10, lhs_params.first_param, rhs_params.first_param) ?
                    (lhs_x < rhs_x) :
                    (lhs_params.first_param < rhs_params.first_param);

    return result;
}

void sweepline_state::move_sweepline(const coordinate& position)
{
    sweepline_end = position;
}

void sweepline_state::insert_edge(const sweepline_state::edge& to_insert)
{
    BOOST_ASSERT(to_insert.first < to_insert.second);

    auto iter = std::lower_bound(intersecting_edges.begin(), intersecting_edges.end(),
                                 to_insert, std::bind(&sweepline_state::edge_comparator, this,
                                                      std::placeholders::_1, std::placeholders::_2));

    if (iter == intersecting_edges.end())
    {
        intersecting_edges.push_back(to_insert);
    }
    else if (edge_comparator(*iter, to_insert))
    {
        intersecting_edges.push_front(to_insert);
    }
    else
    {
        intersecting_edges.insert(iter, to_insert);
    }
}

void sweepline_state::remove_edge(const sweepline_state::edge& to_remove)
{
    BOOST_ASSERT(to_remove.first < to_remove.second);

    auto iter = std::lower_bound(intersecting_edges.begin(), intersecting_edges.end(),
                                 to_remove, std::bind(&sweepline_state::edge_comparator, this,
                                                      std::placeholders::_1, std::placeholders::_2));

    // search for the real position, might have hit an itersection
    while (iter != intersecting_edges.end() && iter->first != to_remove.first && iter->second != to_remove.second)
    {
        iter++;
    }
    BOOST_ASSERT(iter != intersecting_edges.end());
    BOOST_ASSERT(iter->first == to_remove.first && iter->second == to_remove.second);

    intersecting_edges.erase(iter);
}

sweepline_state::edge_iterator sweepline_state::get_first_intersecting(const coordinate& coord) const
{
    BOOST_ASSERT_MSG(geometry::segment_intersection(sweepline_start, sweepline_end,
                                                    sweepline_start, coord).colinear,
                     "Sweepline must be moved forward for intersection search.");

    auto iter = std::lower_bound(intersecting_edges.begin(), intersecting_edges.end(), coord,
                                 [this](const edge& lhs, const coordinate& rhs)
                                 {
                                    auto params = geometry::segment_intersection(sweepline_start, rhs, coordinates[lhs.first], coordinates[lhs.second]);

                                    return params.first_param < 1;
                                 });
    return iter;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_ERFC_HPP
#define STAN_MATH_PRIM_SCAL_FUN_ERFC_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the complementary error function of the specified value.
 *
 * \f[
 * \mbox{erfc}(x) = 1 - \frac{2}{\sqrt{\pi}} \int_0^x e^{-t^2} dt
 * \f]
 *
 * @param[in] x Argument.
 * @return Complementary error function of the argument.
 */
inline double erfc(double x) {
  if (is_nan(x))
    return std::numeric_limits<double>::quiet_NaN();
  return boost::math::erfc(x, boost_policy_t());
}

/**
 * Return the error function of the specified argument.  This
 * version is required to disambiguate erf(int).
 *
 * @param[in] x Argument.
 * @return Complementary error function value of the argument.
 */
inline double erfc(int x) { return erfc(static_cast<double>(x)); }

}  // namespace math
}  // namespace stan
#endif


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G.Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "closed_form_calibration.h"

#include "g2o/types/sclam2d/odometry_measurement.h"

#include <iostream>
#include <limits>

#include <Eigen/SVD>

#define SQR(X)		( std::pow(X,2) )
#define CUBE(X)		( std::pow(X,3) )

namespace g2o {

bool ClosedFormCalibration::calibrate(const MotionInformationVector& measurements, SE2& laserOffset, Eigen::Vector3d& odomParams)
{
  std::vector<VelocityMeasurement, Eigen::aligned_allocator<VelocityMeasurement> > velMeasurements;
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& odomMotion = measurements[i].odomMotion;
    const double& timeInterval = measurements[i].timeInterval;
    MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
    VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
    velMeasurements.push_back(velMeas);
  }

  double J_21, J_22;
  {
    Eigen::MatrixXd A(measurements.size(), 2);
    Eigen::VectorXd x(measurements.size());
    for (size_t i = 0; i < measurements.size(); ++i) {
      const SE2& laserMotion = measurements[i].laserMotion;
      const double& timeInterval = measurements[i].timeInterval;
      const VelocityMeasurement& velMeas = velMeasurements[i];
      A(i, 0) = velMeas.vl() * timeInterval;
      A(i, 1) = velMeas.vr() * timeInterval;
      x(i) = laserMotion.rotation().angle();
    }
    // (J_21, J_22) = (-r_l / b, r_r / b)
    Eigen::Vector2d linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
    //std::cout << linearSolution.transpose() << std::endl;
    J_21 = linearSolution(0);
    J_22 = linearSolution(1);
  }

  // construct M
  Eigen::Matrix<double, 5, 5> M;
  M.setZero();
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& laserMotion = measurements[i].laserMotion;
    const double& timeInterval = measurements[i].timeInterval;
    const VelocityMeasurement& velMeas = velMeasurements[i];
    Eigen::Matrix<double, 2, 5> L;
    double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
    double o_theta_k = omega_o_k * timeInterval;
    double sx = 1.;
    double sy = 0.;
    if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
      sx = sin(o_theta_k)       / o_theta_k;
      sy = (1 - cos(o_theta_k)) / o_theta_k;
    }
    double c_x = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
    double c_y = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
    L(0, 0) = -c_x;
    L(0, 1) = 1 - cos(o_theta_k);
    L(0, 2) = sin(o_theta_k);
    L(0, 3) = laserMotion.translation().x();
    L(0, 4) = -laserMotion.translation().y();
    L(1, 0) = -c_y;
    L(1, 1) = - sin(o_theta_k);
    L(1, 2) = 1 - cos(o_theta_k);
    L(1, 3) = laserMotion.translation().y();
    L(1, 4) = laserMotion.translation().x();
    M.noalias() += L.transpose() * L;
  }
  //std::cout << M << std::endl;

  // compute lagrange multiplier
  // and solve the constrained least squares problem
  double m11 = M(0,0);
  double m13 = M(0,2);
  double m14 = M(0,3);
  double m15 = M(0,4);
  double m22 = M(1,1);
  double m34 = M(2,3);
  double m35 = M(2,4);
  double m44 = M(3,3);

  double a = m11 * SQR(m22) - m22 * SQR(m13);
  double b = 2*m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) - 2*m22 * SQR(m13) * m44 - 2*m11 * m22 * SQR(m34)
    - 2*m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) + 2*m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34)
    + 2*m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
  double c = - 2*m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) + m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44
    + 2*m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 - 2*m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14
    - 2*m11 * m22 * SQR(m35) * m44 + 2*m11 * SQR(m35) * SQR(m34) + m22 * SQR(m14) * SQR(m35) - 2*m13 * SQR(m35) * m34 * m14
    - 2*m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) + m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15)
    + m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 + 2*m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14)
    - SQR(m22) * SQR(m15) * m44;

  // solve the quadratic equation
  double lambda1, lambda2;
  if(a < std::numeric_limits<double>::epsilon()) {
    if(b <= std::numeric_limits<double>::epsilon())
      return false;
    lambda1 = lambda2 = -c/b;
  } else {
    double delta = b*b - 4*a*c;
    if (delta < 0)
      return false;
    lambda1 = 0.5 * (-b-sqrt(delta)) / a;
    lambda2 = 0.5 * (-b+sqrt(delta)) / a;
  }

  Eigen::VectorXd x1 = solveLagrange(M, lambda1);
  Eigen::VectorXd x2 = solveLagrange(M, lambda2);
  double err1 = x1.dot(M * x1);
  double err2 = x2.dot(M * x2);

  const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
  odomParams(0) = - calibrationResult(0) * J_21;
  odomParams(1) = calibrationResult(0) * J_22;
  odomParams(2) = calibrationResult(0);

  laserOffset = SE2(calibrationResult(1), calibrationResult(2), atan2(calibrationResult(4), calibrationResult(3)));

  return true;
}

Eigen::VectorXd ClosedFormCalibration::solveLagrange(const Eigen::Matrix<double,5,5>& M, double lambda)
{
  // A = M * lambda*W (see paper)
  Eigen::Matrix<double,5,5> A;
  A.setZero();
  A(3,3) = A(4,4) = lambda;
  A.noalias() += M;

  // compute the kernel of A by SVD
  Eigen::JacobiSVD< Eigen::Matrix<double,5,5> > svd(A, ComputeFullV);
  Eigen::VectorXd result = svd.matrixV().col(4);
  //for (int i = 0; i < 5; ++i)
  //std::cout << "singular value " << i << " "  << svd.singularValues()(i) << std::endl;
  //std::cout << "kernel base " << result << std::endl;

  // enforce the conditions
  // x_1 > 0
  if (result(0) < 0.)
    result *= -1;
  // x_4^2 + x_5^2 = 1
  double scale = sqrt(pow(result(3), 2) + pow(result(4), 2));
  result /= scale;

  return result;
}

} // end namespace


#include <stdint.h>
#include <Eigen/Core>
#include "mex.h"

using namespace Eigen;
using namespace std;

// TODO openmp version

// NOTE: mxSetProperty and possibly mxGetProperty make copies, even with
// classdef < handle! lame! I believe mxGetField does NOT make a copy, though I
// haven't tested it recently

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
    /* SETUP */
    if (nrhs != 4) { mexErrMsgTxt("wrong number of arguments\n"); }

    //// pull out inputs

    // parameters struct
    if (!mxGetField(prhs[0],0,"learns")) { mexErrMsgTxt("model missing 'learns' field\n"); }
    double *learns = mxGetPr(mxGetField(prhs[0],0,"learns"));
    int num_resources = max(mxGetM(mxGetField(prhs[0],0,"learns")),
            mxGetN(mxGetField(prhs[0],0,"learns")));

    if (!mxGetField(prhs[0],0,"forgets")) { mexErrMsgTxt("model missing 'forgets' field\n"); }
    double *forgets = mxGetPr(mxGetField(prhs[0],0,"forgets"));

    if (!mxGetField(prhs[0],0,"guesses")) { mexErrMsgTxt("model missing 'guesses' field\n"); }
    double *guess = mxGetPr(mxGetField(prhs[0],0,"guesses"));

    if (!mxGetField(prhs[0],0,"slips")) { mexErrMsgTxt("model missing 'slips' field\n"); }
    double *slip = mxGetPr(mxGetField(prhs[0],0,"slips"));

    int num_subparts = max(mxGetM(mxGetField(prhs[0],0,"slips")),
            mxGetN(mxGetField(prhs[0],0,"slips")));

    if (!mxGetField(prhs[0],0,"prior")) { mexErrMsgTxt("model missing 'prior' field\n"); }
    double prior = mxGetScalar(mxGetField(prhs[0],0,"prior"));

    Vector2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        As.col(2*n) << 1-learns[n], learns[n];
        As.col(2*n+1) << forgets[n], 1-forgets[n];
    }

    int32_t *starts = (int32_t *) mxGetData(prhs[1]);
    int32_t *lengths = (int32_t *) mxGetData(prhs[2]);
    int16_t *allresources = (int16_t *) mxGetData(prhs[3]);

    int num_sequences = max(mxGetM(prhs[1]),mxGetN(prhs[1]));

    int bigT = 0;
    for (int k=0; k<num_sequences; k++) {
        bigT += (int) lengths[k];
    }

    //// outputs

    if (nlhs != 2) { mexErrMsgTxt("needs two outputs\n"); }

    // NOTE: these are doubles just to stick with the convention we had going
    // with the Matlab code, but they really should be some integer type. Some
    // implicit casting with the indexing below.
    plhs[0] = mxCreateNumericMatrix(1,bigT,mxINT8_CLASS,mxREAL);
    plhs[1] =  mxCreateNumericMatrix(num_subparts,bigT,mxINT8_CLASS,mxREAL);
    int8_t *all_stateseqs = (int8_t *) mxGetData(plhs[0]);
    int8_t *all_data = (int8_t *) mxGetData(plhs[1]);

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        // NOTE: -1 because Matlab indexing starts at 1
        int32_t sequence_start = starts[sequence_index] - 1;
        int32_t T = lengths[sequence_index];

        int8_t *data = all_data + num_subparts*sequence_start;
        int16_t *resources = allresources + sequence_start;
        int8_t *stateseq = all_stateseqs + sequence_start;

        Vector2d nextstate_distr = initial_distn;
        for (int t=0; t<T; t++) {
            stateseq[t] = nextstate_distr(0) < ((double) rand()) / ((double) RAND_MAX);
            for (int n=0; n<num_subparts; n++) {
                data[n+num_subparts*t] = (stateseq[t] ? slip[n] : (1-guess[n]))
                    < ((double) rand()) / ((double) RAND_MAX);
            }
            nextstate_distr = As.col(2*(resources[t]-1)+stateseq[t]);
        }
    }
}


#define ELPP_STL_LOGGING
#include "easyloggingpp/easylogging++.h"
#include <iostream>
#include <pcl/console/time.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/src/Core/Matrix.h>
#include <string>

INITIALIZE_EASYLOGGINGPP

using namespace pcl;
using namespace std;
using ICPFunPtr = Eigen::Matrix4f (*)(PointCloud<PointXYZI>::Ptr, PointCloud<PointXYZI>::Ptr, Eigen::Matrix4f&);

Eigen::Matrix4f PointToPlane(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud1,
                             pcl::PointCloud<pcl::PointXYZI>::Ptr cloud2,
                             Eigen::Matrix4f& guess)
{
  pcl::PointCloud<pcl::PointNormal>::Ptr src(new pcl::PointCloud<pcl::PointNormal>);
  pcl::copyPointCloud(*cloud1, *src);
  pcl::PointCloud<pcl::PointNormal>::Ptr tgt(new pcl::PointCloud<pcl::PointNormal>);
  pcl::copyPointCloud(*cloud2, *tgt);

  pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est;
  norm_est.setSearchMethod(pcl::search::KdTree<pcl::PointNormal>::Ptr(new pcl::search::KdTree<pcl::PointNormal>));
  norm_est.setKSearch(10);
  norm_est.setInputCloud(tgt);
  norm_est.compute(*tgt);

  pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
  typedef pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal> PointToPlane;
  boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
  icp.setTransformationEstimation(point_to_plane);  // key
  /*
  typedef pcl::registration::CorrespondenceEstimationNormalShooting<PointNormal,
  PointNormal, PointNormal> CorrEstNS;
  CorrEstNS::Ptr corrEst(new CorrEstNS);
  icp.setCorrespondenceEstimation(corrEst);
  */
  icp.setInputSource(src);
  icp.setInputTarget(tgt);
  // icp.setRANSACOutlierRejectionThreshold(ransac_par);
  icp.setRANSACIterations(20);
  icp.setMaximumIterations(100);
  icp.setTransformationEpsilon(1e-3);
  pcl::PointCloud<pcl::PointNormal> output;
  icp.align(output);  // align 的另一个重载可以设置一个初始矩阵guess
  LOG(INFO) << "score: " << icp.getFitnessScore() << endl;
  return icp.getFinalTransformation();
}

Eigen::Matrix4f PointToPoint(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source,
                             pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target,
                             Eigen::Matrix4f& guess)
{
  // ICP
  pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
  // pcl::IterativeClosestPointWithNormals<pcl::PointXYZI, pcl::PointXYZI> icp;
  pcl::search::KdTree<pcl::PointXYZI>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZI>);
  tree1->setInputCloud(cloud_source);
  pcl::search::KdTree<pcl::PointXYZI>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZI>);
  tree2->setInputCloud(cloud_target);
  icp.setSearchMethodSource(tree1);
  icp.setSearchMethodTarget(tree2);
  icp.setInputSource(cloud_source);
  icp.setInputTarget(cloud_target);
  icp.setMaxCorrespondenceDistance(150);  // 1500
  icp.setTransformationEpsilon(1e-3);
  icp.setEuclideanFitnessEpsilon(0.01);  // 0.1
  icp.setMaximumIterations(100);         // 100
  pcl::PointCloud<pcl::PointXYZI> output;
  icp.align(output);
  LOG(INFO) << "score: " << icp.getFitnessScore() << endl;
  return icp.getFinalTransformation();
}

Eigen::Matrix4f icpPlaneToPlane(PointCloud<PointXYZI>::Ptr src, PointCloud<PointXYZI>::Ptr tar, Eigen::Matrix4f& guess)
{
  pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;  // GICP 泛化的ICP，或者叫Plane to Plane
                                                                              // ICP
  icp.setTransformationEpsilon(0.0000000001);
  icp.setMaxCorrespondenceDistance(2.0);
  icp.setMaximumIterations(50);
  icp.setRANSACIterations(20);
  icp.setInputTarget(tar);
  icp.setInputSource(src);
  pcl::PointCloud<pcl::PointXYZI> unused_result;
  icp.align(unused_result, guess);
  LOG(INFO) << "score: " << icp.getFitnessScore();
  return icp.getFinalTransformation();
}

void addNormal(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
               pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals)
{
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

  pcl::search::KdTree<pcl::PointXYZI>::Ptr searchTree(new pcl::search::KdTree<pcl::PointXYZI>);
  searchTree->setInputCloud(cloud);

  pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> normalEstimator;
  normalEstimator.setInputCloud(cloud);
  normalEstimator.setSearchMethod(searchTree);
  normalEstimator.setKSearch(15);
  normalEstimator.compute(*normals);

  pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
}

Eigen::Matrix4f icpPointToPlane(PointCloud<PointXYZI>::Ptr src, PointCloud<PointXYZI>::Ptr tar, Eigen::Matrix4f& guess)
{
  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_source_normals(new pcl::PointCloud<pcl::PointXYZINormal>());
  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_target_normals(new pcl::PointCloud<pcl::PointXYZINormal>());
  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_source_trans_normals(new pcl::PointCloud<pcl::PointXYZINormal>());

  addNormal(tar, cloud_target_normals);
  addNormal(src, cloud_source_normals);

  pcl::IterativeClosestPointWithNormals<pcl::PointXYZINormal, pcl::PointXYZINormal>::Ptr icp(
      new pcl::IterativeClosestPointWithNormals<pcl::PointXYZINormal, pcl::PointXYZINormal>());
  icp->setTransformationEpsilon(0.0000000001);
  icp->setMaxCorrespondenceDistance(2.0);
  icp->setMaximumIterations(50);
  icp->setRANSACIterations(20);
  icp->setInputSource(cloud_source_normals);  //
  icp->setInputTarget(cloud_target_normals);
  icp->align(*cloud_source_trans_normals, guess);  //
  LOG(INFO) << "score: " << icp->getFitnessScore() << endl;
  return icp->getFinalTransformation();
}

Eigen::Matrix4f genTransformation(Eigen::Vector3f& r, Eigen::Vector3f& t)
{
  Eigen::AngleAxisf init_rotation_x(r.x(), Eigen::Vector3f::UnitX());
  Eigen::AngleAxisf init_rotation_y(r.y(), Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf init_rotation_z(r.z(), Eigen::Vector3f::UnitZ());
  Eigen::Translation3f init_translation(t.x(), t.y(), t.z());
  return (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();
}

void displayAngel(Eigen::Matrix4f& transformation)
{
  double rx, ry, rz, tx, ty, tz;
  ry = (180 / M_PI) * atan(-(transformation(2, 0) / sqrt(pow(transformation(0, 0), 2) + pow(transformation(1, 0), 2))));
  rz = (180 / M_PI) * atan((transformation(1, 0) / transformation(0, 0)));
  rx = (180 / M_PI) * atan((transformation(2, 1) / transformation(2, 2)));
  tx = transformation(0, 3);
  ty = transformation(1, 3);
  tz = transformation(2, 3);
  LOG(INFO) << rx << '\n' << ry << '\n' << rz << '\n' << tx << '\n' << ty << '\n' << tz << std::endl;
}

Eigen::Matrix4f
run(PointCloud<PointXYZI>::Ptr src, PointCloud<PointXYZI>::Ptr tar, const std::vector<float>& params, ICPFunPtr icp)
{
  Eigen::Vector3f r(params[0], params[1], params[2]);
  Eigen::Vector3f t(params[3], params[4], params[5]);
  // Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
  Eigen::Matrix4f guess = genTransformation(r, t);
  return icp(src, tar, guess);
}

void initLogger(const std::string& fpath)
{
  el::Configurations conf(fpath);
  el::Loggers::reconfigureAllLoggers(conf);
}

int main(int argc, char** argv)
{
  initLogger("easyloggingpp/log.conf");
  const auto config = YAML::LoadFile("./config/config.yaml");
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target_transform(new pcl::PointCloud<pcl::PointXYZI>());
  pcl::PointCloud<pcl::PointXYZI>::Ptr Final(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI> temp;
  temp.makeShared();

  const auto pcd_source_path = config["pcd_source_path"].as<std::string>();
  const auto pcd_target_path = config["pcd_target_path"].as<std::string>();
  const auto init_params = config["init_params"].as<std::vector<float>>();
  const auto icp_pattern = config["icp_pattern"].as<std::string>();
  // load pcd file
  if (pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_source_path, *cloud_source) == -1)
  {
    LOG(INFO) << "load source failed!" << std::endl;
    return -1;
  }
  LOG(INFO) << "source loaded!" << std::endl;

  if (pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_target_path, *cloud_target) == -1)
  {
    LOG(INFO) << "load target failed!" << std::endl;
    return -1;
  }
  LOG(INFO) << "target loaded!" << std::endl;

  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
  // clang-format off
  std::map<std::string, ICPFunPtr> icp_map{{"icpPlaneToPlane", &icpPlaneToPlane},
                                           {"icpPointToPlane", &icpPointToPlane},
                                           {"PointToPlane", &PointToPlane},
                                           {"PointToPoint", &PointToPoint}};
  // clang-format on
  Eigen::Matrix4f transformation = run(cloud_source, cloud_target, init_params, icp_map[icp_pattern]);
  displayAngel(transformation);
  pcl::transformPointCloud(*cloud_source, *cloud_target_transform, transformation);

  // display
  pcl::visualization::PCLVisualizer p;
  p.setWindowName("Could after calibration");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> tgt_after_transform_h(cloud_target_transform, 0, 255,
                                                                                0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> tgt_h(cloud_target, 0, 0, 255);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> src_h(cloud_source, 255, 255, 255);
  p.addPointCloud(cloud_target_transform, tgt_after_transform_h, "target_after_transform");
  p.addPointCloud(cloud_target, tgt_h, "target");
  p.addPointCloud(cloud_source, src_h, "source");
  p.setSize(1200, 900);
  p.spin();
  return 0;
}


#ifndef AIKIDO_STATESPACE_SO2STATESPACE_HPP_
#define AIKIDO_STATESPACE_SO2STATESPACE_HPP_
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "aikido/statespace/ScopedState.hpp"
#include "aikido/statespace/StateSpace.hpp"

namespace aikido {
namespace statespace {

// Defined in detail/SO2-impl.hpp
template <class>
class SO2StateHandle;

/// The two-dimensional special orthogonal group SO(2), i.e. the space of
/// planar rigid body rotations.
class SO2 : virtual public StateSpace
{
public:
  /// State in SO(2), a planar rotation.
  class State;

  using StateHandle = SO2StateHandle<State>;
  using StateHandleConst = SO2StateHandle<const State>;

  using ScopedState = statespace::ScopedState<StateHandle>;
  using ScopedStateConst = statespace::ScopedState<StateHandleConst>;

  using StateSpace::compose;

  /// Constructs a state space representing SO(2).
  SO2() = default;

  /// Helper function to create a \c ScopedState.
  ///
  /// \return new \c ScopedState.
  ScopedState createState() const;

  /// Creates an identical clone of \c stateIn.
  ScopedState cloneState(const StateSpace::State* stateIn) const;

  /// Returns state as a rotation angle in (-pi, pi].
  ///
  /// \param[in] state State.
  double toAngle(const State* state) const;

  /// Sets state from a rotation angle in (-inf, inf).
  ///
  /// \param[out] state State corresponding to angle.
  /// \param[in] angle Rotation angle in (-inf, inf).
  void fromAngle(State* state, double angle) const;

  /// Returns state as an Eigen rotation.
  ///
  /// \param[in] state State.
  Eigen::Rotation2Dd toRotation(const State* state) const;

  /// Sets state from an Eigen rotation.
  ///
  /// \param[out] state State corresponding to rotation.
  /// \param[in] rotation Eigen rotation.
  void fromRotation(State* state, const Eigen::Rotation2Dd& rotation) const;

  // Documentation inherited.
  std::size_t getStateSizeInBytes() const override;

  // Documentation inherited.
  StateSpace::State* allocateStateInBuffer(void* buffer) const override;

  // Documentation inherited.
  void freeStateInBuffer(StateSpace::State* state) const override;

  // Documentation inherited.
  void compose(
      const StateSpace::State* state1,
      const StateSpace::State* state2,
      StateSpace::State* out) const override;

  // Documentation inherited.
  void getIdentity(StateSpace::State* out) const override;

  // Documentation inherited.
  void getInverse(
      const StateSpace::State* in, StateSpace::State* out) const override;

  // Documentation inherited.
  std::size_t getDimension() const override;

  // Documentation inherited.
  void copyState(
      const StateSpace::State* source,
      StateSpace::State* destination) const override;

  /// Exponential mapping of Lie algebra element to a Lie group element. The
  /// tangent space is parameterized a rotation angle.
  ///
  /// \param[in] tangent Element of the tangent space.
  /// \param[out] out Corresponding element of the Lie group.
  void expMap(
      const Eigen::VectorXd& tangent, StateSpace::State* out) const override;

  /// Log mapping of Lie group element to a Lie algebra element. The tangent
  /// space is parameterized as a rotation angle.
  ///
  /// \param[in] in Element of this Lie group.
  /// \param[out] tangent Corresponding element of the tangent space.
  void logMap(
      const StateSpace::State* in, Eigen::VectorXd& tangent) const override;

  /// Print the angle represented by the state.
  void print(const StateSpace::State* state, std::ostream& os) const override;
};

class SO2::State final : public StateSpace::State
{
public:
  /// Constructs a state from a rotation angle.
  ///
  /// \param[in] angle Rotation angle in (-inf, inf).
  explicit State(double angle = 0.0);

  ~State() = default;

  /// Returns state as a rotation angle in (-pi, pi].
  double toAngle() const;

  /// Sets state from a rotation angle.
  ///
  /// \param[in] angle Rotation angle in (-inf, inf).
  void fromAngle(double angle);

  /// Returns state as an Eigen rotation.
  Eigen::Rotation2Dd toRotation() const;

  /// Sets state from an Eigen rotation.
  ///
  /// \param[in] rotation Eigen rotation.
  void fromRotation(const Eigen::Rotation2Dd& rotation);

private:
  /// Angle bounded in (-pi, pi] representing the SO(2) state.
  double mAngle;

  friend class SO2;
};

} // namespace statespace
} // namespace aikido

#include "detail/SO2-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_SO2STATESPACE_HPP_


////////////////////////////////////////////////////////////////////////////////
// grid.cc
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Create grid meshes, optionally tesselated into tetrahedra or triangles.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  06/20/2015 18:23:24
////////////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <iterator>
#include <string>
#include <vector>
#include <MeshFEM/filters/gen_grid.hh>
#include <MeshFEM/filters/voxels_to_simplices.hh>
#include <MeshFEM/MSHFieldWriter.hh>

#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>

namespace po = boost::program_options;
using namespace std;

[[ noreturn ]] void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: grid CxR[xS] out.msh [options]" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[]) {
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("gridSize",  po::value<string>(), "grid size")
        ("outFile",  po::value<string>(), "output mesh file")
        ;

    po::positional_options_description p;
    p.add("gridSize",   1);
    p.add("outFile",  1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("tesselate,t", "tesselate into tetrahedra or triangles")
        ("minCorner,m", po::value<string>(), "minCorner of the grid bounding box (defaults to 0,0,0)")
        ("maxCorner,M", po::value<string>(), "maxCorner of the grid bounding box (defaults to sx,sy,sz)")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    if (vm.count("help"))
        usage(0, visible_opts);

    if ((vm.count("gridSize") == 0) || (vm.count("outFile") == 0)) {
        cout << "Must specify grid size and output path";
        usage(1, visible_opts);
    }

    if (vm.count("minCorner") != vm.count("maxCorner")) {
        cout << "Must specify full bounding box" << endl;
        usage(1, visible_opts);
    }

    return vm;
}

Point3D parseVector(size_t expectedSize, const string &cstring) {
    auto parseError = runtime_error("Invalid minCorner (must be comma-separated components)");
    vector<string> sizeStrings;
    boost::split(sizeStrings, cstring, boost::is_any_of(","));
    assert(expectedSize <= 3);
    if (sizeStrings.size() != expectedSize) throw parseError;
    Point3D result(Point3D::Zero());
    try {
        for (size_t i = 0; i < expectedSize; ++i)
            result[i] = stof(sizeStrings[i]);
    }
    catch(...) { throw parseError; }
    return result;
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  gridVertices, simplexVertices;
    vector<MeshIO::IOElement> gridElements, simplices;

    vector<string> sizeStrings;
    boost::split(sizeStrings, args["gridSize"].as<string>(),
                 boost::is_any_of("x"));
    vector<size_t> sizes;
    transform(sizeStrings.begin(), sizeStrings.end(), back_inserter(sizes),
            [](const std::string &s) { return std::stoul(s); });

    gen_grid(sizes, gridVertices, gridElements);

    // Transform grid vertices to the requested
    if (args.count("minCorner")) {
        Point3D minCorner = parseVector(sizes.size(), args["minCorner"].as<string>());
        Point3D maxCorner = parseVector(sizes.size(), args["maxCorner"].as<string>());
        // Current grid is [0, sx], ...
        Point3D scale = maxCorner - minCorner;
        for (size_t i = 0; i < sizes.size(); ++i) scale[i] /= sizes[i];
        for (auto &v : gridVertices) v.point = (scale.array() * v.point.array()).matrix() + minCorner;
    }

    string outPath = args["outFile"].as<string>();
    if (args.count("tesselate")) {
        vector<size_t> cellIdx;
        voxels_to_simplices(gridVertices, gridElements, simplexVertices, simplices,
                            cellIdx);
        MSHFieldWriter writer(outPath, simplexVertices, simplices);
        cout << "Writing mesh file..." << endl; // warning that we always write .msh regardless of filename

        ScalarField<double> cell_index(cellIdx.size());
        for (size_t i = 0; i < cellIdx.size(); ++i) cell_index[i] = cellIdx[i];
        writer.addField("cell_index", cell_index, DomainType::PER_ELEMENT);
    }
    else {
        // MeshIO will incorrectly guess the output is a tet mesh in the 2D case...
        MeshIO::MeshType type = (sizes.size() == 2) ? MeshIO::MESH_QUAD : MeshIO::MESH_HEX;
        MeshIO::save(outPath, gridVertices, gridElements, MeshIO::FMT_GUESS, type);
    }

    return 0;
}


/*
Pharmit
Copyright (c) David Ryan Koes, University of Pittsburgh and contributors.
All rights reserved.

Pharmit is licensed under both the BSD 3-clause license and the GNU
Public License version 2. Any use of the code that retains its reliance
on the GPL-licensed OpenBabel library is subject to the terms of the GPL2.

Use of the Pharmit code independently of OpenBabel (or any other
GPL2 licensed software) may choose between the BSD or GPL licenses.

See the LICENSE file provided with the distribution for more information.

*/
/*
 * KSamplePartitioner.cpp
 *
 *  Created on: Oct 17, 2011
 *      Author: dkoes
 */

#include "KSamplePartitioner.h"
#include "ShapeDistance.h"

#include <boost/multi_array.hpp>

#include <ext/algorithm>
using namespace boost;

//when the total number of items (n) divided by kcenters is smaller than the stop size,
//reduce kcenter to something that will create fuller partitions
unsigned KSamplePartitioner::fitKCenterToSize(unsigned  n) const
{
  if(n/kcenters < stopPartitionSize)
  {
    unsigned newKC = (n-1)/stopPartitionSize+1;
    //should never have n < stop, but always divide to be on the safe size
    if(newKC <= 1) newKC = 2;
    return newKC;
  }
  else
    return kcenters;
}

//create an instances of ksamplepartitioner with all of this's settings and the specified data
//initialize to fully represent the data
TopDownPartitioner* KSamplePartitioner::create(const DataViewer* dv) const
{
	TopDownPartitioner* ret = new KSamplePartitioner(dv, fitKCenterToSize(dv->size()), ksamples, centerFind, stopPartitionSize);
	ret->initFromData();
	return ret;
}

//initialize using a slice of the data from ind
TopDownPartitioner* KSamplePartitioner::create(const DataViewer* dv, vector<unsigned>& ind) const
{
	KSamplePartitioner* ret = new KSamplePartitioner(dv, fitKCenterToSize(ind.size()), ksamples, centerFind, stopPartitionSize);
	swap(ret->indices, ind);
	return ret;
}

//find the SINGLE value that is most central in cluster
//this is assumed to be a small partition since we calculate all n^2 distances
void KSamplePartitioner::getCenter(const vector<unsigned>& cluster, const MappableOctTree *& MIV, const MappableOctTree *& MSV) const
{
	unsigned N = cluster.size();
	boost::multi_array<float, 2> distances(boost::extents[N][N]);

	//compute distances
	for (unsigned i = 0; i < N; i++)
	{
		distances[i][i] = 0;
		const MappableOctTree *imiv = data->getMIV(cluster[i]);
		const MappableOctTree *imsv = data->getMSV(cluster[i]);
		for (unsigned j = 0; j < i; j++)
		{
			const MappableOctTree *jmiv = data->getMIV(cluster[j]);
			const MappableOctTree *jmsv = data->getMSV(cluster[j]);
			distances[i][j] = distances[j][i] = shapeDistance(imiv, imsv, jmiv,
						jmsv);
		}
	}

	//what is most central?  the row with the lowest average?
	//or the minimum maximum value?
	float bestave = HUGE_VAL;
	unsigned bestavei = 0;
	float minmaxval = HUGE_VAL;
	unsigned minmaxi = 0;
	for (unsigned i = 0; i < N; i++)
	{
		float ave = 0;
		float max = 0;
		for(unsigned j = 0; j < N; j++)
		{
			ave += distances[i][j];
			if(distances[i][j] > max)
				max = distances[i][j];
		}
		ave /= N;

		if(ave < bestave)
		{
			bestave = ave;
			bestavei = i;
		}
		if(max < minmaxval)
		{
			minmaxval = max;
			minmaxi = i;
		}
	}

	unsigned best;
	if(centerFind == AveCenter)
		best = bestavei;
	else
		best = minmaxi;

	MIV = data->getMIV(cluster[best]);
	MSV = data->getMSV(cluster[best]);
}

void KSamplePartitioner::partition(vector<TopDownPartitioner*>& parts)
{
	//grab k*mult samples
	unsigned nsamples = kcenters*ksamples;
	assert(nsamples > 0);
	nsamples = std::min(nsamples, (unsigned)indices.size());
	//random sample, unfortunately linear in indices size
	vector<unsigned> sampleIndices(nsamples, 0);
	srand(1); //provide determinism, as long as not multi-threaded
	random_sample_n(indices.begin(), indices.end(), sampleIndices.begin(), nsamples);

	//cluster samples
	vector< vector<unsigned> > clusters;
	kCluster(sampleIndices, clusters);

	//compute cluster "centers": MIV/MSV
	unsigned numclusters = clusters.size();

	vector<const MappableOctTree*> MSVcenters(numclusters, NULL);
	vector<const MappableOctTree*> MIVcenters(numclusters, NULL);
	for(unsigned i = 0, n = clusters.size(); i < n; i++)
	{
		getCenter(clusters[i], MIVcenters[i], MSVcenters[i]);
	}

	vector< vector<unsigned> > partitions;
	partitions.resize(numclusters);
	//now add each data iterm to the cluster whose center it is closest to
	for (unsigned i = 0, n = indices.size(); i < n; i++)
	{
		unsigned index = indices[i];
		//TODO: use triangle inequality to make this more efficient
		float mindist = HUGE_VAL;
		unsigned best = 0;
		for(unsigned j = 0; j < numclusters; j++)
		{
			float d = shapeDistance(MIVcenters[j],MSVcenters[j], data->getMIV(index), data->getMSV(index));
			if(d < mindist)
			{
				mindist = d;
				best = j;
			}
		}
		partitions[best].push_back(index);
	}

	//if the centers are identical (boo) then we may only have a single cluster
	unsigned numPart = 0;
	for(unsigned i = 0, n = partitions.size(); i < n; i++)
	{
		if(partitions[i].size() > 0)
			numPart++;
	}

	if(numPart == 1)
	{
		//this is irritating, but if we encounter a group of identical shapes,
		//must split to avoid infinite recursion
		partitions.clear();
		partitions.resize(2);
		for (unsigned i = 0, n = indices.size(); i < n; i++)
		{
			unsigned index = indices[i];
			if(i % 2)
			{
				partitions[0].push_back(index);
			}
			else
			{
				partitions[1].push_back(index);
			}
		}
		numPart = 2;
	}
	else if (numPart > 2)
	{
		for (unsigned i = 0, n = partitions.size(); i < n; i++)
		{
			if (partitions[i].size() == 1)
			{
				unsigned index = partitions[i].front();
				float mindist = HUGE_VAL;
				unsigned best = 0;
				for (unsigned j = 0; j < numclusters; j++)
				{
					if (j == i || partitions[j].size() == 0)
						continue;
					float d = shapeDistance(MIVcenters[j],MSVcenters[j], data->getMIV(index), data->getMSV(index));
					if(d < mindist)
					{
						mindist = d;
						best = j;
					}
				}

				partitions[best].push_back(index);
				partitions[i].clear();
				numPart--;

				if(numPart <= 2)
					break;
			}
		}
	}

	//create new partitions from indices
	for (unsigned i = 0, n = partitions.size(); i < n; i++)
	{
		if(partitions[i].size() > 0)
		{
			parts.push_back(create(data, partitions[i]));
		}
	}

}

//use aglomerative clusters (complete linkage) to create k clusters of variable size
void KSamplePartitioner::kCluster(const vector<unsigned>& indices, vector< vector<unsigned> >& clusters)
{
	//compute pairwise distances between all members
	unsigned N = indices.size();
	boost::multi_array<float, 2> distances(boost::extents[N][N]);
	//compute distances indicesed by position in indices
	clusters.clear();
	clusters.reserve(1 + N);
	for (unsigned i = 0; i < N; i++)
	{
		distances[i][i] = 0;
		const MappableOctTree *imiv = data->getMIV(indices[i]);
		const MappableOctTree *imsv = data->getMSV(indices[i]);
		for (unsigned j = 0; j < i; j++)
		{
			const MappableOctTree *jmiv = data->getMIV(indices[j]);
			const MappableOctTree *jmsv = data->getMSV(indices[j]);
			distances[i][j] = distances[j][i] = shapeDistance(imiv, imsv, jmiv, jmsv);
		}
		clusters.push_back(vector<unsigned>());
		clusters.back().push_back(i);
	}

	//iteratively merge all clusters until we run into size limit
	while (clusters.size() > kcenters)
	{
		//find two clusters with smallest complete linkage distance
		//(minimize the maximum distance)
		float mindist = HUGE_VAL;
		unsigned besti = 0;
		unsigned bestj = 0;
		for (unsigned i = 0, n = clusters.size(); i < n; i++)
		{
			for (unsigned j = 0; j < i; j++)
			{
				float maxdist = 0;
				//find the max distance between i and j
				for (unsigned I = 0, ni = clusters[i].size(); I < ni; I++)
				{
					for (unsigned J = 0, nj = clusters[j].size(); J < nj; J++)
					{
						float d = distances[clusters[i][I]][clusters[j][J]];
						if (d > maxdist)
						{
							maxdist = d;
						}
					}
				}
				if (maxdist < mindist)
				{
					mindist = maxdist;
					besti = i;
					bestj = j;
				}
			}
		}

		if (besti == bestj)
			break; //couldn't merge any

		//move besti and bestj to end of vector
		unsigned last = clusters.size() - 1;
		swap(clusters[besti], clusters[last]);
		swap(clusters[bestj], clusters[last - 1]);

		//insert into second to last
		clusters[last - 1].insert(clusters[last - 1].end(), clusters[last].begin(),
				clusters[last].end());
		clusters.pop_back(); //remove
	}

	//clusters is now populated with indicies into indices
	//replace with actual indices
	for(unsigned i = 0, n = clusters.size(); i < n; i++)
	{
		for(unsigned j = 0, m = clusters[i].size(); j < m; j++)
		{
			clusters[i][j] = indices[clusters[i][j]];
		}
	}

}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_NO_STATIC_ASSERT
#include "product.h"
#include <Eigen/LU>

// regression test for bug 447
template<int>
void product1x1()
{
  Matrix<float,1,3> matAstatic;
  Matrix<float,3,1> matBstatic;
  matAstatic.setRandom();
  matBstatic.setRandom();
  VERIFY_IS_APPROX( (matAstatic * matBstatic).coeff(0,0),
                    matAstatic.cwiseProduct(matBstatic.transpose()).sum() );

  MatrixXf matAdynamic(1,3);
  MatrixXf matBdynamic(3,1);
  matAdynamic.setRandom();
  matBdynamic.setRandom();
  VERIFY_IS_APPROX( (matAdynamic * matBdynamic).coeff(0,0),
                    matAdynamic.cwiseProduct(matBdynamic.transpose()).sum() );
}

template<typename TC, typename TA, typename TB>
const TC& ref_prod(TC &C, const TA &A, const TB &B)
{
  for(Index i=0;i<C.rows();++i)
    for(Index j=0;j<C.cols();++j)
      for(Index k=0;k<A.cols();++k)
        C.coeffRef(i,j) += A.coeff(i,k) * B.coeff(k,j);
  return C;
}

template<typename T, int Rows, int Cols, int Depth, int OC, int OA, int OB>
typename internal::enable_if<! ( (Rows ==1&&Depth!=1&&OA==ColMajor)
                              || (Depth==1&&Rows !=1&&OA==RowMajor)
                              || (Cols ==1&&Depth!=1&&OB==RowMajor)
                              || (Depth==1&&Cols !=1&&OB==ColMajor)
                              || (Rows ==1&&Cols !=1&&OC==ColMajor)
                              || (Cols ==1&&Rows !=1&&OC==RowMajor)),void>::type
test_lazy_single(int rows, int cols, int depth)
{
  Matrix<T,Rows,Depth,OA> A(rows,depth); A.setRandom();
  Matrix<T,Depth,Cols,OB> B(depth,cols); B.setRandom();
  Matrix<T,Rows,Cols,OC>  C(rows,cols);  C.setRandom();
  Matrix<T,Rows,Cols,OC>  D(C);
  VERIFY_IS_APPROX(C+=A.lazyProduct(B), ref_prod(D,A,B));
}

template<typename T, int Rows, int Cols, int Depth, int OC, int OA, int OB>
typename internal::enable_if<  ( (Rows ==1&&Depth!=1&&OA==ColMajor)
                              || (Depth==1&&Rows !=1&&OA==RowMajor)
                              || (Cols ==1&&Depth!=1&&OB==RowMajor)
                              || (Depth==1&&Cols !=1&&OB==ColMajor)
                              || (Rows ==1&&Cols !=1&&OC==ColMajor)
                              || (Cols ==1&&Rows !=1&&OC==RowMajor)),void>::type
test_lazy_single(int, int, int)
{
}

template<typename T, int Rows, int Cols, int Depth>
void test_lazy_all_layout(int rows=Rows, int cols=Cols, int depth=Depth)
{
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,ColMajor,ColMajor,ColMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,RowMajor,ColMajor,ColMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,ColMajor,RowMajor,ColMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,RowMajor,RowMajor,ColMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,ColMajor,ColMajor,RowMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,RowMajor,ColMajor,RowMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,ColMajor,RowMajor,RowMajor>(rows,cols,depth) ));
  CALL_SUBTEST(( test_lazy_single<T,Rows,Cols,Depth,RowMajor,RowMajor,RowMajor>(rows,cols,depth) ));
}

template<typename T>
void test_lazy_l1()
{
  int rows = internal::random<int>(1,12);
  int cols = internal::random<int>(1,12);
  int depth = internal::random<int>(1,12);

  // Inner
  CALL_SUBTEST(( test_lazy_all_layout<T,1,1,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,1,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,1,3>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,1,8>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,1,9>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,1,-1>(1,1,depth) ));

  // Outer
  CALL_SUBTEST(( test_lazy_all_layout<T,2,1,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,2,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,2,2,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,3,3,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,4,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,8,1>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,-1,1>(4,cols) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,7,-1,1>(7,cols) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,8,1>(rows) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,3,1>(rows) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,-1,1>(rows,cols) ));
}

template<typename T>
void test_lazy_l2()
{
  int rows = internal::random<int>(1,12);
  int cols = internal::random<int>(1,12);
  int depth = internal::random<int>(1,12);

  // mat-vec
  CALL_SUBTEST(( test_lazy_all_layout<T,2,1,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,2,1,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,1,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,1,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,5,1,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,1,5>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,1,6>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,6,1,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,8,1,8>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,1,4>(rows) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,1,-1>(4,1,depth) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,1,-1>(rows,1,depth) ));

  // vec-mat
  CALL_SUBTEST(( test_lazy_all_layout<T,1,2,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,2,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,4,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,4,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,5,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,4,5>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,4,6>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,6,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,8,8>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,-1, 4>(1,cols) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1, 4,-1>(1,4,depth) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,1,-1,-1>(1,cols,depth) ));
}

template<typename T>
void test_lazy_l3()
{
  int rows = internal::random<int>(1,12);
  int cols = internal::random<int>(1,12);
  int depth = internal::random<int>(1,12);
  // mat-mat
  CALL_SUBTEST(( test_lazy_all_layout<T,2,4,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,2,6,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,3,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,8,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,5,6,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,2,5>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,7,6>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,6,8,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,8,3,8>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,6,4>(rows) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,3,-1>(4,3,depth) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,-1,6,-1>(rows,6,depth) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,8,2,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,5,2,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,4,2>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,8,4,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,6,5,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,4,5>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,3,4,6>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,2,6,4>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,7,8,8>() ));
  CALL_SUBTEST(( test_lazy_all_layout<T,8,-1, 4>(8,cols) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,3, 4,-1>(3,4,depth) ));
  CALL_SUBTEST(( test_lazy_all_layout<T,4,-1,-1>(4,cols,depth) ));
}

template<typename T,int N,int M,int K>
void test_linear_but_not_vectorizable()
{
  // Check tricky cases for which the result of the product is a vector and thus must exhibit the LinearBit flag,
  // but is not vectorizable along the linear dimension.
  Index n = N==Dynamic ? internal::random<Index>(1,32) : N;
  Index m = M==Dynamic ? internal::random<Index>(1,32) : M;
  Index k = K==Dynamic ? internal::random<Index>(1,32) : K;

  {
    Matrix<T,N,M+1> A; A.setRandom(n,m+1);
    Matrix<T,M*2,K> B; B.setRandom(m*2,k);
    Matrix<T,1,K> C;
    Matrix<T,1,K> R;

    C.noalias() = A.template topLeftCorner<1,M>() * (B.template topRows<M>()+B.template bottomRows<M>());
    R.noalias() = A.template topLeftCorner<1,M>() * (B.template topRows<M>()+B.template bottomRows<M>()).eval();
    VERIFY_IS_APPROX(C,R);
  }

  {
    Matrix<T,M+1,N,RowMajor> A; A.setRandom(m+1,n);
    Matrix<T,K,M*2,RowMajor> B; B.setRandom(k,m*2);
    Matrix<T,K,1> C;
    Matrix<T,K,1> R;

    C.noalias() = (B.template leftCols<M>()+B.template rightCols<M>())        * A.template topLeftCorner<M,1>();
    R.noalias() = (B.template leftCols<M>()+B.template rightCols<M>()).eval() * A.template topLeftCorner<M,1>();
    VERIFY_IS_APPROX(C,R);
  }
}

template<int Rows>
void bug_1311()
{
  Matrix< double, Rows, 2 > A;  A.setRandom();
  Vector2d b = Vector2d::Random() ;
  Matrix<double,Rows,1> res;
  res.noalias() = 1. * (A * b);
  VERIFY_IS_APPROX(res, A*b);
  res.noalias() = 1.*A * b;
  VERIFY_IS_APPROX(res, A*b);
  res.noalias() = (1.*A).lazyProduct(b);
  VERIFY_IS_APPROX(res, A*b);
  res.noalias() = (1.*A).lazyProduct(1.*b);
  VERIFY_IS_APPROX(res, A*b);
  res.noalias() = (A).lazyProduct(1.*b);
  VERIFY_IS_APPROX(res, A*b);
}

void test_product_small()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( product(Matrix<float, 3, 2>()) );
    CALL_SUBTEST_2( product(Matrix<int, 3, 17>()) );
    CALL_SUBTEST_8( product(Matrix<double, 3, 17>()) );
    CALL_SUBTEST_3( product(Matrix3d()) );
    CALL_SUBTEST_4( product(Matrix4d()) );
    CALL_SUBTEST_5( product(Matrix4f()) );
    CALL_SUBTEST_6( product1x1<0>() );

    CALL_SUBTEST_11( test_lazy_l1<float>() );
    CALL_SUBTEST_12( test_lazy_l2<float>() );
    CALL_SUBTEST_13( test_lazy_l3<float>() );

    CALL_SUBTEST_21( test_lazy_l1<double>() );
    CALL_SUBTEST_22( test_lazy_l2<double>() );
    CALL_SUBTEST_23( test_lazy_l3<double>() );

    CALL_SUBTEST_31( test_lazy_l1<std::complex<float> >() );
    CALL_SUBTEST_32( test_lazy_l2<std::complex<float> >() );
    CALL_SUBTEST_33( test_lazy_l3<std::complex<float> >() );

    CALL_SUBTEST_41( test_lazy_l1<std::complex<double> >() );
    CALL_SUBTEST_42( test_lazy_l2<std::complex<double> >() );
    CALL_SUBTEST_43( test_lazy_l3<std::complex<double> >() );

    CALL_SUBTEST_7(( test_linear_but_not_vectorizable<float,2,1,Dynamic>() ));
    CALL_SUBTEST_7(( test_linear_but_not_vectorizable<float,3,1,Dynamic>() ));
    CALL_SUBTEST_7(( test_linear_but_not_vectorizable<float,2,1,16>() ));

    CALL_SUBTEST_6( bug_1311<3>() );
    CALL_SUBTEST_6( bug_1311<5>() );
  }

#ifdef EIGEN_TEST_PART_6
  {
    // test compilation of (outer_product) * vector
    Vector3f v = Vector3f::Random();
    VERIFY_IS_APPROX( (v * v.transpose()) * v, (v * v.transpose()).eval() * v);
  }

  {
    // regression test for pull-request #93
    Eigen::Matrix<double, 1, 1> A;  A.setRandom();
    Eigen::Matrix<double, 18, 1> B; B.setRandom();
    Eigen::Matrix<double, 1, 18> C; C.setRandom();
    VERIFY_IS_APPROX(B * A.inverse(), B * A.inverse()[0]);
    VERIFY_IS_APPROX(A.inverse() * C, A.inverse()[0] * C);
  }

  {
    Eigen::Matrix<double, 10, 10> A, B, C;
    A.setRandom();
    C = A;
    for(int k=0; k<79; ++k)
      C = C * A;
    B.noalias() = (((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)) * ((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)))
                * (((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)) * ((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A))*((A*A)*(A*A)));
    VERIFY_IS_APPROX(B,C);
  }
#endif
}


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/test/minimal.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <iterator>
#include <iostream>
#include <algorithm>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/random.hpp>
#include <boost/random/linear_congruential.hpp>
#include <fstream>

using namespace boost;

struct EdgeProperty
{
  std::size_t component;
};

static bool any_errors = false;

template<typename Graph, typename Vertex>
void
check_articulation_points(const Graph& g, std::vector<Vertex> art_points)
{
  std::vector<int> components(num_vertices(g));
  int basic_comps =
    connected_components(g,
                         make_iterator_property_map(components.begin(),
                                                    get(vertex_index, g),
                                                    int()));

  std::vector<Vertex> art_points_check;

  typename graph_traits<Graph>::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    Graph g_copy(g);
    Vertex victim = vertex(get(vertex_index, g, *vi), g_copy);
    clear_vertex(victim, g_copy);
    remove_vertex(victim, g_copy);

    int copy_comps =
      connected_components
        (g_copy,
         make_iterator_property_map(components.begin(),
                                    get(vertex_index, g_copy),
                                    int()));

    if (copy_comps > basic_comps)
      art_points_check.push_back(*vi);
  }

  std::sort(art_points.begin(), art_points.end());
  std::sort(art_points_check.begin(), art_points_check.end());

  BOOST_CHECK(art_points == art_points_check);
  if (art_points != art_points_check) {
    std::cerr << "ERROR!" << std::endl;
    std::cerr << "\tComputed: ";
    std::size_t i;
    for (i = 0; i < art_points.size(); ++i)
      std::cout << art_points[i] << ' ';
    std::cout << std::endl << "\tExpected: ";
    for (i = 0; i < art_points_check.size(); ++i)
      std::cout << art_points_check[i] << ' ';
    std::cout << std::endl;
    any_errors = true;
  } else std::cout << "OK." << std::endl;
}

typedef adjacency_list<listS, vecS, undirectedS,
                       no_property, EdgeProperty> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;

bool test_graph(Graph& g) { // Returns false on failure
  std::vector<Vertex> art_points;

  std::cout << "Computing biconnected components & articulation points... ";
  std::cout.flush();

  std::size_t num_comps =
    biconnected_components(g,
                           get(&EdgeProperty::component, g),
                           std::back_inserter(art_points)).first;

  std::cout << "done.\n\t" << num_comps << " biconnected components.\n"
            << "\t" << art_points.size() << " articulation points.\n"
            << "\tTesting articulation points...";
  std::cout.flush();

  check_articulation_points(g, art_points);

  if (any_errors) {
    std::ofstream out("biconnected_components_test_failed.dot");

    out << "graph A {\n" << "  node[shape=\"circle\"]\n";

    for (std::size_t i = 0; i < art_points.size(); ++i) {
      out << art_points[i] << " [ style=\"filled\" ];" << std::endl;
    }

    graph_traits<Graph>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
      out << source(*ei, g) << " -- " << target(*ei, g)
          << "[label=\"" << g[*ei].component << "\"]\n";
    out << "}\n";
  }

  return any_errors;
}

int test_main(int argc, char* argv[])
{
  std::size_t n = 100;
  std::size_t m = 500;
  std::size_t seed = 1;

  if (argc > 1) n = lexical_cast<std::size_t>(argv[1]);
  if (argc > 2) m = lexical_cast<std::size_t>(argv[2]);
  if (argc > 3) seed = lexical_cast<std::size_t>(argv[3]);

  {
    Graph g(n);
    minstd_rand gen(seed);
    generate_random_graph(g, n, m, gen);
    if (test_graph(g)) return 1;
  }

  {
    Graph g(4);
    add_edge(2, 3, g);
    add_edge(0, 3, g);
    add_edge(0, 2, g);
    add_edge(1, 0, g);
    if (test_graph(g)) return 1;
  }

  return 0;
}


//  Copyright (c) 2018-2019 Cem Bassoy
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <iostream>
#include <algorithm>
#include <vector>
#include <boost/numeric/ublas/tensor/algorithms.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>
#include <boost/numeric/ublas/tensor/strides.hpp>
#include "utility.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE ( test_tensor_algorithms,
                        * boost::unit_test::depends_on("test_extents")
                        * boost::unit_test::depends_on("test_strides"))

using test_types  = zip<int,long,float,double,std::complex<float>>::with_t<boost::numeric::ublas::first_order, boost::numeric::ublas::last_order>;
using test_types2 = std::tuple<int,long,float,double,std::complex<float>>;

struct fixture
{
	using extents_type = boost::numeric::ublas::shape;
	fixture()
	  : extents {
	      extents_type{1,1}, // 1
	      extents_type{1,2}, // 2
	      extents_type{2,1}, // 3
	      extents_type{2,3}, // 4
	      extents_type{2,3,1}, // 5
	      extents_type{4,1,3}, // 6
	      extents_type{1,2,3}, // 7
	      extents_type{4,2,3}, // 8
	      extents_type{4,2,3,5} } // 9
	{
	}
	std::vector<extents_type> extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_copy, value,  test_types2, fixture )
{
	using namespace boost::numeric;
	using value_type   = value;
	using vector_type  = std::vector<value_type>;

	for(auto const& n : extents) {

		auto a  = vector_type(n.product());
		auto b  = vector_type(n.product());
		auto c  = vector_type(n.product());

		auto wa = ublas::strides<ublas::first_order>(n);
		auto wb = ublas::strides<ublas::last_order> (n);
		auto wc = ublas::strides<ublas::first_order>(n);

		auto v = value_type{};
		for(auto i = 0ul; i < a.size(); ++i, v+=1){
			a[i]=v;
		}

		ublas::copy( n.size(), n.data(), b.data(), wb.data(), a.data(), wa.data() );
		ublas::copy( n.size(), n.data(), c.data(), wc.data(), b.data(), wb.data() );

		for(auto i = 1ul; i < c.size(); ++i)
			BOOST_CHECK_EQUAL( c[i], a[i] );

		using size_type = typename ublas::strides<ublas::first_order>::value_type;
		size_type const*const p0 = nullptr;
		BOOST_CHECK_THROW( ublas::copy( n.size(), p0, c.data(), wc.data(), b.data(), wb.data() ), std::length_error );
		BOOST_CHECK_THROW( ublas::copy( n.size(), n.data(), c.data(), p0, b.data(), wb.data() ), std::length_error );
		BOOST_CHECK_THROW( ublas::copy( n.size(), n.data(), c.data(), wc.data(), b.data(), p0 ), std::length_error );

		value_type* c0 = nullptr;
		BOOST_CHECK_THROW( ublas::copy( n.size(), n.data(), c0, wc.data(), b.data(), wb.data() ), std::length_error );
	}

	// special case rank == 0
	{
		auto n = ublas::shape{};

		auto a  = vector_type(n.product());
		auto b  = vector_type(n.product());
		auto c  = vector_type(n.product());

		auto wa = ublas::strides<ublas::first_order>(n);
		auto wb = ublas::strides<ublas::last_order> (n);
		auto wc = ublas::strides<ublas::first_order>(n);

		ublas::copy( n.size(), n.data(), b.data(), wb.data(), a.data(), wa.data() );
		ublas::copy( n.size(), n.data(), c.data(), wc.data(), b.data(), wb.data() );

		BOOST_CHECK_NO_THROW( ublas::copy( n.size(), n.data(), c.data(), wc.data(), b.data(), wb.data() ) );

	}

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_transform, value,  test_types2, fixture )
{
	using namespace boost::numeric;
	using value_type   = value;
	using vector_type  = std::vector<value_type>;

	for(auto const& n : extents) {

		auto a  = vector_type(n.product());
		auto b  = vector_type(n.product());
		auto c  = vector_type(n.product());

		auto wa = ublas::strides<ublas::first_order>(n);
		auto wb = ublas::strides<ublas::last_order> (n);
		auto wc = ublas::strides<ublas::first_order>(n);

		auto v = value_type{};
		for(auto i = 0ul; i < a.size(); ++i, v+=1){
			a[i]=v;
		}

		ublas::transform( n.size(), n.data(), b.data(), wb.data(), a.data(), wa.data(), [](value_type const& a){ return a + value_type(1);} );
		ublas::transform( n.size(), n.data(), c.data(), wc.data(), b.data(), wb.data(), [](value_type const& a){ return a - value_type(1);} );

		for(auto i = 1ul; i < c.size(); ++i)
			BOOST_CHECK_EQUAL( c[i], a[i] );

	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_accumulate, value,  test_types2, fixture )
{
	using namespace boost::numeric;
	using value_type   = value;
	using vector_type  = std::vector<value_type>;

	for(auto const& n : extents) {

		auto const s = n.product();

		auto a  = vector_type(n.product());
		//		auto b  = vector_type(n.product());
		//		auto c  = vector_type(n.product());

		auto wa = ublas::strides<ublas::first_order>(n);
		//		auto wb = ublas::strides<ublas::last_order> (n);
		//		auto wc = ublas::strides<ublas::first_order>(n);

		auto v = value_type{};
		for(auto i = 0ul; i < a.size(); ++i, v+=value_type(1)){
			a[i]=v;
		}

		auto acc = ublas::accumulate( n.size(), n.data(), a.data(), wa.data(), v);

		BOOST_CHECK_EQUAL( acc, value_type( s*(s+1) / 2 )  );

		auto acc2 = ublas::accumulate( n.size(), n.data(), a.data(), wa.data(), v,
		                               [](auto const& l, auto const& r){return l + r; });

		BOOST_CHECK_EQUAL( acc2, value_type( s*(s+1) / 2 )  );

	}
}

template<class V>
void init(std::vector<V>& a)
{
	auto v = V(1);
	for(auto i = 0u; i < a.size(); ++i, ++v){
		a[i] = v;
	}
}

template<class V>
void init(std::vector<std::complex<V>>& a)
{
	auto v = std::complex<V>(1,1);
	for(auto i = 0u; i < a.size(); ++i){
		a[i] = v;
		v.real(v.real()+1);
		v.imag(v.imag()+1);
	}
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_algorithms_trans, value,  test_types, fixture )
{
	using namespace boost::numeric;
	using value_type  = typename value::first_type;
	using layout_type = typename value::second_type;
	using vector_type  = std::vector<value_type>;
	using strides_type = ublas::strides<layout_type>;
	using extents_type = ublas::shape;
	using size_type = typename extents_type::value_type;
	using permutation_type = std::vector<size_type>;

	for(auto const& n : extents) {

		auto p   = n.size();
		auto s   = n.product();

		auto pi  = permutation_type(p);
		auto a   = vector_type(s);
		auto b1  = vector_type(s);
		auto b2  = vector_type(s);
		auto c1  = vector_type(s);
		auto c2  = vector_type(s);

		auto wa = strides_type(n);

		init(a);

		// so wie last-order.
		for(auto i = size_type(0), j = p; i < n.size(); ++i, --j)
			pi[i] = j;

		auto nc = typename extents_type::base_type (p);
		for(auto i = 0u; i < p; ++i)
			nc[pi[i]-1] = n[i];

		auto wc = strides_type(extents_type(nc));
		auto wc_pi = typename strides_type::base_type (p);
		for(auto i = 0u; i < p; ++i)
			wc_pi[pi[i]-1] = wc[i];

		ublas::copy ( p, n.data(),            c1.data(), wc_pi.data(), a.data(), wa.data());
		ublas::trans( p, n.data(), pi.data(), c2.data(), wc.data(),    a.data(), wa.data() );

		if(!std::is_compound_v<value_type>)
			for(auto i = 0ul; i < s; ++i)
				BOOST_CHECK_EQUAL( c1[i], c2[i] );

		auto nb = typename extents_type::base_type (p);
		for(auto i = 0u; i < p; ++i)
			nb[pi[i]-1] = nc[i];

		auto wb = strides_type (extents_type(nb));
		auto wb_pi = typename strides_type::base_type (p);
		for(auto i = 0u; i < p; ++i)
			wb_pi[pi[i]-1] = wb[i];

		ublas::copy ( p, nc.data(),            b1.data(), wb_pi.data(), c1.data(), wc.data());
		ublas::trans( p, nc.data(), pi.data(), b2.data(), wb.data(),    c2.data(), wc.data() );

		if(!std::is_compound_v<value_type>)
			for(auto i = 0ul; i < s; ++i)
				BOOST_CHECK_EQUAL( b1[i], b2[i] );

		for(auto i = 0ul; i < s; ++i)
			BOOST_CHECK_EQUAL( a[i], b2[i] );

	}
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef NETWORKS_HPP
#define NETWORKS_HPP

// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>

// Define Namespace
namespace nn = torch::nn;
namespace po = boost::program_options;

// Function Prototype
void weights_init(nn::Module &m);
void LinearLayer(nn::Sequential &sq, const size_t in_dim, const size_t out_dim, const bool ReLU);

// -------------------------------------------------
// struct{AutoEncoder1dImpl}(nn::Module)
// -------------------------------------------------
struct AutoEncoder1dImpl : nn::Module{
private:
    nn::Sequential encoder, decoder;
public:
    AutoEncoder1dImpl(){}
    AutoEncoder1dImpl(po::variables_map &vm);
    torch::Tensor forward(torch::Tensor x);
};

TORCH_MODULE(AutoEncoder1d);

#endif


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

#include <Utils/DataStructures/OccupiedMolecularOrbitals.h>
#include <Utils/Scf/LcaoUtils/HFWaveFunctionOverlap.h>
#include <Eigen/Dense>

namespace Scine {
namespace Utils {

namespace LcaoUtils {

double HFWaveFunctionOverlap::calculateOrthonormalOverlap(const OccupiedMolecularOrbitals& c1,
                                                          const OccupiedMolecularOrbitals& c2) {
  // Convert to uhf to make comparison of RHF - UHF possible
  // TODO: make more efficient if no conversion is needed
  auto unrestricted1 = c1.toUnrestricted();
  auto unrestricted2 = c2.toUnrestricted();
  return unrestrictedOrthonormalOverlap(unrestricted1, unrestricted2);
}

double HFWaveFunctionOverlap::calculateNonOrthonormalOverlap(const OccupiedMolecularOrbitals& c1,
                                                             const OccupiedMolecularOrbitals& c2, const Eigen::MatrixXd& s) {
  // Convert to uhf to make comparison of RHF - UHF possible
  // TODO: make more efficient if no conversion is needed
  auto unrestricted1 = c1.toUnrestricted();
  auto unrestricted2 = c2.toUnrestricted();
  return unrestrictedNonOrthonormalOverlap(unrestricted1, unrestricted2, s);
}

double HFWaveFunctionOverlap::unrestrictedOrthonormalOverlap(const OccupiedMolecularOrbitals& c1,
                                                             const OccupiedMolecularOrbitals& c2) {
  double f1 = orthonormalContribution(c1.alphaMatrix(), c2.alphaMatrix());
  double f2 = orthonormalContribution(c1.betaMatrix(), c2.betaMatrix());
  // Return a product of determinants:
  // The determinant of a block diagonal matrix is the product of the determinants of the block
  return f1 * f2;
}

double HFWaveFunctionOverlap::unrestrictedNonOrthonormalOverlap(const OccupiedMolecularOrbitals& c1,
                                                                const OccupiedMolecularOrbitals& c2,
                                                                const Eigen::MatrixXd& s) {
  double f1 = nonOrthonormalContribution(c1.alphaMatrix(), c2.alphaMatrix(), s);
  double f2 = nonOrthonormalContribution(c1.betaMatrix(), c2.betaMatrix(), s);
  // Return a product of determinants:
  // The determinant of a block diagonal matrix is the product of the determinants of the block
  return f1 * f2;
}

double HFWaveFunctionOverlap::orthonormalContribution(const Eigen::MatrixXd& m1, const Eigen::MatrixXd& m2) {
  if (m1.cols() != m2.cols()) {
    throw std::runtime_error("Not possible to calculate overlap between systems with different number of electrons.");
  }
  return (m1.transpose() * m2).determinant();
}

double HFWaveFunctionOverlap::nonOrthonormalContribution(const Eigen::MatrixXd& m1, const Eigen::MatrixXd& m2,
                                                         const Eigen::MatrixXd& s) {
  if (m1.cols() != m2.cols()) {
    throw std::runtime_error("Not possible to calculate overlap between systems with different number of electrons.");
  }
  return (m1.transpose() * s * m2).determinant();
}

} // namespace LcaoUtils
} // namespace Utils
} // namespace Scine


#include <iostream>
#include <time.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "pcl/point_cloud.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/features/fpfh.h"

using namespace pcl;

SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33>

align(PointCloud<PointXYZ>::Ptr cloud1, PointCloud<PointXYZ>::Ptr cloud2,
PointCloud<FPFHSignature33>::Ptr features1, PointCloud<FPFHSignature33>::Ptr features2,
int max_sacia_iterations, double min_correspondence_dist, double max_correspondence_dist) {

	SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
	Eigen::Matrix4f final_transformation;
	sac_ia.setInputCloud(cloud2);
	sac_ia.setSourceFeatures(features2);
	sac_ia.setInputTarget(cloud1);
	sac_ia.setTargetFeatures(features1);
	sac_ia.setMaximumIterations(max_sacia_iterations);
	sac_ia.setMinSampleDistance(min_correspondence_dist);
	sac_ia.setMaxCorrespondenceDistance(max_correspondence_dist);
	PointCloud<PointXYZ> finalcloud;
	sac_ia.align(finalcloud);
	sac_ia.getCorrespondenceRandomness();
	return sac_ia;
}


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using boost::multiprecision::cpp_int;

int main() {
	int n, count; cpp_int cppi;
	while (1) {
		cin >> n; if (n == 0) break;
		count = 0; cppi = 1;
		for (int i = 2; i <= n; i += 2) cppi *= i;
		for (int i = 5; i <= n; i += 10) cppi *= i;
		while (cppi % 10 == 0) { cppi /= 10; count++; }
		cout << count << endl;
	}
	return 0;
}