#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>

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

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"

#ifndef SIGMA
#define SIGMA (1.0)
#endif
#define E_MAX (2.0)

/*
template <unsigned int i>
struct phi
{
  static std::complex<double>
  operator () ( std::complex<double> const & z ) {
    static std::valarray<std::complex<double>> coeff(i);
    coeff[0] = 1.;

    for ( unsigned int k=1 ; k<coeff.size() ; ++k ) {
      coeff[k] = coeff[k-1] * z / (double(k));
    }

    return (std::exp(z) - std::accumulate( std::begin(coeff) , std::end(coeff) , std::complex<double>(0.,0.) ))/(std::pow(z,i));
  }
};
*/
template <unsigned int i>
std::complex<double>
phi ( std::complex<double> const & _z )
{
  std::valarray<std::complex<double>> coeff(i);
  coeff[0] = 1.;

  std::complex<double> z = _z;
  if ( _z == 0. ) { z = std::complex<double>(1.,0.); }

  for ( unsigned int k=1 ; k<coeff.size() ; ++k ) {
    coeff[k] = coeff[k-1] * z / (double(k));
  }
  //std::copy(std::begin(coeff),std::end(coeff),std::ostream_iterator<std::complex<double>>(std::cout," . "));
  //std::cout << std::endl;

  if ( z != 0. ) {
    return (std::exp(z) - std::accumulate( std::begin(coeff) , std::end(coeff) , std::complex<double>(0.,0.) ))/(std::pow(z,i));
  }
  return coeff[i-1];
}

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)

auto
maxwellian ( double rho , double u , double T ) {
  //std::cout << rho << " " << u << " " << T << std::endl;
  //std::cout << rho/(std::sqrt(2.*math::pi<double>()*T)) << std::endl;
  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,char**)
{
	std::size_t Nx = 135, Nv = 256 , Nb_iter=10;
	field<double,1> f(boost::extents[Nv][Nx]);

	f.range.v_min = -8.; f.range.v_max = 8.;
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;

  const double Kx = 0.5;
	//f.range.x_min = 0.; f.range.x_max = 20.*math::pi<double>();
  f.range.x_min = 0.; f.range.x_max = 2./Kx*math::pi<double>(); //10.0*math::pi<double>();
  //f.range.x_min = -8.; f.range.x_max = 8.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;

  //field<double,1> f_sol = f;
  //field<double,1> f_ini = f;

	ublas::vector<double> v (Nv,0.);
  ublas::vector<double> E (Nx,0.),rho(Nx);
  for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  //for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = -Xi(i); }

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

  double np = 0.9 , nb = 0.2 , ui = 4.5;
  double alpha = 0.1;
  double Tc = 0.01; //, u = 4.5;
  //auto Mmu = maxwellian(1.,-u,1.) , Mpu = maxwellian(1.,u,1.) , M0c = maxwellian(1.,0.,Tc);

  ui = 2.;
  alpha = 0.2;
  Tc = 0.01;
  auto landau_M = maxwellian(1.,0.,1.);
  auto db_M1 = maxwellian(0.5,-ui,1.) , db_M2 = maxwellian(0.5,ui,1.);
  auto bot_M1 = maxwellian(1.-alpha,0.,1.) , bot_M2 = maxwellian(alpha,ui,0.25);
  auto tb_MC = maxwellian(1.-alpha,0.,Tc) , tb_M1 = maxwellian(0.5*alpha,ui,1.) , tb_M2 = maxwellian(0.5*alpha,-ui,1.);
  auto v10_MC = maxwellian(1.-alpha,0,Tc) , v10_Mh = maxwellian(alpha,0.,1.);

  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] = ((1.-alpha)*M0c(Xi(i),Vk(k)) + 0.5*alpha*( Mpu(Xi(i),Vk(k)) + Mmu(Xi(i),Vk(k)) ))*(1.+0.01*std::cos(0.5*Xi(i)));

      //// landau damping : Kx=0.5
      //f[k][i] = landau_M(Xi(i),Vk(k))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// double beam Kx=0.2, ui=2.4 ou Kx=0.2, ui=4.5
      //f[k][i] = (db_M1(Xi(i),Vk(k))+db_M2(Xi(i),Vk(k)))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// bot Kx=0.5 , alpha=0.2 , ui=4.5
      //f[k][i] = (bot_M1(Xi(i),Vk(k)) + bot_M2(Xi(i),Vk(k)))*(1.+0.01*std::cos(Kx*Xi(i)));
      //// tb Kx=0.5 , ui=4. , alpha=0.2 , Tc=0.01
      f[k][i] = tb_MC(Xi(i),Vk(k)) + (tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
      //// v10 Kx=0.5 , alpha=0.2
      //f[k][i] = v10_MC(Xi(i),Vk(k)) + ( std::pow(Vk(k),10)*v10_Mh(Xi(i),Vk(k))/945. )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
  }
  f.write("vphl/kin/init_tb.dat");

  poisson<double> poisson_solver(Nx,l);
  rho = f.density();
  E = poisson_solver(rho);

  //double Tf = 60.;//2*math::pi<double>();
  const double Tf = 200.;
  int i_t=0;

  double current_time = 0.;
  // SIGMA is the CFL number
  double dt = f.step.dv;//std::min( 0.1 , SIGMA*f.step.dv/std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << f.range.v_min << "\n";
  std::cout << "v_max: " << f.range.v_max << "\n";
  std::cout << "x_min: " << f.range.x_min << "\n";
  std::cout << "x_max: " << f.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << f.step.dx << "\n";
  std::cout << "dv: " << f.step.dv << "\n";
  std::cout << "Tf: " << Tf << "\n";
  std::cout << "f_0: " << "\"tb\"" << "\n";
  std::cout << std::endl;

  std::ofstream info("info.yaml");

  info << "Nx: " << Nx << "\n";
  info << "Nv: " << Nv << "\n";
  info << "v_min: " << f.range.v_min << "\n";
  info << "v_max: " << f.range.v_max << "\n";
  info << "x_min: " << f.range.x_min << "\n";
  info << "x_max: " << f.range.x_max << "\n";
  info << "dt: " << dt << "\n";
  info << "dx: " << f.step.dx << "\n";
  info << "dv: " << f.step.dv << "\n";
  info << "Tf: " << Tf << "\n";
  info << "f_0: " << "\"tb\"" << "\n";
  info << std::endl;
  info.close();

  std::vector<double> ee;   ee.reserve(int(std::ceil(Tf/dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(Tf/dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(Tf/dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(Tf/dt))+1);

  // space scheme
  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
  lawson::RK33<poisson<double>> rk(Nx,Nv,l,f.shape(),v,kx,weno);

  rk.E = rk.poisson_solver(f.density());
  Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
  double electric_energy = 0.;
  for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
  ee.push_back( std::sqrt(electric_energy) );
  H.push_back( energy(f,rk.E) );
  times.push_back( 0. );

  while (  current_time < Tf ) {
    std::cout<<" ["<<std::setw(5)<<i_t<<"] "<< current_time <<"\r"<<std::flush;

    f = rk(f,dt);

    // end of time loop

    // MONITORING
    rk.E = rk.poisson_solver(f.density());
    Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    electric_energy = 0.;
    for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
    H.push_back( energy(f,rk.E) );

    dt = std::min( 0.1 , SIGMA*f.step.dv/Emax[i_t] );

    // increment time
    ++i_t;
    current_time += dt;
    times.push_back( current_time );
	} // while (  i_t*dt < Tf )

//#define FOLDER "lukas/landau/"
#define FOLDER "vphl/kin/"
#define SPACE_SCHEME "weno"

  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt <<std::endl;

  std::stringstream ss; ss << FOLDER << "vp_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  f.write("vphl/kin/vp_tb.dat");

  ss.str(std::string());
  std::ofstream of;
  std::size_t count = 0;
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };
  //ss << FOLDER << "ee_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  of.open("vphl/kin/ee_tb.dat"); ss.str(std::string());
  for ( auto i=0; i<ee.size() ; ++i ) {
    of << times[i] <<" " << ee[i] << "\n";
  }
  of.close();
  //ss << FOLDER << "H_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  of.open("vphl/kin/H_tb.dat"); ss.str(std::string());
  for ( auto i=0; i<H.size() ; ++i ) {
    of << times[i] <<" " << (H[i]-H[0])/std::abs(H[0]) << "\n";
  }

  //double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  //std::cout << dt << " " << std::abs((h-H[0])/std::abs(H[0])) << "\n";

  of.close();
  //ss << FOLDER << "Emax_" << rk.label << "_" << SPACE_SCHEME << ".dat";
  of.open("vphl/kin/Emax_tb.dat"); ss.str(std::string());
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

	return 0;
}


/*
 * LibHEOM: Copyright (c) Tatsushi Ikeda
 * This library is distributed under BSD 3-Clause License.
 * See LINCENSE.txt for licence.
 *------------------------------------------------------------------------*/

// Note: In order to avoid compile errors caused by the two phase name
//       lookup rule regarding template class, all class variables in
//       this source have this-> modifier.

#include "redfield.h"

#include <Eigen/SparseLU>

#include "const.h"
// #include "mkl_wrapper.h"

namespace libheom
{

// calculate Fourier-Laplace transform of quantum correlation function.
// The exponent is +1j*omega*t.
template<typename T>
inline T correlation
/**/(redfield<T>& rf,
     int u,
     real_t<T> omega)
{
  if (rf.use_corr_func && rf.use_corr_func[u]) {
    return rf.corr_func[u](omega);
  } else {

    Eigen::SparseMatrix<T, Eigen::RowMajor> I(rf.len_gamma[u],rf.len_gamma[u]);
    I.setIdentity();
    Eigen::SparseLU<Eigen::SparseMatrix<T, Eigen::RowMajor>> solver;
    solver.compute(rf.gamma[u] - i_unit<T>()*omega*I);
    if (solver.info() != Eigen::Success) {
      std::cerr << "[Error] LU decomposition failed. " << std::endl;
      std::exit(1);
      return zero<T>();
    }
    return static_cast<T>(rf.sigma[u].transpose()*(rf.s[u] + i_unit<T>()*rf.a[u])*solver.solve(rf.phi_0[u])) + rf.S_delta[u];

  }
}

template<typename T>
void redfield<T>::init_aux_vars
/**/()
{
  qme<T>::init_aux_vars();

  this->Lambda.reset(new lil_matrix<T>[this->n_noise]);
  for (int s = 0; s < this->n_noise; ++s) {
    this->Lambda[s].set_shape(this->n_state, this->n_state);
    for (auto& V_ijv : this->V[s].data) {
      int i = V_ijv.first;
      for (auto& V_jv: V_ijv.second) {
        int j = V_jv.first;
        T val = V_jv.second;
        if (val != zero<T>()) {
          real_t<T> omega_ji;
          try {
            omega_ji = std::real(this->H.data[j][j] - this->H.data[i][i]);
          } catch (std::out_of_range&) {
            continue;
          }
          T corr = correlation(*this, s, omega_ji);
          this->Lambda[s].data[i][j] += val*corr;
        }
      }
    }
  }
}

//========================================================================
// redfield Module (Hilbert space expression)
//========================================================================

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_h<T, matrix_type, num_state>::init_aux_vars
/**/()
{
  redfield<T>::init_aux_vars();

  if (this->secular) {
    std::cerr << "[Error] Secular approximation is supported only in Liouville space." << std::endl;
    std::exit(1);
  }

  this->H.template dump<num_state>(this->H_impl);
  matrix_type<T, num_state> H_c_impl;
  this->H_c.template dump<num_state>(H_c_impl);
  this->H_impl += H_c_impl;

  this->V_impl.reset(new matrix_hilb[this->n_noise]);
  this->Lambda_impl.reset(new matrix_hilb[this->n_noise]);
  this->Lambda_dagger_impl.reset(new matrix_hilb[this->n_noise]);

  for (int s = 0; s < this->n_noise; ++s) {
    this->V[s].template dump<num_state>(this->V_impl[s]);
    this->Lambda[s].template dump<num_state>(this->Lambda_impl[s]);
    auto tmp = this->Lambda[s].hermite_conjugate();
    tmp.template dump<num_state>(this->Lambda_dagger_impl[s]);
  }
}

// template<typename T, template <typename> class matrix_type>
// void redfield_h<T, matrix_type>::ConstructCommutator(
//     lil_matrix<T>& x,
//     T coef_l,
//     T coef_r,
//     std::function<void(int)> callback,
//     int interval_callback) {
//   this->X_impl = static_cast<matrix_type<T>>(this->x);
//   this->coef_l_X = coef_l;
//   this->coef_r_X = coef_r;
// }

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_h<T, matrix_type, num_state>::calc_diff
/**/(ref<dense_vector<T,Eigen::Dynamic>>              drho_dt_raw,
     const ref<const dense_vector<T,Eigen::Dynamic>>& rho_raw,
     real_t<T> alpha,
     real_t<T> beta)
{
  auto n_state = this->n_state;

  auto rho     = Eigen::Map<const dense_matrix<T,num_state>>(rho_raw.data(),n_state,n_state);
  auto drho_dt = Eigen::Map<dense_matrix<T,num_state>>(drho_dt_raw.data(),n_state,n_state);
  dense_matrix<T,num_state> tmp(n_state,n_state);

  drho_dt *= beta;
  drho_dt.noalias() += -alpha*i_unit<T>()*this->H_impl*rho;
  drho_dt.noalias() += +alpha*i_unit<T>()*rho*this->H_impl;

  for (int s = 0; s < this->n_noise; ++s) {
    tmp.noalias()  = +i_unit<T>()*this->Lambda_impl[s]*rho;
    tmp.noalias() += -i_unit<T>()*rho*this->Lambda_dagger_impl[s];
    drho_dt.noalias() += alpha*i_unit<T>()*this->V_impl[s]*tmp;
    drho_dt.noalias() -= alpha*i_unit<T>()*tmp*this->V_impl[s];
  }
}

// template<typename T, template <typename, int> class matrix_type, int num_state>
// void redfield_h<T, matrix_type, num_state>::ApplyCommutator(T* rho_raw) {
//   DenseMatrixWrapper<T> rho(this->n_state, this->n_state, rho_raw);
//   DenseMatrixWrapper<T> sub(this->n_state, this->n_state, this->sub_vector.data());

//   gemm(this->coef_l_X, this->X_impl, rho, zero<T>(), sub);
//   gemm(this->coef_r_X, rho, this->X_impl, one <T>(), sub);
// }

//========================================================================
// redfield Module (Liouville space expression)
//========================================================================

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_l<T, matrix_type, num_state>::init_aux_vars
/**/()
{
  redfield<T>::init_aux_vars();

  this->n_state_liou = this->n_state*this->n_state;

  this->L.set_shape(this->n_state_liou, this->n_state_liou);
  kron_identity_right(+i_unit<T>(), this->H, zero<T>(), this->L);
  kron_identity_left (-i_unit<T>(), this->H, one<T>(), this->L);

  this->Phi.  reset(new lil_matrix<T>[this->n_noise]);
  this->Theta.reset(new lil_matrix<T>[this->n_noise]);

  for (int s = 0; s < this->n_noise; ++s){
    this->Phi[s].set_shape(this->n_state_liou, this->n_state_liou);
    kron_identity_right(+i_unit<T>(), this->V[s], zero<T>(), this->Phi[s]);
    kron_identity_left (-i_unit<T>(), this->V[s], one<T>(),  this->Phi[s]);

    this->Theta[s].set_shape(this->n_state_liou, this->n_state_liou);
    kron_identity_right(+i_unit<T>(), this->Lambda[s],
                        zero<T>(), this->Theta[s]);
    kron_identity_left (-i_unit<T>(), this->Lambda[s].hermite_conjugate(),
                        one<T>(),  this->Theta[s]);
  }

  this->R_redfield.set_shape(this->n_state_liou, this->n_state_liou);
  kron_identity_right(+i_unit<T>(), this->H_c, zero<T>(), this->R_redfield);
  kron_identity_left (-i_unit<T>(), this->H_c, one<T>(),  this->R_redfield);
  for (int s = 0; s < this->n_noise; ++s) {
    gemm(-one<T>(), this->Phi[s], this->Theta[s], one<T>(), this->R_redfield);
  }
  if (this->secular) {
    dense_matrix<T,num_state_liou> i_omega;
    this->L.optimize();
    this->L.template dump<num_state_liou>(i_omega);
    this->R_redfield.optimize();
    for (auto& ijv : this->R_redfield.data) {
      int i = ijv.first;
      for (auto& jv: ijv.second) {
        int j = jv.first;
        if (abs(i_omega(i,i) - i_omega(j,j))
            >= std::numeric_limits<typename T::value_type>::epsilon()) {
          jv.second = zero<T>();
        }
      }
    }
    this->R_redfield.optimize();
  }
  axpy(one<T>(), this->L, this->R_redfield);
  this->R_redfield.optimize();

  R_redfield.template dump<num_state_liou>(this->R_redfield_impl);
}

// template<typename T, template <typename> class matrix_type>
// void redfield_l<T, matrix_type>::ConstructCommutator(
//     lil_matrix<T>& x,
//     T coef_l,
//     T coef_r,
//     std::function<void(int)> callback,
//     int interval_callback) {
//   this->X_impl = static_cast<matrix_type<T>>(this->x);
//   this->coef_l_X = coef_l;
//   this->coef_r_X = coef_r;
// }

template<typename T,
         template <typename, int> class matrix_type,
         int num_state>
void redfield_l<T, matrix_type, num_state>::calc_diff
/**/(ref<dense_vector<T,Eigen::Dynamic>>              drho_dt_raw,
     const ref<const dense_vector<T,Eigen::Dynamic>>& rho_raw,
     real_t<T> alpha,
     real_t<T> beta)
{
  auto n_state_liou   = this->n_state_liou;
  auto rho     = blk<T,num_state_liou,1>::value(rho_raw,    0,0,n_state_liou,1);
  auto drho_dt = blk<T,num_state_liou,1>::value(drho_dt_raw,0,0,n_state_liou,1);

  drho_dt *= beta;
  drho_dt.noalias() += -alpha*this->R_redfield_impl*rho;
}

// template<typename T, template <typename, int> class matrix_type, int num_state>
// void redfield_l<T, matrix_type, num_state>::ApplyCommutator(T* rho_raw) {
//   DenseMatrixWrapper<T> rho(this->n_state, this->n_state, rho_raw);
//   DenseMatrixWrapper<T> sub(this->n_state, this->n_state, this->sub_vector.data());

//   gemm(this->coef_l_X, this->X_impl, rho, zero<T>(), sub);
//   gemm(this->coef_r_X, rho, this->X_impl, one <T>(), sub);
// }

}

// Explicit instantiations
namespace libheom
{

#define DECLARE_EXPLICIT_INSTANTIATIONS(qme_type, T, matrix_type, num_state) \
  template void qme_type<T, matrix_type, num_state>::init_aux_vars();        \
  template void qme_type<T, matrix_type, num_state>::calc_diff(              \
      ref<dense_vector<T, Eigen::Dynamic>> drho_dt,                          \
      const ref<const dense_vector<T, Eigen::Dynamic>>& rho,                 \
      real_t<T> alpha, real_t<T> beta);
// template void qme_type<T, matrix_type>::ConstructCommutator(            \
//     lil_matrix<T>& x,                                                  \
//     T coef_l,                                                         \
//     T coef_r,                                                         \
//     std::function<void(int)> callback,                                \
//     int interval_callback);                                           \
// template void qme_type<T, matrix_type>::ApplyCommutator(ref<dense_vector<T>> rho);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  csr_matrix,   Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, csr_matrix,   Eigen::Dynamic);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  csr_matrix,   Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, dense_matrix, Eigen::Dynamic);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, csr_matrix,   Eigen::Dynamic);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  csr_matrix,   2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, csr_matrix,   2);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  csr_matrix,   2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, dense_matrix, 2);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, csr_matrix,   2);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex64,  csr_matrix,   3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_h, complex128, csr_matrix,   3);

DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex64,  csr_matrix,   3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, dense_matrix, 3);
DECLARE_EXPLICIT_INSTANTIATIONS(redfield_l, complex128, csr_matrix,   3);

}


