#include <functional>
#include <cmath>

#include <boost/math/tools/roots.hpp>

#include "PROPOSAL/Constants.h"
#include "PROPOSAL/decay/LeptonicDecayChannel.h"
#include "PROPOSAL/math/RandomGenerator.h"
#include "PROPOSAL/particle/Particle.h"

using namespace PROPOSAL;

/******************************************************************************
*                         LeptonicDecayChannelApprox                         *
******************************************************************************/

const std::string LeptonicDecayChannelApprox::name_ = "LeptonicDecayChannelApprox";

// ------------------------------------------------------------------------- //
LeptonicDecayChannelApprox::LeptonicDecayChannelApprox(const ParticleDef& lepton,
                                           const ParticleDef& neutrino,
                                           const ParticleDef& anti_neutrino)
    : DecayChannel()
    , massive_lepton_(lepton)
    , neutrino_(neutrino)
    , anti_neutrino_(anti_neutrino)
{
}

// ------------------------------------------------------------------------- //
LeptonicDecayChannelApprox::~LeptonicDecayChannelApprox() {}

// ------------------------------------------------------------------------- //
LeptonicDecayChannelApprox::LeptonicDecayChannelApprox(const LeptonicDecayChannelApprox& mode)
    : DecayChannel(mode)
    , massive_lepton_(mode.massive_lepton_)
    , neutrino_(mode.neutrino_)
    , anti_neutrino_(mode.anti_neutrino_)
{
}

// ------------------------------------------------------------------------- //
bool LeptonicDecayChannelApprox::compare(const DecayChannel& channel) const
{
    const LeptonicDecayChannelApprox* leptonic = dynamic_cast<const LeptonicDecayChannelApprox*>(&channel);

    if (!leptonic)
        return false;
    else if (massive_lepton_ != leptonic->massive_lepton_)
        return false;
    else if (neutrino_ != leptonic->neutrino_)
        return false;
    else if (anti_neutrino_ != leptonic->anti_neutrino_)
        return false;
    else
        return true;
}

// ------------------------------------------------------------------------- //
double LeptonicDecayChannelApprox::DecayRate(double x, double parent_mass, double E_max, double right_side)
{
    (void)parent_mass;
    (void)E_max;

    return x * x * x * (1. - 0.5 * x) - right_side;
}

// ------------------------------------------------------------------------- //
double LeptonicDecayChannelApprox::DifferentialDecayRate(double x, double parent_mass, double E_max)
{
    (void)parent_mass;
    (void)E_max;

    return (3 - 2 * x) * x * x;
}

// ------------------------------------------------------------------------- //
std::pair<double, double> LeptonicDecayChannelApprox::function_and_derivative(double x,
                                                                        double parent_mass,
                                                                        double E_max,
                                                                        double right_side)
{
    return std::make_pair(DecayRate(x, parent_mass, E_max, right_side), DifferentialDecayRate(x, parent_mass, E_max));
}

// ------------------------------------------------------------------------- //
double LeptonicDecayChannelApprox::FindRootBoost(double min, double parent_mass, double E_max, double right_side)
{
    double max        = 1;
    double x_start    = 0.5;
    int binary_digits = 6;
    // in older versions a max_step was set to 40, which were the max number of int steps
    // int max_steps = 40;

    return boost::math::tools::newton_raphson_iterate(
        std::bind(&LeptonicDecayChannelApprox::function_and_derivative, this, std::placeholders::_1, parent_mass, E_max, right_side),
        x_start,
        min,
        max,
        binary_digits);
}

// ------------------------------------------------------------------------- //
DecayChannel::DecayProducts LeptonicDecayChannelApprox::Decay(const Particle& particle)
{
    double parent_mass = particle.GetMass();

    DecayProducts products;
    products.push_back(new Particle(massive_lepton_));
    products.push_back(new Particle(neutrino_));
    products.push_back(new Particle(anti_neutrino_));

    // Sample energy from decay rate
    double emax       = (parent_mass * parent_mass + massive_lepton_.mass * massive_lepton_.mass) / (2 * parent_mass);
    double x_min      = massive_lepton_.mass / emax;
    // double f_min      = x_min * x_min * x_min * (1 - 0.5 * x_min);
    // double right_side = f_min + (0.5 - f_min) * RandomGenerator::Get().RandomDouble();

    double f_min      = DecayRate(x_min, parent_mass, emax, 0.0);
    double f_max      = DecayRate(1.0, parent_mass, emax, 0.0);
    double right_side = f_min + (f_max - f_min) * RandomGenerator::Get().RandomDouble();

    double find_root = FindRootBoost(x_min, parent_mass, emax, right_side);

    double lepton_energy   = std::max(find_root * emax, massive_lepton_.mass);
    double lepton_momentum = std::sqrt((lepton_energy - massive_lepton_.mass) * (lepton_energy + massive_lepton_.mass));

    // Sample directions For the massive letpon
    products[0]->SetDirection(GenerateRandomDirection());
    products[0]->SetMomentum(lepton_momentum);

    // Sample directions For the massless letpon
    double energy_neutrinos   = parent_mass - lepton_energy;
    double virtual_mass       = std::sqrt((energy_neutrinos - lepton_momentum) * (energy_neutrinos + lepton_momentum));
    double momentum_neutrinos = 0.5 * virtual_mass;
    Vector3D direction        = GenerateRandomDirection();

    products[1]->SetDirection(direction);
    products[1]->SetMomentum(momentum_neutrinos);

    Vector3D opposite_direction = -direction;
    opposite_direction.CalculateSphericalCoordinates();
    products[2]->SetDirection(opposite_direction);
    products[2]->SetMomentum(momentum_neutrinos);

    // Boost neutrinos to lepton frame
    // double beta = lepton_momentum / energy_neutrinos;
    double gamma = energy_neutrinos / virtual_mass;
    double betagamma = lepton_momentum / virtual_mass;
    Boost(*products[1], products[0]->GetDirection(), gamma, betagamma);
    Boost(*products[2], products[0]->GetDirection(), gamma, betagamma);

    // Boost all products in Lab frame (the reason, why the boosting goes in the negative direction of the particle)
    Boost(products, -particle.GetDirection(), particle.GetEnergy()/particle.GetMass(), particle.GetMomentum()/particle.GetMass());

    CopyParticleProperties(products, particle);

    return products;
}

// ------------------------------------------------------------------------- //
// Print
// ------------------------------------------------------------------------- //

// ------------------------------------------------------------------------- //
void LeptonicDecayChannelApprox::print(std::ostream& os) const
{
    os << "Massive lepton:\n" << massive_lepton_ << '\n';
    os << "Neutrino:\n" << neutrino_ << '\n';
    os << "Anti neutrino:\n" << anti_neutrino_ << '\n';
}

/******************************************************************************
 *                          LeptonicDecayChannel                              *
 ******************************************************************************/

const std::string LeptonicDecayChannel::name_ = "LeptonicDecayChannel";

// ------------------------------------------------------------------------- //
LeptonicDecayChannel::LeptonicDecayChannel(const ParticleDef& lepton,
                                                 const ParticleDef& neutrino,
                                                 const ParticleDef& anti_neutrino)
    : LeptonicDecayChannelApprox(lepton, neutrino, anti_neutrino)
{
}

// ------------------------------------------------------------------------- //
LeptonicDecayChannel::~LeptonicDecayChannel() {}

// ------------------------------------------------------------------------- //
LeptonicDecayChannel::LeptonicDecayChannel(const LeptonicDecayChannel& mode)
    : LeptonicDecayChannelApprox(mode)
{
}

// ------------------------------------------------------------------------- //
double LeptonicDecayChannel::DecayRate(double x, double M, double E_max, double right_side)
{
    double M2 = M * M;
    double m  = massive_lepton_.mass;
    double m2 = m * m;

    double E_l     = E_max * x;
    double sqrt_EM = std::sqrt(E_l * E_l - m2);

    return 1.5 * m2 * m2 * M * std::log(sqrt_EM + E_l) +
           sqrt_EM * ((M2 + m2 - M * E_l) * (E_l * E_l - m2) - 1.5 * M * E_l * m2) - right_side;
}

// ------------------------------------------------------------------------- //
double LeptonicDecayChannel::DifferentialDecayRate(double x, double M, double E_max)
{
    double m   = massive_lepton_.mass;
    double E_l = E_max * x;

    return E_max * std::sqrt(E_l * E_l - m * m) * (M * E_l * (3.0 * M - 4.0 * E_l) + m * m * (3.0 * E_l - 2 * M));
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>
#include <utility>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

struct surimi {
  ublas::vector<double> E;

  surimi ( std::size_t nx , double )
    : E(nx,1.)
  { ; }

  ~surimi ()
  { ; }

  ublas::vector<double>
  operator () ( ublas::vector<double> const& )
  {
    return E;
  }
};

auto
disque ( double x0 , double v0 , double R ) {
  return [=](double x,double v){ return ( SQ( x-x0 ) + SQ(v-v0) < R*R ) ? 1. : 0.; };
}

auto
test2 ( double vmin , double vmax , double kx ) {
  return [=](double x,double v){ return ( vmin < v && v < vmax ) ? std::cos(2.*math::pi<double>()*kx*x) : 0.; };
}

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main ( int argc , char const * argv[] )
{
  std::filesystem::path p("config.init");
  if ( argc > 1 )
    { p = argv[1]; }
  auto c = config(p);
  c.name = "cm_e10m3";

  c.create_output_directory();
  std::ofstream ofconfig( c.output_dir / "config.init" );
  ofconfig << c << "\n";
  ofconfig.close();

/* ------------------------------------------------------------------------- */
  field<double,1> f(boost::extents[c.Nv][c.Nx]);
  field<double,1> f_sol(boost::extents[c.Nv][c.Nx]);

  complex_field<double,1> hf(boost::extents[c.Nv][c.Nx]);
  fft::spectrum_ d(c.Nx);

  std::vector<double> mass;

  const double Kx = 0.5;
  f.range.v_min = -3.; f.range.v_max = 3.;
  f.range.x_min = -3.; f.range.x_max = 3.;
  f.compute_steps();

  f_sol.range = f.range;
  f_sol.compute_steps();

  ublas::vector<double> v (c.Nv,1.); // velocity in x direction, transport at speed 1

  ublas::vector<double> kx(c.Nx);
  {
    double l = f.range.len_x();
    for ( auto i=0 ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  auto c0 = disque(0.,0.,1.);
  auto d0 = test2(-1.,1.,1./f.range.len_x());

  double m=0;
  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] = c0(Xi(i),Vk(k));
      f_sol[k][i] = c0(Xi(i)-c.Tf,Vk(k)-c.Tf);

      m += f[k][i]*f.step.dx*f.step.dv;
      //f[k][i] = d0(Xi(i),Vk(k));
      //f_sol[k][i] = d0(Xi(i)-c.Tf,Vk(k)-c.Tf);
    }
  }
  std::cout << m << std::endl;
  //d.fft(f[int(c.Nv/2.)].begin());
  //std::copy( d.begin() , d.end() , std::ostream_iterator<std::complex<double>>(std::cout," "));

  f.write( c.output_dir / ("init_"+c.name+".dat") );
  f_sol.write( c.output_dir / ("sol_"+c.name+".dat") );

  iteration::iteration<double> iter;
  iter.iter = 0;
  iter.current_time = 0.;
  //iter.dt = 0.050*f.step.dv;
  //iter.dt = 2.*std::sqrt(2.)*f.step.dv;
  iter.dt = 0.150*f.step.dv;

  std::vector<double> normL2; normL2.reserve(100);
  auto L2 = [&]( field<double,1>const& f )->double {
    return std::pow(
            std::accumulate( f.origin() , f.origin()+f.num_elements() , 0. ,
              []( double s , double fik )->double { return s + fik*fik; }
          ),2)*f.step.dx*f.step.dv;
  };

  // space scheme
  auto wenol = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return wenolin::trp_v(f,E); };
  auto weno  = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return weno::trp_v(f,E); };
  auto cd2   = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return o2::trp_v(f,E); };

  // time scheme init
  //expRK::HochbruckOstermann<surimi> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2); // 0.250 , 0.501 , 1.702
  expRK::CoxMatthews<surimi> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2); // 0.150 , 0.450 , 1.351
  //expRK::RK22<surimi> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2);
  //lawson::RK33<surimi> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,weno);

  //while (  iter.current_time < c.Tf ) {
  while ( iter.iter < 101 ) {
    std::cout << "\r" << iteration::time(iter) << std::flush;

    normL2.push_back(L2(f));
    f = rk(f,iter.dt);

    ++iter.iter;
    //if ( iter.current_time + iter.dt > c.Tf ) { iter.dt = c.Tf-iter.current_time; }
    iter.current_time += iter.dt;
  }
  std::cout << "\r" << time(iter) << std::endl;

  f.write( c.output_dir / ("vp_"+c.name+".dat") );

  /*
  field<double,1> f_diff(boost::extents[c.Nv][c.Nx]);
  f_diff.range = f.range;
  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f_diff[k][i] = f[k][i]-d0(Xi(i)-c.Tf,Vk(k)-c.Tf);
    }
  }
  f_diff.write( c.output_dir / ("diff_"+c.name+".dat") );
  */

  std::ofstream of( c.output_dir / ("normL2"+c.name+".dat") );
  std::transform(
    normL2.begin() , normL2.end() ,
    std::ostream_iterator<std::string>(of,"\n") ,
    [&,count=0](double x) mutable {
      std::stringstream ss; ss << count << " " << iter.dt*(count++) << " " << x;
      return ss.str();
    }
  );
  of.close();

  return 0;
}