/*

   Recon Code for Cartesian and Non-Cartesian Data
   (in process of reorganizing for template + 4D recons)

 */

#include "ArrayTemplates.cpp"
#include "tictoc.hpp"
#include <omp.h>
#include <armadillo>
#include <cmath>

using namespace std;
using namespace NDarray;

int dideal_recon_2D_CG(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	int max_iter);

int dideal_recon_2D_Gradient(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	float lambda_lowrank,
	int max_iter);

void lowrank_thresh( Array< complex<float>, 4> &image, float thresh);

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

	// Hard Coded
	int Nt = 20;
	int Nr = 6692*2;
	int Ny = 128;
	int Nx = 128;
	int Ns = 3;
	int max_iter = 50;
	float lambda_space= 0.000;
	float lambda_time = 0.000;
	float lambda_lowrank = 0.000;
	float *freqs = new float[16];
	freqs[0]=-42;
	freqs[1]=-653;
	freqs[2]=-462;
	freqs[3]=-302;

#define float_flag(name,val)    }else if(strcmp(name,argv[pos]) == 0){ pos++; val = atof(argv[pos]);
#define int_flag(name,val)    }else if(strcmp(name,argv[pos]) == 0){ pos++; val = atoi(argv[pos]);
	for(int pos=0; pos < argc; pos++){
		if (strcmp("-h", argv[pos] ) == 0) {

			int_flag("-Nt",Nt);
			int_flag("-Nr",Nr);
			int_flag("-Ny",Ny);
			int_flag("-Nx",Nx);
			int_flag("-Ns",Ns);
			int_flag("-max_iter",max_iter);
			float_flag("-lambda_space",lambda_space);
			float_flag("-lambda_time",lambda_time);
			float_flag("-lambda_lowrank",lambda_lowrank);
			float_flag("-f0",freqs[0]);
			float_flag("-f1",freqs[1]);
			float_flag("-f2",freqs[2]);
			float_flag("-f3",freqs[3]);
			float_flag("-f4",freqs[4]);
			float_flag("-f5",freqs[5]);
			float_flag("-f6",freqs[6]);
		}
	}

	cout << "Imaging Parameters" << endl;
	cout << "  Nx = " << Nx << endl;
	cout << "  Ny = " << Ny << endl;
	cout << "  Nt = " << Nt << endl;
	cout << "  Ns = " << Ns << endl;
	cout << "  Nr = " << Nr << endl;
	cout << "Recon Parameters" << endl;
	for(int sp = 0; sp <Ns; sp++){
	cout << "  f" << sp << " = " << freqs[sp] << endl;
	}
	cout << "  lambda_space = " << lambda_space << endl;
	cout << "  lambda_time = " << lambda_time << endl;
	cout << "  lambda_lowrank = " << lambda_lowrank << endl;
	cout << "  max_iter = " << max_iter << endl;

	// Allocate Memory and Copy Values
	cout << "Kx" << endl << flush;
	Array< float,2>kx(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(kx,"Kx.dat");
	kx*=(float)1.0 / (float)Nx;

	cout << "Ky" << endl << flush;
	Array< float,2>ky(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(ky,"Ky.dat");
	ky*=(float)1.0 / (float)Ny;

	cout << "Kt" << endl << flush;
	Array< float,2>kt(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(kt,"Kt.dat");

	cout << "Kdata" << endl << flush;
	Array< complex<float> ,2>kdata(Nr,Nt,ColumnMajorArray<2>());
	ArrayRead(kdata,"Kdata.dat");

	cout << "Fieldmap" << endl << flush;
	Array< float,3>fieldmap(Nx,Ny,Nt,ColumnMajorArray<3>());
	ArrayRead(fieldmap,"FieldMap.dat");

	if(lambda_lowrank == 0){
		dideal_recon_2D_CG( kx, ky, kdata, fieldmap, kt, freqs, Ns,lambda_space,lambda_time,max_iter);
	}else{
		dideal_recon_2D_Gradient( kx, ky, kdata, fieldmap, kt, freqs, Ns,lambda_space,lambda_time,lambda_lowrank,max_iter);
	}

	return(0);
}

// Generate Images from Kdata
void transpose_dideal(
	const Array< complex<float>,2 > &diff_data,
	Array< complex<float>,4 > &X,
	const Array< float, 2 > &kx,
	const Array< float, 2 > &ky,
	const Array< float, 2 > &kt,
	const Array< float, 3 > &fieldmap,
	float *freqs){

	int Ns = X.length(fourthDim);
	int Nt = diff_data.length(secondDim); // Time Frame
	int Nr = diff_data.length(firstDim);  // Readout position

	// Spatial Coordinates
	int Nx = fieldmap.length(firstDim);
	int Ny = fieldmap.length(secondDim);
	float cx = (float)Ny/2.0;
	float cy = (float)Nx/2.0;
	const float pic=-6.28318530718;

	X = 0;

	float fov = (float)(Nx*Nx)/(4.0);
	// Loop Kx/Ky
	tictoc T;
	T.tic();

#pragma omp parallel for schedule(static,1)
	for(int j=0; j<Ny; j++){

		// cout << "," << j << flush;
		float y = (float)j - cy;

		for(int i=0; i<Nx; i++){
			float x = (float)i - cx;
			float rad = x*x + y*y;
			if( rad > fov) continue;

			for(int species=0; species< Ns; species++){
				for(int t=0; t< Nt; t++){
					float Pf = freqs[species]+fieldmap(i,j,t);
					complex<float>temp(0.0,0.0);
					for(int rpos=0; rpos< Nr; rpos++){
						// Loop over image
						complex<float>basis = polar<float>((float)1.0, pic*(kx(rpos,t)*x + ky(rpos,t)*y + kt(rpos,t)*Pf));
						temp+= ( diff_data(rpos,t)*basis);
					}
					X(i,j,t,species)=temp;	// This takes all the time
				}// Nt
			}
		}
	}//Image Loop
	cout << "Transpose Took = " << T << endl;
}

// Generate Images from Kdata
void forward_dideal(
	Array< complex<float>,2 > &diff_data,
	const Array< complex<float>,4 > &X,
	const Array< float, 2 > &kx,
	const Array< float, 2 > &ky,
	const Array< float, 2 > &kt,
	const Array< float, 3 > &fieldmap,
	float *freqs){

	int Ns = X.length(fourthDim);
	int Nt = diff_data.length(secondDim); // Time Frame
	int Nr = diff_data.length(firstDim);  // Readout position

	// Spatial Coordinates
	int Nx = fieldmap.length(firstDim);
	int Ny = fieldmap.length(secondDim);
	float cx = (float)Ny/2.0;
	float cy = (float)Nx/2.0;
	const float pic= 6.28318530718;

	diff_data = 0;

	float fov = (float)(Nx*Nx)/(4.0);
	// Loop Kx/Ky

	tictoc T;
	T.tic();

#pragma omp parallel for
	for(int rpos=0; rpos< Nr; rpos++){
		for(int t=0; t< Nt; t++){

			// Position in k-t space
			float Kx = kx(rpos,t);
			float Ky = ky(rpos,t);
			float Kt = kt(rpos,t);

			// Loop over image
			for(int j=0; j<Ny; j++){
				float y = ((float)j - cy);
				float Py = y*Ky;
				for(int i=0; i<Nx; i++){
					float x =((float)i - cx);
					float Px = x*Kx;

					float rad = x*x + y*y;
					if( rad > fov) continue;
					for(int species=0; species< Ns; species++){
						complex<float>basis = polar<float>((float)1.0, pic*(Px + Py + Kt*( freqs[species]+fieldmap(i,j,t) )));
						diff_data(rpos,t) += (X(i,j,t,species)*basis);
					}
			}}//Image Loop

	}}// K-t  + Species Looop
	cout << "Forward Took = " << T << endl;
}

int dideal_recon_2D_CG(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	int max_iter){

	// Inputs
	//   kx      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   ky      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   kdata   	[Nr,Nt ]   		  = Matrix with Nt readouts of length Nr x Coils
	//   fieldmap	[ResX,ResY,Nt]    = Fieldmap
	//   kt      	[Nr,Nt]           = Readout times
	//   freqs   	[Ns x 1]          = Offsets for Species
	//   max_iter   [scalar]     		maximum iterations

	// Dimensions of Problem
	int Nr = ky.length(firstDim);
	int Nt = ky.length(secondDim);
	int Ny = fieldmap.length(firstDim);
	int Nx = fieldmap.length(secondDim);
	cout << "Array Size = " << Nx << " x " << Ny << " x " << Nt << " x " << Ns << endl;

	// ------------------------------------
	// Iterative Soft Thresholding  x(n+1)=  thresh(   x(n) - E*(Ex(n) - d)  )
	//  Designed to not use memory
	// Uses gradient descent x(n+1) = x(n) - ( R'R ) / ( R'E'E R) * Grad  [ R = E'(Ex-d)]
	// ------------------------------------

	// Final Image Solution
	Array< complex<float>,4 >X(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	X = 0;

	// Residue
	Array< complex<float>,4 >R(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	R = 0;;

	// Residue
	Array< complex<float>,4 >Reg(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Reg = 0;

	// Temp variable for E'ER
	Array< complex<float>,4 >P(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	P = 0;;

	// Temp variable for E'ER
	Array< complex<float>,2 >diff_data(Nr,Nt,ColumnMajorArray<2>());
	diff_data = 0;

	// RHS (CG)
	Array< complex<float>,4 >LHS(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());

	cout << "Init CG" << endl;

	LHS= complex<float>(0,0);
	R= complex<float>(0,0);
	P= complex<float>(0,0);

	transpose_dideal(kdata,R,kx,ky,ktimes,fieldmap,freqs);
	R = -R;
	P = R;

	// Conjugate Gradient
	float error0=0.0;
	float reg_scale2=0.0;
	for(int iteration =0; iteration< max_iter; iteration++){

		cout << "\nIteration = " << iteration << endl;

		diff_data = complex<float>(0,0);
		forward_dideal(diff_data,P,kx,ky,ktimes,fieldmap,freqs);

		LHS = complex<float>(0,0);
		transpose_dideal(diff_data,LHS,kx,ky,ktimes,fieldmap,freqs);

		// Convolve with TV
		Reg = complex<float>(0.0,0.0);
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){

						// Space
						Reg(i,j,t,sp)=complex<float>(4.0,0)*P(i,j,t,sp);
						Reg(i,j,t,sp)-=P( (i+1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=P( (i-1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=P( i,(j+1+Ny)%Ny,t,sp);
						Reg(i,j,t,sp)-=P( i,(j-1+Ny)%Ny,t,sp);

						//Time
						Reg(i,j,t,sp) +=lambda_time*complex<float>(2.0,0)*P(i,j,t,sp);
						Reg(i,j,t,sp) -=lambda_time*P(i,j,(Nt+t-1)%Nt,sp);
						Reg(i,j,t,sp) -=lambda_time*P(i,j,(Nt+t+1)%Nt,sp);
		}}}}

		if(iteration==0){
			error0 = ArrayEnergy(R);
			reg_scale2 = lambda_space*sqrt( error0 );
		}
		Reg *= reg_scale2;
		LHS += Reg;

		complex< float> sum_R0_R0(0.0,0.0);
		complex< float> sum_R_R(0.0,0.0);
		complex< float> sum_P_LHS(0.0,0.0);

		// Calc R'R and P'*LHS
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){
						sum_R0_R0 += norm( R(i,j,t,sp));
						sum_P_LHS += conj( P(i,j,t,sp))*LHS(i,j,t,sp);
		}}}}
		complex< float> scale = sum_R0_R0 / sum_P_LHS;

		// Calc R'R and P'*LHS
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){
						X(i,j,t,sp) += ( scale*P(i,j,t,sp) );
						R(i,j,t,sp) -= ( scale*LHS(i,j,t,sp) );
						sum_R_R += norm( R(i,j,t,sp) );
		}}}}

		cout << "Sum R'R = " << sum_R_R << endl;
		complex< float> scale2 = sum_R_R / sum_R0_R0;

		// Take step size
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){
						P(i,j,t,sp) = R(i,j,t,sp) + scale2*P(i,j,t,sp);
		}}}}

		char fname[80];
		for(int sp=0; sp<Ns; sp++){
			sprintf(fname,"Species_%d.dat",sp);
			Array< complex<float>,3> TempS = X( Range::all(),Range::all(),Range::all(),sp);
			ArrayWrite( TempS,fname);
		}
	}

	return(0);
}

int dideal_recon_2D_Gradient(
	Array< float,2 >&kx,
	Array< float,2 >&ky,
	Array< complex<float>,2 >&kdata,
	Array< float,3 >&fieldmap,
	Array< float,2 >&ktimes,
	float *freqs,
	int Ns,
	float lambda_space,
	float lambda_time,
	float lambda_lowrank,
	int max_iter){

	// Inputs
	//   kx      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   ky      	[Nr,Nt ]          = Matrix with Nt readouts of length Nr
	//   kdata   	[Nr,Nt ]   		  = Matrix with Nt readouts of length Nr x Coils
	//   fieldmap	[ResX,ResY,Nt]    = Fieldmap
	//   kt      	[Nr,Nt]           = Readout times
	//   freqs   	[Ns x 1]          = Offsets for Species
	//   max_iter   [scalar]     		maximum iterations

	// Dimensions of Problem
	int Nr = ky.length(firstDim);
	int Nt = ky.length(secondDim);
	int Ny = fieldmap.length(firstDim);
	int Nx = fieldmap.length(secondDim);
	cout << "Array Size = " << Nx << " x " << Ny << " x " << Nt << " x " << Ns << endl;

	// ------------------------------------
	// Iterative Soft Thresholding  x(n+1)=  thresh(   x(n) - E*(Ex(n) - d)  )
	//  Designed to not use memory
	// Uses gradient descent x(n+1) = x(n) - ( R'R ) / ( R'E'E R) * Grad  [ R = E'(Ex-d)]
	// ------------------------------------

	// Final Image Solution
	Array< complex<float>,4 >X(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Array< complex<float>,4 >R(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Array< complex<float>,4 >Reg(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());
	Array< complex<float>,2 >diff_data(Nr,Nt,ColumnMajorArray<2>());
	Array< complex<float>,4 >P(Nx,Ny,Nt,Ns,ColumnMajorArray<4>());

	R= complex<float>(0,0);
	X= complex<float>(0,0);

	// Conjugate Gradient
	float error0=0.0;
	float reg_scale2=0.0;
	for(int iteration =0; iteration< max_iter; iteration++){

		cout << "Iteration = " << iteration << endl;

		// Get data
		diff_data = complex<float>(0,0);
		forward_dideal(diff_data,X,kx,ky,ktimes,fieldmap,freqs);

		// Difference
		diff_data -= kdata;

		// Get Residue
		R= complex<float>(0,0);
		transpose_dideal(diff_data,R,kx,ky,ktimes,fieldmap,freqs);

		// Convolve with TV
		Reg = complex<float>(0.0,0.0);
		for(int sp=0; sp<Ns; sp++){
			for(int t=0;t<Nt;t++){
				for(int j=0;j<Ny;j++){
					for(int i=0;i<Nx;i++){

						// Space
						Reg(i,j,t,sp)=lambda_space*complex<float>(4.0,0)*X(i,j,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( (i+1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( (i-1+Nx)%Nx,j,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( i,(j+1+Ny)%Ny,t,sp);
						Reg(i,j,t,sp)-=lambda_space*X( i,(j-1+Ny)%Ny,t,sp);

						//Time
						Reg(i,j,t,sp) +=lambda_time*complex<float>(2.0,0)*X(i,j,t,sp);
						Reg(i,j,t,sp) -=lambda_time*X(i,j,(Nt+t-1)%Nt,sp);
						Reg(i,j,t,sp) -=lambda_time*X(i,j,(Nt+t+1)%Nt,sp);

		}}}}

		if(iteration==0){
			error0 = ArrayEnergy(R);
			reg_scale2 = sqrt( error0 );
		}
		Reg *= reg_scale2;
		R += Reg;

		// Now get Scale
		diff_data = complex<float>(0,0);
		forward_dideal(diff_data,R,kx,ky,ktimes,fieldmap,freqs);
		P = complex<float>(0,0);
		transpose_dideal(diff_data,P,kx,ky,ktimes,fieldmap,freqs);

		cout << "Energy = " << ArrayEnergy(R)/error0 << endl;

		P *= conj(R);
		cout << "RhR = " << ArrayEnergy(R) << endl;
		cout << "RhP = " << sum(P) << endl;

		complex<float>scale = ArrayEnergy(R)/sum(P);
		cout << "Scale = " << scale << endl;

		R *= scale;
		X -= R;

		{
			char fname[80];
			for(int sp=0; sp<Ns; sp++){
				sprintf(fname,"Species_%d.dat.slice",sp);
				Array< complex<float>,2> TempS = X( Range::all(),Range::all(),(int)(Nt/2),sp);
				ArrayWriteMagAppend(TempS,fname);
			}
		}

		if(lambda_lowrank > 0){
			lowrank_thresh(X,lambda_lowrank);
		}

		{
			char fname[80];
			for(int sp=0; sp<Ns; sp++){
				sprintf(fname,"Species_%d.dat.slice",sp);
				Array< complex<float>,2> TempS = X( Range::all(),Range::all(),(int)(Nt/2),sp);
				ArrayWriteMagAppend(TempS,fname);
			}
		}
	}

	char fname[80];
	for(int sp=0; sp<Ns; sp++){
		sprintf(fname,"Species_%d.dat",sp);
		Array< complex<float>,3> TempS = X( Range::all(),Range::all(),Range::all(),sp);
		ArrayWrite( TempS,fname);
	}

	return(0);
}

void lowrank_thresh( Array< complex<float>,4> &image, float thresh){

	int Nx = image.length(firstDim);
	int Ny = image.length(secondDim);
	int Nt = image.length(thirdDim);
	int Ns = image.length(fourthDim);
	int Np = Nx*Ny*Ns;

	// Copy image into matrix
	arma::cx_mat A;
	A.zeros(Nx*Ny*Ns,Nt);

	for(int t =0; t < Nt; t++){
		int count = 0;

		for(int s =0; s<Ns; s++){
		for(int j =0; j<Ny; j++){
		for(int i =0; i<Nx; i++){
			A(count,t) = image(i,j,t,s);
			count++;
		}}}
	}

	// SVD
	cout << "Svd" << endl;
	arma::cx_mat U;
	arma::vec s;
	arma::cx_mat V;
	arma::svd(U,s,V,A);

	arma::mat S;
	S.zeros(Np,Nt); // Pixels x Coils

	cout << "Thresh with " << thresh << endl;
	float smax = thresh*max(s);
	for(int pos =0; pos< min(Nt,Np); pos++){
		S(pos,pos)=   max( s(pos) - smax, 0.0 );
	}

	A = U*S*V.t();

	for(int t =0; t < Nt; t++){
		int count = 0;

		for(int s =0; s<Ns; s++){
		for(int j =0; j<Ny; j++){
		for(int i =0; i<Nx; i++){
			image(i,j,t,s) = A(count,t);
			count++;
		}}}
	}
}


//
//  GPUTSDFVolume.hpp
//  A GPU Based TSDF
//
//  Created by Dave on 11/03/2016.
//  Copyright © 2016 Sindesso. All rights reserved.
//

#ifndef TSDFVolumeCPU_hpp
#define TSDFVolumeCPU_hpp

#include "Camera.hpp"

//#include <Eigen/Core>

#include <cstdint>
#include <iostream>
#include <string>
#include <cmath>

typedef struct {
    float m11, m21, m31, m41;
    float m12, m22, m32, m42;
    float m13, m23, m33, m43;
    float m14, m24, m34, m44;
} Mat44CPU;

typedef struct {
    float m11, m21, m31;
    float m12, m22, m32;
    float m13, m23, m33;
} Mat33CPU;

struct float3cpu {
    float x, y, z;
};

struct int3cpu {
    int x, y, z;
};

struct dim3cpu {
    unsigned int x, y, z;
};

struct uchar3cpu {
    unsigned char x, y, z;
};

inline float f3_dot(const float3cpu& f1, const float3cpu& f2) {
    return f1.x * f2.x + f1.y * f2.y + f1.z * f2.z;
}

/**
 * Subtract one float3 from another
 * @param f1 First float3
 * @param f2 Second float3
 * @return f1 - f2
 */
inline float3cpu f3_sub( const float3cpu& f1, const float3cpu& f2 ) {
    float3cpu f{f1.x-f2.x, f1.y-f2.y, f1.z-f2.z};
    return f;
}

/**
 * Add one float3 to another
 * @param f1 First float3
 * @param f2 Second float3
 * @return f1 + f2
 */
inline float3cpu f3_add( const float3cpu& f1, const float3cpu& f2 ) {
    float3cpu f{f2.x+f1.x, f2.y+f1.y, f2.z+f1.z};
    return f;
}

/**
 * Multiply a float3 by a scalar value
 * @param s The scalar
 * @param vec The float3
 */
inline float3cpu f3_mul_scalar( const float& scalar, const float3cpu& vec ) {
    float3cpu f{vec.x * scalar, vec.y * scalar, vec.z * scalar};
    return f;
}

/**
 * Perform per element division of f1 by v2
 * @param f1 the first float3 (numerators)
 * @param f2 The second float3 (denominators)
 * @return f1 ./ f2
 */
inline float3cpu f3_div_elem(const float3cpu& f1, const float3cpu& f2 ) {
    float3cpu f{f1.x/f2.x, f1.y/f2.y, f1.z/f2.z};
    return f;
}

/**
 * Perform per element division of f1 by v2
 * @param f1 the first float3 (numerators)
 * @param i2 The second float3 (denominators)
 * @return f1 ./ f2
 */
inline float3cpu f3_div_elem( const float3cpu& f, const dim3cpu& i ) {
    float3cpu ff{f.x/i.x, f.y/i.y, f.z/i.z};
    return ff;
}

/**
 * Normalise a float3
 * @param vec The float3 vector
 */
inline float3cpu f3_normalise(float3cpu vec) {
    float l = sqrt( vec.x*vec.x+vec.y*vec.y+vec.z*vec.z);
    vec.x /= l;
    vec.y /= l;
    vec.z /= l;
    return vec;
}

/**
 * Normalise a float3
 * @param vec The float3 vector
 */
inline float f3_norm( float3cpu vec ) {
    return sqrt(vec.x*vec.x+vec.y*vec.y+vec.z*vec.z);
}

/**
 * Perform a matrix multiplication
 * @param mat33 The matrix
 * @param vec3 The vector
 * @return an output vector
 */
inline float3cpu m3_f3_mul( const Mat33CPU& mat, const float3cpu& vec ) {
    float3cpu result;
    result.x = mat.m11 * vec.x + mat.m12 * vec.y + mat.m13 * vec.z;
    result.y = mat.m21 * vec.x + mat.m22 * vec.y + mat.m23 * vec.z;
    result.z = mat.m31 * vec.x + mat.m32 * vec.y + mat.m33 * vec.z;
    return result;
}

/**
 * Perform a matrix multiplication
 * @param mat33 The matrix
 * @param vec3 The vector
 * @return an output vector
 */
inline float3cpu m3_i3_mul( const Mat33CPU& mat, const int3cpu& vec ) {
    float3cpu result;
    result.x = mat.m11 * vec.x + mat.m12 * vec.y + mat.m13 * vec.z;
    result.y = mat.m21 * vec.x + mat.m22 * vec.y + mat.m23 * vec.z;
    result.z = mat.m31 * vec.x + mat.m32 * vec.y + mat.m33 * vec.z;
    return result;
}

/**
 * Project down into pixel coordinates
 * given the inv_pose and K matrices
 * plus a point in world coords
 */
inline int3cpu world_to_pixel( const Mat44CPU & inv_pose, const Mat33CPU& k, const float3cpu& point ) {
    float3cpu cam_point;
    cam_point.x = inv_pose.m11 * point.x + inv_pose.m12 * point.y + inv_pose.m13 * point.z + inv_pose.m14;
    cam_point.y = inv_pose.m21 * point.x + inv_pose.m22 * point.y + inv_pose.m23 * point.z + inv_pose.m24;
    cam_point.z = inv_pose.m31 * point.x + inv_pose.m32 * point.y + inv_pose.m33 * point.z + inv_pose.m34;
    float w = inv_pose.m41 * point.x + inv_pose.m42 * point.y + inv_pose.m43 * point.z + inv_pose.m44;
    cam_point.x /= w;
    cam_point.y /= w;
    cam_point.z /= w;

    float3cpu image_point {cam_point.x / cam_point.z, cam_point.y / cam_point.z, 1.0f};
    int3cpu pixel;
    pixel.x = static_cast<uint>( round( k.m11 * image_point.y + k.m12 * image_point.z + k.m13 ) );
    pixel.y = static_cast<uint>( round( k.m21 * image_point.y + k.m22 * image_point.z + k.m23 ) );
    pixel.z = static_cast<uint>( round( k.m31 * image_point.y + k.m32 * image_point.z + k.m33 ) ); // Should be 1

    return pixel;
}

class TSDFVolumeCPU {
public:
    struct DeformationNode {
        float3cpu  translation;
        float3cpu  rotation;
    };

    struct Float3 {
        float x;
        float y;
        float z;
        inline Float3( const float3cpu& rhs ) {
            x = rhs.x;
            y = rhs.y;
            z = rhs.z;
        }
        inline Float3( float fx=0.0f, float fy=0.0f, float fz=0.0f ) {
            x = fx;
            y = fy;
            z = fz;
        }
        inline operator float3cpu() const {return float3cpu{x, y, z}; }

        inline Float3 operator -( const Float3& rhs ) const {
            return float3cpu{x - rhs.x, y - rhs.y, z - rhs.z};
        }
        inline Float3 operator +( const Float3& rhs ) const {
            return float3cpu{x + rhs.x, y + rhs.y, z + rhs.z};
        }
        inline Float3 operator /( const float scalar ) const {
            return float3cpu{x / scalar, y / scalar, z / scalar};
        }
        inline Float3 operator *( const Float3& rhs ) const {
            return float3cpu{x * rhs.x, y * rhs.y, z * rhs.z};
        }
        inline float norm( ) const {
            return std::sqrt( x*x + y*y + z*z );
        }

    };

    struct Int3 {
        int16_t x;
        int16_t y;
        int16_t z;
    };

    struct  UInt3 {
        unsigned int x;
        unsigned int y;
        unsigned int z;

        inline UInt3( const dim3cpu& rhs ) : x{rhs.x}, y{rhs.y}, z{rhs.z} {};
        inline UInt3( uint32_t x, uint32_t y, uint32_t z ) : x{x}, y{y}, z{z} {};
        inline operator dim3cpu() const {return dim3cpu{x, y, z}; }
    };

    ~TSDFVolumeCPU();

    /**
    * Make a TSDFVolume with the given dimensions (voxels) and physcial dimensions
    * @param size The number of voxels in each X,Y and Z dimension
    * @param physical_size The size ( in mm ) of the space contained in the volume
    */
    TSDFVolumeCPU( const UInt3& size = UInt3{64, 64, 64},
                const Float3& physical_size = Float3 { 3000.0f, 3000.0f, 3000.0f} );

    /**
     * Make a TSDFVolume with the given dimensins and physical dimensions
     * @param volume_x X dimension in voxels
     * @param volume_y Y dimension in voxels
     * @param volume_z Z dimension in voxels
     * @param psize_x Physical size in X dimension in mm
     * @param psize_y Physical size in Y dimension in mm
     * @param psize_z Physical size in Z dimension in mm
     */
    TSDFVolumeCPU( uint16_t volume_x, uint16_t volume_y, uint16_t volume_z, float psize_x, float psize_y, float psize_z );

    /**
     * Load a TSDFVolume from the specified file. The volume must previously have been saved
     */
    TSDFVolumeCPU( const std::string& file_name );

    /**
     * Set the size of the volume. This will delete any existing values and resize the volume, clearing it when done.
     * Volume offset is maintained
     * @param volume_x X dimension in voxels
     * @param volume_y Y dimension in voxels
     * @param volume_z Z dimension in voxels
     * @param psize_x Physical size in X dimension in mm
     * @param psize_y Physical size in Y dimension in mm
     * @param psize_z Physical size in Z dimension in mm
     */
    void set_size( uint16_t volume_x, uint16_t volume_y, uint16_t volume_z, float psize_x, float psize_y, float psize_z);

    void set_truncation_distance(float d);

    /**
     * @return the size of this space in voxels.
     */
    inline UInt3 size( ) const { return (UInt3) m_size; }

    /**
     * @return the dimensions of each voxel in mm
     */
    inline Float3 voxel_size( ) const { return (Float3) m_voxel_size; }

    /**
     * @return the physical size of the volume in world coords (mm)
     */
    inline Float3 physical_size( ) const { return (Float3) m_physical_size; }

    /**
     * @return the truncation distance (mm)
     */
    inline float truncation_distance( ) const { return m_truncation_distance; }

    /**
     * Offset the TSDF volume in space by the given offset. By default, the bottom, left, front corner of
     * voxel (0,0,0) is at world coordinate (0,0,0). This moves that point to the new world coordinate by a
     * @param ox X offset in mm
     * @param oy Y offset in mm
     * @param oz Z offset in mm
     */
    inline void offset( float ox, float oy, float oz ) {
        m_offset.x = ox;
        m_offset.y = oy;
        m_offset.z = oz;
    }

    /**
     * @return the offset f the TSDF volume in space
     */
    inline Float3 offset( ) const { return (Float3) m_offset; }

    /**
     * Clear the voxel and weight data
     */
    void clear( );

#pragma mark - Data access

 /**
     *
     */
    inline size_t index( int x, int y, int z ) const {
        return x + (y * m_size.x) + (z * m_size.x * m_size.y);
    };

    /**
     * @return pointer to translation data
     */
    inline DeformationNode *  deformation() const {
        return m_deformation_nodes;
    }

    /**
     * Set the deformation data for this space
     * @param data Data in host memory space; Assumed to be vx*vy*vz DeformationNode
     */
    void set_deformation( DeformationNode *deformation);

    /**
     * Return pointer to distance data
     * @return Pointer to distance data
     */
    inline const float * distance_data() const {
        return m_distances;
    }

    /**
     * Set the distance data for the TSDF in one call
     * @param distance_data Pointer to enough floats to populate the TSFD
     */
    void set_distance_data(float * distance_data);

    /**
     * Return pointer to weight data
     * @return Pointer to weight data
     */
    inline const float * weight_data() const {
        return m_weights;
    }

    /**
     * Set the weight data for the TSDF in one call
     * @param weight_data Pointer to enough floats to populate the TSFD
     */
    void set_weight_data( const float * weight_data );

    /**
     * @return the global rotation of the TSDF deformation
     * as a vector of 3 Euler angles (X then Y then Z - also Tait-Bryan angles)
     */
    inline float3cpu global_rotation( ) const {
        return m_global_rotation;
    }

    /**
     * @return the global translation of the TSDF deformation
     */
    inline float3cpu global_translation( ) const {
        return m_global_translation;
    }

#pragma mark - Deform a set of points
    /**
     * Apply the volume's deformation field t the given set of points, modifying them in place
     */
    void deform_mesh( const int num_points, float3cpu * points ) const;

#pragma mark - Integrate new depth data
    /**
     * Integrate a range map into the TSDF
     * @param depth_map Pointer to width*height depth values where 0 is an invalid depth and positive values are expressed in mm
     * @param width The horiontal dimension of the depth_map
     * @param height The height of the depth_map
     * @param camera The camera from which the depth_map was taken
     */
    void integrate( const uint16_t * depth_map, uint32_t width, uint32_t height, const Camera & camera );

#pragma mark - Import/Export

    /**
     * Save the TSDF to file
     * @param The filename
     * @return true if the file saved OK otherwise false.
     */
    bool save_to_file( const std::string & file_name) const;

    /**
     * Load the given TSDF file
     * @param The filename
     * @return true if the file saved OK otherwise false.
     */
    bool load_from_file( const std::string & file_name);

#pragma mark - Rendering
    void raycast( uint16_t width, uint16_t height, const Camera& camera, Eigen::Matrix<float, 3, Eigen::Dynamic>& vertices, Eigen::Matrix<float, 3, Eigen::Dynamic>& normals ) const ;

private:
    /**
     * Deallocate storage for this TSDF
     */
    void deallocate( );

    // Size of voxel grid
    dim3cpu                    m_size;

    // Physical size of space represented in mm
    float3cpu                  m_physical_size;

    // Offset of physical grid in world coordinates
    float3cpu                  m_offset;

    // Size of a voxel
    float3cpu m_voxel_size;

    //  Truncation distance
    float m_truncation_distance;

    // Max weight for a voxel
    float m_max_weight;

    // Per grid point data
    float                   *m_distances;

    // Colour data, RGB as 3xuchar
    uchar3cpu                  *m_colours;

    //  Confidence weight for distance and colour
    float                   *m_weights;

    // Deformation field
    DeformationNode         *m_deformation_nodes;

    // Global translation
    float3cpu                  m_global_translation;

    // Global rotation
    float3cpu                  m_global_rotation;
};
#endif /* TSDFVolumeCPU_hpp */


/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

Copyright (c) 2019 Panda Team
*/
#ifndef _METRIC_DISTANCE_K_RANDOM_VOI_CPP
#define _METRIC_DISTANCE_K_RANDOM_VOI_CPP

#include <algorithm>
#include <cmath>
#include <iostream>
#include <random>
#include <unordered_set>
#include <vector>

#include <boost/functional/hash.hpp>

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

#include "../../space/tree.hpp"
#include "VOI.hpp"

namespace metric {

namespace {
    template <typename T>
    void add_noise(std::vector<std::vector<T>>& data)
    {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::normal_distribution<T> dis(0, 1);
        double c = 1e-10;
        for (auto& v : data) {
            std::transform(v.begin(), v.end(), v.begin(), [&gen, c, &dis](T e) {
                auto g = dis(gen);
                auto k = e + c * g;
                return k;
            });
        }
    }

    template <typename T1, typename T2>
    T1 log(T1 logbase, T2 x)
    {
        return std::log(x) / std::log(logbase);
    }

    template <typename T>
    void print_vec(const std::vector<T>& v)
    {
        std::cout << "[";
        for (auto d : v) {
            std::cout << d << ", ";
        }
        std::cout << "]";
    }

    template <typename Node_ptr, typename Distance>
    void print(const std::vector<std::pair<Node_ptr, Distance>>& data)
    {
        std::cout << "[";
        for (auto& v : data) {
            print_vec(v.first->data);
            std::cout << " dist=" << v.second << "]";
        }
        std::cout << "]" << std::endl;
    }
    template <typename T>
    void combine(
        const std::vector<std::vector<T>>& X, const std::vector<std::vector<T>>& Y, std::vector<std::vector<T>>& XY)
    {
        std::size_t N = X.size();
        std::size_t dx = X[0].size();
        std::size_t dy = Y[0].size();
        XY.resize(N);
        for (std::size_t i = 0; i < N; i++) {
            XY[i].resize(dx + dy);
            std::size_t k = 0;
            for (std::size_t j = 0; j < dx; j++, k++) {
                XY[i][k] = X[i][j];
            }
            for (std::size_t j = 0; j < dy; j++, k++) {
                XY[i][k] = Y[i][j];
            }
        }
    }
    template <typename T>
    std::vector<T> unique(const std::vector<T>& data)
    {
        std::unordered_set<std::size_t> hashes;
        std::vector<T> result;
        result.reserve(data.size());
        std::copy_if(data.begin(), data.end(), std::back_inserter(result),
            [&hashes](const T& i) { return hashes.insert(boost::hash_value(i)).second; });
        return result;
    }

}  // namespace

template <typename T, typename Metric, typename L>
typename std::enable_if<!std::is_integral<T>::value, T>::type entropy(
    std::vector<std::vector<T>> data, std::size_t k, L logbase, Metric metric)
{
    if (data.empty() || data[0].empty()) {
        return 0;
    }
    if (data.size() < k + 1)
        throw std::invalid_argument("number of points in dataset must be larger than k");

    T p = 1;
    T N = data.size();
    T d = data[0].size();
    T two = 2.0;  // this is in order to make types match the log template function
    T cb = d * log(logbase, two);

    if constexpr (!std::is_same<Metric, typename metric::Chebyshev<T>>::value) {
        if constexpr (std::is_same<Metric, typename metric::Euclidian<T>>::value) {
            p = 2;
        } else if constexpr (std::is_same<Metric, typename metric::P_norm<T>>::value) {
            p = metric.p;
        }
        cb = cb + d * log(logbase, std::tgamma(1 + 1 / p)) - log(logbase, std::tgamma(1 + d / p));
    }

    add_noise(data);
    metric::Tree<std::vector<T>, Metric> tree(data, -1, metric);
    T entropyEstimate = boost::math::digamma(N) - boost::math::digamma(k) + cb + d * log(logbase, two);
    for (std::size_t i = 0; i < N; i++) {
        auto res = tree.knn(data[i], k + 1);
        entropyEstimate += d / N * log(logbase, res.back().second);
    }
    return entropyEstimate;
}

// Kozachenko-Leonenko estimator based on https://hal.archives-ouvertes.fr/hal-00331300/document (Shannon diff. entropy,
// q = 1)

template <typename T, typename Metric = metric::Euclidian<T>, typename L = T>  // TODO check if L = T is correct
typename std::enable_if<!std::is_integral<T>::value, T>::type entropy_kl(
    std::vector<std::vector<T>> data, std::size_t k = 3, L logbase = 2, Metric metric = Metric())
{
    if (data.empty() || data[0].empty())
        return 0;
    if (data.size() < k + 1)
        throw std::invalid_argument("number of points in dataset must be larger than k");
    if constexpr (!std::is_same<Metric, typename metric::Euclidian<T>>::value)
        throw std::logic_error("entropy function is now implemented only for Euclidean distance");

    metric::Tree<std::vector<T>, Metric> tree(data, -1, metric);

    size_t N = data.size();
    size_t m = data[0].size();
    T two = 2.0;  // this is in order to make types match the log template function
    T sum = 0;
    auto Pi = boost::math::constants::pi<T>();
    T half_m = m / two;
    auto coeff = (N - 1) * exp(-boost::math::digamma(k + 1)) * std::pow(Pi, half_m) / boost::math::tgamma(half_m + 1);

    for (std::size_t i = 0; i < N; i++) {
        auto neighbors = tree.knn(data[i], k + 1);
        auto ro = neighbors.back().second;
        sum = sum + log(logbase, coeff * std::pow(ro, m));
    }

    return sum;
}

template <typename T>
std::pair<std::vector<double>, std::vector<std::vector<T>>> pluginEstimator(const std::vector<std::vector<T>>& Y)
{
    std::vector<std::vector<T>> uniqueVal = unique(Y);
    std::vector<double> counts(uniqueVal.size());
    for (std::size_t i = 0; i < counts.size(); i++) {
        for (std::size_t j = 0; j < Y.size(); j++) {
            if (Y[j] == uniqueVal[i])
                counts[i]++;
        }
    }
    std::size_t length = Y.size() * Y[0].size();
    std::transform(counts.begin(), counts.end(), counts.begin(), [&length](auto& i) { return i / length; });

    return std::make_pair(counts, uniqueVal);
}

template <typename T, typename Metric>
typename std::enable_if<!std::is_integral<T>::value, T>::type mutualInformation(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, int k, Metric metric, int version)
{
    T N = Xc.size();

    if (N < k + 1 || Yc.size() < k + 1)
        throw std::invalid_argument("number of points in dataset must be larger than k");

    auto X = Xc;
    auto Y = Yc;
    add_noise(X);
    add_noise(Y);
    std::vector<std::vector<T>> XY;
    combine(X, Y, XY);
    metric::Tree<std::vector<T>, Metric> tree(XY, -1, metric);
    auto entropyEstimate = boost::math::digamma(k) + boost::math::digamma(N);
    if (version == 2) {
        entropyEstimate -= 1 / static_cast<double>(k);
    }

    metric::Tree<std::vector<T>, Metric> xTree(X, -1, metric);

    for (std::size_t i = 0; i < N; i++) {
        auto res = tree.knn(XY[i], k + 1);
        auto neighbor = res.back().first;
        auto dist = res.back().second;
        std::size_t nx = 0;
        if (version == 1) {
            auto dist_eps = std::nextafter(
                dist, std::numeric_limits<decltype(dist)>::max());  // this is instead of replacing < with <= in Tree //
                // added by Max F in order to match Julia code logic
                // without updating Tree
            nx = xTree.rnn(X[i], dist_eps).size();  // we include points that lay on the sphere
        } else if (version == 2) {
            auto ex = metric(X[neighbor->ID], X[i]);
            auto ex_eps = std::nextafter(
                ex, std::numeric_limits<decltype(ex)>::max());  // this it to include the most distant point into the
                // sphere // added by Max F in order to match Julia code
                // logic without updating Tree
            auto rnn_set = xTree.rnn(X[i], ex_eps);
            nx = rnn_set.size();  // replaced ex by ex_eps by Max F
        } else {
            throw std::runtime_error("this version not allowed");
        }
        entropyEstimate -= 1.0 / N * boost::math::digamma(static_cast<double>(nx));
    }
    return entropyEstimate;
}

template <typename T>
typename std::enable_if<std::is_integral<T>::value, T>::type mutualInformation(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, T logbase)
{
    std::vector<std::vector<T>> XY;
    combine(Xc, Yc, XY);
    return entropy<T>(Xc, logbase)
        + entropy<T>(Yc,
            logbase)  // entropy overload for integers is not implemented yet
        - entropy<T>(XY, logbase);
}

template <typename T, typename Metric>
typename std::enable_if<!std::is_integral<T>::value, T>::type variationOfInformation(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, int k, T logbase)
{
    return entropy<T, Metric>(Xc, k, logbase, Metric()) + entropy<T, Metric>(Yc, k, logbase, Metric())
        - 2 * mutualInformation<T>(Xc, Yc, k);
}

template <typename T>
typename std::enable_if<!std::is_integral<T>::value, T>::type variationOfInformation_normalized(
    const std::vector<std::vector<T>>& Xc, const std::vector<std::vector<T>>& Yc, int k, T logbase)
{
    using Cheb = metric::Chebyshev<T>;
    auto mi = mutualInformation<T>(Xc, Yc, k);
    return 1 - (mi / (entropy<T, Cheb>(Xc, k, logbase, Cheb()) + entropy<T, Cheb>(Yc, k, logbase, Cheb()) - mi));
}

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    using Cheb = metric::Chebyshev<El>;
    return entropy<El, Cheb>(a, k, logbase, Cheb()) + entropy<El, Cheb>(b, k, logbase, Cheb())
        - 2 * mutualInformation<El>(a, b, k);
}

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI_normalized<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    using Cheb = metric::Chebyshev<El>;
    auto mi = mutualInformation<El>(a, b, this->k);
    return 1
        - (mi
            / (entropy<El, Cheb>(a, this->k, this->logbase, Cheb())
                + entropy<El, Cheb>(b, this->k, this->logbase, Cheb()) - mi));
}

// VOI based on Kozachenko-Leonenko entropy estimator

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI_kl<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    std::vector<std::vector<El>> ab;
    combine(a, b, ab);
    return 2 * entropy_kl(ab) - entropy_kl(a) - entropy_kl(b);
}

template <typename V>
template <template <class, class> class Container, class Allocator_inner, class Allocator_outer, class El>
typename std::enable_if<!std::is_integral<El>::value, V>::type VOI_normalized_kl<V>::operator()(
    const Container<Container<El, Allocator_inner>, Allocator_outer>& a,
    const Container<Container<El, Allocator_inner>, Allocator_outer>& b) const
{
    auto entropy_a = entropy_kl(a);
    auto entropy_b = entropy_kl(b);
    std::vector<std::vector<El>> ab;
    combine(a, b, ab);
    auto joint_entropy = entropy_kl(ab);
    auto mi = entropy_a + entropy_b - joint_entropy;
    return 1 - (mi / (entropy_a + entropy_b - mi));
}

}  // namespace metric
#endif


/**
 * @file Tools/Math/RotationMatrix.hpp
 * Delcaration of class RotationMatrix
 * @author Martin Kallnik
 * @author Thomas Kindler
 * @author Max Risler
 * @author Alexis Tsogias
 */

#pragma once

#include <Eigen/Dense>

using namespace Eigen;

/**
 * Representation for 3x3 RotationMatrices
 */
class RotationMatrix : public Matrix3f
{
public:
  RotationMatrix() : Matrix3f(Matrix3f::Identity()) {}
  RotationMatrix(const Matrix3f& other) : Matrix3f(other) {}
  RotationMatrix(const AngleAxisf& angleAxis) : Matrix3f(angleAxis.toRotationMatrix()) {}
  RotationMatrix(const Quaternionf& quat) : Matrix3f(quat.toRotationMatrix()) {}

  RotationMatrix& operator=(const Matrix3f& other)
  {
    Matrix3f::operator=(other);
    return *this;
  }

  RotationMatrix& operator=(const AngleAxisf& angleAxis)
  {
    Matrix3f::operator=(angleAxis.toRotationMatrix());
    return *this;
  }

  RotationMatrix& operator=(const Quaternionf& quat)
  {
    Matrix3f::operator=(quat.toRotationMatrix());
    return *this;
  }

  /**
   * Multiplication of this matrix by vector.
   * @param  vector  The vector this one is multiplied by
   * @return         A new vector containing the result
   */
  Vector3f operator*(const Vector3f& vector) const
  {
    return Matrix3f::operator*(vector);
  }

  /**
   * Multiplication of this rotation matrix by another rotation matrix.
   * @param  other  The other matrix this one is multiplied by
   * @return        A new matrix containing the result
   *                of the calculation.
   */
  RotationMatrix operator*(const RotationMatrix& other) const
  {
    return RotationMatrix(Base::operator*(other));
  }

  RotationMatrix& operator*=(const AngleAxisf& rot)
  {
    Matrix3f::operator*=(rot.toRotationMatrix());
    return *this;
  }

  RotationMatrix& operator*=(const Quaternionf& rot)
  {
    Matrix3f::operator*=(rot.toRotationMatrix());
    return *this;
  }

  RotationMatrix& operator*=(const RotationMatrix& rot)
  {
    Matrix3f::operator*=(rot);
    return *this;
  }

  /**
   * Invert the matrix.
   *
   * @note: Inverted rotation matrix is transposed matrix.
   */
  RotationMatrix& invert()
  {
    transposeInPlace();
    return *this;
  }

  RotationMatrix inverse() const
  {
    return RotationMatrix(transpose());
  }

  void normalize()
  {
    *this = Quaternionf(*this).normalized();
  }

  RotationMatrix normalized() const
  {
    return Quaternionf(*this).normalized();
  }

  /**
   * Converts the rotation matrix into an angleAxis.
   * @return The rotation matrix as angleAxis.
   */
  AngleAxisf getAngleAxis() const;

  /**
   * Converts the rotation matrix into an angleAxis in single vector format.
   * @return The rotation matrix as angleAxis.
   */
  // Vector3f getPackedAngleAxis() const;

  /**
   * Rotation around the x-axis.
   *
   * @param   angle  The angle this pose will be rotated by
   * @return  A reference to this object after the calculation.
   */
  RotationMatrix& rotateX(const float angle);

  /**
   * Rotation around the y-axis.
   *
   * @param   angle  The angle this pose will be rotated by
   * @return  A reference to this object after the calculation.
   */
  RotationMatrix& rotateY(const float angle);

  /**
   * Rotation around the z-axis.
   *
   * @param   angle  The angle this pose will be rotated by
   * @return  A reference to this object after the calculation.
   */
  RotationMatrix& rotateZ(const float angle);

  /**
   * Get the x-angle of a RotationMatrix.
   *
   * @return  The angle around the x-axis between the original
   *          and the rotated z-axis projected on the y-z-plane
   */
  float getXAngle() const;

  /**
   * Get the y-angle of a RotationMatrix.
   *
   * @return  The angle around the y-axis between the original
   *          and the rotated x-axis projected on the x-z-plane
   */
  float getYAngle() const;

  /**
   * Get the z-angle of a RotationMatrix.
   *
   * @return  The angle around the z-axis between the original
   *          and the rotated x-axis projected on the x-y-plane
   */
  float getZAngle() const;

  /**
   * Create and return a RotationMatrix, rotated around x-axis
   *
   * @param   angle
   * @return  rotated RotationMatrix
   */
  static RotationMatrix aroundX(const float angle);

  /**
   * Create and return a RotationMatrix, rotated around y-axis
   *
   * @param   angle
   * @return  rotated RotationMatrix
   */
  static RotationMatrix aroundY(const float angle);

  /**
   * Create and return a RotationMatrix, rotated around z-axis
   *
   * @param   angle
   * @return  rotated RotationMatrix
   */
  static RotationMatrix aroundZ(const float angle);

  /**
   * Creates a RotationMatrix rotated around z, y and x (in this order!).
   * Equivalent to fromRotationZ(z).rotateY(y).rotateX(x);
   */
  static RotationMatrix fromEulerAngles(const float x, const float y, const float z);

  /**
   * Creates a RotationMatrix rotatied around the z, y and x components of the Vector3 (in this order!).
   * Equivalent to fromRotationZ(rotation.z).rotateY(rotation.y).rotateX(rotation.x);
   */
  static RotationMatrix fromEulerAngles(const Vector3f rotation);

private:
  // The following is a hack in order to keep the kicks from the Kickengine working...
  friend class KickViewWidget;
  friend class KickEngineData;
  friend class StableKickEngineData;
  friend class RotationMatrix_getPackedAngleAxisFaulty_Test;

  Vector3f getPackedAngleAxisFaulty() const;

  static void reg();
};


#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/LU>
#include <limits>
#include "rose499/spline.hpp"

using namespace Eigen;

constexpr int Spline::PolyOrder;
constexpr int Spline::CoeffCount;

namespace
{
    constexpr int ResampleCount = 9;

    using ValueType = Spline::ValueType;
    using MatrixXT = SimulatorTypes::MatrixXT;
    using VectorXT = SimulatorTypes::VectorXT;
    using PolySpace = Array<ValueType, 1, Spline::CoeffCount>;

    PolySpace polydiff_power(PolySpace poly, int order)
    {
        PolySpace prevPoly = poly;
        PolySpace dpoly = PolySpace::Constant(0);
        PolySpace powerCoeff = PolySpace::Constant(1);
        for(auto i = 0; i < order; i++)
        {
            Array<ValueType, 1, Dynamic> power(1, Spline::CoeffCount - i);
            power.setLinSpaced(0, Spline::PolyOrder - i);
            powerCoeff.tail(Spline::CoeffCount - i) = powerCoeff.tail(Spline::CoeffCount - i) * power;
        }
        dpoly.tail(Spline::CoeffCount - order) = poly.head(Spline::CoeffCount - order);
        dpoly = dpoly * powerCoeff;
        return dpoly;
    }

    PolySpace polydiffshift(PolySpace poly)
    {
        PolySpace dpoly = PolySpace::Constant(0);
        dpoly.head(Spline::CoeffCount - 1) = poly.tail(Spline::CoeffCount - 1);
        dpoly.head(Spline::CoeffCount - 1) *= Array<ValueType, Spline::CoeffCount - 1, 1>::LinSpaced(1, Spline::PolyOrder);
        return dpoly;
    }

    /**
        Builds an equality matrix that only provides end point guarantees and
        C^2 continuity everywhere.
    **/
    void
    polyBuildSoftEqualityMatrix(    MatrixXT& Aeq,
                                    VectorXT& beq,
                                    Matrix<ValueType, 2, Dynamic> const & waypoints,
                                    double direction = std::numeric_limits<double>::infinity() )
    {
        constexpr size_t DirectionConstraintCount = 1;
        constexpr auto CoeffCount = Spline::CoeffCount;

        auto const waypointCount = waypoints.cols();
        auto const segmentCount = waypointCount - 1;

        bool const supportDirection = std::isfinite(direction);

        Aeq.resize( 4*(waypointCount-2)                                         // Differential Constraints
                        + 2*waypointCount                                       // Equality Constraints
                        + (supportDirection ? DirectionConstraintCount : 0)     // Direction Constraint
                    , 2*CoeffCount*segmentCount );
        beq.resize(Aeq.rows(), 1);

        Aeq.setZero();
        beq.setZero();

        PolySpace powers = PolySpace::LinSpaced(0, Spline::PolyOrder);
        auto row = 0;
        for(auto indSeg = 0; indSeg < segmentCount; ++indSeg)
        {
            auto col = 2*CoeffCount*indSeg;

            ValueType leftLambda = (indSeg * 1.0) / segmentCount;
            ValueType rightLambda = ((indSeg + 1.0) * 1.0) / segmentCount;

            PolySpace leftTerms = PolySpace::Constant(leftLambda);
            PolySpace rightTerms = PolySpace::Constant(rightLambda);
            leftTerms = pow(leftTerms, powers);
            rightTerms = pow(rightTerms, powers);

            PolySpace dLeftTerms = polydiff_power(leftTerms, 1);
            PolySpace ddLeftTerms = polydiff_power(leftTerms, 2);

            PolySpace dRightTerms = polydiff_power(rightTerms, 1);
            PolySpace ddRightTerms = polydiff_power(rightTerms, 2);

            if( indSeg < segmentCount - 1 )
            {
                // Right Equality (C0)
                Aeq.block<1, CoeffCount>(row + 0, col) = rightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -rightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = rightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -rightTerms;
                row += 2;

                // Right Differential Continuity (C1)
                Aeq.block<1, CoeffCount>(row + 0, col) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -dRightTerms;
                row += 2;

                // Right Differential Continuity (C2)
                Aeq.block<1, CoeffCount>(row + 0, col) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -ddRightTerms;
                row += 2;
            }
            else
            {
                // Hard Right Equality
                Aeq.block<1, CoeffCount>(row + 0, col) = rightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = rightTerms;
                beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg + 1);
                row += 2;
            }

            if( indSeg == 0 )
            {
                // Left Equality (C0)
                Aeq.block<1, CoeffCount>(row + 0, col) = leftTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = leftTerms;
                beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg);
                row += 2;

                if( supportDirection )
                {
                    while(direction > M_PI) direction -= 2*M_PI;
                    while(direction < -M_PI) direction += 2*M_PI;

                    if( std::abs(direction) <= std::atan2(1.0, 1.0)
                     || std::abs(direction - M_PI) <= std::atan2(1.0, 1.0)
                     || std::abs(direction + M_PI) <= std::atan2(1.0, 1.0) )
                    {
                        // Small tangent.
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col) = std::tan(direction)*dLeftTerms;
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col + CoeffCount) = -dLeftTerms;
                    }
                    else
                    {
                        // Large tangent
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col) = -dLeftTerms;
                        Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col + CoeffCount) = dLeftTerms / std::tan(direction);
                    }
                }
            }
        }
    }

    /**
        Builds the equality matrix that is used in least squares to achieve
        C0, C1, and C2 equality at waypoints.

    **/
    void
    polyBuildEqualityMatrix( MatrixXT& Aeq,
                             VectorXT& beq,
                             Matrix<ValueType, 2, Dynamic> const & waypoints,
                             double direction = std::numeric_limits<double>::infinity() )
    {
        constexpr size_t DirectionConstraintCount = 2;
        constexpr auto CoeffCount = Spline::CoeffCount;

        auto const waypointCount = waypoints.cols();
        auto const segmentCount = waypointCount - 1;

        bool const supportDirection = std::isfinite(direction);

        Aeq.resize( 4*(segmentCount-1)                                          // Differential Constraints
                        + 4*(segmentCount)                                      // Equality Constraints
                        + (supportDirection ? DirectionConstraintCount : 0)     // Direction Constraint
                    , 2*CoeffCount*segmentCount );
        beq.resize(Aeq.rows(), 1);

        Aeq.setZero();
        beq.setZero();

        PolySpace powers = PolySpace::LinSpaced(0, Spline::PolyOrder);
        auto row = 0;
        for(auto indSeg = 0; indSeg < segmentCount; ++indSeg)
        {
            auto col = 2*CoeffCount*indSeg;

            ValueType leftLambda = (indSeg * 1.0) / segmentCount;
            ValueType rightLambda = ((indSeg + 1.0) * 1.0) / segmentCount;

            PolySpace leftTerms = PolySpace::Constant(leftLambda);
            PolySpace rightTerms = PolySpace::Constant(rightLambda);
            leftTerms = pow(leftTerms, powers);
            rightTerms = pow(rightTerms, powers);

            PolySpace dLeftTerms = polydiff_power(leftTerms, 1);
            PolySpace ddLeftTerms = polydiff_power(leftTerms, 2);

            PolySpace dRightTerms = polydiff_power(rightTerms, 1);
            PolySpace ddRightTerms = polydiff_power(rightTerms, 2);

            // Left Equality (C0)
            Aeq.block<1, CoeffCount>(row + 0, col) = leftTerms;
            Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = leftTerms;
            beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg);
            row += 2;

            // Right Equality (C0)
            Aeq.block<1, CoeffCount>(row + 0, col) = rightTerms;
            Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = rightTerms;
            beq.block<2, 1>(row, 0) = waypoints.block<2, 1>(0, indSeg + 1);
            row += 2;

            if( (indSeg == 0) && supportDirection )
            {
                Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(0, col) = dLeftTerms;
                Aeq.bottomRows(DirectionConstraintCount).block<1, CoeffCount>(1, col + CoeffCount) = dLeftTerms;

                Matrix<ValueType, 2, 1> vec;
                vec(0) = std::cos(direction);
                vec(1) = std::sin(direction);

                beq.tail(DirectionConstraintCount) = vec;
            }

            if( indSeg < segmentCount - 1 )
            {
                // Right Differential Continuity (C1)
                Aeq.block<1, CoeffCount>(row + 0, col) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = dRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -dRightTerms;
                row += 2;

                // Right Differential Continuity (C2)
                Aeq.block<1, CoeffCount>(row + 0, col) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 0, col + 2 * CoeffCount) = -ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + CoeffCount) = ddRightTerms;
                Aeq.block<1, CoeffCount>(row + 1, col + 3 * CoeffCount) = -ddRightTerms;
                row += 2;
            }
        }
    }

    void
    polyBuildLSQMatrix( MatrixXT& A,
                        VectorXT& b,
                        Matrix<ValueType, 2, Dynamic> const & waypoints )
    {
        constexpr auto CoeffCount = Spline::CoeffCount;
        constexpr auto IntermediateSampleCount = 9;
        const auto waypointCount = waypoints.cols();
        const auto segmentCount = waypointCount - 1;
        const auto sampleCount = segmentCount * IntermediateSampleCount;
        using SampleRow = Matrix<ValueType, 1, IntermediateSampleCount>;

        A.resize(2*sampleCount, 2*CoeffCount*segmentCount);
        b.resize(A.rows());

        A.setZero();
        b.setZero();

        PolySpace powers = PolySpace::LinSpaced(0, Spline::PolyOrder);
        size_t row = 0;
        size_t colX = 0, colY = CoeffCount;
        for(auto si = 0; si < segmentCount; ++si)
        {
            SampleRow sampleX = Matrix<ValueType, 1, IntermediateSampleCount+2>::LinSpaced(waypoints(0, si), waypoints(0, si+1)).middleCols(1, IntermediateSampleCount);
            SampleRow sampleY = Matrix<ValueType, 1, IntermediateSampleCount+2>::LinSpaced(waypoints(1, si), waypoints(1, si+1)).middleCols(1, IntermediateSampleCount);
            SampleRow sampleLambda = Matrix<ValueType, 1, IntermediateSampleCount+2>::LinSpaced(si * 1.0 / segmentCount, (si+1) * 1.0 / segmentCount).middleCols(1, IntermediateSampleCount);

            for(auto i = 0; i < sampleLambda.cols(); ++i)
            {
                PolySpace terms = PolySpace::Constant(sampleLambda(i));
                terms = pow(terms, powers);

                // Equality at the lambda
                A.block<1, CoeffCount>(row + 0, colX) = terms;
                A.block<1, CoeffCount>(row + 1, colY) = terms;
                b(row + 0) = sampleX(i);
                b(row + 1) = sampleY(i);

                row += 2;
            }

            colX += 2 * CoeffCount;
            colY += 2 * CoeffCount;
        }
    }

    /**
     * Splines a 5th order polynomial through the waypoints that ensures C2
     * conditions are met as well as a direction requirement at lambda=0
     *
     * The system is normally underdetermined so polyfit normally calculates the
     * right pseduo-inverse and performs a full pivot LU decomposition in order to
     * solve for the coefficients.
     *
     * Implements https://stanford.edu/class/ee103/lectures/constrained-least-squares/constrained-least-squares_slides.pdf
     */
    Matrix<ValueType, Dynamic, Spline::CoeffCount>
    polyfit(Matrix<ValueType, 2, Dynamic> waypoints, double direction = std::numeric_limits<double>::infinity() )
    {
        constexpr auto CoeffCount = Spline::CoeffCount;

        auto const waypointCount = waypoints.cols();
        auto const segmentCount = waypointCount - 1;

        MatrixXT Aeq, A;
        VectorXT beq, b;

        polyBuildSoftEqualityMatrix(Aeq, beq, waypoints, direction);
        polyBuildLSQMatrix(A, b, waypoints);

        // Moore-Penrose (Right) Pseudo-Inverse
        //c = Aeq.transpose() * (Aeq * Aeq.transpose()).fullPivLu().solve(beq);

        // Build full solution matrix F * (x, z)' = (2A'b, beq)'
        MatrixXT F( A.cols() + Aeq.rows(),
                    A.cols() + Aeq.rows() );
        VectorXT v( F.rows() );

        F.setZero();
        v.setZero();

        F.block(0, 0, A.cols(), A.cols()) = 2 * A.transpose() * A;
        F.block(0, A.cols(), Aeq.cols(), Aeq.rows()) = Aeq.transpose();
        F.block(A.cols(), 0, Aeq.rows(), Aeq.cols()) = Aeq;

        v.head(A.cols()) = 2 * A.transpose() * b;
        v.tail(beq.rows()) = beq;

        VectorXT d(F.rows(), 1);
        if( F.rows() > 100 )
        {
            SparseMatrix<ValueType, ColMajor> Fs;
            Fs = F.sparseView();

            SparseLU< SparseMatrix<ValueType, ColMajor> > solver;
            solver.analyzePattern(Fs);
            solver.factorize(Fs);
            d = solver.solve(v);
        }
        else
        {
            d = F.fullPivHouseholderQr().solve(v);
        }
        VectorXT c = d.head(Aeq.cols());
        /*
        MatrixXT c(Aeq.cols(), 1);

        // Moore-Penrose (Right) Pseudo-Inverse
        c = Aeq.transpose() * (Aeq * Aeq.transpose()).fullPivLu().solve(beq);
        */
        // Map the resulting coefficient vector into our matrix form of the polynomial
        // spline!
        Matrix<ValueType, Dynamic, CoeffCount> result(2 * segmentCount, CoeffCount);
        result = Map< Matrix<ValueType, Dynamic, CoeffCount, RowMajor> >(c.data(), result.rows(), result.cols());

        return result;
    }
}