#pragma once

#include <math.h>
#include <boost/serialization/version.hpp>
#include "types/Point.hpp"

struct RANSACLine
{
   Point p1, p2;
   /**
    * Line defined in terms of
    * t1 * x + t2 * y + t3 = 0
    */
   int t1, t2, t3;
   float var;

   RANSACLine(Point p1, Point p2, float var = 0) : p1(p1), p2(p2), var(var)
   {
      t1 = p2.y() - p1.y();
      t2 = p1.x() - p2.x();
      t3 = p1.y() * (p2.x() - p1.x()) - p1.x() * (p2.y() - p1.y());
   }

   RANSACLine() {};

   RANSACLine(const RANSACLine &other) {
      this->t1 = other.t1;
      this->t2 = other.t2;
      this->t3 = other.t3;
      this->var = other.var;

      this->p1(0, 0) = other.p1(0, 0);
      this->p1(1, 0) = other.p1(1, 0);
      this->p2(0, 0) = other.p2(0, 0);
      this->p2(1, 0) = other.p2(1, 0);
   }

   template<class Archive>
   void serialize(Archive &ar, const unsigned int file_version)
   {
      ar & t1 & t2 & t3;
      ar & var;
      if (file_version >= 1) {
        ar & p1 & p2;
      } else {
        p1 = Point (0, 0);
        p2 = Point (0, 0);
      }
   }
};

inline std::ostream& operator<<(std::ostream& os, const RANSACLine& line) {
   os << line.p1;
   os << line.p2;
   os.write((char*) &(line.t1), sizeof(int));
   os.write((char*) &(line.t2), sizeof(int));
   os.write((char*) &(line.t3), sizeof(int));
   os.write((char*) &(line.var), sizeof(float));

   return os;
}

inline std::istream& operator>>(std::istream& is, RANSACLine& line) {
   is >> line.p1;
   is >> line.p2;
   is.read((char*) &(line.t1), sizeof(int));
   is.read((char*) &(line.t2), sizeof(int));
   is.read((char*) &(line.t3), sizeof(int));
   is.read((char*) &(line.var), sizeof(float));

   return is;
}

BOOST_CLASS_VERSION(RANSACLine, 1);

struct RANSACCircle
{
   PointF centre;
   float radius;
   float var;

   RANSACCircle(PointF centre, float radius, float var = 0)
      : centre(centre), radius(radius), var(var)
   {
   }

   RANSACCircle(const Point p1, const Point p2, const Point p3, float var = 0)
      : var(var)
   {
      float bx = p1.x(); float by = p1.y();
      float cx = p2.x(); float cy = p2.y();
      float dx = p3.x(); float dy = p3.y();

      float temp = cx*cx+cy*cy;
      float bc   = (bx*bx + by*by - temp)/2.0;
      float cd   = (temp - dx*dx - dy*dy)/2.0;
      float det  = (bx-cx)*(cy-dy)-(cx-dx)*(by-cy);

      centre = PointF();
      radius = 0;

      if (fabs(det) < 1.0e-6) {
         this->radius = std::numeric_limits<float>::quiet_NaN();
         return;
      }

      det = 1 / det;
      centre.x() = (bc*(cy-dy)-cd*(by-cy))*det;
      centre.y() = ((bx-cx)*cd-(cx-dx)*bc)*det;
      cx = centre.x(); cy = centre.y();
      radius = sqrt((cx-bx)*(cx-bx)+(cy-by)*(cy-by));
   }

   RANSACCircle(const Point p1, const Point p2, float radius, float var = 0)
      : radius(radius), var(var)
   {
		/* TODO(carl) Find a faster way to do this */
/*
      PointF v = (p2 - p1).cast<float>();
      PointF n = PointF(v.y(), -v.x()).normalized();
      PointF m = (p2 + p1).cast<float>() / 2;
      float dist2 = ((p1.x()- p2.x()) * (p1.x()- p2.x())) +
         ((p1.y()- p2.y()) * (p1.y()- p2.y()));
      float d = sqrt(radius*radius - (dist2 / 4));
      PointF newN = PointF(n.x() * d, n.y() *d);
      centre = (m + (newN));
*/

      centre = PointF();
      if (p1 == p2) {
         this->radius = std::numeric_limits<float>::quiet_NaN();
         return;
      };

      const float x1 = p1.x(), x2 = p2.x(), y1 = p1.y(), y2 = p2.y();
      if (y1 != y2) {
         const float k1 = (x2 - x1) / (y1 - y2);
         const float k2 = (x1*x1 + y1*y1 - x2*x2 - y2*y2) / (2*(y1 - y2));

         const float a = 1 + k1*k1;
         const float b = -2*x1 + 2*(k1*(k2 - y1));
         const float c = x1*x1 + (k2 - y1)*(k2 - y1) - radius*radius;
         const float d = b*b - 4*a*c;

         if (d < 0) {
            this->radius = std::numeric_limits<float>::quiet_NaN();
            return;
         }

         const float x = (-b + (y1 > y2 ? sqrtf(d) : -sqrtf(d))) / (2*a);
         const float y = k1*x + k2;
         centre.x() = x;
         centre.y() = y;

      } else {
         const float k1 = (y2 - y1) / (x1 - x2);
         const float k2 = (y1*y1 + x1*x1 - y2*y2 - x2*x2) / (2*(x1 - x2));

         const float a = 1 + k1*k1;
         const float b = -2*y1 + 2*(k1*(k2 - x1));
         const float c = y1*y1 + (k2 - x1)*(k2 - x1) - radius*radius;
         const float d = b*b - 4*a*c;

         if (d < 0) {
            this->radius = std::numeric_limits<float>::quiet_NaN();
            return;
         }

         const float x = (-b + (x1 > x2 ? sqrtf(d) : -sqrtf(d))) / (2*a);
         const float y = k1*x + k2;
         centre.x() = x;
         centre.y() = y;
      }
   }

   RANSACCircle() {};

   template<class Archive>
   void serialize(Archive &ar, const unsigned int file_version)
   {
      ar & centre;
      ar & radius;
      ar & var;
   }

};

BOOST_CLASS_VERSION(RANSACCircle, 0);


// Copyright 2017 Emanuele Palazzolo (emanuele.palazzolo@uni-bonn.de)
#include "fastcd/point_covariance2d.h"
#include <Eigen/Eigenvalues>

namespace fastcd {

PointCovariance2d::PointCovariance2d() {}

PointCovariance2d::PointCovariance2d(const Eigen::Vector2d &point,
                       const Eigen::Matrix2d &covariance, double chi_square)
    : point_(point), covariance_(covariance) {
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> es(covariance_);
  eigenvalues_ = es.eigenvalues();
  eigenvectors_ = es.eigenvectors();
  angle_ = atan2(eigenvectors_(1, 1), eigenvectors_(1, 0));
  if (angle_ < 0) angle_ += 2 * M_PI;
  half_major_axis_size_ = sqrt(chi_square) * sqrt(eigenvalues_(1));
  half_minor_axis_size_ = sqrt(chi_square) * sqrt(eigenvalues_(0));
  Eigen::Matrix2d R;
  R << cos(angle_), -sin(angle_), sin(angle_), cos(angle_);
  Eigen::Vector2d xaxis, yaxis, vertex;
  yaxis << 0, 1;
  xaxis << 1, 0;
  vertex = R * half_major_axis_size_ * xaxis + point;
  vertices_.push_back(vertex);
  vertex = -R * half_major_axis_size_ * xaxis + point;
  vertices_.push_back(vertex);
  vertex = R * half_minor_axis_size_ * yaxis + point;
  vertices_.push_back(vertex);
  vertex = -R * half_minor_axis_size_ * yaxis + point;
  vertices_.push_back(vertex);
}

Eigen::Vector2d PointCovariance2d::Point() { return point_; }

Eigen::Matrix2d PointCovariance2d::Covariance() { return covariance_; }

Eigen::Vector2d PointCovariance2d::Eigenvalues() { return eigenvalues_; }

Eigen::Matrix2d PointCovariance2d::Eigenvectors() { return eigenvectors_; }

double PointCovariance2d::Angle() { return angle_; }

double PointCovariance2d::HalfMajorAxisSize() { return half_major_axis_size_; }

double PointCovariance2d::HalfMinorAxisSize() { return half_minor_axis_size_; }

std::vector<Eigen::Vector2d> PointCovariance2d::Vertices() { return vertices_; }

}  // namespace fastcd


// 作者：陈露
// current  version： V1.0 191219
// previous version： V1.0 191210
// 描述：point cloud odometry based on ICP

// ROS --------------------------------
#include <ros/ros.h>

#include <std_msgs/Bool.h>
#include <std_msgs/ColorRGBA.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float64MultiArray.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>

#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>