Spline::Spline(Matrix<Spline::ValueType, 2, Dynamic> points)
  : mSplineCount(points.cols() - 1),
    mPoly(2 * mSplineCount, CoeffCount),
    mDPoly(2 * mSplineCount, CoeffCount),
    mDDPoly(2 * mSplineCount, CoeffCount)
{
    mPoly = ::polyfit(points);
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDPoly.row(i) = ::polydiffshift(mPoly.row(i));
    }
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDDPoly.row(i) = ::polydiffshift(mDPoly.row(i));
    }
    approximateSelf();
}

Spline::Spline(Eigen::Matrix<Spline::ValueType, 2, Eigen::Dynamic> points, double direction)
  : mSplineCount(points.cols() - 1),
    mPoly(2 * mSplineCount, CoeffCount),
    mDPoly(2 * mSplineCount, CoeffCount),
    mDDPoly(2 * mSplineCount, CoeffCount)
{
    mPoly = ::polyfit(points, direction);
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDPoly.row(i) = ::polydiffshift(mPoly.row(i));
    }
    for( auto i = 0; i < mPoly.rows(); ++i )
    {
        mDDPoly.row(i) = ::polydiffshift(mDPoly.row(i));
    }
    approximateSelf();
}

Spline::Spline()
  : mSplineCount(1),
    mPoly(2 * mSplineCount, CoeffCount),
    mDPoly(2 * mSplineCount, CoeffCount),
    mDDPoly(2 * mSplineCount, CoeffCount)
{
    mPoly.setZero();
    mDPoly.setZero();
    mDDPoly.setZero();
}