#include <sensor_msgs/PointCloud2.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <jsk_recognition_msgs/BoundingBox.h>

// C++ ------------------------------
#include <iostream>
#include <boost/thread/thread.hpp>

// eigen -----------------------------
#include <Eigen/Dense>

// pcl -------------------------------
#include <pcl/common/common_headers.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl-1.8/pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

#include <pcl/range_image/range_image.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/visualization/range_image_visualizer.h>

#include <pcl/registration/gicp.h>
#include <pcl/registration/icp.h>

#include <pcl/filters/filter.h>

#define PI 3.1415926

pcl::PointCloud<pcl::PointXYZ>::Ptr incloud(new pcl::PointCloud<pcl::PointXYZ>), source(new pcl::PointCloud<pcl::PointXYZ>), reference(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr incloudFiltered(new pcl::PointCloud<pcl::PointXYZ>);
bool isInitialized = false;

ros::Publisher referenceCloud_pub, readingCloud_pub;
ros::Publisher icpPoseVariation_pub, odomVariation_pub;

Eigen::Vector3d odomTCurrent;
Eigen::Vector4d odomRCurrent;

bool updateICP()
{
    // G-ICP
    // pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    // icp.setTransformationEpsilon(0.01);
    // icp.setMaxCorrespondenceDistance(0.3);
    // icp.setMaximumIterations(30);
    // icp.setRANSACIterations(0);

    // ICP
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setInputSource(source);
    icp.setInputTarget(reference);
    pcl::PointCloud<pcl::PointXYZ> unused;
    icp.align(unused); //这里kinect出来的有序点云貌似不可以直接进行demo里的半径过滤法会报错

    Eigen::Matrix4f transM = icp.getFinalTransformation();
    std::cout << "icp transformation = " << std::endl << icp.getFinalTransformation() << std::endl;

    // rotation matrix to quaternion
    Eigen::Matrix3d t_R;
    t_R(0,0) = transM(0,0);
    t_R(1,0) = transM(1,0);
    t_R(2,0) = transM(2,0);
    t_R(0,1) = transM(0,1);
    t_R(1,1) = transM(1,1);
    t_R(2,1) = transM(2,1);
    t_R(0,2) = transM(0,2);
    t_R(1,2) = transM(1,2);
    t_R(2,2) = transM(2,2);
    std::cout << "rotation matrix = " << std::endl << t_R << std::endl;

    Eigen::Quaterniond t_Q;
    t_Q = t_R;
    std::cout << "quaternion = " << std::endl << t_Q.coeffs() << std::endl;

    // quaternion to euler_angle(ZYX-RPY)
    Eigen::Vector3d eulerAngle = t_Q.matrix().eulerAngles(2,1,0);
    std::cout << "euler angle in radian= " << std::endl << eulerAngle << std::endl;
    Eigen::Vector3d eulerAngleInDegree = t_Q.matrix().eulerAngles(2,1,0);
    eulerAngleInDegree(0) = eulerAngle(0)*180/PI;
    eulerAngleInDegree(1) = eulerAngle(1)*180/PI;
    eulerAngleInDegree(2) = eulerAngle(2)*180/PI;
    std::cout << "euler angle in degree= " << std::endl << eulerAngleInDegree << std::endl;

    // translation
    Eigen::Vector3d translation;
    translation(0) = transM(0,3);
    translation(1) = transM(1,3);
    translation(2) = transM(2,3);
    std::cout << "translation = " << std::endl << translation << std::endl;

    float positionDelta = 0.05, orientationDelta = PI/20;
    if(translation(0)>positionDelta || translation(1)>positionDelta || translation(2)>positionDelta)
    {
        ROS_ERROR("position changed large!");
    }

    if(eulerAngle(0)>orientationDelta || eulerAngle(1)>orientationDelta || eulerAngle(2)>orientationDelta)
    {
        ROS_WARN("orientation changed large!");
    }

    std_msgs::Float32MultiArray icpPoseVariation;
    icpPoseVariation.data.push_back(translation(0));
    icpPoseVariation.data.push_back(translation(1));
    icpPoseVariation.data.push_back(translation(2));
    icpPoseVariation.data.push_back(eulerAngleInDegree[0]);
    icpPoseVariation.data.push_back(eulerAngleInDegree[1]);
    icpPoseVariation.data.push_back(eulerAngleInDegree[2]);
    icpPoseVariation_pub.publish(icpPoseVariation);
}

void pointcloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
    // pcl::PointCloud<pcl::PointXYZ>::ptr source(new pcl::PointCloud<pcl::PointXYZ>), reference(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*cloud, *incloud);

    if(!incloud->is_dense)
    {
        //ROS_WARN("input cloud is_dense=false!");
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*incloud, *incloudFiltered, indices);
    }
    else
    {
        *incloudFiltered = *incloud;
    }

    Eigen::Vector4d odomRK1, odomRK2; // quatertion
    Eigen::Vector3d odomTK1, odomTK2; // translation
    Eigen::Vector3d odomREK1, odomREK2; // euler angle
    if(!isInitialized)
    {
        pcl::copyPointCloud(*incloudFiltered, *source);

        odomTK1 = odomTCurrent;
        odomRK1 = odomRCurrent;

        isInitialized = true;
        return;
    }
    odomTK2 = odomTCurrent;
    odomRK2 = odomRCurrent;
    std::cout << "odom position previous = " << odomTK1 << std::endl;
    std::cout << "odom orientation previous = " << odomRK1 << std::endl;
    std::cout << "odom position current = " << odomTK2 << std::endl;
    std::cout << "odom orientation current = " << odomRK2 << std::endl;
    std::cout << std::endl;

    pcl::copyPointCloud(*source, *reference);
    pcl::copyPointCloud(*incloudFiltered, *source);

    // pub reference cloud & reading cloud.
    sensor_msgs::PointCloud2 referenceCloud, readingCloud;
    pcl::toROSMsg(*source, readingCloud);
    referenceCloud_pub.publish(readingCloud);
    pcl::toROSMsg(*reference, referenceCloud);
    readingCloud_pub.publish(referenceCloud);

    updateICP();

    // odom variation
    std_msgs::Float32MultiArray odomVariation;
    odomVariation.data.push_back(odomTK2[0] - odomTK1[0]);
    odomVariation.data.push_back(odomTK2[1] - odomTK1[1]);
    odomVariation.data.push_back(odomTK2[2] - odomTK1[2]);

    tf::Quaternion q( odomRK1[0], odomRK1[1], odomRK1[2], odomRK1[3]);
    tf::Matrix3x3 m(q);
    m.getRPY(odomREK1[0], odomREK1[1], odomREK1[2]);

    tf::Quaternion q2( odomRK2[0], odomRK2[1], odomRK2[2], odomRK2[3]);
    tf::Matrix3x3 m2(q2);
    m2.getRPY(odomREK2[0], odomREK2[1], odomREK2[2]);
    odomVariation.data.push_back(odomREK2[0] - odomREK1[0]);
    odomVariation.data.push_back(odomREK2[1] - odomREK1[1]);
    odomVariation.data.push_back(odomREK2[2] - odomREK1[2]);

    // publish pose variation
    odomVariation_pub.publish(odomVariation);

    odomTK1 = odomTK2;
    odomRK1 = odomRK2;
}

void odom_callback(const nav_msgs::OdometryConstPtr& odom)
{
    odomTCurrent(0) = odom->pose.pose.position.x;
    odomTCurrent(1) = odom->pose.pose.position.y;
    odomTCurrent(2) = odom->pose.pose.position.z;

    odomRCurrent(0) = odom->pose.pose.orientation.x;
    odomRCurrent(1) = odom->pose.pose.orientation.y;
    odomRCurrent(2) = odom->pose.pose.orientation.z;
    odomRCurrent(3) = odom->pose.pose.orientation.w;
}

// ========================================= publisher

int main(int argc, char** argv)
{
    ros::init(argc, argv, "icp_pose");
    ros::NodeHandle nh("~");
    //ros::NodeHandle nh;

    // Subscriber
    //ros::Subscriber cloud_sub = nh.subscribe("/bluerov2h_follower/depth/points2", 10, &pointcloud_callback);
    ros::Subscriber cloud_sub = nh.subscribe("/cloud_centroid/radius_outlier_removal_points2", 10, &pointcloud_callback);
    ros::Subscriber odom_sub = nh. subscribe("/bluerov2h_leader/pose_gt", 20, &odom_callback);

    // publisher
    referenceCloud_pub = nh.advertise<sensor_msgs::PointCloud2>("reference_cloud", 10);
    readingCloud_pub = nh.advertise<sensor_msgs::PointCloud2>("reading_cloud", 10);
    icpPoseVariation_pub = nh.advertise<std_msgs::Float32MultiArray>("icp_pose_variation", 10);
    odomVariation_pub = nh.advertise<std_msgs::Float32MultiArray>("odom_variation", 10);

    ros::AsyncSpinner spinner(2);
    ros::Rate rate(1);
    spinner.start();
    while(ros::ok())
    {
        ROS_INFO("*********************************************");
        //spinner.start();
        ros::spinOnce();
        rate.sleep();
    }

    spinner.stop();

    return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// 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)

#ifndef GENFILE_QUANTILE_NORMALISING_CROSS_COHORT_COVARIATE_MAPPING_HPP
#define GENFILE_QUANTILE_NORMALISING_CROSS_COHORT_COVARIATE_MAPPING_HPP

#include <boost/math/distributions/normal.hpp>
#include "genfile/CrossCohortCovariateValueMapping.hpp"
#include "genfile/ContinuousVariableCrossCohortCovariateValueMapping.hpp"

namespace genfile {
	class QuantileNormalisingCrossCohortCovariateValueMapping: public ContinuousVariableCrossCohortCovariateValueMapping
		// A mapping which scales its given values (which must be real numbers) so that
		// they are approximately normally distributed with mean 0 and variance 1.
	{
	public:
		QuantileNormalisingCrossCohortCovariateValueMapping(
			std::string const& column_name
		) ;

		void add_source( CohortIndividualSource const& source ) ;
		std::size_t get_number_of_distinct_mapped_values() const ;

		Entry get_unmapped_value( Entry const& level ) const ;
		Entry get_mapped_value( Entry const& entry ) const ;

		// std::string get_summary( std::string const& prefix = "" ) const ;

		virtual std::string get_mapping_name() const { return "quantile normalised" ; }

	private:
		typedef std::map< double, double > Mapping ;
		Mapping m_mapping ;
		Mapping m_reverse_mapping ;
	private:
		void analyse_values( Histogram const& entries ) ;
		boost::math::normal m_normal_distribution ;
	} ;
}

#endif


#pragma once

#include <array>
#include <iostream>
#include <mutex>
#include <vector>

#include <Eigen/Core>

#include "camera.hpp"
#include "eigen_json.hpp"
#include "float.hpp"

namespace swr {

struct Light {
    Light() = default;
    Light(const nlohmann::json& json)
    {
        from_json(json["position"], position);
        from_json(json["color"], intensity);
    }

    Vector3F position;
    Vector3F intensity;
};

struct Material {
    Material() = default;
    Material(const nlohmann::json& json)
    {
        from_json(json["ambient"], ambient_color);
        from_json(json["diffuse"], diffuse_color);
        from_json(json["specular"], specular_color);
        specular_exponent = json["shininess"];
    }

    Vector3F ambient_color;
    Vector3F diffuse_color;
    Vector3F specular_color;
    Float specular_exponent; // Also called "shininess"
};

class VertexAttributes {
public:
    VertexAttributes(Float x = 0, Float y = 0, Float z = 0, Float w = 1)
        : VertexAttributes(Vector3F(x, y, z), w)
    {
    }
    VertexAttributes(const Vector3F& pos, Float w = 1)
    {
        position.head<3>() = pos;
        position.w() = w;
    }

    // Interpolates the vertex attributes
    static VertexAttributes interpolate(
        const VertexAttributes& a,
        const VertexAttributes& b,
        const VertexAttributes& c,
        const Float alpha,
        const Float beta,
        const Float gamma)
    {
        VertexAttributes r;
        r.position =
            alpha * a.position + beta * b.position + gamma * c.position;
        r.color = alpha * a.color + beta * b.color + gamma * c.color;
        r.bc << alpha, beta, gamma;
        return r;
    }

    Vector4F position;
    Vector3F normal;
    Vector3F color;
    Vector3F bc;
    Material material;
};

class FragmentAttributes {
public:
    FragmentAttributes(Float r = 0, Float g = 0, Float b = 0, Float a = 1)
        : FragmentAttributes(Vector3F(r, g, b), a)
    {
    }
    FragmentAttributes(const Vector3F& rgb, Float a = 1)
    {
        color.head<3>() = rgb;
        color(3) = a;
    }
    Float depth;
    Vector4F color;
};

class FrameBufferAttributes {
public:
    FrameBufferAttributes(
        uint8_t r = 0, uint8_t g = 0, uint8_t b = 0, uint8_t a = 255)
        : FrameBufferAttributes(Eigen::Matrix<uint8_t, 4, 1>(r, g, b, a))
    {
    }
    FrameBufferAttributes(const Eigen::Matrix<uint8_t, 4, 1>& color)
        : color(color)
        , depth(std::numeric_limits<Float>::infinity())
    {
    }
    Float depth;
    Eigen::Matrix<uint8_t, 4, 1> color;
    std::mutex lock;
};

typedef Eigen::Matrix<FrameBufferAttributes, Eigen::Dynamic, Eigen::Dynamic>
    FrameBuffer;

inline FrameBuffer create_frame_buffer(
    const Eigen::Vector2i& resolution,
    const Eigen::Matrix<uint8_t, 4, 1>& bg_color)
{
    FrameBuffer frame_buffer(resolution.x(), resolution.y());
    for (int i = 0; i < frame_buffer.rows(); i++) {
        for (int j = 0; j < frame_buffer.cols(); j++) {
            frame_buffer(i, j).color = bg_color;
        }
    }
    return frame_buffer;
}

class UniformAttributes {
public:
    Matrix4F M;
    Matrix4F ST;
    std::vector<Light> lights;
    Vector3F ambient;
    Camera camera;
};

} // namespace swr


#include <algorithm>
#include <array>
#include <cassert>
#include <exception>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/operators.hpp>

template<typename ForwardIt>
auto is_element_unique(ForwardIt first, ForwardIt last) -> bool {
    for (; first != last; ++first) {
        if (last != std::find(first + 1, last, *first)) {
            return false;
        }
    }
    return true;
}

template<typename ForwardIt>
auto permutation_parity(ForwardIt first1, ForwardIt last1, ForwardIt first2, ForwardIt last2) -> int {
    assert(std::distance(first1, last1) == std::distance(first2, last2));

    std::vector<bool> mark(std::distance(first1, last1));
    int counter = 0;
    bool done = false;
    do {
        // find next unmark pos
        auto unmark = std::find(mark.begin(), mark.end(), false);
        if (unmark == mark.end()) {
            done = true;
        }
        else {
            auto pos1 = std::next(first1, std::distance(mark.begin(), unmark));
            auto f = pos1;
            bool end_cycle = false;
            do {
                // find next pos in the cycle
                // only accept if it is not marked
                bool found = false;
                auto pos2 = first2;
                long long dis = 0;
                do {
                    pos2 = std::find(pos2, last2, *pos1);
                    if (pos2 == last2) return -1;
                    dis = std::distance(first2, pos2);
                    found = (mark[dis] == false);
                    ++pos2;
                } while (!found);

                pos1 = std::next(first1, dis);
                mark[dis] = true;
                if (*f == *pos1) {
                    end_cycle = true;
                }
                else {
                    ++counter;
                }
            } while (!end_cycle);
        }
    } while (!done);

    return counter;
}

namespace puzzle {

    struct direction {
        enum type {
            begin = 0,
            left = begin,
            top,
            right,
            bottom,
            end
        };
    };

    namespace board {

        // contains size * size numbers
        // 1 special number called blank square
        template<typename Derived>
        class base_board {
            using mytype = base_board;
            using derived_type = Derived;

        protected:
            base_board() = default;
            ~base_board() = default;

        public:
            // slide blank square
            auto slide(direction::type d) -> bool {
                auto&& buffer = this->buffer();
                auto bpos = blankpos();
                auto& row = bpos.first;
                auto& col = bpos.second;

                using std::swap;
                switch (d) {
                case direction::left:
                    if (col == 0)
                        return false;
                    swap(buffer[row][col], buffer[row][col - 1]);
                    --col;
                    blankpos(bpos);
                    break;

                case direction::top:
                    if (row == 0)
                        return false;
                    swap(buffer[row][col], buffer[row - 1][col]);
                    --row;
                    blankpos(bpos);
                    break;

                case direction::right:
                    if (col == size() - 1)
                        return false;
                    swap(buffer[row][col], buffer[row][col + 1]);
                    ++col;
                    blankpos(bpos);
                    break;

                case direction::bottom:
                    if (row == size() - 1)
                        return false;
                    swap(buffer[row][col], buffer[row + 1][col]);
                    ++row;
                    blankpos(bpos);
                    break;

                default:
                    throw std::invalid_argument{ "invalid slide direction" };
                    break;
                }

                return true;
            }

            // find position of a number in board
            // return (-1, -1) if cannot find
            auto find(int num) const noexcept -> std::pair<int, int> {
                auto&& buffer = this->buffer();
                for (auto row = 0; row < size(); ++row) {
                    for (auto col = 0; col < size(); ++col) {
                        if (buffer[row][col] == num) {
                            return std::make_pair(row, col);
                        }
                    }
                }
                return std::make_pair(-1, -1);
            }

            // get & set blank square position
            auto blankpos() const noexcept -> decltype(auto) {
                return static_cast<derived_type const *>(this)->blankpos();
            }

            void blankpos(std::pair<int, int> const& bpos) noexcept {
                static_cast<derived_type*>(this)->blankpos(bpos);
            }

            auto size() const noexcept -> decltype(auto) {
                return static_cast<derived_type const *>(this)->size();
            }

            // read only
            auto operator[](int index) const noexcept -> decltype(auto) {
                return buffer()[index];
            }

            // used by std::map
            auto operator<(mytype const& other) const noexcept -> bool {
                return buffer() < other.buffer();
            }

            auto operator==(mytype const& other) const noexcept -> bool {
                return buffer() == other.buffer();
            }

            auto operator!=(mytype const& other) const noexcept -> bool {
                return !(*this == other);
            }

            // output mytype to ostream
            friend std::ostream& operator<<(std::ostream& os, mytype const& board) {
                auto width = os.width();
                for (auto r = 0; r < board.size(); ++r) {
                    for (auto c = 0; c < board.size(); ++c) {
                        os << std::setw(width) << board.buffer()[r][c];
                    }
                    os << "\n";
                }
                return os;
            }

        private:
            auto buffer() noexcept -> decltype(auto) {
                return static_cast<derived_type *>(this)->buffer();
            }

            auto buffer() const noexcept -> decltype(auto) {
                return static_cast<derived_type const *>(this)->buffer();
            }
        };

        template<int N>
        class aa_board : public base_board<aa_board<N>>
        {
            static_assert(N > 0, "invalid size");

            using mybase = base_board<aa_board<N>>;
            using buffer_type = std::array<std::array<int, N>, N>;

            friend mybase;

        public:
            aa_board() = default;
            aa_board(buffer_type const& buffer, std::pair<int, int> const& blankpos) :
                mybase{},
                m_buffer{ buffer },
                m_brow{ blankpos.first },
                m_bcol{ blankpos.second }
            {
                // check blank square position
                if ((0 <= m_brow && m_brow <= size() - 1) && (0 <= m_brow && m_brow <= size() - 1)) {
                    // then it's OK
                }
                else {
                    throw std::invalid_argument{ "invalid blank position" };
                }
            }

        public: // required by base_board
            auto size() const noexcept -> int {
                return N;
            }

            auto blankpos() const noexcept {
                assert(m_brow != -1 && m_bcol != -1);
                return std::make_pair(m_brow, m_bcol);
            }

            void blankpos(std::pair<int, int> const& bpos) noexcept {
                m_brow = bpos.first;
                m_bcol = bpos.second;
            }

        private: // required by base_board
            auto buffer() noexcept -> buffer_type& {
                return m_buffer;
            }

            auto buffer() const noexcept -> buffer_type const& {
                return m_buffer;
            }

        private:
            buffer_type m_buffer{};
            int m_brow = -1;
            int m_bcol = -1;
        };

        class vv_board : public base_board<vv_board>
        {
            using mybase = base_board<vv_board>;
            using buffer_type = std::vector<std::vector<int>>;

            friend mybase;

        public:
            vv_board() = default;
            vv_board(buffer_type const& buffer, std::pair<int, int> const& blankpos) :
                mybase{},
                m_buffer{ buffer },
                m_brow{ blankpos.first },
                m_bcol{ blankpos.second }
            {
                // check buffer, number of cols must be equal to number of rows
                for (auto& row : m_buffer) {
                    if (static_cast<int>(row.size()) != size()) {
                        throw std::invalid_argument{ "invalid board" };
                    }
                }

                // check blank square position
                if ((0 <= m_brow && m_brow <= size() - 1) && (0 <= m_brow && m_brow <= size() - 1)) {
                    // then it's OK
                }
                else {
                    throw std::invalid_argument{ "invalid blank position" };
                }
            }

        public: // required by base_board
            auto size() const noexcept -> int {
                return static_cast<int>(m_buffer.size());
            }

            auto blankpos() const noexcept {
                assert(m_brow != -1 && m_bcol != -1);
                return std::make_pair(m_brow, m_bcol);
            }

            void blankpos(std::pair<int, int> const& bpos) noexcept {
                m_brow = bpos.first;
                m_bcol = bpos.second;
            }

        private: // required by base_board
            auto buffer() noexcept -> buffer_type& {
                return m_buffer;
            }

            auto buffer() const noexcept -> buffer_type const& {
                return m_buffer;
            }

        private:
            buffer_type m_buffer{};
            int m_brow = -1;
            int m_bcol = -1;
        };

        template<typename Board>
        auto num_of_neighbors(Board const& board) noexcept -> int {
            auto bpos = board.blankpos();
            auto row = bpos.first;
            auto col = bpos.second;

            int non = 0;

            int r1 = row - 1;
            int r2 = row + 1;
            int c1 = col - 1;
            int c2 = col + 1;

            auto is_valid_pos = [&board](int r, int c) -> bool {
                return (0 <= r && r <= board.size() - 1) && (0 <= c && c <= board.size() - 1);
            };

            if (is_valid_pos(r1, c1)) ++non;
            if (is_valid_pos(r1, c2)) ++non;
            if (is_valid_pos(r2, c1)) ++non;
            if (is_valid_pos(r2, c2)) ++non;

            assert(non >= 2);

            return non;
        }

        template<typename Board, typename T>
        auto make_board(T&& buffer, int size, int blank_num = 0) {
            int row = -1;
            int col = -1;
            for (auto r = 0; r < size; ++r) {
                for (auto c = 0; c < size; ++c) {
                    if (buffer[r][c] == blank_num) {
                        row = r;
                        col = c;
                    }
                }
            }

            if (row == -1 || col == -1)
                throw std::invalid_argument{ "cannot find blank square" };

            return Board{ buffer, std::make_pair(row, col) };
        }

    } // namespace board

    namespace graph {

        template<typename Board>
        class neighbor_iterator : public boost::forward_iterator_helper<neighbor_iterator<Board>, Board>
        {
        public:
            neighbor_iterator() = default;
            neighbor_iterator(Board const& board, direction::type d) :
                m_board{ board },
                m_direction{ d }
            {}

            auto operator==(neighbor_iterator const& other) const noexcept -> bool {
                if (m_direction == direction::end && other.m_direction == direction::end) {
                    return true;
                }
                return m_direction == other.m_direction && m_board == other.m_board;
            }

            auto operator++() -> neighbor_iterator& {
                m_direction = static_cast<direction::type>(m_direction + 1);
                return *this;
            }

            auto operator*() const noexcept -> std::pair<Board, Board> {
                auto temp = m_board;
                temp.slide(m_direction);
                return std::make_pair(m_board, temp);
            }

        private:
            Board m_board;
            direction::type m_direction = direction::end;
        };

        /*
        model of
            Graph
            IncidenceGraph
        */
        template<typename Board>
        struct board_graph {
            // Graph concept requirements
            using vertex_descriptor         = Board;
            using edge_descriptor           = std::pair<Board, Board>;
            using directed_category         = boost::undirected_tag;
            using edge_parallel_category    = boost::disallow_parallel_edge_tag;
            using traversal_category        = boost::incidence_graph_tag;

            // IncidenceGraph concept requirements
            using out_edge_iterator         = neighbor_iterator<Board>;
            using degree_size_type          = int;
        };

        // IncidenceGraph concept requirements
        template<typename Board,
            typename Graph = board_graph<Board> // just make the name shorter
        >
        auto out_edges(typename Graph::vertex_descriptor const& board, board_graph<Board> const&)
            -> std::pair<typename Graph::out_edge_iterator, typename Graph::out_edge_iterator>
        {
            return std::make_pair(
                typename Graph::out_edge_iterator{ board, direction::begin },
                typename Graph::out_edge_iterator{ board, direction::end }
            );
        }

        template<typename Board,
            typename Graph = board_graph<Board>
        >
        auto out_degree(typename Graph::vertex_descriptor const& board, board_graph<Board> const&)
            -> typename Graph::degree_size_type
        {
            return num_of_neighbors(board);
        }

        /* boost graph already provides two below functions

        template<typename Board,
            typename Graph = board_graph<Board>
        >
        auto source(typename Graph::edge_descriptor const& e, board_graph<Board> const&)
            -> typename Graph::vertex_descriptor
        {
            return e.first;
        }

        template<typename Board,
            typename Graph = board_graph<Board>
        >
        auto target(typename Graph::edge_descriptor const& e, board_graph<Board> const&)
            -> typename Graph::vertex_descriptor
        {
            return e.second;
        }
        */

        // found exception
        struct found : public std::exception
        {};

        struct exceed_limit : public std::exception
        {};

        template<typename Board>
        class visitor : public boost::default_astar_visitor
        {
        public:
            visitor(Board const& goal, int *counter = nullptr, int vertices_limit = -1) noexcept :
                m_goal{ goal },
                m_counter{ counter },
                m_limit{ vertices_limit }
            {}

            void examine_vertex(Board const& board, board_graph<Board> const&) {
                if (m_limit > 0) --m_limit;
                if (m_counter) ++*m_counter;

                if (board == m_goal)
                    throw found{};

                if (m_limit == 0)
                    throw exceed_limit{};
            }

        private:
            Board const& m_goal;
            int *m_counter = nullptr;
            int m_limit = -1;
        };

        template<typename Board>
        class manhattan_heuristic : public boost::astar_heuristic<board_graph<Board>, int>
        {
        public:
            manhattan_heuristic(Board const& goal) :
                m_goal{ goal }
            {}

            auto operator()(Board const& board) -> int {
                int sum = 0;
                for (auto row = 0; row < board.size(); ++row) {
                    for (auto m_col = 0; m_col < board.size(); ++m_col) {
                        auto pos = m_goal.find(board[row][m_col]);
                        sum += (std::abs(row - pos.first) + std::abs(m_col - pos.second)) << 1;
                    }
                }
                return sum;
            }

        private:
            Board const& m_goal;
        };

    } // namespace graph

    // a map with default value
    template <typename Key, typename Value>
    class default_map {
    public:
        using key_type = Key;
        using data_type = Value;
        using value_type = std::pair<Key, Value>;

        default_map(Value const& defaultValue)
            : defaultValue(defaultValue)
        {}

        auto operator[](Key const& k) -> Value& {
            if (m.find(k) == m.end()) {
                m[k] = defaultValue;
            }
            return m[k];
        }

    private:
        std::map<Key, Value> m;
        Value const defaultValue;
    };

}

// graph_traits partial specialization for board_graph
namespace boost {

    template<typename Board>
    struct graph_traits<puzzle::graph::board_graph<Board>> {
        using G                         = puzzle::graph::board_graph<Board>;

        using vertex_descriptor         = typename G::vertex_descriptor;
        using edge_descriptor           = typename G::edge_descriptor;
        using out_edge_iterator         = typename G::out_edge_iterator;

        using directed_category         = typename G::directed_category;
        using edge_parallel_category    = typename G::edge_parallel_category;
        using traversal_category        = typename G::traversal_category;

        using degree_size_type          = typename G::degree_size_type;

        using in_edge_iterator          = void;
        using vertex_iterator           = void;
        using vertices_size_type        = void;
        using edge_iterator             = void;
        using edges_size_type           = void;
    };

}

namespace puzzle {

    template<typename Board>
    auto check_board(Board const& start, Board const& goal) -> bool {
        // must have same size
        if (start.size() != goal.size()) {
            return false;
        }
        auto const size = start.size();

        // must have same blank number
        auto sbpos = start.blankpos();
        auto gbpos = goal.blankpos();
        if (start[sbpos.first][sbpos.second] != goal[gbpos.first][gbpos.second]) {
            return false;
        }

        std::vector<int> vstart;
        std::vector<int> vgoal;
        vstart.reserve(size * size);
        vgoal.reserve(size * size);
        for (auto r = 0; r < size; ++r) {
            for (auto c = 0; c < size; ++c) {
                vstart.push_back(start[r][c]);
                vgoal.push_back(goal[r][c]);
            }
        }

        // each number can appear only once
        if (!is_element_unique(vstart.begin(), vstart.end()) || !is_element_unique(vgoal.begin(), vgoal.end())) {
            return false;
        }

        // both boards have same set of numbers
        if (!std::is_permutation(vstart.begin(), vstart.end(), vgoal.begin(), vgoal.end())) {
            return false;
        }

        return true;
    }

    template<typename Board>
    auto solvable(Board const& start, Board const& goal) -> bool {
        if (!check_board(start, goal)) {
            throw std::invalid_argument{ "invalid boards, cannot check solvability" };
        }

        auto const size = start.size();

        std::vector<int> vstart;
        std::vector<int> vgoal;
        vstart.reserve(size * size);
        vgoal.reserve(size * size);
        for (auto r = 0; r < size; ++r) {
            for (auto c = 0; c < size; ++c) {
                vstart.push_back(start[r][c]);
                vgoal.push_back(goal[r][c]);
            }
        }

        // compute parity of permutation
        auto parity = permutation_parity(vstart.begin(), vstart.end(), vgoal.begin(), vgoal.end());
        if (parity == -1) {
            return false;
        }

        // compute taxicab distance
        auto sbpos = start.blankpos();
        auto gbpos = goal.blankpos();
        auto tabdis = std::abs(sbpos.first - gbpos.first) + std::abs(sbpos.second - gbpos.second);

        return (parity + tabdis) % 2 == 0;
    }

    template<typename Board>
    auto solve(Board const& start, Board const& goal, int *counter = nullptr, int limit = -1) -> std::vector<Board> {
        using PredecessorMap = boost::associative_property_map<std::map<Board, Board>>;
        using DistanceMap = boost::associative_property_map<puzzle::default_map<Board, int>>;
        using WeightMap = boost::associative_property_map<puzzle::default_map<std::pair<Board, Board>, int>>;
        using VertexIndexMap = boost::associative_property_map<std::map<Board, int>>;
        using RankMap = boost::associative_property_map<std::map<Board, int>>;
        using ColorMap = boost::associative_property_map<std::map<Board, boost::default_color_type>>;

        if (!check_board(start, goal)) {
            throw std::invalid_argument{ "invalid boards" };
        }

        if (!solvable(start, goal)) {
            throw std::runtime_error{ "unsolvable" };
        }

        puzzle::graph::board_graph<Board> graph;

        std::map<Board, Board> pmx{};
        puzzle::default_map<Board, int> dmx{ std::numeric_limits<int>::max() };
        puzzle::default_map<std::pair<Board, Board>, int> wmx{ 1 };
        std::map<Board, int> vimx{};
        std::map<Board, int> rmx{};
        std::map<Board, boost::default_color_type> cmx{};

        PredecessorMap pm{ pmx };

        try {
            boost::astar_search_no_init(
                graph,
                start,
                puzzle::graph::manhattan_heuristic<Board>{ goal },
                boost::visitor(puzzle::graph::visitor<Board>{ goal, counter, limit })
                    .weight_map(WeightMap{ wmx })
                    .vertex_index_map(VertexIndexMap{ vimx })
                    .predecessor_map(pm)
                    .distance_map(DistanceMap{ dmx })
                    .rank_map(RankMap{ rmx })
                    .color_map(ColorMap{ cmx })
                    .distance_compare(std::less<void>{})
                    .distance_combine(std::plus<void>{})
            );
        }
        catch (puzzle::graph::found const&) {
            std::vector<Board> path;

            auto current = goal;
            bool done = false;
            do {
                path.push_back(current);
                if (current == start) {
                    done = true;
                }
                else {
                    current = pm[current];
                }
            } while (!done);

            return path;
        }
        catch (puzzle::graph::exceed_limit const&) {
            // ignore
        }

        return std::vector<Board>{};
    }

} // namespace puzzle

int main() {
    try {
        std::array<std::array<int, 4>, 4> g{ {
            { 1,  2,  3,  4 },
            { 5,  6,  7,  8 },
            { 9, 10, 11, 12 },
            { 13, 14, 15, 0 }
        } };

        std::array<std::array<int, 4>, 4> s{ {
            { 7, 15,  2, 10 },
            { 3,  0,  8,  5 },
            { 6, 11, 13,  1 },
            { 14, 12, 4,  9 }
        } };

        auto goal = puzzle::board::make_board<puzzle::board::aa_board<4>>(g, 4);
        auto start = puzzle::board::make_board<puzzle::board::aa_board<4>>(s, 4);

        int counter = 0;
        auto path = puzzle::solve(start, goal, &counter);

        std::cout
            << "number of tests: " << counter << "\n"
            << "number of steps: " << path.size() - 1 << "\n\n";

        std::cout
            << "start puzzle:\n" << std::setw(3) << start << "\n\n"
            << "goal puzzle:\n" << std::setw(3) << goal << "\n\n";

        // print all intermediate blank square positions
        for (auto it = path.rbegin(), eit = path.rend(); it != eit; ++it) {
            auto pos = it->blankpos();
            std::cout << "(" << pos.first << "," << pos.second << ") -> ";
        }
        std::cout << "done\n";
    }
    catch (std::exception const& e) {
        std::cout << e.what();
    }
}


#include <stan/math/rev/mat.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/mat/fun/util.hpp>
#include <test/unit/math/rev/mat/util.hpp>

#ifdef STAN_OPENCL
#include <boost/random/mersenne_twister.hpp>
#endif

TEST(AgradRevMatrix, mdivide_left_tri_val) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;

  matrix_v Av(2, 2);
  matrix_v Av_inv(2, 2);
  matrix_d Ad(2, 2);
  matrix_v I;

  Av << 2.0, 0.0, 5.0, 7.0;
  Ad << 2.0, 0.0, 5.0, 7.0;

  I = mdivide_left_tri<Eigen::Lower>(Av, Av);
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);

  I = mdivide_left_tri<Eigen::Lower>(Av, Ad);
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);

  I = mdivide_left_tri<Eigen::Lower>(Ad, Av);
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);

  Av_inv = mdivide_left_tri<Eigen::Lower>(Av);
  I = Av * Av_inv;
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);

  Av << 2.0, 3.0, 0.0, 7.0;
  Ad << 2.0, 3.0, 0.0, 7.0;

  I = mdivide_left_tri<Eigen::Upper>(Av, Av);
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);

  I = mdivide_left_tri<Eigen::Upper>(Av, Ad);
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);

  I = mdivide_left_tri<Eigen::Upper>(Ad, Av);
  EXPECT_NEAR(1.0, I(0, 0).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(0, 1).val(), 1.0E-12);
  EXPECT_NEAR(0.0, I(1, 0).val(), 1.0E-12);
  EXPECT_NEAR(1.0, I(1, 1).val(), 1.0e-12);
}