Matrix<Spline::ValueType, 2, 2> Spline::frame(ValueType parameter, uint32_t derivative) const
{
    // Min ops, 89 froats

    Matrix<ValueType, 2, 2> basis;
    Matrix<ValueType, 2, 1> tangent = (*this)(parameter, 1);
    ValueType speed = tangent.norm();

    if(derivative >= 0)
    {
        basis.col(0) = tangent / speed;

        // Rotate the first column by 90 degrees (right handed frame) to form the
        // full basis for R^2
        basis.col(1) = basis.col(0).reverse();
        basis(0, 1) = -basis(0, 1);
    }

    if(derivative >= 1)
    {
        Matrix<ValueType, 2, 1> accel = (*this)(parameter, 2);
        basis.col(0) = ( accel - accel.dot(basis.col(0)) * basis.col(0) ) / speed;
    }

    if(derivative >= 2)
        throw InvalidParameterException();

    // Rotate the first column by 90 degrees (right handed frame) to form the
    // full basis for R^2
    basis.col(1) = basis.col(0).reverse();
    basis(0, 1) = -basis(0, 1);
    return basis;
}

Spline::ValueType Spline::nearestPoint(Matrix<ValueType, 2, 1> point, ValueType lambdaStar) const
{
    auto error =
        [point, this](ValueType l) -> ValueType {
            return (point - this->operator()(l)).squaredNorm();
        };
    auto gradient =
        [point, this](ValueType l) -> ValueType {
            Matrix<ValueType, 2, 1> verr = point - this->operator()(l);
            return -2*verr.dot(this->operator()(l, 1));
        };

    ValueType lambda = lambdaStar;
    ValueType alpha = static_cast<ValueType>(1.0);
    alpha = std::min(alpha, alpha / speed(lambda));

    ValueType improvement = 1;
    int i = 500;
    while(std::abs(gradient(lambda)) > 1e-6 && (i > 0))
    {
        ValueType estimate = lambda - alpha * gradient(lambda);
        improvement = error(estimate) - error(lambda);
        if( improvement < 0 )
        {
            lambda = estimate;
            alpha *= 1.2;
        }
        else
            alpha *= 0.7;
        --i;
    }

    return std::max(std::min(lambda, static_cast<ValueType>(1.0)), static_cast<ValueType>(0.0));
}