TEST(AgradRevMatrix, mdivide_left_tri_lower_grad_vv) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;

  matrix_d Ad(2, 2), Ad_tmp(2, 2);
  matrix_d Bd(2, 2), Bd_tmp(2, 2);
  matrix_d Cd(2, 2);

  Ad << 2.0, 0.0, 5.0, 7.0;
  Bd << 12.0, 13.0, 15.0, 17.0;

  size_type i, j, k, l;
  for (i = 0; i < Bd.rows(); i++) {
    for (j = 0; j < Bd.cols(); j++) {
      matrix_v A(2, 2);
      matrix_v B(2, 2);
      matrix_v C;
      VEC g;

      for (k = 0; k < 4; k++) {
        A(k) = Ad(k);
        B(k) = Bd(k);
      }

      C = mdivide_left_tri<Eigen::Lower>(A, B);
      AVEC x = createAVEC(A(0, 0), A(1, 0), A(0, 1), A(1, 1), B(0, 0), B(1, 0),
                          B(0, 1), B(1, 1));
      C(i, j).grad(x, g);

      for (l = 0; l < Ad_tmp.cols(); l++) {
        for (k = 0; k < Ad_tmp.rows(); k++) {
          if (k >= l) {
            Ad_tmp.setZero();
            Ad_tmp(k, l) = 1.0;
            Cd = -mdivide_left_tri<Eigen::Lower>(
                Ad, multiply(Ad_tmp, mdivide_left_tri<Eigen::Lower>(Ad, Bd)));
            EXPECT_NEAR(Cd(i, j), g[k + l * Ad_tmp.rows()], 1.0E-12);
          } else {
            EXPECT_NEAR(0.0, g[k + l * Ad_tmp.rows()], 1.0E-12);
          }
        }
      }
      for (k = 0; k < 4; k++) {
        Bd_tmp.setZero();
        Bd_tmp(k) = 1.0;
        Cd = mdivide_left_tri<Eigen::Lower>(Ad, Bd_tmp);
        EXPECT_NEAR(Cd(i, j), g[4 + k], 1.0E-12);
      }
    }
  }
}