int Spline::splineIndexUsed(ValueType parameter) const
{
    return std::max(0, std::min(mSplineCount - 1, (int)std::floor(mSplineCount * parameter)));
}

Spline::ValueType Spline::speed(ValueType parameter) const
{
    return (*this)(parameter, 1).norm();
}

Matrix<Spline::ValueType, 2, 1> Spline::operator() (ValueType parameter, uint32_t derivative) const
{
    using Vector = Matrix<Spline::ValueType, 2, 1>;
    using PolySpace = Array<Spline::ValueType, CoeffCount, 1>;

    // Identically zero derivative
    if( derivative > PolyOrder )
        return Vector::Zero();

    // Calculate x^0, x^1, ..., x^5
    PolySpace value = PolySpace::Constant(parameter);
    PolySpace powers = PolySpace::LinSpaced(0, PolyOrder);
    value = pow(value, powers);

    // Choose spline based on parameter value.
    int indSpline = splineIndexUsed(parameter);

    // And evaluate!
    switch(derivative)
    {
    case 0:
        return mPoly.block<2, CoeffCount>(2 * indSpline, 0) * value.matrix();

    case 1:
        return mDPoly.block<2, CoeffCount>(2 * indSpline, 0) * value.matrix();

    case 2:
        return mDDPoly.block<2, CoeffCount>(2 * indSpline, 0) * value.matrix();

    default:
        throw InvalidParameterException();
    };
}