TEST(AgradRevMatrix, mdivide_left_tri_lower_grad_dv) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;

  matrix_d Ad(2, 2), Ad_tmp(2, 2);
  matrix_d Bd(2, 2), Bd_tmp(2, 2);
  matrix_d Cd(2, 2);

  Ad << 2.0, 0.0, 5.0, 7.0;
  Bd << 12.0, 13.0, 15.0, 17.0;

  size_type i, j, k;
  for (i = 0; i < Bd.rows(); i++) {
    for (j = 0; j < Bd.cols(); j++) {
      matrix_v B(2, 2);
      matrix_v C;
      VEC g;

      for (k = 0; k < 4; k++) {
        B(k) = Bd(k);
      }

      C = mdivide_left_tri<Eigen::Lower>(Ad, B);
      AVEC x = createAVEC(B(0, 0), B(1, 0), B(0, 1), B(1, 1));
      C(i, j).grad(x, g);

      for (k = 0; k < 4; k++) {
        Bd_tmp.setZero();
        Bd_tmp(k) = 1.0;
        Cd = mdivide_left_tri<Eigen::Lower>(Ad, Bd_tmp);
        EXPECT_NEAR(Cd(i, j), g[k], 1.0E-12);
      }
    }
  }
}

TEST(AgradRevMatrix, mdivide_left_tri_lower_grad_vd) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;

  matrix_d Ad(2, 2), Ad_tmp(2, 2);
  matrix_d Bd(2, 2), Bd_tmp(2, 2);
  matrix_d Cd(2, 2);

  Ad << 2.0, 0.0, 5.0, 7.0;
  Bd << 12.0, 13.0, 15.0, 17.0;

  size_type i, j, k, l;
  for (i = 0; i < Bd.rows(); i++) {
    for (j = 0; j < Bd.cols(); j++) {
      matrix_v A(2, 2);
      matrix_v C;
      VEC g;

      for (k = 0; k < 4; k++) {
        A(k) = Ad(k);
      }

      C = mdivide_left_tri<Eigen::Lower>(A, Bd);
      AVEC x = createAVEC(A(0, 0), A(1, 0), A(0, 1), A(1, 1));
      C(i, j).grad(x, g);

      for (l = 0; l < Ad_tmp.cols(); l++) {
        for (k = 0; k < Ad_tmp.rows(); k++) {
          if (k >= l) {
            Ad_tmp.setZero();
            Ad_tmp(k, l) = 1.0;
            Cd = -mdivide_left_tri<Eigen::Lower>(
                Ad, multiply(Ad_tmp, mdivide_left_tri<Eigen::Lower>(Ad, Bd)));
            EXPECT_NEAR(Cd(i, j), g[k + l * Ad_tmp.rows()], 1.0E-12);
          } else {
            EXPECT_NEAR(0.0, g[k + l * Ad_tmp.rows()], 1.0E-12);
          }
        }
      }
    }
  }
}
TEST(AgradRevMatrix, mdivide_left_tri_upper_grad_vv) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;

  matrix_d Ad(2, 2), Ad_tmp(2, 2);
  matrix_d Bd(2, 2), Bd_tmp(2, 2);
  matrix_d Cd(2, 2);

  Ad << 2.0, 3.0, 0.0, 7.0;
  Bd << 12.0, 13.0, 15.0, 17.0;

  size_type i, j, k, l;
  for (i = 0; i < Bd.rows(); i++) {
    for (j = 0; j < Bd.cols(); j++) {
      matrix_v A(2, 2);
      matrix_v B(2, 2);
      matrix_v C;
      VEC g;

      for (k = 0; k < 4; k++) {
        A(k) = Ad(k);
        B(k) = Bd(k);
      }

      C = mdivide_left_tri<Eigen::Upper>(A, B);
      AVEC x = createAVEC(A(0, 0), A(1, 0), A(0, 1), A(1, 1), B(0, 0), B(1, 0),
                          B(0, 1), B(1, 1));
      C(i, j).grad(x, g);

      for (l = 0; l < Ad_tmp.cols(); l++) {
        for (k = 0; k < Ad_tmp.rows(); k++) {
          if (k <= l) {
            Ad_tmp.setZero();
            Ad_tmp(k, l) = 1.0;
            Cd = -mdivide_left_tri<Eigen::Upper>(
                Ad, multiply(Ad_tmp, mdivide_left_tri<Eigen::Upper>(Ad, Bd)));
            EXPECT_NEAR(Cd(i, j), g[k + l * Ad_tmp.rows()], 1.0E-12);
          } else {
            EXPECT_NEAR(0.0, g[k + l * Ad_tmp.rows()], 1.0E-12);
          }
        }
      }
      for (k = 0; k < 4; k++) {
        Bd_tmp.setZero();
        Bd_tmp(k) = 1.0;
        Cd = mdivide_left_tri<Eigen::Upper>(Ad, Bd_tmp);
        EXPECT_NEAR(Cd(i, j), g[4 + k], 1.0E-12);
      }
    }
  }
}

TEST(AgradRevMatrix, mdivide_left_tri_upper_grad_dv) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;

  matrix_d Ad(2, 2), Ad_tmp(2, 2);
  matrix_d Bd(2, 2), Bd_tmp(2, 2);
  matrix_d Cd(2, 2);

  Ad << 2.0, 3.0, 0.0, 7.0;
  Bd << 12.0, 13.0, 15.0, 17.0;

  size_type i, j, k;
  for (i = 0; i < Bd.rows(); i++) {
    for (j = 0; j < Bd.cols(); j++) {
      matrix_v B(2, 2);
      matrix_v C;
      VEC g;

      for (k = 0; k < 4; k++) {
        B(k) = Bd(k);
      }

      C = mdivide_left_tri<Eigen::Upper>(Ad, B);
      AVEC x = createAVEC(B(0, 0), B(1, 0), B(0, 1), B(1, 1));
      C(i, j).grad(x, g);

      for (k = 0; k < 4; k++) {
        Bd_tmp.setZero();
        Bd_tmp(k) = 1.0;
        Cd = mdivide_left_tri<Eigen::Upper>(Ad, Bd_tmp);
        EXPECT_NEAR(Cd(i, j), g[k], 1.0E-12);
      }
    }
  }
}