void Spline::approximateSelf()
{
    Spline& self = *this;

    const double maxDelta = 1.0 / (2.0 * mSplineCount);
    double startLambda = 0;
    double endLambda = maxDelta;

    mApproximation.clear();
    do
    {
        Spline::Line currentSegment;
        Matrix<ValueType, 2, 1> start = self(startLambda);
        Matrix<ValueType, 2, 1> end = self(endLambda);

        double normedIntegral = 0.0;
        do
        {
            normedIntegral = 0.0;
            endLambda = startLambda / 2.0 + endLambda / 2.0;
            end = self(endLambda);

            Matrix<ValueType, 2, 1> midpoint = self(startLambda / 2.0 + endLambda / 2.0);

            // Integrate numerically
            for(int i = 0; (i < 1000) && (normedIntegral <= 0.1); ++i)
            {
                Matrix<ValueType, 3, 1> dError = Matrix<ValueType, 3, 1>::Constant(0);
                Matrix<ValueType, 3, 1> dLine = Matrix<ValueType, 3, 1>::Constant(0);
                dError.head(2) = self(startLambda + i * (endLambda - startLambda) / 1000)
                               - (start + i * (end - start) / 1000);
                dLine.head(2)  = (end - start) / 1000;
                normedIntegral += dError.cross(dLine).norm();
            }
        } while( normedIntegral > 0.1 );

        geom::set<0, 0>(currentSegment, start[0]);
        geom::set<0, 1>(currentSegment, start[1]);
        geom::set<1, 0>(currentSegment, end[0]);
        geom::set<1, 1>(currentSegment, end[1]);

        startLambda = endLambda;
        endLambda = std::min(endLambda + maxDelta, 1.0);

        mApproximation.push_back(currentSegment);
    } while( startLambda < 1.0 );

    mArcLength = 0;
    for(auto&& line : mApproximation)
    {
        mArcLength += geom::length(line);
    }
}

Matrix<Spline::ValueType, Dynamic, Spline::CoeffCount> Spline::poly() const { return mPoly; }
Matrix<Spline::ValueType, Dynamic, Spline::CoeffCount> Spline::dpoly() const{ return mDPoly; }
Matrix<Spline::ValueType, Dynamic, Spline::CoeffCount> Spline::ddpoly() const{ return mDDPoly; }
Spline::ApproximateSpline const & Spline::approximation() const { return mApproximation; }
Spline::ValueType Spline::arclength() const { return mArcLength; }


#pragma once
#include <autoppl/util/var_expr_traits.hpp>
#include <autoppl/variable.hpp>
#include <autoppl/math/math.hpp>
#include <iostream>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <autoppl/program_analysis/UncertainIntervals/IntervalAnalysis.hpp>
#include <autoppl/global_data.hpp>

namespace ppl {
namespace expr {

#if __cplusplus <= 201703L
template <class BinaryOp, class LHSVarExprType, class RHSVarExprType>
#else
template <class BinaryOp, util::var_expr LHSVarExprType, util::var_expr RHSVarExprType>
#endif
struct BinaryOpNode :
    util::VarExpr<BinaryOpNode<BinaryOp, LHSVarExprType, RHSVarExprType>>
{
#if __cplusplus <= 201703L
	static_assert(util::assert_is_var_expr_v<LHSVarExprType>);
	static_assert(util::assert_is_var_expr_v<RHSVarExprType>);
#endif

	using value_t = std::common_type_t<
		typename util::var_expr_traits<LHSVarExprType>::value_t,
		typename util::var_expr_traits<RHSVarExprType>::value_t
			>;

	BinaryOpNode(const LHSVarExprType& lhs, const RHSVarExprType& rhs)
		: lhs_{lhs}, rhs_{rhs}
	{ assert(lhs.size() == rhs.size() || lhs.size() == 1 || rhs.size() == 1); }

    value_t get_value(size_t i = 0) const {
        auto lhs_value = lhs_.get_value(i);
        auto rhs_value = rhs_.get_value(i);
        return BinaryOp::evaluate(lhs_value, rhs_value);
    }

    size_t size() const { return std::max(lhs_.size(), rhs_.size()); }

    /**
     * Returns ad expression of the binary operation.
     */
    template <class VecRefType, class VecADVarType>
    auto get_ad(const VecRefType& keys,
                const VecADVarType& vars,
                size_t idx = 0) const
    {
        return BinaryOp::evaluate(lhs_.get_ad(keys, vars, idx),
                                  rhs_.get_ad(keys, vars, idx));
    }

    std::set<boost::uuids::uuid> getDeps(){
	std::set<boost::uuids::uuid> used_vars;
	std::set<boost::uuids::uuid> lhs_used_vars = lhs_.getDeps(); //runtime polymorphism: whatever class mean_ is should have it's own getDeps() function
	used_vars.insert(lhs_used_vars.begin(),lhs_used_vars.end());
	std::set<boost::uuids::uuid> rhs_used_vars = rhs_.getDeps();
	used_vars.insert(rhs_used_vars.begin(),rhs_used_vars.end());
	return used_vars;
    }

   DeterministicInterval getInterval(){
        return BinaryOp::getInterval(lhs_, rhs_);
   }

private:
	LHSVarExprType lhs_;
	RHSVarExprType rhs_;

};

struct AddOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x + y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return add(x.getInterval(),y.getInterval());
	}

};

struct SubOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x - y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return sub(x.getInterval(),y.getInterval());
	}

};

struct MultOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x * y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return mult(x.getInterval(),y.getInterval());
	}

};

struct DivOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x / y;
	}

};

struct MaxOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x >= y) ? x : y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return max(x.getInterval(),y.getInterval());
	}

};

struct MinOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x <= y) ? x : y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return min(x.getInterval(),y.getInterval());
	}

};

struct GTEOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x >= y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0.,1.);
	}

};

struct LTEOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x <= y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0.,1.);
	}

};

struct GTOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x > y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0.,1.);
	}

};

struct LTOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x < y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0,1);
	}

};

struct EQOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x == y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0,1);
	}

};

struct SqrtOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return ppl::math::sqrt(x*y);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return sqrt_prod(x.getInterval(),y.getInterval());
	}

};

struct BooleanSwitchOp {
	//left value is a boolean switch
	template <class BooleanSwitchType, class RHSValueType>
	static auto evaluate(BooleanSwitchType x, RHSValueType y)
	{
		return x*y;
	}

	template <class BooleanSwitchType, class RHSValueType>
	static auto getInterval(BooleanSwitchType x, RHSValueType y)
	{
		return y.getInterval();
	}

};

struct MixtureMaxOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x >= y) ? x : y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return union_(x.getInterval(),y.getInterval());
	}

};

} // namespace expr
} // namespace ppl


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef RK_HPP
#define RK_HPP

// Runge-Kutta

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

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
rk(F f, ub::vector<T>& init, T start, T end) {

	ub::vector<T> x, dx1, dx2, dx3, dx4, x1, x2, x3;

	T t = start;
	T dt = end - start;

	x = init;
	dx1 = f(x, t) * dt;
	x1 = x + dx1 / 2.;
	dx2 = f(x1, t + dt/2.) * dt;
	x2 = x + dx2 / 2.;
	dx3 = f(x2, t + dt/2.) * dt;
	x3 = x + dx3;
	dx4 = f(x3, t + dt) * dt;

	init = x + dx1 / 6. + dx2 / 3. + dx3 / 3. + dx4 / 6.;
}

} // namespace kv

#endif // RK_HPP