TEST(AgradRevMatrix, mdivide_left_tri_upper_grad_vd) {
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;

  matrix_d Ad(2, 2), Ad_tmp(2, 2);
  matrix_d Bd(2, 2), Bd_tmp(2, 2);
  matrix_d Cd(2, 2);

  Ad << 2.0, 3.0, 0.0, 7.0;
  Bd << 12.0, 13.0, 15.0, 17.0;

  size_type i, j, k, l;
  for (i = 0; i < Bd.rows(); i++) {
    for (j = 0; j < Bd.cols(); j++) {
      matrix_v A(2, 2);
      matrix_v C;
      VEC g;

      for (k = 0; k < 4; k++) {
        A(k) = Ad(k);
      }

      C = mdivide_left_tri<Eigen::Upper>(A, Bd);
      AVEC x = createAVEC(A(0, 0), A(1, 0), A(0, 1), A(1, 1));
      C(i, j).grad(x, g);

      for (l = 0; l < Ad_tmp.cols(); l++) {
        for (k = 0; k < Ad_tmp.rows(); k++) {
          if (k <= l) {
            Ad_tmp.setZero();
            Ad_tmp(k, l) = 1.0;
            Cd = -mdivide_left_tri<Eigen::Upper>(
                Ad, multiply(Ad_tmp, mdivide_left_tri<Eigen::Upper>(Ad, Bd)));
            EXPECT_NEAR(Cd(i, j), g[k + l * Ad_tmp.rows()], 1.0E-12);
          } else {
            EXPECT_NEAR(0.0, g[k + l * Ad_tmp.rows()], 1.0E-12);
          }
        }
      }
    }
  }
}

TEST(AgradRevMatrix, check_varis_on_stack) {
  using stan::math::value_of;
  stan::math::matrix_v A(2, 2);
  A << 2.0, 0.0, 5.0, 7.0;

  test::check_varis_on_stack(stan::math::mdivide_left_tri<Eigen::Lower>(A, A));
  test::check_varis_on_stack(
      stan::math::mdivide_left_tri<Eigen::Lower>(A, value_of(A)));
  test::check_varis_on_stack(
      stan::math::mdivide_left_tri<Eigen::Lower>(value_of(A), A));
}

#ifdef STAN_OPENCL
#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(stan::math::value_of(A(i)), stan::math::value_of(B(i)), DELTA);

boost::random::mt19937 rng;
#define MDIVIDE_OPENCL_OVERRIDE 0
#define MDIVIDE_CPU_OVERRIDE INT_MAX
TEST(AgradRevMatrix, mdivide_left_tri_val_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;
  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  int size = 111;
  matrix_v Av(size, size);
  matrix_d Ad(size, size);
  matrix_v I, I_cl;
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Ad(i, j) = stan::math::uniform_rng(-5, 5, rng);
      Av(i, j) = Ad(i, j);
    }
    Ad(i, i) = 20.0;
    Av(i, i) = Ad(i, i);
    for (int j = i + 1; j < size; j++) {
      Ad(i, j) = 0.0;
      Av(i, j) = Ad(i, j);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  I_cl = mdivide_left_tri<Eigen::Lower>(Av, Av);
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  I = mdivide_left_tri<Eigen::Lower>(Av, Av);
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  I_cl = mdivide_left_tri<Eigen::Lower>(Av, Ad);
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  I = mdivide_left_tri<Eigen::Lower>(Av, Ad);
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  I_cl = mdivide_left_tri<Eigen::Lower>(Ad, Av);
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  I = mdivide_left_tri<Eigen::Lower>(Ad, Av);
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  matrix_v Av_inv_cl = mdivide_left_tri<Eigen::Lower>(Av);
  I_cl = Av * Av_inv_cl;
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  matrix_v Av_inv = mdivide_left_tri<Eigen::Lower>(Av);
  I = Av * Av_inv;
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Ad(i, j) = 0.0;
      Av(i, j) = Ad(i, j);
    }
    Ad(i, i) = 20.0;
    Av(i, i) = Ad(i, i);
    for (int j = i + 1; j < size; j++) {
      Ad(i, j) = stan::math::uniform_rng(-5, 5, rng);
      Av(i, j) = Ad(i, j);
    }
  }

  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  I_cl = mdivide_left_tri<Eigen::Upper>(Av, Av);
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  I = mdivide_left_tri<Eigen::Upper>(Av, Av);
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  I_cl = mdivide_left_tri<Eigen::Upper>(Av, Ad);
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  I = mdivide_left_tri<Eigen::Upper>(Av, Ad);
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  I_cl = mdivide_left_tri<Eigen::Upper>(Ad, Av);
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  I = mdivide_left_tri<Eigen::Upper>(Ad, Av);
  EXPECT_MATRIX_NEAR(I, I_cl, 1.0E-12);

  // restore default value
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}

TEST(AgradRevMatrix, mdivide_left_tri_lower_grad_vv_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;

  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;
  int size = 256;

  matrix_v Av(size, size);
  matrix_v Bv(size, size);
  matrix_v Cv(size, size);
  matrix_v Cv_cl(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Av(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    Av(i, i) = 20.0;
    for (int j = i + 1; j < size; j++) {
      Av(i, j) = 0.0;
    }
    for (int j = 0; j < size; j++) {
      Bv(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  Cv_cl = mdivide_left_tri<Eigen::Lower>(Av, Bv);
  Cv_cl(0, 0).grad();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  Cv = mdivide_left_tri<Eigen::Lower>(Av, Bv);
  Cv(0, 0).grad();
  EXPECT_MATRIX_NEAR(Cv, Cv_cl, 1.0E-12);
  EXPECT_MATRIX_NEAR(Cv.adj(), Cv_cl.adj(), 1.0E-12);
  stan::math::recover_memory();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}

TEST(AgradRevMatrix, mdivide_left_tri_lower_grad_dv_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;

  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;
  int size = 213;

  matrix_d Ad(size, size);
  matrix_v Bv(size, size);
  matrix_v Cv(size, size);
  matrix_v Cv_cl(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Ad(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    Ad(i, i) = 20.0;
    for (int j = i + 1; j < size; j++) {
      Ad(i, j) = 0.0;
    }
    for (int j = 0; j < size; j++) {
      Bv(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  Cv_cl = mdivide_left_tri<Eigen::Lower>(Ad, Bv);
  Cv_cl(0, 0).grad();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  Cv = mdivide_left_tri<Eigen::Lower>(Ad, Bv);
  Cv(0, 0).grad();
  EXPECT_MATRIX_NEAR(Cv, Cv_cl, 1.0E-12);
  EXPECT_MATRIX_NEAR(Cv.adj(), Cv_cl.adj(), 1.0E-12);
  stan::math::recover_memory();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}

TEST(AgradRevMatrix, mdivide_left_tri_lower_grad_vd_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;

  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;
  int size = 250;

  matrix_d Av(size, size);
  matrix_v Bd(size, size);
  matrix_v Cv(size, size);
  matrix_v Cv_cl(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Av(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    Av(i, i) = 20.0;
    for (int j = i + 1; j < size; j++) {
      Av(i, j) = 0.0;
    }
    for (int j = 0; j < size; j++) {
      Bd(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  Cv_cl = mdivide_left_tri<Eigen::Lower>(Av, Bd);
  Cv_cl(0, 0).grad();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  Cv = mdivide_left_tri<Eigen::Lower>(Av, Bd);
  Cv(0, 0).grad();
  EXPECT_MATRIX_NEAR(Cv, Cv_cl, 1.0E-12);
  EXPECT_MATRIX_NEAR(Cv.adj(), Cv_cl.adj(), 1.0E-12);
  stan::math::recover_memory();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}

TEST(AgradRevMatrix, mdivide_left_tri_upper_grad_vv_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;

  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;
  int size = 299;

  matrix_v Av(size, size);
  matrix_v Bv(size, size);
  matrix_v Cv(size, size);
  matrix_v Cv_cl(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Av(i, j) = 0.0;
    }
    Av(i, i) = 20.0;
    for (int j = i + 1; j < size; j++) {
      Av(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    for (int j = 0; j < size; j++) {
      Bv(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  Cv_cl = mdivide_left_tri<Eigen::Upper>(Av, Bv);
  Cv_cl(0, 0).grad();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  Cv = mdivide_left_tri<Eigen::Upper>(Av, Bv);
  Cv(0, 0).grad();
  EXPECT_MATRIX_NEAR(Cv, Cv_cl, 1.0E-12);
  EXPECT_MATRIX_NEAR(Cv.adj(), Cv_cl.adj(), 1.0E-12);
  stan::math::recover_memory();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}

TEST(AgradRevMatrix, mdivide_left_tri_upper_grad_dv_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;

  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;
  int size = 301;

  matrix_d Ad(size, size);
  matrix_v Bv(size, size);
  matrix_v Cv(size, size);
  matrix_v Cv_cl(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Ad(i, j) = 0.0;
    }
    Ad(i, i) = 20.0;
    for (int j = i + 1; j < size; j++) {
      Ad(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    for (int j = 0; j < size; j++) {
      Bv(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  Cv_cl = mdivide_left_tri<Eigen::Upper>(Ad, Bv);
  Cv_cl(0, 0).grad();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  Cv = mdivide_left_tri<Eigen::Upper>(Ad, Bv);
  Cv(0, 0).grad();
  EXPECT_MATRIX_NEAR(Cv, Cv_cl, 1.0E-12);
  EXPECT_MATRIX_NEAR(Cv.adj(), Cv_cl.adj(), 1.0E-12);
  stan::math::recover_memory();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}

TEST(AgradRevMatrix, mdivide_left_tri_upper_grad_vd_cl) {
  int temp = stan::math::opencl_context.tuning_opts()
                 .tri_inverse_size_worth_transfer;

  using stan::math::matrix_d;
  using stan::math::matrix_v;
  using stan::math::mdivide_left_tri;
  using stan::math::multiply;
  int size = 300;

  matrix_d Av(size, size);
  matrix_v Bd(size, size);
  matrix_v Cv(size, size);
  matrix_v Cv_cl(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      Av(i, j) = 0.0;
    }
    Av(i, i) = 20.0;
    for (int j = i + 1; j < size; j++) {
      Av(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    for (int j = 0; j < size; j++) {
      Bd(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
  }
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_OPENCL_OVERRIDE;
  Cv_cl = mdivide_left_tri<Eigen::Upper>(Av, Bd);
  Cv_cl(0, 0).grad();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = MDIVIDE_CPU_OVERRIDE;
  Cv = mdivide_left_tri<Eigen::Upper>(Av, Bd);
  Cv(0, 0).grad();
  EXPECT_MATRIX_NEAR(Cv, Cv_cl, 1.0E-12);
  EXPECT_MATRIX_NEAR(Cv.adj(), Cv_cl.adj(), 1.0E-12);
  stan::math::recover_memory();
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = temp;
}
#endif


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/fold_left.hpp>
#include <boost/hana/fold_right.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/plus.hpp>
namespace hana = boost::hana;

static_assert(hana::fold_left(hana::make_pair(1, 3), 0, hana::plus) == 4, "");
static_assert(hana::fold_right(hana::make_pair(1, 3), 0, hana::minus) == -2, "");

int main() { }


#include <boost/random/hyperexponential_distribution.hpp>


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2021, Institut de Robotica i Informatica Industrial (CSIC-UPC)
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef EAGLE_MPC_SBFDDP_HPP_
#define EAGLE_MPC_SBFDDP_HPP_

#include <Eigen/Dense>

#include "crocoddyl/core/solver-base.hpp"
#include "crocoddyl/core/solvers/ddp.hpp"
#include "crocoddyl/core/solvers/fddp.hpp"
#include "crocoddyl/core/optctrl/shooting.hpp"
#include "crocoddyl/core/actuation/squashing/smooth-sat.hpp"
#include "crocoddyl/core/actuation/actuation-squashing.hpp"
#include "crocoddyl/core/activations/quadratic-barrier.hpp"
#include "crocoddyl/core/activations/weighted-quadratic-barrier.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/core/integrator/euler.hpp"
#include "crocoddyl/core/integrator/rk4.hpp"

#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "crocoddyl/multibody/actions/contact-fwddyn.hpp"

#include "eagle_mpc/factory/diff-action.hpp"
#include "eagle_mpc/utils/log.hpp"

namespace eagle_mpc
{
class SolverSbFDDP : public crocoddyl::SolverFDDP
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    explicit SolverSbFDDP(boost::shared_ptr<crocoddyl::ShootingProblem>         problem,
                          boost::shared_ptr<crocoddyl::SquashingModelSmoothSat> squashing_model);
    virtual ~SolverSbFDDP();

    virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs     = crocoddyl::DEFAULT_VECTOR,
                       const std::vector<Eigen::VectorXd>& init_us     = crocoddyl::DEFAULT_VECTOR,
                       const std::size_t                   maxiter     = 100,
                       const bool                          is_feasible = false,
                       const double                        regInit     = 1e-9) override;

    const std::vector<Eigen::VectorXd>& getSquashControls() const;

    void          set_convergence_init(const double& convergence_init);
    const double& get_convergence_init() const;

    private:
    void barrierInit();
    void squashingUpdate();
    void barrierUpdate();

    bool                   solveFDDP(const std::size_t& maxiter, const bool& is_feasible, const double& reginit);
    bool                   solveDDP(const std::size_t& maxiter, const bool& is_feasible, const double& reginit);
    const Eigen::Vector2d& expectedImprovementDDP();
    double                 tryStepDDP(const double& steplength = 1);
    void                   forwardPassDDP(const double& steplength);

    IntegratedActionModelTypes   getIntegratedModelType(boost::shared_ptr<crocoddyl::ActionModelAbstract> int_action);
    DifferentialActionModelTypes getDifferentialModelType(
        boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> diff_action);
    boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> getDifferentialModelFromIntegrated(
        boost::shared_ptr<crocoddyl::ActionModelAbstract> int_action);
    boost::shared_ptr<crocoddyl::CostModelSum> getCostsFromDifferentialModel(
        boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> diff_action);

    IntegratedActionModelTypes   getIntegratedDataType(boost::shared_ptr<crocoddyl::ActionDataAbstract> int_action);
    DifferentialActionModelTypes getDifferentialDataType(
        boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> diff_action);
    boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> getDifferentialDataFromIntegrated(
        boost::shared_ptr<crocoddyl::ActionDataAbstract> int_action);
    boost::shared_ptr<crocoddyl::ActuationDataAbstract> getActuationDataFromDifferential(
        boost::shared_ptr<crocoddyl::DifferentialActionDataAbstract> diff_action);

    void fillSquashedOutputs();

    boost::shared_ptr<crocoddyl::SquashingModelSmoothSat>                 squashing_model_;
    boost::shared_ptr<crocoddyl::ActuationSquashingModel>                 actuation_;
    boost::shared_ptr<crocoddyl::ActivationBounds>                        barrier_act_bounds_;
    boost::shared_ptr<crocoddyl::ActivationModelWeightedQuadraticBarrier> barrier_activation_;
    boost::shared_ptr<crocoddyl::ResidualModelControl>                    squash_barr_residual_;
    boost::shared_ptr<crocoddyl::CostModelResidual>                       squash_barr_cost_;
    boost::shared_ptr<crocoddyl::SolverDDP>                               ddp_;

    // Variables for downcasting
    // models
    boost::shared_ptr<crocoddyl::IntegratedActionModelEuler>                euler_;
    boost::shared_ptr<crocoddyl::IntegratedActionModelRK4>                  rk4_;
    boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics>    free_;
    boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> contact_;
    boost::shared_ptr<crocoddyl::CostModelSum>                              costs_;

    // datas
    boost::shared_ptr<crocoddyl::IntegratedActionDataEuler>                euler_d_;
    boost::shared_ptr<crocoddyl::IntegratedActionDataRK4>                  rk4_d_;
    boost::shared_ptr<crocoddyl::DifferentialActionDataFreeFwdDynamics>    free_d_;
    boost::shared_ptr<crocoddyl::DifferentialActionDataContactFwdDynamics> contact_d_;
    boost::shared_ptr<crocoddyl::ActuationSquashingData>                   actuation_squashing_d_;

    double smooth_;
    double smooth_init_;
    double smooth_mult_;

    Eigen::VectorXd barrier_quad_weights_;
    Eigen::VectorXd barrier_quad_weights_aux_;
    double          barrier_weight_;

    double convergence_;
    double convergence_init_;
    double convergence_stop_;
    double convergence_mult_;

    std::size_t max_iters_;
    double      reg_init_;
    std::size_t total_iters_;

    std::vector<Eigen::VectorXd> us_squash_;  //!< Control trajectory

    double th_acceptnegstep_;  // FDDP solver
};

}  // namespace eagle_mpc

#endif


//
// Copyright 2013 Christian Henning
//
// 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
//
#ifndef BOOST_GIL_IO_TEST_MANDEL_HPP
#define BOOST_GIL_IO_TEST_MANDEL_HPP

#include <boost/gil.hpp>

using namespace std;
using namespace boost;
using namespace gil;

// Models a Unary Function
template <typename P>   // Models PixelValueConcept
struct mandelbrot_fn
{
    using point_t = boost::gil::point_t;
    using const_t = mandelbrot_fn;
    using value_type = P;
    using reference = value_type;
    using const_reference = value_type;
    using argument_type = point_t;
    using result_type = reference;
    static constexpr bool is_mutable = false;

    value_type                    _in_color,_out_color;
    point_t                       _img_size;
    static const int MAX_ITER=100;        // max number of iterations

    mandelbrot_fn() {}
    mandelbrot_fn(const point_t& sz, const value_type& in_color, const value_type& out_color) : _in_color(in_color), _out_color(out_color), _img_size(sz) {}

    std::ptrdiff_t width()  { return _img_size.x; }
    std::ptrdiff_t height() { return _img_size.y; }

    result_type operator()(const point_t& p) const {
        // normalize the coords to (-2..1, -1.5..1.5)
        // (actually make y -1.0..2 so it is asymmetric, so we can verify some view factory methods)
        double t=get_num_iter(point<double>(p.x/(double)_img_size.x*3-2, p.y/(double)_img_size.y*3-1.0f));//1.5f));
        t=pow(t,0.2);

        value_type ret;
        for (int k=0; k<num_channels<P>::value; ++k)
            ret[k]=(typename channel_type<P>::type)(_in_color[k]*t + _out_color[k]*(1-t));
        return ret;
    }

private:
    double get_num_iter(const point<double>& p) const {
        point<double> Z(0,0);
        for (int i=0; i<MAX_ITER; ++i) {
            Z = point<double>(Z.x*Z.x - Z.y*Z.y + p.x, 2*Z.x*Z.y + p.y);
            if (Z.x*Z.x + Z.y*Z.y > 4)
                return i/(double)MAX_ITER;
        }
        return 0;
    }
};

template< typename Pixel >
struct mandel_view
{
    using deref_t = mandelbrot_fn<Pixel>;
    using locator_t= virtual_2d_locator<deref_t, false>;
    using my_virt_view_t = image_view<locator_t>;
    using type = my_virt_view_t;
};

template< typename Pixel >
typename mandel_view< Pixel >::type create_mandel_view( unsigned int width
                                                      , unsigned int height
                                                      , const Pixel& in
                                                      , const Pixel& out
                                                      )
{
    using view_t = typename mandel_view<Pixel>::type;
    using deref_t = typename mandel_view<Pixel>::deref_t;
    using locator_t = typename mandel_view<Pixel>::locator_t;

    point_t dims( width, height );
    return view_t( dims
                 , locator_t( point_t( 0, 0 )
                            , point_t( 1, 1 )
                            , deref_t( dims
                                     , in
                                     , out
                                     )
                            )
                 );
}

#endif // BOOST_GIL_IO_TEST_MANDEL_HPP


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <CGAL/Polyhedron_3.h>

#include <CGAL/boost/graph/graph_traits_Linear_cell_complex_for_combinatorial_map.h>

#include <CGAL/boost/graph/copy_face_graph.h>

#include <iostream>
#include <fstream>
#include <iterator>
#include <boost/unordered_map.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef CGAL::Polyhedron_3<Kernel>                       Source;
typedef boost::graph_traits<Source>::vertex_descriptor   sm_vertex_descriptor;
typedef boost::graph_traits<Source>::halfedge_descriptor sm_halfedge_descriptor;
typedef boost::graph_traits<Source>::face_descriptor     sm_face_descriptor;

typedef CGAL::Exact_predicates_exact_constructions_kernel Other_kernel;
typedef Other_kernel::Point_3                             Point;
typedef CGAL::Linear_cell_complex_traits<3, Other_kernel> LCC_traits;
typedef CGAL::Linear_cell_complex_for_bgl_combinatorial_map_helper
         <2, 3, LCC_traits>::type LCC;

int main(int argc, char* argv[])
{
  Source S;

  std::ifstream in((argc>1)?argv[1]:"cube.off");
  in >> S;

  // Note that the vertex_point property of the Source and Target1
  // come from different kernels.
  typedef LCC Target1;
  Target1 T1;
  {
    CGAL::copy_face_graph(S, T1);
    CGAL::write_off("lcc.off", T1);
  }

  S.clear();
  {
    typedef boost::graph_traits<Target1>::vertex_descriptor   source_vertex_descriptor;
    typedef boost::graph_traits<Target1>::halfedge_descriptor source_halfedge_descriptor;

    typedef boost::graph_traits<Source>::vertex_descriptor   tm_vertex_descriptor;
    typedef boost::graph_traits<Source>::halfedge_descriptor tm_halfedge_descriptor;

    boost::unordered_map<source_vertex_descriptor, tm_vertex_descriptor> v2v;
    boost::unordered_map<source_halfedge_descriptor, tm_halfedge_descriptor> h2h;

    CGAL::copy_face_graph(T1, S, CGAL::parameters::vertex_to_vertex_output_iterator(std::inserter(v2v, v2v.end()))
                                                  .halfedge_to_halfedge_output_iterator(std::inserter(h2h, h2h.end())));
    std::ofstream out("reverse.off");
    out << S;
  }
  return 0;
}