#ifndef CRADLE_IMAGING_GRADIENT_HPP
#define CRADLE_IMAGING_GRADIENT_HPP

#include <boost/range/functions.hpp>
#include <cradle/math/interpolate.hpp>

namespace cradle {

template<class Color>
void
compute_gradient(
    std::vector<Color>& gradient,
    std::vector<Color> const& colors,
    std::vector<double> const& positions)
{
    assert(positions.size() == colors.size());
    regular_grid<1, double> grid(0, 1, unsigned(gradient.size()));
    interpolate(&gradient, grid, colors, positions);
}

template<class Color>
void
compute_gradient(
    std::vector<Color>& gradient,
    std::vector<Color> const& colors,
    std::vector<double> const& positions,
    linear_function<double> const& scale)
{
    assert(positions.size() == colors.size());
    regular_grid<1, double> grid(
        scale.intercept, scale.slope, unsigned(gradient.size()));
    interpolate(&gradient, grid, colors, positions);
}

} // namespace cradle

#endif


//---------------------------------------------------------------------------//
//!
//! \file   Utility_EvaporationDistribution.hpp
//! \author Aaron Tumulak
//! \brief  Evaporation distribution class declaration. Modified by Alex
//!         Robinson to accommodate units.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_EVAPORATION_DISTRIBUTION_HPP
#define UTILITY_EVAPORATION_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/units/physical_dimensions/energy.hpp>

// FRENSIE Includes
#include "Utility_UnivariateDistribution.hpp"

namespace Utility{

/*! The unit-aware evaporation distribution class
 * \ingroup univariate_distributions
 */
template<typename IndependentUnit, typename DependentUnit>
class UnitAwareEvaporationDistribution : public UnitAwareUnivariateDistribution<IndependentUnit,DependentUnit>
{
  // Only allow construction when the independent unit corresponds to energy
  RESTRICT_UNIT_TO_BOOST_DIMENSION( IndependentUnit, energy_dimension );

  // Typedef for base type
  typedef UnitAwareUnivariateDistribution<IndependentUnit,DependentUnit> BaseType;

  // The distribution multiplier unit traits typedef
  typedef UnitTraits<typename UnitTraits<DependentUnit>::template GetMultipliedUnitType<typename UnitTraits<IndependentUnit>::InverseUnit>::type> DistMultiplierUnitTraits;

  // The distribution multiplier quantity type
  typedef typename DistMultiplierUnitTraits::template GetQuantityType<double>::type DistMultiplierQuantity;

  // The distribution normalization quantity type
  typedef typename BaseType::DistNormQuantity DistNormQuantity;

  // Typedef for QuantityTraits<double>
  typedef QuantityTraits<double> QT;

  // Typedef for QuantityTraits<IndepQuantity>
  typedef QuantityTraits<typename BaseType::IndepQuantity> IQT;

  // Typedef for QuantityTraits<InverseIndepQuantity>
  typedef QuantityTraits<typename BaseType::InverseIndepQuantity> IIQT;

  // Typedef for QuantityTraits<DepQuantity>
  typedef QuantityTraits<typename BaseType::DepQuantity> DQT;

  // Typedef for QuantityTraits<DistMultiplierQuantity>
  typedef QuantityTraits<DistMultiplierQuantity> DMQT;

public:

  //! This distribution type
  typedef UnitAwareEvaporationDistribution<IndependentUnit,DependentUnit> ThisType;

  //! The independent quantity type
  typedef typename BaseType::IndepQuantity IndepQuantity;

  //! The inverse independent quantity type
  typedef typename BaseType::InverseIndepQuantity InverseIndepQuantity;

  //! The dependent quantity type
  typedef typename BaseType::DepQuantity DepQuantity;

  //! Constructor
  template<typename InputIndepQuantityA = IndepQuantity,
	   typename InputIndepQuantityB = IndepQuantity,
	   typename InputIndepQuantityC = IndepQuantity>
  UnitAwareEvaporationDistribution(
                 const InputIndepQuantityA incident_energy =
                 ThisType::getDefaultIncidentEnergy<InputIndepQuantityA>(),
                 const InputIndepQuantityB nuclear_temperature =
                 ThisType::getDefaultNuclearTemperature<InputIndepQuantityB>(),
                 const InputIndepQuantityC restriction_energy =
                 ThisType::getDefaultRestrictionEnergy<InputIndepQuantityC>(),
                 const double constant_multiplier =
                 ThisType::getDefaultConstantMultiplier() );

  //! Copy constructor
  template<typename InputIndepUnit, typename InputDepUnit>
  UnitAwareEvaporationDistribution( const UnitAwareEvaporationDistribution<InputIndepUnit,InputDepUnit>& dist_instance );

  //! Construct distribution from a unitless dist. (potentially dangerous)
  static UnitAwareEvaporationDistribution fromUnitlessDistribution( const UnitAwareEvaporationDistribution<void,void>& unitless_distribution );

  //! Assignment operator
  UnitAwareEvaporationDistribution& operator=( const UnitAwareEvaporationDistribution& dist_instance );

  //! Destructor
  ~UnitAwareEvaporationDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  DepQuantity evaluate( const IndepQuantity indep_var_value ) const override;

  //! Evaluate the PDF
  InverseIndepQuantity evaluatePDF( const IndepQuantity indep_var_value ) const override;

  //! Return a sample from the distribution
  IndepQuantity sample() const override;

  //! Return a sample from the distribution
  static IndepQuantity sample( const IndepQuantity incident_energy,
			       const IndepQuantity nuclear_temperature,
			       const IndepQuantity restriction_energy );

  //! Return a random sample from the distribution, and record the number of trials
  IndepQuantity sampleAndRecordTrials( DistributionTraits::Counter& trials ) const override;

  //! Return a random sample from the corresponding CDF and record the number of trials
  static IndepQuantity sampleAndRecordTrials(
                                       const IndepQuantity incident_energy,
                                       const IndepQuantity nuclear_temperature,
                                       const IndepQuantity restriction_energy,
                                       DistributionTraits::Counter& trials );

  //! Return the upper bound of the distribution independent variable
  IndepQuantity getUpperBoundOfIndepVar() const override;

  //! Return the lower bound of the distribution independent variable
  IndepQuantity getLowerBoundOfIndepVar() const override;

  //! Return the distribution type
  UnivariateDistributionType getDistributionType() const override;

  //! Test if the distribution is continuous
  bool isContinuous() const override;

  //! Method for placing the object in an output stream
  void toStream( std::ostream& os ) const override;

  //! Equality comparison operator
  bool operator==( const UnitAwareEvaporationDistribution& other ) const;

  //! Inequality comparison operator
  bool operator!=( const UnitAwareEvaporationDistribution& other ) const;

protected:

  //! Copy constructor (copying from unitless distribution only)
  UnitAwareEvaporationDistribution( const UnitAwareEvaporationDistribution<void,void>& unitless_dist_instance, int );

  //! Test if the dependent variable can be zero within the indep bounds
  bool canDepVarBeZeroInIndepBounds() const override;

  //! Get the default incident energy
  template<typename InputIndepQuantity>
  static InputIndepQuantity getDefaultIncidentEnergy()
  { return QuantityTraits<InputIndepQuantity>::one(); }

  //! Get the default nuclear temperature
  template<typename InputIndepQuantity>
  static InputIndepQuantity getDefaultNuclearTemperature()
  { return QuantityTraits<InputIndepQuantity>::one(); }

  //! Get the default restriction energy
  template<typename InputIndepQuantity>
  static InputIndepQuantity getDefaultRestrictionEnergy()
  { return QuantityTraits<InputIndepQuantity>::zero(); }

  //! Get the default constant multiplier
  static double getDefaultConstantMultiplier()
  { return 1.0; }

private:

  // Calculate the normalization constant of the distribution
  void calculateNormalizationConstant();

  // Verify that the shape parameters are valid
  template<typename InputIndepQuantity, typename InputDistMultQuantity>
  static void verifyValidShapeParameters( InputIndepQuantity& incident_energy,
                                          InputIndepQuantity& nuclear_temp,
                                          InputIndepQuantity& restriction_energy,
                                          InputDistMultQuantity& multiplier );

  // Save the distribution to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const;

  // Load the distribution from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version );

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // All possible instantiations are friends
  template<typename FriendIndepUnit, typename FriendDepUnit>
  friend class UnitAwareEvaporationDistribution;

  // The distribution type
  static const UnivariateDistributionType distribution_type = EVAPORATION_DISTRIBUTION;

  // The incident neutron energy of the distribution
  IndepQuantity d_incident_energy;

  // The nuclear temperature of the distribution
  IndepQuantity d_nuclear_temperature;

  // The restriction energy of the distribution
  IndepQuantity d_restriction_energy;

  // The distribution multiplier
  DistMultiplierQuantity d_multiplier;

  // The distribution normalization constant
  DistNormQuantity d_norm_constant;
};

/*! The evaporation distribution (unit-agnostic)
 * \ingroup univariate_distributions
 */
typedef UnitAwareEvaporationDistribution<void,void> EvaporationDistribution;

} // end Utility namespace

BOOST_SERIALIZATION_DISTRIBUTION2_VERSION( UnitAwareEvaporationDistribution, 0 );
BOOST_SERIALIZATION_DISTRIBUTION2_EXPORT_STANDARD_KEY( EvaporationDistribution );

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_EvaporationDistribution_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_EVAPORATION_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end Utility_EvaporationDistribution.hpp
//---------------------------------------------------------------------------//


#include <cassert>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include <iomanip>
#include <armadillo>

#include "constant.hpp"
#include "mt_random.hpp"

using namespace std;

namespace willow {

// global variables
const  int m_nnhc  = 4;
static int m_nbead;
static int m_nstep;
static int m_irst;
static int m_nref;
static double m_dt;
static double m_dt_ref;
static double m_gfree_nm;

static double m_Dmorse; // (De) in Morse potential: De (1 - exp(-a*x))**2
static double m_ZPE;
static double m_omega;
static double m_omega_p2;
static double m_temp_nm;
static double m_beta_nm;
static double m_ekin_nm;

const  double delt_x = 4.e-3;

static arma::vec prob_dx;      // (0:399)

static double     m_mass;

static arma::mat  m_tmat_nm;  // (m_nbead, m_nbead)
static arma::vec  m_fict_mass;// (m_nbead)

static arma::vec  m_rbath_nm; // (m_nnhc)
static arma::mat  m_vbath_nm; //
static arma::mat  m_qmass_nm; //

// one-dimensional system
// Cartesian Coordinate
static arma::vec  m_pos_qm;   // (m_nbead)
static arma::vec  m_grd_qm;   // (m_nbead)

// Normal Mode
static arma::vec  m_pos_nm;   // (m_nbead)
static arma::vec  m_vel_nm;   // (m_nbead)
static arma::vec  m_grd_nm;   // (m_nbead)
static arma::vec  m_grd_nm_spr; //(m_nbead)

static void sample_rho ()
{

  // QM (beads)

  for (auto ib = 0; ib < m_nbead; ++ib) {

    // distribution of beads around pos(0)
    // \rho(x) = \varrho (x1 - X1)
    double dx   = m_pos_qm(ib);
    int    id1  = (int) round(dx/delt_x) + 200;
    if (id1 >= 0 && id1 < 400) prob_dx (id1) += 1;

  }

}

void nm_nhc_integrate ()
{
  // Nose-Hoover Chain Method

  const double dt_ref  = m_dt_ref;
  const double dt_ref2 = 0.5*dt_ref;
  const double dt_ref4 = 0.5*dt_ref2;
  const double dt_ref8 = 0.5*dt_ref4;

  // m_nnhc = 4
  arma::vec4 rbath;
  arma::vec4 vbath;
  arma::vec4 fbath;
  arma::vec4 qmass;

  // ---
  //
  double ekin_nm = 0.0;

  for (size_t ib = 1; ib < m_nbead; ++ib) {

    double mass = m_fict_mass (ib);
    double v    = m_vel_nm(ib);

    ekin_nm += mass * v * v;

  }

  //---

  vbath = m_vbath_nm;
  rbath = m_rbath_nm;
  qmass = m_qmass_nm;

  // ihc = 0
  fbath(0) = (ekin_nm - m_gfree_nm*m_ekin_nm)/qmass(0);

  for (size_t ihc = 1; ihc < m_nnhc; ++ihc) {
    fbath(ihc) =
      (qmass(ihc-1)*vbath(ihc-1)*vbath(ihc-1)	- m_ekin_nm) / qmass(ihc);
  }

  // Update Thermostat Velocities

  vbath(m_nnhc-1) = vbath(m_nnhc-1) + fbath(m_nnhc-1)*dt_ref4;

  for (auto ihc = 1; ihc < m_nnhc; ++ihc) {
    const auto jhc     = m_nnhc - ihc;
    const double vfact = exp (-vbath(jhc)*dt_ref8);
    const double vtmp  = vbath(jhc-1);
    vbath(jhc-1) = vtmp*vfact*vfact + fbath(jhc-1)*vfact*dt_ref4;
  }

  // Update atomic velocities
  const double pvfact = exp(-vbath(0)*dt_ref2);

  ekin_nm  = ekin_nm*pvfact*pvfact;

  // Update thermostat forces

  fbath(0) = (ekin_nm - m_gfree_nm*m_ekin_nm)/qmass(0);

  for (auto ihc = 0; ihc < m_nnhc; ++ihc) {
    rbath(ihc) += vbath(ihc)*dt_ref2;
  }

  // Update Thermostat velocities

  for (auto ihc = 0; ihc < m_nnhc-1; ++ihc) {
    const double vfact = exp(-vbath(ihc+1)*dt_ref8);
    const double vtmp  = vbath(ihc);

    vbath(ihc) = vtmp*vfact*vfact + fbath(ihc)*vfact*dt_ref4;
    fbath(ihc+1) =
      (qmass(ihc)*vbath(ihc)*vbath(ihc) - m_ekin_nm)/qmass(ihc+1);
  }

  vbath(m_nnhc-1) += fbath(m_nnhc-1)*dt_ref4;

  // backup
  m_vbath_nm = vbath;
  m_rbath_nm = rbath;

  // update velocities of normal modes
  for (auto ib = 1; ib < m_nbead; ++ib)
    m_vel_nm(ib) *= pvfact;

}

void nm_pos_update ()
{

  m_pos_nm += m_dt_ref * m_vel_nm;

}

void nm_grad_spring ()
{

  //
  // centroid gradient is zero
  //
  m_grd_nm_spr(0) = 0.0;

  //
  // Gradients from the Springs between 'neighboring' beads
  //

  for (size_t ib = 1; ib < m_nbead; ++ib) {

    const double fact = m_fict_mass(ib)*m_omega_p2;

    m_grd_nm_spr(ib) = fact * m_pos_nm(ib);
  }

}

void nm_pos_trans ()
{
  //
  // normal mode (nm) ---> Cartesian (qm)
  // pos_nm --->  pos_qm

  for (auto ib = 0; ib < m_nbead; ++ib) {

    double pos_x = 0.0;

    for (auto jb = 0; jb < m_nbead; ++jb) {
      pos_x += m_tmat_nm(ib,jb)*m_pos_nm(jb);
    }

    m_pos_qm(ib) = pos_x;
  }

}

void nm_grad_trans ()
{

  m_grd_nm.zeros();

  for (size_t ib = 0; ib < m_nbead; ++ib) {
    for (size_t jb = 0; jb < m_nbead; ++jb) {
      m_grd_nm(ib) += m_tmat_nm(jb,ib)*m_grd_qm(jb);
    }
  }

}

void nm_pos_init ()
{

  {// centroid particle
    m_pos_nm(0) = 0.0;
  }

  { // beads : normal mode
    const double dbead = m_nbead;
    const double usigma = 0.02*ang2bohr; // sigma_x = 0.02 A

    for (size_t ib = 1; ib < m_nbead; ++ib) {

      double mass05 = sqrt(m_fict_mass(ib));
      m_pos_nm(ib) = usigma*rnd::gaus_dev()/mass05;

    }
  } // beads

}

void nm_vel_init ()
{

  // ---- centroid ----
  {
    // Here, vel_nm(ib = 0) zero
    m_vel_nm(0) = 0.0;
  }

  { // velocities for bead particles (or nm-mode particles)

    for (size_t ib = 1; ib < m_nbead; ++ib) {

      double mass   = m_fict_mass(ib);
      double vsigma = sqrt (m_ekin_nm/mass);

      double v = vsigma*rnd::gaus_dev();

      m_vel_nm(ib) = v;

    }

    // --- one particle on one-dimensional harmonic oscillator
    // no correction of translational and rotational motions

    // Scale Velocity
    double ekin_nm = 0.0;

    for (size_t ib = 1; ib < m_nbead; ++ib) {

      double mass  = m_fict_mass(ib);
      double v     = m_vel_nm(ib);
      ekin_nm += mass * v*v;

    }

    double temp  = ekin_nm / (m_gfree_nm*boltz);
    double scale = sqrt(m_temp_nm / temp);

    for (size_t ib = 1; ib < m_nbead; ++ib) {

      m_vel_nm(ib) *= scale;

    }

  } // velocities for bead particles

}

void nm_vel_update ()
{

  // (ib = 0) belongs to the centroid velocity

  //---
  const double dt2 = 0.5*m_dt;

  for (size_t ib = 1; ib < m_nbead; ++ib) {
    double mass   = m_fict_mass(ib);
    double factor = dt2/mass;
    m_vel_nm(ib) -= factor*m_grd_nm(ib);
  }

}

void nm_vel_spring_update ()
{

  double dt2 = 0.5*m_dt_ref;

  for (size_t ib = 1; ib < m_nbead; ++ib) {

    double mass   = m_fict_mass(ib);
    double factor = dt2/mass;
    m_vel_nm(ib) -=
      factor*m_grd_nm_spr(ib);
  }

}

double nm_pot_grad ()
{

  // beads: normal mode ---> cartesian
  //        pos_nm ----> pos_qm
  nm_pos_trans ();

  double u_vib = 0.0;

  m_grd_qm.zeros();

  //
  // force constant: reduced_mass*omega*omega
  //

  double k_val = m_mass*m_omega*m_omega;

  // Harmonic Oscilltor
  // U = 0.5 * k * x**2
  //
  for (size_t ib = 0; ib < m_nbead; ib++) {

    // call your potential.
    double dx      = m_pos_qm(ib);
    double en_harm = 0.5*k_val*dx*dx;
    m_grd_qm(ib)   =  k_val*dx;

    u_vib += en_harm;
  } // ib

  // ---
  double d_nbead = m_nbead;
  u_vib /= d_nbead;

  m_grd_qm /= d_nbead;

  //
  // cartesian gradient ---> normal mode gradient
  //
  nm_grad_trans ();

  return u_vib;

}

void nm_report (const int& istep,
		const double& u_vib,
		double& E_eff)
{
  // kinetic energy for beads
  double ekin_nm = 0.0;

  for (auto ib = 1; ib < m_nbead; ++ib) {
    double mass = m_fict_mass(ib);
    double v    = m_vel_nm(ib);
    ekin_nm += mass*v*v;
  }

  ekin_nm = 0.5*ekin_nm;
  double temp_nm = 2.0*ekin_nm/(m_gfree_nm*boltz);

  // Harmonic Potential of springs between neighboring beads

  double qkin_nm = 0.0;
  for (auto ib = 1; ib < m_nbead; ++ib) {
    double fact  = 0.5*m_fict_mass(ib)*m_omega_p2;
    double q     = m_pos_nm(ib);
    qkin_nm += fact*q*q;

  }

  double ebath_nm = 0.0;

  arma::vec4 qmass = m_qmass_nm;
  ebath_nm += 0.5*qmass(0)*m_vbath_nm(0)*m_vbath_nm(0);
  ebath_nm += m_gfree_nm*m_ekin_nm*m_rbath_nm(0);

  for (auto ihc = 1; ihc < m_nnhc; ++ihc) {
    ebath_nm += 0.5*qmass(ihc)*m_vbath_nm(ihc)*m_vbath_nm(ihc);
    ebath_nm += m_ekin_nm*m_rbath_nm(ihc);
  }

  E_eff = qkin_nm + u_vib; // <E_eff> = E_ZPE
  double H_sys = ekin_nm + E_eff; // Hamiltonian of the system
  double H_tot = H_sys + ebath_nm; // Total H.

  // unit convert
  E_eff *= au_kcal;
  H_sys *= au_kcal;
  H_tot *= au_kcal;

  printf (" %8d %14.6f %14.6f %14.6f %14.6f %10.2f \n",
	  (istep+1), H_tot, H_sys, E_eff,
	  u_vib*au_kcal, temp_nm);

  fflush (stdout);

}

void read_restart_nm (int& istep0)
{

  // read a restart file
  std::string str_rst;

  std::ifstream ifs_rst ("pimdrr.sav");
  assert (ifs_rst.good());

  std::ostringstream oss;

  oss << ifs_rst.rdbuf();

  str_rst = oss.str();

  std::istringstream is (str_rst);

  std::string line;
  std::getline (is, line); // istep
  std::istringstream istep_ss (line);
  istep_ss >> istep0;

  // ib = 0 is for centroids
  for (auto ib = 1; ib < m_nbead; ++ib) {
    std::getline (is, line);
    std::istringstream iss_pos (line);

    iss_pos >> m_pos_nm(ib);

    std::getline (is, line);
    std::istringstream iss_vel (line);

    iss_vel >> m_vel_nm(ib);
  }

  for (auto ihc = 0; ihc < m_nnhc; ++ihc) {
    std::getline (is,line);
    std::istringstream iss (line);

    iss >> m_rbath_nm(ihc);
  }

  // position and velocity of the centroid particle
  m_pos_nm(0) = 0.0;
  m_vel_nm(0) = 0.0;

}

void write_restart_nm (const int& istep)
{

  FILE *ofs_rst = fopen ("pimdrr.sav", "w");

  fprintf( ofs_rst, " %10d \n", istep+1);

  // ib = 0 is for centroids
  for (auto ib = 1; ib < m_nbead; ++ib) {
    fprintf (ofs_rst, "  %E   \n", m_pos_nm(ib) );
    fprintf (ofs_rst, "  %E   \n", m_vel_nm(ib) );
  }

  for (auto ihc = 0; ihc < m_nnhc; ++ihc) {
    fprintf (ofs_rst, "  %E   %E   %E \n",
	     m_rbath_nm(ihc),
	     m_vbath_nm(ihc),
	     m_qmass_nm(ihc) );
  }

  fclose (ofs_rst);

}

void write_prob_bin (int& nsamp)
{

  double zeta2   = m_mass*m_omega; // (unit 1/bohr**2)

  arma::vec p_dx = prob_dx/(m_nbead*(nsamp+1)*delt_x);

  std::ofstream ofs_rho ("prob_bin_rho.dat");
  // psi_0
  // zeta2 = mass*omega*(x*x)
  // debug
  const double zt    = sqrt(zeta2/M_PI);

  for (auto ib = 0; ib < 400; ++ib) {
    double  x  = (ib - 200)*delt_x;
    double  x2 =  x* x;
    double rho =  zt*exp(-zeta2*x2);
    ofs_rho << x << "   "  << p_dx(ib)
	   << "  " << rho << endl;
  }

  ofs_rho.close();

}

void wpimd_run ()
{

  //
  // This is a sample code,
  // in which WPIMD is running at T (thermal temperature) = 0 K.
  //

  int istep0 = 0;

  if (m_irst == 1) {
    read_restart_nm (istep0);
  }

  // initial gradients and potential energy
  double u_vib = nm_pot_grad ();

  double e_eff = 0.0;
  double sum_e_eff = 0.0;
  int    ncount    = 0;

  nm_grad_spring ();

  if (m_irst == 0)
    nm_report (istep0-1, u_vib, e_eff);

  for (auto istep = 0; istep < m_nstep; ++istep) {

    nm_vel_update ();

    for (auto iref = 0; iref < m_nref; ++iref) {
      nm_nhc_integrate ();
      nm_vel_spring_update();
      nm_pos_update ();
      nm_grad_spring();
      nm_vel_spring_update();
      nm_nhc_integrate ();
    }

    u_vib   = nm_pot_grad ();

    sample_rho ();

    nm_vel_update ();

    if ( (istep+1)%500 == 0) {
      nm_report (istep+istep0, u_vib, e_eff);
      sum_e_eff += e_eff;
      ncount++;
    }

    if ( (istep+1)%5000 == 0) {
      write_restart_nm   (istep+istep0);
      write_prob_bin (istep);
    }

  }

  cout << "AVE E_eff " << sum_e_eff / ncount << endl;

}

void wpimd_init ()
{

  // one-dimensional system
  m_gfree_nm = m_nbead;

  // ZPE = 0.5 * hbar * omega
  // ZPE(au) = 0.5 * omega
  m_omega    = 2.0*m_ZPE;

  // Eq. (14)
  // temperature for the bead motions
  m_temp_nm  = m_omega/(m_nbead*boltz);

  double dbead = m_nbead;
  double omega_p = sqrt(dbead)*boltz*m_temp_nm;
  m_omega_p2 = omega_p * omega_p;
  m_beta_nm  = 1.0 / (boltz*m_temp_nm);
  m_ekin_nm  = boltz*m_temp_nm;

  // mem alloc

  m_tmat_nm   = arma::mat (m_nbead, m_nbead, arma::fill::zeros);
  m_fict_mass = arma::vec (m_nbead, arma::fill::zeros);

  m_rbath_nm  = arma::vec (m_nnhc,  arma::fill::zeros);
  m_vbath_nm  = arma::vec (m_nnhc,  arma::fill::zeros);
  m_qmass_nm  = arma::vec (m_nnhc,  arma::fill::zeros);

  // one-dimensional system
  m_pos_qm    = arma::vec (m_nbead, arma::fill::zeros);
  m_pos_nm    = arma::vec (m_nbead, arma::fill::zeros);

  m_vel_nm    = arma::vec (m_nbead, arma::fill::zeros);

  m_grd_qm    = arma::vec (m_nbead, arma::fill::zeros);
  m_grd_nm    = arma::vec (m_nbead, arma::fill::zeros);
  m_grd_nm_spr= arma::vec (m_nbead, arma::fill::zeros);

  // --- initiate the normal mode matrix.
  for (size_t i = 0; i < m_nbead; ++i) {
    m_tmat_nm(i, 0) = 1.0;
  }

  for (size_t i = 0; i < m_nbead/2; ++i) {
    m_tmat_nm(2*i,   m_nbead-1) = -1.0;
    m_tmat_nm(2*i+1, m_nbead-1) =  1.0;
  }

  double dnorm = sqrt (2.0);

  for (size_t i = 0; i < m_nbead; ++i) {
    const double di    = i+1;
    const double phase = 2.0*di*(M_PI/dbead);
    for (size_t j = 0; j < (m_nbead-2)/2; ++j) {
      const double dj    = j+1;
      m_tmat_nm(i, 2*j+1) = dnorm*cos(phase*dj);
      m_tmat_nm(i, 2*j+2) = dnorm*sin(phase*dj);
    }
  }

  // --- mass init ---
  double mass = m_mass;

  m_fict_mass(0)         = mass;
  m_fict_mass(m_nbead-1) = 4.0*dbead*mass;

  for (auto ib = 1; ib < m_nbead/2; ++ib) {
    double val = 2.0*(1.0 - cos (2.0*ib*(M_PI/dbead)))*dbead*mass;
    m_fict_mass(2*ib-1) = val;
    m_fict_mass(2*ib  ) = val;
  }

  // bath init for beads

  m_qmass_nm(0) = m_gfree_nm*m_ekin_nm/m_omega_p2;

  for (size_t ihc = 1; ihc < m_nnhc; ++ihc) {
    m_qmass_nm(ihc) = m_ekin_nm/m_omega_p2;
  }

  nm_pos_init ();
  nm_vel_init ();

  prob_dx      = arma::vec(400, arma::fill::zeros);

}

void read_input (const std::string& fname)
{

  std::ifstream is_input(fname);
  std::ostringstream oss;
  oss << is_input.rdbuf();

  std::istringstream ss (oss.str());

  m_nstep = 1000;
  m_dt   = 0.5; // fsec
  m_irst = 0;

  m_nbead = 8;
  m_nref  = 10;
  m_ZPE   = 0.0; // kcal/mol

  m_mass  = 1.0*amu2au; // amu --> au

  std::string line;

  while(getline(ss, line)) {
    std::istringstream iss (line);
    std::string keyword;
    std::string val;
    iss >> keyword >> val;

    if (keyword == "nstep") {
      m_nstep = stoi (val);
    }
    else if (keyword == "dt") {
      m_dt = stod(val);
    }
    else if (keyword == "l_restart") {
      m_irst = stoi(val);
    }
    else if (keyword == "nbead") {
      m_nbead = stoi(val);
    }
    else if (keyword == "nref") {
      m_nref  = stoi(val);
    }
    else if (keyword == "ZPE") {
      m_ZPE  = stod(val)/au_kcal; // kcal/mol ---> au
    }
    else if (keyword == "mass1") {
      m_mass = stod(val)*amu2au; // amu --> au
    }
    //else if (keyword == "mass2") {
    //  m_mass(1) = stod(val)*amu2au; // amu --> au
    //}

  }

  // -- time : [fs] ---> [au]
  m_dt  = m_dt * (1.0e-15/au_time);

  m_dt_ref = m_dt/m_nref;

}

} // namespace willow

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

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

  //--- read an input file --
  const std::string fname = (argc > 1) ? argv[1] : "sample1.inp";

  willow::read_input (fname);

  willow::wpimd_init ();
  willow::wpimd_run ();

  return 0;

}


/*
 * Copyright (c) 2019 Nobuyuki Umetani
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "delfem2/v23m3q.h"

#if defined(__APPLE__) && defined(__MACH__)
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#elif defined(__MINGW32__) // probably I'm using Qt and don't want to use GLUT
#include <GL/glu.h>
#elif defined(_WIN32)
#include <windows.h>
#include <GL/glu.h>
#else
#include <GL/glu.h>
#endif

#if defined(__APPLE__) && defined(__MACH__)
#include "Eigen/Dense"
#else
#include <Eigen/Dense>
#endif

#include "delfem2/eigen/eigen_rigidbody.h"

void myUpdateMinMaxXYZ
(double bb[6],
 const CVector3 p)
{
  const double x = p.x;
  const double y = p.y;
  const double z = p.z;
  if( bb[0] > bb[1] ){
    bb[0] = bb[1] = x;
    bb[2] = bb[3] = y;
    bb[4] = bb[5] = z;
    return;
  }
  bb[0] = (bb[0] < x) ? bb[0] : x;
  bb[1] = (bb[1] > x) ? bb[1] : x;
  bb[2] = (bb[2] < y) ? bb[2] : y;
  bb[3] = (bb[3] > y) ? bb[3] : y;
  bb[4] = (bb[4] < z) ? bb[4] : z;
  bb[5] = (bb[5] > z) ? bb[5] : z;
}

// -----------------------------------------------

void EdEd_Potential
(double& energy,
 CVector3& dEdu,
 CVector3& dEdw,
 const CVector3& cg,
 const CVector3& u,
 const double mass,
 const CVector3& g       // floor normal
)
{
  energy = mass*(u*g);
  dEdu = mass*g;
  dEdw = CVector3(0,0,0);
}

void EdEddE_Exforce
(double& energy,
 CVector3& dEdu, CVector3& dEdw,
 CMatrix3&  ddEddu,  CMatrix3&  ddEddw, CMatrix3&  ddEdudw,
 const CVector3& cg, // the center of gravity
 const CVector3& pex, // external force position
 const CVector3& fex, // external force
 const CVector3& u, // displacement
 const CMatrix3& R // rigid rotation
)
{
  CVector3 Rv = R * (pex-cg);
  CVector3 qex = Rv + cg + u; // current external force position
  energy = +qex*fex;
  dEdu = +fex;
  dEdw = +Rv^fex;
  ddEddu  = CMatrix3(0.0);
  ddEddw  = +Mat3_Spin(fex)*Mat3_Spin(Rv);
  ddEdudw = CMatrix3(0.0);
}

void EdEddE_Contact
(double& energy,
 CVector3& dEdu, CVector3& dEdw,
 CMatrix3&  ddEddu,  CMatrix3&  ddEddw, CMatrix3&  ddEdudw,
 const CVector3& cg, // the center of gravity
 const CVector3& cp, // contact position
 const CVector3& u, // displacement
 const CMatrix3& R, // rigid rotation
 const double cont_stiff,
 const CVector3& n       // floor normal
)
{
  CVector3 Rv = R * (cp-cg);
  CVector3 cq = Rv + cg + u;
  energy = 0.5*(cq*n)*(cq*n)*cont_stiff;
  dEdu = ((cq*n)*cont_stiff)*n;
  dEdw = ((cq*n)*cont_stiff)*(Rv^n);
  ddEddu  = cont_stiff*Mat3_OuterProduct(n,n);
  ddEddw  = cont_stiff*Mat3_OuterProduct(Rv^n,Rv^n) + ((cq*n)*cont_stiff)*Mat3_Spin(n)*Mat3_Spin(Rv);
  ddEdudw = cont_stiff*Mat3_OuterProduct(Rv^n,n);
}

void EdEddE_ContactFriction
(double& energy,
 CVector3& dEdu, CVector3& dEdw,
 CMatrix3&  ddEddu,  CMatrix3&  ddEddw, CMatrix3&  ddEdudw,
 const CVector3& cg, // the center of gravity
 const CVector3& cp, // contact position
 const CVector3& u, // displacement
 const CMatrix3& R, // rigid rotation
 const double cont_stiff
 )
{
  CVector3 Rv = R * (cp-cg);
  CVector3 cq = Rv + cg + u;
  energy = 0.5*(cq-cp)*(cq-cp)*cont_stiff;
  dEdu = cont_stiff*(cq-cp);
  dEdw = cont_stiff*(Rv^(cq-cp));
  ddEddu  = cont_stiff*CMatrix3::Identity();
  //    ddEddw  = -cont_stiff*CMatrix3::OuterProduct(Rv,Rv) + cont_stiff*CMatrix3::Spin(Rv)*CMatrix3::Spin(cq-cp);
  ddEddw  = -cont_stiff*Mat3_Spin(Rv)*Mat3_Spin(Rv) + cont_stiff*Mat3_Spin(cq-cp)*Mat3_Spin(Rv);
  ddEdudw = cont_stiff*Mat3_Spin(Rv);
}

void EdEddE_Joint
(double& energy,
 CVector3& dEdu0, CVector3& dEdw0, CVector3& dEdu1, CVector3& dEdw1,
 ////
 CMatrix3& ddEdu0du0,  CMatrix3& ddEdu0dw0, CMatrix3& ddEdu0du1, CMatrix3& ddEdu0dw1, CMatrix3& ddEdw0dw0,
 CMatrix3& ddEdw0du1,  CMatrix3& ddEdw0dw1, CMatrix3& ddEdu1du1, CMatrix3& ddEdu1dw1, CMatrix3& ddEdw1dw1,
 ////
 const double trans_stiff,
 const double rot_stiff,
 const CVector3& pj,
 ////
 const CVector3& cg0,  const CVector3& u0,  const CMatrix3& R0,
 const CVector3& cg1,  const CVector3& u1,  const CMatrix3& R1
 )
{
  CVector3 Rv0 = R0 * (pj-cg0);
  CVector3 qj0 = Rv0 + cg0 + u0; // after deformation joint pos relative to rigid body 0

  CVector3 Rv1 = R1 * (pj-cg1);
  CVector3 qj1 = Rv1 + cg1 + u1; // after deformation joint pos relative to rigid body 1

  energy = 0.5*trans_stiff*(qj0 - qj1).DLength();

  dEdu0 = trans_stiff*(qj0-qj1);
  dEdw0 = trans_stiff*(Rv0^(qj0-qj1));

  dEdu1 = trans_stiff*(qj1-qj0);
  dEdw1 = trans_stiff*(Rv1^(qj1-qj0));

  ddEdu0du0 =  trans_stiff*CMatrix3::Identity();
  ddEdu0du1 = -trans_stiff*CMatrix3::Identity();
  ddEdu0dw0 = +trans_stiff*Mat3_Spin(Rv0);
  ddEdu0dw1 = -trans_stiff*Mat3_Spin(Rv1);

  ddEdw0dw0 = -trans_stiff*Mat3_Spin(Rv0)*Mat3_Spin(Rv0) + trans_stiff*Mat3_Spin(qj0-qj1)*Mat3_Spin(Rv0);
  ddEdw0du1 = +trans_stiff*Mat3_Spin(Rv0);
  ddEdw0dw1 = +trans_stiff*Mat3_Spin(Rv1)*Mat3_Spin(Rv0);

  ddEdu1du1 =  trans_stiff*CMatrix3::Identity();
  ddEdu1dw1 = +trans_stiff*Mat3_Spin(Rv1);

  ddEdw1dw1 = -trans_stiff*Mat3_Spin(Rv1)*Mat3_Spin(Rv1) + trans_stiff*Mat3_Spin(qj1-qj0)*Mat3_Spin(Rv1);

  CVector3 av(0,0,0);
  CMatrix3 davdw0, davdw1;
  {
    for(unsigned int i=0;i<3;i++){
      CVector3 r0( R0.mat[0*3+i], R0.mat[1*3+i], R0.mat[2*3+i] );
      CVector3 r1( R1.mat[0*3+i], R1.mat[1*3+i], R1.mat[2*3+i] );
      av += (r0^r1);
      davdw0 += Mat3_Spin(r1)*Mat3_Spin(r0);
      davdw1 -= Mat3_Spin(r0)*Mat3_Spin(r1);
    }
    av *= 0.5;
    davdw0 *= 0.5;
    davdw1 *= 0.5;
  }
  energy += 0.5*rot_stiff*av.DLength();

  CMatrix3 m0,m1,m2;
  for(unsigned int i=0;i<3;i++){
    CVector3 r0( R0.mat[0*3+i], R0.mat[1*3+i], R0.mat[2*3+i] );
    CVector3 r1( R1.mat[0*3+i], R1.mat[1*3+i], R1.mat[2*3+i] );
    dEdw0 += 0.5*rot_stiff*r0^(r1^av);
    dEdw1 -= 0.5*rot_stiff*r1^(r0^av);
    ddEdw0dw0 += 0.5*rot_stiff*( Mat3_Spin(r1^av)*Mat3_Spin(r0) + Mat3_Spin(r0)*Mat3_Spin(r1)*davdw0 );
    ddEdw1dw1 -= 0.5*rot_stiff*( Mat3_Spin(r0^av)*Mat3_Spin(r1) + Mat3_Spin(r1)*Mat3_Spin(r0)*davdw1 );
    ddEdw0dw1 -= 0.5*rot_stiff*(  Mat3_Spin(r1)*Mat3_Spin(av)*Mat3_Spin(r0)
                                + Mat3_Spin(r1)*Mat3_Spin(r0)*davdw0 );
  }
}

CVector3 rand_vec(double s)
{
  CVector3 v;
  v.x = s*rand()/(RAND_MAX+1.0);
  v.y = s*rand()/(RAND_MAX+1.0);
  v.z = s*rand()/(RAND_MAX+1.0);
  return v;
}

CMatrix3 rand_rot()
{
  double s = 3.5;
  CVector3 v;
  v.x = s*rand()/(RAND_MAX+1.0);
  v.y = s*rand()/(RAND_MAX+1.0);
  v.z = s*rand()/(RAND_MAX+1.0);
  CMatrix3 R;  R.SetRotMatrix_Cartesian(v.x,v.y,v.z);
  return R;
}

void CheckDiff_Contact()
{
  double energy = 0.0;
  CVector3 dEdu,dEdw;
  CMatrix3 ddEddu, ddEddw, ddEdudw;
  double epsilon = 1.0e-5;
  const double cont_stiff = 1.0e+3;
  CVector3 cg = rand_vec(1.0);
  CVector3 cp = rand_vec(1.0);
  CVector3 u  = rand_vec(1.0);
  CMatrix3  R  = rand_rot();
  CVector3 n(0,0,1);
  EdEddE_Contact(energy,
                 dEdu,dEdw,  ddEddu,ddEddw,ddEdudw,
                 cg,cp,u,R,
                 cont_stiff,n);
  for(unsigned int idim=0;idim<3;idim++){
    CVector3 dEdu_, dEdw_;
    CMatrix3 ddEddu_, ddEddw_, ddEdudw_;
    double energy_ = 0.0;
    CVector3 u_ = u;
    u_[idim] += epsilon;
    EdEddE_Contact(energy_,  dEdu_,dEdw_,   ddEddu_,ddEddw_,ddEdudw_,
                   cg,cp,u_,R,
                   cont_stiff,n);
    std::cout << "dEdu " << idim << " --> "  << (energy_-energy)/epsilon << " " << dEdu[idim] << std::endl;
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEddu  " << idim << " " << jdim << " --> " << (dEdu_[jdim]-dEdu[jdim])/epsilon << " " << ddEddu.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEdudw " << idim << " " << jdim << " --> " << (dEdw_[jdim]-dEdw[jdim])/epsilon << " " << ddEdudw.mat[jdim*3+idim] << std::endl;
    }
  }
  for(unsigned int idim=0;idim<3;idim++){
    CVector3 dEdu_, dEdw_;
    CMatrix3 ddEddu_, ddEddw_, ddEdudw_;
    double energy_ = 0.0;
    CVector3 w(0,0,0);
    w[idim] = epsilon;
    CMatrix3 dR; dR.SetRotMatrix_Cartesian(w.x,w.y,w.z);
    CMatrix3 R_ = dR*R;
    EdEddE_Contact(energy_,  dEdu_,dEdw_,   ddEddu_,ddEddw_,ddEdudw_,
                   cg,cp,u,R_,
                   cont_stiff,n);
    std::cout << "dEdw " << idim << " --> " << (energy_-energy)/epsilon << " " << dEdw[idim] << std::endl;
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEdudw " << idim << " " << jdim << " --> " << (dEdu_[jdim]-dEdu[jdim])/epsilon << " " << ddEdudw.mat[idim*3+jdim] << std::endl;
    }
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEddw  " << idim << " " << jdim << " --> " << (dEdw_[jdim]-dEdw[jdim])/epsilon << " " << ddEddw.mat[jdim*3+idim] << std::endl;
    }
  }
}

void CheckDiff_ContactFriction()
{
  double energy = 0.0;
  CVector3 dEdu,dEdw;
  CMatrix3 ddEddu, ddEddw, ddEdudw;
  double epsilon = 1.0e-5;
  const double cont_stiff = 1.0e+3;
  CVector3 cg = rand_vec(1.0);
  CVector3 cp = rand_vec(1.0);
  CVector3 u  = rand_vec(1.0);
  CMatrix3  R  = rand_rot();
  EdEddE_ContactFriction(energy,
                         dEdu,dEdw,  ddEddu,ddEddw,ddEdudw,
                         cg,cp,u,R,
                         cont_stiff);
  for(unsigned int idim=0;idim<3;idim++){
    CVector3 dEdu_, dEdw_;
    CMatrix3 ddEddu_, ddEddw_, ddEdudw_;
    double energy_ = 0.0;
    CVector3 u_ = u;
    u_[idim] += epsilon;
    EdEddE_ContactFriction(energy_,  dEdu_,dEdw_,   ddEddu_,ddEddw_,ddEdudw_,
                           cg,cp,u_,R,
                           cont_stiff);
    std::cout << "dEdu " << idim << " --> "  << (energy_-energy)/epsilon << " " << dEdu[idim] << std::endl;
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEddu  " << idim << " " << jdim << " --> " << (dEdu_[jdim]-dEdu[jdim])/epsilon << " " << ddEddu.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEdudw " << idim << " " << jdim << " --> " << (dEdw_[jdim]-dEdw[jdim])/epsilon << " " << ddEdudw.mat[jdim*3+idim] << std::endl;
    }
  }
  for(unsigned int idim=0;idim<3;idim++){
    CVector3 dEdu_, dEdw_;
    CMatrix3 ddEddu_, ddEddw_, ddEdudw_;
    double energy_ = 0.0;
    CVector3 w(0,0,0);
    w[idim] = epsilon;
    CMatrix3 dR; dR.SetRotMatrix_Cartesian(w.x,w.y,w.z);
    CMatrix3 R_ = dR*R;
    EdEddE_ContactFriction(energy_,  dEdu_,dEdw_,   ddEddu_,ddEddw_,ddEdudw_,
                           cg,cp,u,R_,
                           cont_stiff);
    std::cout << "dEdw " << idim << " --> " << (energy_-energy)/epsilon << " " << dEdw[idim] << std::endl;
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEdudw " << idim << " " << jdim << " --> " << (dEdu_[jdim]-dEdu[jdim])/epsilon << " " << ddEdudw.mat[idim*3+jdim] << std::endl;
    }
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEddw  " << idim << " " << jdim << " --> " << (dEdw_[jdim]-dEdw[jdim])/epsilon << " " << ddEddw.mat[jdim*3+idim] << std::endl;
    }

  }
}

void CheckDiff_Exforce()
{
  double energy = 0.0;
  CVector3 dEdu,dEdw;
  CMatrix3 ddEddu, ddEddw, ddEdudw;
  double epsilon = 1.0e-5;
  CVector3 cg = rand_vec(1.0);
  CVector3 u  = rand_vec(1.0);
  CVector3 fex = rand_vec(1.0);
  CVector3 pex = rand_vec(1.0);
  CMatrix3  R  = rand_rot();
  CVector3 n(0,0,1);
  EdEddE_Exforce(energy,
                 dEdu,dEdw,  ddEddu,ddEddw,ddEdudw,
                 cg,pex,fex,u,R);
  for(unsigned int idim=0;idim<3;idim++){
    CVector3 dEdu_, dEdw_;
    CMatrix3 ddEddu_, ddEddw_, ddEdudw_;
    double energy_ = 0.0;
    CVector3 u_ = u;
    u_[idim] += epsilon;
    EdEddE_Exforce(energy_,  dEdu_,dEdw_,   ddEddu_,ddEddw_,ddEdudw_,
                   cg,pex,fex,u_,R);
    std::cout << "dEdu " << idim << " --> "  << (energy_-energy)/epsilon << " " << dEdu[idim] << std::endl;
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEddu  " << idim << " " << jdim << " --> " << (dEdu_[jdim]-dEdu[jdim])/epsilon << " " << ddEddu.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEdudw " << idim << " " << jdim << " --> " << (dEdw_[jdim]-dEdw[jdim])/epsilon << " " << ddEdudw.mat[jdim*3+idim] << std::endl;
    }
  }
  for(unsigned int idim=0;idim<3;idim++){
    CVector3 dEdu_, dEdw_;
    CMatrix3 ddEddu_, ddEddw_, ddEdudw_;
    double energy_ = 0.0;
    CVector3 w(0,0,0);
    w[idim] = epsilon;
    CMatrix3 dR; dR.SetRotMatrix_Cartesian(w.x,w.y,w.z);
    CMatrix3 R_ = dR*R;
    EdEddE_Exforce(energy_,  dEdu_,dEdw_,   ddEddu_,ddEddw_,ddEdudw_,
                   cg,pex,fex,u,R_);
    std::cout << "dEdw " << idim << " --> " << (energy_-energy)/epsilon << " " << dEdw[idim] << std::endl;
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEdudw " << idim << " " << jdim << " --> " << (dEdu_[jdim]-dEdu[jdim])/epsilon << " " << ddEdudw.mat[idim*3+jdim] << std::endl;
    }
    for(unsigned int jdim=0;jdim<3;jdim++){
      std::cout << " ddEddw  " << idim << " " << jdim << " --> " << (dEdw_[jdim]-dEdw[jdim])/epsilon << " " << ddEddw.mat[jdim*3+idim] << std::endl;
    }
  }
}

void CheckDiff_Joint()
{
  double energy = 0.0;
  CVector3 dEdu0(0,0,0);
  CVector3 dEdw0(0,0,0);
  CVector3 dEdu1(0,0,0);
  CVector3 dEdw1(0,0,0);
  CMatrix3 ddEdu0du0;
  CMatrix3 ddEdu0dw0;
  CMatrix3 ddEdu0du1;
  CMatrix3 ddEdu0dw1;
  CMatrix3 ddEdw0dw0;
  CMatrix3 ddEdw0du1;
  CMatrix3 ddEdw0dw1;
  CMatrix3 ddEdu1du1;
  CMatrix3 ddEdu1dw1;
  CMatrix3 ddEdw1dw1;
  double epsilon = 1.0e-5;
  const double trans_stiff = 1.0e+3;
  const double rot_stiff = 1.0e+3;
  CVector3 pj  = rand_vec(1.0);
  ////
  CVector3 cg0 = rand_vec(1.0);
  CVector3 cp0 = rand_vec(1.0);
  CVector3 u0  = rand_vec(1.0);
  CMatrix3  R0  = rand_rot();
  ////
  CVector3 cg1 = rand_vec(1.0);
  CVector3 cp1 = rand_vec(1.0);
  CVector3 u1  = rand_vec(1.0);
  CMatrix3  R1  = rand_rot();
  ////
  EdEddE_Joint(energy,
               dEdu0, dEdw0, dEdu1,dEdw1,
               ddEdu0du0, ddEdu0dw0, ddEdu0du1, ddEdu0dw1, ddEdw0dw0, ddEdw0du1, ddEdw0dw1, ddEdu1du1, ddEdu1dw1, ddEdw1dw1,
               trans_stiff, rot_stiff, pj,
               cg0, u0, R0,
               cg1, u1, R1);
  for(unsigned int kdim=0;kdim<3;kdim++){
    CVector3 dEdu0_, dEdw0_, dEdu1_, dEdw1_;
    CMatrix3 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_;
    double energy_ = 0.0;
    CVector3 u0_ = u0;
    u0_[kdim] += epsilon;
    EdEddE_Joint(energy_, dEdu0_,dEdw0_, dEdu1_,dEdw1_,
                 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_,
                 trans_stiff, rot_stiff, pj,
                 cg0, u0_, R0,
                 cg1, u1,  R1);
    std::cout << "dEdu0: " << kdim << " -->  " << (energy_-energy)/epsilon << " " << dEdu0[kdim] << std::endl;
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu0du0 " << kdim << " " << idim << " --> " << (dEdu0_[idim]-dEdu0[idim])/epsilon << " " << ddEdu0du0.mat[idim*3+kdim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdw0du0 " << kdim << " " << idim << " --> " << (dEdw0_[idim]-dEdw0[idim])/epsilon << " " << ddEdu0dw0.mat[idim*3+kdim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << "*ddEdu1du0 " << kdim << " " << idim << " --> " << (dEdu1_[idim]-dEdu1[idim])/epsilon << " " << ddEdu0du1.mat[idim*3+kdim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << "*ddEdw1du0 " << kdim << " " << idim << " --> " << (dEdw1_[idim]-dEdw1[idim])/epsilon << " " << ddEdu0dw1.mat[idim*3+kdim] << std::endl;
    }
  }
  for(unsigned int jdim=0;jdim<3;jdim++){
    CVector3 dEdu0_, dEdw0_, dEdu1_, dEdw1_;
    CMatrix3 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_;
    double energy_ = 0.0;
    CVector3 w(0,0,0);
    w[jdim] = epsilon;
    CMatrix3 dR; dR.SetRotMatrix_Cartesian(w.x,w.y,w.z);
    CMatrix3 R0_ = dR*R0;
    EdEddE_Joint(energy_, dEdu0_,dEdw0_, dEdu1_,dEdw1_,
                 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_,
                 trans_stiff, rot_stiff, pj,
                 cg0, u0, R0_,
                 cg1, u1, R1);
    std::cout << "dEdw0: " << jdim << " -->  " << (energy_-energy)/epsilon << " " << dEdw0[jdim] << std::endl;
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu0dw0 " << jdim << " " << idim << " --> " << (dEdu0_[idim]-dEdu0[idim])/epsilon << " " << ddEdu0dw0.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdw0dw0 " << jdim << " " << idim << " --> " << (dEdw0_[idim]-dEdw0[idim])/epsilon << " " << ddEdw0dw0.mat[idim*3+jdim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu1dw0 " << jdim << " " << idim << " --> " << (dEdu1_[idim]-dEdu1[idim])/epsilon << " " << ddEdw0du1.mat[idim*3+jdim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdw1dw0 " << jdim << " " << idim << " --> " << (dEdw1_[idim]-dEdw1[idim])/epsilon << " " << ddEdw0dw1.mat[idim*3+jdim] << std::endl;
    }
  }

  for(unsigned int jdim=0;jdim<3;jdim++){
    CVector3 dEdu0_, dEdw0_, dEdu1_, dEdw1_;
    CMatrix3 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_;
    double energy_ = 0.0;
    CVector3 u1_ = u1;
    u1_[jdim] += epsilon;
    EdEddE_Joint(energy_, dEdu0_,dEdw0_, dEdu1_,dEdw1_,
                 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_,
                 trans_stiff, rot_stiff, pj,
                 cg0, u0, R0,
                 cg1, u1_,R1);
    std::cout << "dEdu1: " << jdim << " -->  " << (energy_-energy)/epsilon << " " << dEdu1[jdim] << std::endl;
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu0du1 " << jdim << " " << idim << " --> " << (dEdu0_[idim]-dEdu0[idim])/epsilon << " " << ddEdu0du1.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdw0du1 " << jdim << " " << idim << " --> " << (dEdw0_[idim]-dEdw0[idim])/epsilon << " " << ddEdw0du1.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu1du1 " << jdim << " " << idim << " --> " << (dEdu1_[idim]-dEdu1[idim])/epsilon << " " << ddEdu1du1.mat[idim*3+jdim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << "*ddEdw1du1 " << jdim << " " << idim << " --> " << (dEdw1_[idim]-dEdw1[idim])/epsilon << " " << ddEdu1dw1.mat[idim*3+jdim] << std::endl;
    }
  }

  for(unsigned int jdim=0;jdim<3;jdim++){
    CVector3 dEdu0_, dEdw0_, dEdu1_, dEdw1_;
    CMatrix3 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_, ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_, ddEdu1du1_, ddEdu1dw1_, ddEdw1dw1_;
    double energy_ = 0.0;
    CVector3 w(0,0,0);
    w[jdim] = epsilon;
    CMatrix3 dR; dR.SetRotMatrix_Cartesian(w.x,w.y,w.z);
    CMatrix3 R1_ = dR*R1;
    EdEddE_Joint(energy_, dEdu0_,dEdw0_, dEdu1_,dEdw1_,
                 ddEdu0du0_, ddEdu0dw0_, ddEdu0du1_, ddEdu0dw1_,
                 ddEdw0dw0_, ddEdw0du1_, ddEdw0dw1_,
                 ddEdu1du1_, ddEdu1dw1_,
                 ddEdw1dw1_,
                 trans_stiff, rot_stiff, pj,
                 cg0, u0, R0,
                 cg1, u1, R1_);
    std::cout << "dEdw1: " << jdim << " -->  " << (energy_-energy)/epsilon << " " << dEdw1[jdim] << std::endl;
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu0dw1 " << jdim << " " << idim << " --> " << (dEdu0_[idim]-dEdu0[idim])/epsilon << " " << ddEdu0dw1.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdw0dw1 " << jdim << " " << idim << " --> " << (dEdw0_[idim]-dEdw0[idim])/epsilon << " " << ddEdw0dw1.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdu1dw1 " << jdim << " " << idim << " --> " << (dEdu1_[idim]-dEdu1[idim])/epsilon << " " << ddEdu1dw1.mat[jdim*3+idim] << std::endl;
    }
    for(unsigned int idim=0;idim<3;idim++){
      std::cout << " ddEdw1dw1 " << jdim << " " << idim << " --> " << (dEdw1_[idim]-dEdw1[idim])/epsilon << " " << ddEdw1dw1.mat[idim*3+jdim] << std::endl;
    }
  }

}

///////////////////////////////////////////////////////////////////////////

CRigidBodyAssembly_Static::CRigidBodyAssembly_Static()
{
  nitr = 30;
  damping_ratio = 0.01;
  ////
  n = CVector3(0,1,0); // normal direction of floor (should be an unit vector)
  gravity = CVector3(0,-10,0); // gravity
  ////
  cont_stiff = 1.0e+4; // contact_stiffness (insensitive)
  trans_stiff = 1.0e+4; // joint_translation_stiffness (insensitive)
  rot_stiff = 1.0e+9; // joint_rotation_stiffness (insensitive)

  scale_force = 0.10; //0.5;
  scale_torque = 0.05; //0.5;

  is_draw_deformed = false; //false
  is_draw_skeleton = false; //false;
  is_draw_force = false; //false;
  is_draw_section = true; //true;
  is_draw_grid = false; //true;
  is_draw_section_moment = false; //false;

//  SetExample();
//  this->Solve();
  this->is_draw_skeleton = true;
  this->is_draw_force = true;
  this->is_draw_grid = true;
}

CRigidBodyAssembly_Static::CRigidBodyAssembly_Static
(const std::vector<CRigidBody>& aRB,
 const std::vector<CJoint>& aJ)
{

  nitr = 30;
  damping_ratio = 0.01;
  ////
  n = CVector3(0,1,0); // normal direction of floor (should be an unit vector)
  gravity = CVector3(0,-10,0); // gravity
  ////
  cont_stiff = 1.0e+4; // contact_stiffness (insensitive)
  trans_stiff = 1.0e+4; // joint_translation_stiffness (insensitive)
  rot_stiff = 1.0e+9; // joint_rotation_stiffness (insensitive)

  scale_force = 0.10; //0.5;
  scale_torque = 0.05; //0.5;

  this->aRigidBody = aRB;
  this->aJoint = aJ;
//  this->Solve();
  this->is_draw_skeleton = true;
  this->is_draw_force = true;
  this->is_draw_grid = true;
}

void CRigidBodyAssembly_Static::AddRigidBody(const double centre_of_mass[3],
                  const double mass,
                  const std::vector<double>& contact_points)
{
  const std::vector<double> pcg(centre_of_mass,centre_of_mass+3);
  CRigidBody rb(mass, pcg);
  const int ncp = (int)contact_points.size()/3;
  for(int icp=0;icp<ncp;icp++) {
    CVector3 pc(contact_points[icp*3+0],contact_points[icp*3+1],contact_points[icp*3+2]);
    rb.aCP.push_back(pc);
  }
  aRigidBody.push_back(rb);
}

void CRigidBodyAssembly_Static::AddJoint(const double position[3],
              const int body_index1,
              const int body_index2){
  CJoint j(body_index1, body_index2,
           std::vector<double>(position,position+3) );
//  j.p = CVector3(position[0], position[1], position[2]);
//  j.irb0 = body_index1;
//  j.irb1 = body_index2;
  aJoint.push_back(j);
}

void CRigidBodyAssembly_Static::ClearProblem(){
  aRigidBody.clear();
  aJoint.clear();
//  aPlate.clear();
}

void AddMatrix(Eigen::MatrixXd& M,
               unsigned int i0, unsigned int j0,
               const CMatrix3& m,
               bool isnt_inverse)
{
  if( isnt_inverse ){
    for(unsigned int idim=0;idim<3;idim++){
      for(unsigned int jdim=0;jdim<3;jdim++){
        M(i0+idim,j0+jdim) += m.mat[idim*3+jdim];
      }
    }
  }
  else{
    for(unsigned int idim=0;idim<3;idim++){
      for(unsigned int jdim=0;jdim<3;jdim++){
        M(i0+idim,j0+jdim) += m.mat[jdim*3+idim];
      }
    }
  }
}

void EdEddE_Total
(double& E,
 Eigen::VectorXd& dE,
 Eigen::MatrixXd& ddE,
 ////
 const std::vector<CRigidBody>& aRigidBody,
 const std::vector<CJoint>& aJoint,
 ////
 const CVector3 n,
 const CVector3 gravity,
 const double cont_stiff,
 const double trans_stiff,
 const double rot_stiff,
 bool is_friction
 )
{
  E = 0;
  ddE.setConstant(0);
  dE.setConstant(0); // u0,w0, u1,w1, ....

  ////
  for(unsigned int irb=0;irb<aRigidBody.size();irb++){
    const CRigidBody& rb = aRigidBody[irb];
    CVector3 cg = rb.cg;
    {
      double e = 0;
      CVector3 du, dw;
      EdEd_Potential(e, du, dw,
                       cg, rb.u, rb.m,
                       gravity);
      E += e;
      for(unsigned int idim=0;idim<3;idim++){
        dE[irb*6+0+idim] += du[idim];
        dE[irb*6+3+idim] += dw[idim];
      }
    }
    for(std::size_t icp=0;icp<rb.aCP.size();icp++){
      CVector3 cp = rb.aCP[icp];
      double e = 0;
      CVector3 du, dw;
      CMatrix3 ddu,ddw,dudw;
      if( is_friction ){
        EdEddE_ContactFriction(e,
                                 du,dw,  ddu,ddw,dudw,
                                 cg,cp,rb.u,rb.R,
                                 cont_stiff);
      }
      else{
        EdEddE_Contact(e,
                         du,dw,  ddu,ddw,dudw,
                         cg,cp,rb.u,rb.R,
                         cont_stiff,n);
      }
      E += e;
      for(unsigned int idim=0;idim<3;idim++){
        dE[irb*6+0+idim] += du[idim];
        dE[irb*6+3+idim] += dw[idim];
      }
      for(unsigned int idim=0;idim<3;idim++){
        for(unsigned int jdim=0;jdim<3;jdim++){
          ddE(irb*6+0+idim,irb*6+0+jdim) += ddu.mat[idim*3+jdim];
        }
      }
      AddMatrix(ddE, irb*6+0, irb*6+0, ddu,  true );
      AddMatrix(ddE, irb*6+0, irb*6+3, dudw, false);
      ////
      AddMatrix(ddE, irb*6+3, irb*6+0, dudw, true );
      AddMatrix(ddE, irb*6+3, irb*6+3, ddw,  true );
    }
    for(std::size_t iexf=0;iexf<rb.aExForce.size();iexf++){
      CVector3 pex = rb.aExForce[iexf].first;
      CVector3 fex = rb.aExForce[iexf].second;
      double e = 0;
      CVector3 du, dw;
      CMatrix3 ddu,ddw,dudw;
      EdEddE_Exforce(e,
                       du,dw,  ddu,ddw,dudw,
                       cg,pex,fex, rb.u,rb.R);
      E += e;
      for(int idim=0;idim<3;idim++){
        dE[irb*6+0+idim] += du[idim];
        dE[irb*6+3+idim] += dw[idim];
      }
      for(unsigned int idim=0;idim<3;idim++){
        for(unsigned int jdim=0;jdim<3;jdim++){
          ddE(irb*6+0+idim,irb*6+0+jdim) += ddu.mat[idim*3+jdim];
        }
      }
      AddMatrix(ddE, irb*6+0, irb*6+0, ddu,  true );
      AddMatrix(ddE, irb*6+0, irb*6+3, dudw, false);
      ////
      AddMatrix(ddE, irb*6+3, irb*6+0, dudw, true );
      AddMatrix(ddE, irb*6+3, irb*6+3, ddw,  true );
    }
  }
  for(const auto & joint : aJoint){
    CVector3 pj = joint.p;
    int irb0 = joint.irb0;
    int irb1 = joint.irb1;
    const CRigidBody& rb0 = aRigidBody[irb0];
    const CRigidBody& rb1 = aRigidBody[irb1];
    {
      double e = 0;
      CVector3 du0, dw0, du1, dw1;
      CMatrix3 du0du0, du0dw0, du0du1, du0dw1, dw0dw0, dw0du1, dw0dw1, du1du1, du1dw1, dw1dw1;
      EdEddE_Joint(e,
                     du0, dw0, du1, dw1,
                     du0du0, du0dw0, du0du1, du0dw1, dw0dw0, dw0du1, dw0dw1, du1du1, du1dw1, dw1dw1,
                     trans_stiff, rot_stiff, pj,
                     rb0.cg, rb0.u, rb0.R,
                     rb1.cg, rb1.u, rb1.R);
      E += e;
      for(int idim=0;idim<3;idim++){
        dE[irb0*6+0+idim] += du0[idim];
        dE[irb0*6+3+idim] += dw0[idim];
      }
      for(int idim=0;idim<3;idim++){
        dE[irb1*6+0+idim] += du1[idim];
        dE[irb1*6+3+idim] += dw1[idim];
      }
      AddMatrix(ddE, irb0*6+0, irb0*6+0, du0du0, true );
      AddMatrix(ddE, irb0*6+0, irb0*6+3, du0dw0, false);
      AddMatrix(ddE, irb0*6+0, irb1*6+0, du0du1, false);
      AddMatrix(ddE, irb0*6+0, irb1*6+3, du0dw1, false);
      ////
      AddMatrix(ddE, irb0*6+3, irb0*6+0, du0dw0, true );
      AddMatrix(ddE, irb0*6+3, irb0*6+3, dw0dw0, true );
      AddMatrix(ddE, irb0*6+3, irb1*6+0, dw0du1, false);
      AddMatrix(ddE, irb0*6+3, irb1*6+3, dw0dw1, false);
      ////
      AddMatrix(ddE, irb1*6+0, irb0*6+0, du0du1, true );
      AddMatrix(ddE, irb1*6+0, irb0*6+3, dw0du1, true );
      AddMatrix(ddE, irb1*6+0, irb1*6+0, du1du1, true );
      AddMatrix(ddE, irb1*6+0, irb1*6+3, du1dw1, false);
      ////
      AddMatrix(ddE, irb1*6+3, irb0*6+0, du0dw1, true );
      AddMatrix(ddE, irb1*6+3, irb0*6+3, dw0dw1, true );
      AddMatrix(ddE, irb1*6+3, irb1*6+0, du1dw1, true );
      AddMatrix(ddE, irb1*6+3, irb1*6+3, dw1dw1, true );
    }
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////

// solve one iteration
void CRigidBodyAssembly_Static::SolveOneIteration()
/*
(
 std::vector<CRigidBody>& aRigidBody,
 std::vector<CJoint>& aJoint,
 ////
 double damping_ratio,
 ////
 const CVector3 n,
 const CVector3 gravity,
 const double cont_stiff,
 const double trans_stiff,
 const double rot_stiff)*/
{
  int nDof = (int)aRigidBody.size()*6;
  double E = 0;
	Eigen::VectorXd dE(nDof);
	Eigen::MatrixXd ddE(nDof,nDof);
  EdEddE_Total(E,dE,ddE,
               aRigidBody,aJoint,
               n,gravity,cont_stiff,trans_stiff,rot_stiff,true);
  std::cout << "energy : " << E << std::endl;
  /////
	Eigen::VectorXd b(nDof);
	Eigen::MatrixXd A(nDof,nDof);
  b = -dE;
  A = ddE;
  for(int i=0;i<nDof;i++){
    A(i,i) += damping_ratio;
  }
	Eigen::VectorXd x = A.partialPivLu().solve(b);
  for(std::size_t irb=0;irb<aRigidBody.size();irb++){
    CRigidBody& rb = aRigidBody[irb];
    for(int idim=0;idim<3;idim++){
      rb.u[idim] += x(irb*6+0+idim);
    }
    {
      CVector3 w;
      w.x = x(irb*6+3+0);
      w.y = x(irb*6+3+1);
      w.z = x(irb*6+3+2);
      CMatrix3 dR; dR.SetRotMatrix_Cartesian(w.x,w.y,w.z);
      rb.R = dR*rb.R;
    }
  }
}

void CRigidBodyAssembly_Static::Solve_InterPlane()
/*
(std::vector<CRigidBody>& aRigidBody,
 std::vector<CJoint>& aJoint,
 ////
 double damping_ratio,
 int nitr,
 ////
 const CVector3 n,
 const CVector3 gravity,
 const double cont_stiff,
 const double trans_stiff,
 const double rot_stiff)
 */
{
  for(int itr=0;itr<nitr;itr++){
    //SolveOneIteration(aRigidBody, aJoint, damping_ratio, n,gravity,cont_stiff,trans_stiff,rot_stiff);
      SolveOneIteration();
  }
  ComputeForces();
}

void CRigidBodyAssembly_Static::ComputeForces()
/*
(std::vector<CRigidBody>& aRigidBody,
 std::vector<CJoint>& aJoint,
 ////
 const CVector3 n,
 const double cont_stiff,
 const double trans_stiff,
 const double rot_stiff)
 */
{

  for(auto & irb : aRigidBody){
    CRigidBody& rb = irb;
    const int ncp = (int)irb.aCP.size();
    rb.aCForce.resize(ncp);
    for(int icp=0;icp<ncp;icp++){
      const CVector3& cp = irb.aCP[icp];
      CVector3 Rv = rb.R * (cp-rb.cg);
      CVector3 cq = Rv + rb.cg + rb.u;
      rb.aCForce[icp] = ((cq*n)*cont_stiff)*n;
    }
  }
  for(auto & joint : aJoint){
    CVector3 pj = joint.p;
    int irb0 = joint.irb0;
    int irb1 = joint.irb1;
    const CRigidBody& rb0 = aRigidBody[irb0];
    const CRigidBody& rb1 = aRigidBody[irb1];

    CVector3 cg0 = rb0.cg;
    CVector3 u0  = rb0.u;
    CMatrix3  R0  = rb0.R;

    CVector3 cg1 = rb1.cg;
    CVector3 u1  = rb1.u;
    CMatrix3  R1  = rb1.R;

    CVector3 trans_f; // translation_force
    {
      CVector3 Rv0 = R0 * (pj-cg0);
      CVector3 qj0 = Rv0 + cg0 + u0; // after deformation joint pos relative to rigid body 0
      CVector3 Rv1 = R1 * (pj-cg1);
      CVector3 qj1 = Rv1 + cg1 + u1; // after deformation joint pos relative to rigid body 1
      trans_f = trans_stiff*(qj0 - qj1);
    }

    CVector3 torque_f; // rotation_force
    {
      CVector3 av(0,0,0);
      for(int i=0;i<3;i++){
        CVector3 r0( R0.mat[0*3+i], R0.mat[1*3+i], R0.mat[2*3+i] );
        CVector3 r1( R1.mat[0*3+i], R1.mat[1*3+i], R1.mat[2*3+i] );
        av += (r0^r1);
      }
      av *= 0.5;
      torque_f = rot_stiff*av;
    }
    joint.linear = trans_f;
    joint.torque = torque_f;
  }
}

//void CRigidBodyAssembly_Static::PrintJointForce()
/*
(const std::vector<CRigidBody>& aRigidBody,
 const std::vector<CJoint>& aJoint,
 ////
 const double trans_stiff,
 const double rot_stiff)
 */
/*
{
  std::cout << "force on joint" << std::endl;

  for(unsigned int ij=0;ij<aJoint.size();ij++){
    const CJoint& joint = aJoint[ij];
    CVector3 pj = joint.p;
    int irb0 = joint.irb0;
    int irb1 = joint.irb1;
    const CRigidBody& rb0 = aRigidBody[irb0];
    const CRigidBody& rb1 = aRigidBody[irb1];

    CVector3 cg0 = rb0.cg;
    CVector3 u0  = rb0.u;
    CMatrix3  R0  = rb0.R;

    CVector3 cg1 = rb1.cg;
    CVector3 u1  = rb1.u;
    CMatrix3  R1  = rb1.R;

    CVector3 trans_f; // translation_force
    {
      CVector3 Rv0 = R0 * (pj-cg0);
      CVector3 qj0 = Rv0 + cg0 + u0; // after deformation joint pos relative to rigid body 0
      CVector3 Rv1 = R1 * (pj-cg1);
      CVector3 qj1 = Rv1 + cg1 + u1; // after deformation joint pos relative to rigid body 1
      trans_f = trans_stiff*(qj0 - qj1);
    }

    CVector3 torque_f; // rotation_force
    {
      CVector3 av(0,0,0);
      for(unsigned int i=0;i<3;i++){
        CVector3 r0( R0.mat[0*3+i], R0.mat[1*3+i], R0.mat[2*3+i] );
        CVector3 r1( R1.mat[0*3+i], R1.mat[1*3+i], R1.mat[2*3+i] );
        av += (r0^r1);
      }
      av *= 0.5;
      torque_f = rot_stiff*av;
    }

    std::cout << "force of joint: " << ij << std::endl;
    std::cout << "  trans_force:  " << trans_f.x << " " << trans_f.y << " " << trans_f.z << std::endl;
    std::cout << "  torque_force: " << torque_f.x << " " << torque_f.y << " " << torque_f.z << std::endl;
  }
}
 */

// Setting problem here
void CRigidBodyAssembly_Static::SetExample()
{
  aRigidBody.clear();
  aJoint.clear();

  { // making plane 0
    CRigidBody rb(1.0, CVector3(0,1,0).stlvec() );
    aRigidBody.push_back(rb);
  }
  { // making plane 1
    CRigidBody rb(0.1, CVector3(-1, 0.5, -1).stlvec());
    rb.addCP(CVector3(-1.1,0,-1.1).stlvec());
    aRigidBody.push_back(rb);
  }
  { // making plane 2
    CRigidBody rb(0.1, CVector3(-1,0.5,+1).stlvec() );
    rb.addCP( CVector3(-1.1,0,+1.1).stlvec() );
    aRigidBody.push_back(rb);
  }
  { // making plane 3
    CRigidBody rb(0.1, CVector3(+1,0.5,-1).stlvec() );
    rb.addCP( CVector3(+1.1,0,-1.1).stlvec() );
    aRigidBody.push_back(rb);
  }
  { // making plane 4
    CRigidBody rb(0.1, CVector3(+1,0.5,+1).stlvec() );
    rb.addCP( CVector3(+1.1,0,+1.1).stlvec() );
    aRigidBody.push_back(rb);
  }

  { // joint 0
    CJoint jt(0,1, CVector3(-1,+1,-1).stlvec() );
    aJoint.push_back(jt);
  }
  { // joint 1
    CJoint jt(0,2, CVector3(-1,+1,+1).stlvec() );
    aJoint.push_back(jt);
  }
  { // joint 2
    CJoint jt(0,3, CVector3(+1,+1,-1).stlvec() );
    aJoint.push_back(jt);
  }
  { // joint 3
    CJoint jt(0,4, CVector3(+1,+1,+1).stlvec() );
    aJoint.push_back(jt);
  }
}

static void myGlVertex3d(const CVector3& v){
  ::glVertex3d(v.x,v.y,v.z);
}

std::vector<double> CRigidBodyAssembly_Static::MinMaxXYZ() const
{
  double bb[6] = {1,-1, 0,0, 0,0};
  for(const auto & rb : aRigidBody){
    myUpdateMinMaxXYZ(bb, rb.cg);
  }
  for(const auto & j : aJoint){
    myUpdateMinMaxXYZ(bb, j.p);
  }
  std::vector<double> res(bb,bb+6);
  return res;
}


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

#include "NDDODensityGuess.h"
#include <Sparrow/Implementations/Nddo/Utils/ParameterUtils/ElementParameters.h>
#include <Utils/DataStructures/DensityMatrix.h>
#include <Utils/DataStructures/MatrixWithDerivatives.h>
#include <Utils/Math/DerivOrderEnum.h>
#include <Utils/Scf/MethodInterfaces/OverlapCalculator.h>
#include <Utils/Typenames.h>
#include <Eigen/Core>

namespace Scine {
namespace Sparrow {

namespace nddo {

NDDODensityGuess::NDDODensityGuess(const Utils::ElementTypeCollection& elements, const ElementParameters& elementParameters,
                                   Utils::OverlapCalculator& overlapCalculator, const int& nElectrons, const int& nAOs)
  : elements_(elements),
    elementParameters_(elementParameters),
    overlapCalculator_(overlapCalculator),
    nElectrons_(nElectrons),
    nAOs_(nAOs) {
}

Utils::DensityMatrix NDDODensityGuess::calculateGuess() const {
  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(nAOs_, nAOs_);

  // stewart1990:
  // The guess is very crude: all off-diagonal matrix elements are set to zero, and
  // all on-diagonal terms on any atom are set equal to the core charge of that atom divided by the
  // number of atomic orbitals.

  // Alain:
  // Same as above, but, for off-diagonal elements,
  // use overlap matrix times factor for first guess of density matrix.
  // Division by two found by testing, seems to work better with it than without
  if (nAOs_ != 0) {
    overlapCalculator_.calculateOverlap(Utils::DerivativeOrder::Zero);
    P = overlapCalculator_.getOverlap().getMatrixXd() * nElectrons_ / (2 * nAOs_);
  }

  for (int i = 0; i < nAOs_; i++)
    for (int j = i + 1; j < nAOs_; j++)
      P(i, j) = P(j, i);

  // stewart 1990
  int index = 0;
  for (auto e : elements_) {
    double nEl = elementParameters_.get(e).coreCharge();
    auto nAOs = elementParameters_.get(e).nAOs();
    for (int j = 0; j < nAOs; j++) {
      P(index, index) = nEl / nAOs;
      index++;
    }
  }

  Utils::DensityMatrix d;
  d.setDensity(std::move(P), nElectrons_);
  return d;
}

} // namespace nddo
} // namespace Sparrow
} // namespace Scine


#ifndef CAFFE_MULTI_STAGE_MEANFIELD_LAYER_HPP_
#define CAFFE_MULTI_STAGE_MEANFIELD_LAYER_HPP_

#include <vector>

#include "caffe/blob.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"
#include "caffe/util/modified_permutohedral.hpp"
#include "caffe/layers/softmax_layer.hpp"
#include "caffe/layers/eltwise_layer.hpp"
#include "caffe/layers/split_layer.hpp"
#include "caffe/layers/meanfield_iteration.hpp"
#include <boost/shared_array.hpp>

namespace caffe {

/*!
 *  \brief     The Caffe layer that implements the CRF-RNN described in the paper:
 *             Conditional Random Fields as Recurrent Neural Networks. IEEE ICCV 2015.
 *
 *  \authors   Sadeep Jayasumana, Bernardino Romera-Paredes, Shuai Zheng, Zhizhong Su.
 *  \version   1.0
 *  \date      2015
 *  \copyright Torr Vision Group, University of Oxford.
 *  \details   If you use this code, please consider citing the paper:
 *             Shuai Zheng, Sadeep Jayasumana, Bernardino Romera-Paredes, Vibhav Vineet, Zhizhong Su, Dalong Du,
 *             Chang Huang, Philip H. S. Torr. Conditional Random Fields as Recurrent Neural Networks. IEEE ICCV 2015.
 *
 *             For more information about CRF-RNN, please visit the project website http://crfasrnn.torr.vision.
 */
template <typename Dtype>
class MultiStageMeanfieldLayer : public Layer<Dtype> {

 public:
  explicit MultiStageMeanfieldLayer(const LayerParameter& param) : Layer<Dtype>(param) {}

  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const {
    return "MultiStageMeanfield";
  }
  virtual inline int ExactNumBottomBlobs() const { return 3; }
  virtual inline int ExactNumTopBlobs() const { return 1; }

 protected:
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  virtual void compute_spatial_kernel(float* const output_kernel);
  virtual void compute_bilateral_kernel(const Blob<Dtype>* const image_data_start, const int n, float* const output_kernel);

  int count_;
  int num_;
  int channels_;
  int length_;
  int height_;
  int width_;
  int num_pixels_;

  Dtype theta_alpha_;
  Dtype theta_beta_;
  Dtype theta_gamma_;
  int num_iterations_;

  boost::shared_array<Dtype> norm_feed_;
  Blob<Dtype> spatial_norm_;
  Blob<Dtype> bilateral_norms_;

  vector<Blob<Dtype>*> split_layer_bottom_vec_;
  vector<Blob<Dtype>*> split_layer_top_vec_;
  vector<shared_ptr<Blob<Dtype> > > split_layer_out_blobs_;
  vector<shared_ptr<Blob<Dtype> > > iteration_output_blobs_;
  vector<shared_ptr<MeanfieldIteration<Dtype> > > meanfield_iterations_;

  shared_ptr<SplitLayer<Dtype> > split_layer_;

  shared_ptr<ModifiedPermutohedral> spatial_lattice_;
  boost::shared_array<float> bilateral_kernel_buffer_;
  vector<shared_ptr<ModifiedPermutohedral> > bilateral_lattices_;
};

}  // namespace caffe

#endif  // CAFFE_MULTI_STAGE_MEANFIELD_LAYER_HPP_


#pragma once

#include "prng.hh"
#include "hgd.hh"
#include "aes.hh"
#include "sha.hh"
#include "hmac.hh"
#include "zz.hh"

#include <string>
#include <map>

#include <NTL/ZZ.h>

class ope_domain_range {
 public:
    ope_domain_range(const NTL::ZZ &d_arg,
                     const NTL::ZZ &r_lo_arg,
                     const NTL::ZZ &r_hi_arg)
        : d(d_arg), r_lo(r_lo_arg), r_hi(r_hi_arg) {}
    NTL::ZZ d, r_lo, r_hi;
};

class OPE {
 public:
    OPE(const std::string &keyarg, size_t plainbits, size_t cipherbits)
    : key(keyarg), pbits(plainbits), cbits(cipherbits), aesk(aeskey(key)) {}

    NTL::ZZ encrypt(const NTL::ZZ &ptext);
    NTL::ZZ encrypt(int ptext);
    NTL::ZZ decrypt(const NTL::ZZ &ctext);

 private:
    static std::string aeskey(const std::string &key) {
        auto v = sha256::hash(key);
        v.resize(16);
        return v;
    }

    std::string key;
    size_t pbits, cbits;

    AES aesk;
    std::map<NTL::ZZ, NTL::ZZ> dgap_cache;

    template<class CB>
    ope_domain_range search(CB go_low);

    template<class CB>
    ope_domain_range lazy_sample(const NTL::ZZ &d_lo, const NTL::ZZ &d_hi,
                                 const NTL::ZZ &r_lo, const NTL::ZZ &r_hi,
                                 CB go_low, blockrng<AES> *prng);
};


/********************************************************************************
 *
 * This file is part of the Geneva library collection. The following license
 * applies to this file:
 *
 * ------------------------------------------------------------------------------
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * ------------------------------------------------------------------------------
 *
 * Note that other files in the Geneva library collection may use a different
 * license. Please see the licensing information in each file.
 *
 ********************************************************************************
 *
 * Geneva was started by Dr. Rüdiger Berlich and was later maintained together
 * with Dr. Ariel Garcia under the auspices of Gemfony scientific. For further
 * information on Gemfony scientific, see http://www.gemfomy.eu .
 *
 * The majority of files in Geneva was released under the Apache license v2.0
 * in February 2020.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 ********************************************************************************/

#pragma once

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard headers go here

#include <cstdlib>
#include <iomanip>
#include <ctime>
#include <cmath>
#include <iostream>
#include <sstream>
#include <cassert>
#include <algorithm>
#include <random>

// Boost headers go here
#include <boost/math/constants/constants.hpp>
#include <boost/utility.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/cast.hpp>

// Hap headers go here
#include "common/GCommonMathHelperFunctions.hpp"
#include "hap/GHapEnums.hpp"
#include "hap/GRandomDefines.hpp"
#include "hap/GRandomFactory.hpp"

/******************************************************************************/

namespace Gem {
namespace Hap {

/******************************************************************************/
/**
 * This class defines ways of obtaining different random number distributions
 * from "raw" random numbers, which can be obtained in derived classes using
 * various different ways.
 */
class GRandomBase
{
public:
	 /** @brief Helps to use this object as a generator for C++11 std::distributions */
	 using result_type = G_BASE_GENERATOR::result_type;

	 /***************************************************************************/
	 /** @brief The standard constructor */
	 G_API_HAP GRandomBase();
	 /** @brief A standard destructor */
	 virtual G_API_HAP ~GRandomBase();
	 /** @brief Retrieves a "raw" random item item */
	 G_API_HAP GRandomBase::result_type operator()();

	/***************************************************************************/
	// Prevent copying
	GRandomBase(const GRandomBase&) = delete;
	GRandomBase(const GRandomBase&&) = delete;
	GRandomBase& operator=(const GRandomBase&) = delete;
	GRandomBase& operator=(const GRandomBase&&) = delete;

	 /***************************************************************************/
	 /**
	  * This function is part of the standard interface of C++11 random number
	  * engines. It returns the minimum value returned by the generator. Since
	  * this class acts as a proxy for a wrapped generator or a generator running
	  * as a factory, we simply return the base generators min()-Value.
	  */
	 static constexpr G_API_HAP result_type (min)() {
		 return (G_BASE_GENERATOR::min)();
	 }

	 /***************************************************************************/
	 /**
	  * This function is part of the standard interface of C++11 random number
	  * engines. It returns the maximum value returned by the generator. Since
	  * this class acts as a proxy for a wrapped generator or a generator running
	  * as a factory, we simply return the base generators max()-Value.
	  */
	 static constexpr G_API_HAP result_type (max)() {
		 return (G_BASE_GENERATOR::max)();
	 }

private:
	 /***************************************************************************/
	 /** @brief Uniformly distributed integer numbers in the range min/max */
	 virtual G_API_HAP result_type int_random() = 0;
};

/******************************************************************************/

} /* namespace Hap */
} /* namespace Gem */


/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE DataFile test suite
#include <boost/test/included/unit_test.hpp>
#include <EvoNet/io/DataFile.h>
// #include <filesystem> C++ 17

using namespace EvoNet;
using namespace std;

BOOST_AUTO_TEST_SUITE(DataFile1)

BOOST_AUTO_TEST_CASE(constructor)
{
  DataFile* ptr = nullptr;
  DataFile* nullPointer = nullptr;
  ptr = new DataFile();
  BOOST_CHECK_NE(ptr, nullPointer);
}

BOOST_AUTO_TEST_CASE(destructor)
{
  DataFile* ptr = nullptr;
	ptr = new DataFile();
  delete ptr;
}

BOOST_AUTO_TEST_CASE(storeAndLoadBinary)
{
  DataFile data;

  // std::path data_path = std::current_path().replace_filename("data");  C++ 17
  // data_path /= "DataFileTest.dat";  C++ 17
  std::string filename = "DataFileTest.dat";

  Eigen::Tensor<float, 3> random_dat(2,2,2);
  random_dat.setRandom();
	data.storeDataBinary<float, 3>(filename, random_dat);
	// data.storeDataBinary(data_path.string(), random_dat);  C++ 17

  Eigen::Tensor<float, 3> test_dat(2,2,2);
	data.loadDataBinary<float, 3>(filename, test_dat);
	// data.loadDataBinary(data_path.string(), test_dat);  C++ 17

  BOOST_CHECK_CLOSE(test_dat(0, 0, 0), random_dat(0, 0, 0), 1e-6);
}

BOOST_AUTO_TEST_SUITE_END()


#include "model.h"

#include "dim.h"
#include "label.h"
#include "print.h"

#ifdef CMAKE_USE_DLIB
#include <dlib/dnn.h>
#include <dlib/matrix.h>
#include <dlib/memory_manager.h>
#include <dlib/statistics.h>
#include <dlib/svm_threaded.h>
#endif

#include <algorithm>
#include <cassert>
#include <random>
#include <vector>

namespace model
{

auto count_fequencies( const auto & d )
{
    std::unordered_map<int, double> ret;

    // Count instances of each class.
    unsigned datapoints {};
    for( const auto & kv : d.first )
    {
        ret.emplace( kv.first, kv.second.size() );
        datapoints += kv.second.size();
    }

    // Normalize values so that they sum up to 1.
    for( auto & kv : ret )
    {
        kv.second /= datapoints;
    }

    return ret;
}

RandomChance::RandomChance( const dat::Dataset & d )
    : _probs{ count_fequencies( d ) }
{
}

RandomChance::RandomChance( const dat::DatasetCompressed & d )
    : _probs{ count_fequencies( d ) }
{
}

label::Num RandomChance::predict( const dat::Spectrum & ) const
{
    std::random_device generator;
    std::uniform_real_distribution<double> distribution( 0, 1 );
    const auto rand = distribution( generator );

    // Comulutive probability distribution.
    double total {};
    for( const auto & kv : _probs )
    {
        total += kv.second;
        if( rand <= total )
        {
            return kv.first;
        }
    }

    throw Exception{ "Logical error: falling off RandomChance::predict()." };
}

std::vector< label::Num > construct_labels( const dat::Dataset & d )
{
    const auto size = count( d );

    std::vector< label::Num > labels;
    labels.reserve( size );

    dat::apply( [ & ] ( label::Num l, const dat::Spectrum & )
        {
            labels.push_back( l );
        }
             , d );
    assert( labels.size() == size );

    return labels;
}

#ifdef CMAKE_USE_DLIB
Correlation::Correlation( const dat::Dataset & d )
    : _training_set{ d }
    , _labels{ construct_labels( d ) }
{
}

std::vector<double> compute_correlation_row( const dat::Dataset & train
                                           , const dat::Spectrum & test )
{
    std::vector<double> ret;

    const std::vector<double> vtest{ test._y.begin(), test._y.end() };
    dat::apply( [ &vtest, &ret ] ( int, const dat::Spectrum & s)
        {
            std::vector<double> vtrain{ s._y.begin(), s._y.end() };
            const auto r_xy = dlib::correlation( vtrain, vtest );
            ret.push_back( r_xy );
        }
             , train );

    return ret;
}

label::Num Correlation::predict( const dat::Spectrum & test ) const
{
    const auto row = compute_correlation_row( _training_set, test );

    const auto m = std::max_element( row.cbegin(), row.cend() );
    assert( m != row.cend() );

    const auto index = std::distance( row.begin(), m );
    assert( static_cast<size_t>( index ) < _labels.size() );
    const auto l = _labels[ static_cast<size_t>( index ) ];

    return l;
}

struct SVM::Impl
{
    using Kernel = dlib::linear_kernel< dat::DlibSample >;
    using Classifier = dlib::multiclass_linear_decision_function< Kernel, label::Num >;
    using Trainer = dlib::svm_multiclass_linear_trainer< Kernel, label::Num >;

    Impl( const dat::Dataset & d )
        : _svm{ [ & ] () -> Classifier
            {
                if( d.first.empty() )
                {
                    return {};
                }

                dat::DlibFlattened all;
                std::vector< label::Num > i( d.first.size(), 0 );
                dat::apply( [ & ] ( label::Num l, const dat::Spectrum & s )
                {
                    addto( all, l, s );
                }         , d );

                Trainer trainer;
                trainer.set_num_threads( 10 );
                trainer.set_c( 1e0 );

                const Classifier svm{ trainer.train( all.first, all.second ) };
                return svm;
            } () }
    {
    }

    label::Num predict( const dat::Spectrum & test ) const
    {
        const auto s = dat::to_dlib_sample( test );
        const auto ret = _svm.predict( s );
        return ret.first;  // what is ret.second?
    }

private:
    void addto( dat::DlibFlattened & f, label::Num l, const dat::Spectrum & s )
    {
        f.first.push_back( to_dlib_sample( s ) );
        f.second.push_back( l );
    }

private:
    const Classifier _svm;
};

SVM::SVM( const dat::Dataset & d )
    : _impl{ std::make_unique< Impl >( d ) }
{
}

label::Num SVM::predict( const dat::Spectrum & test ) const
{
    return _impl->predict( test );
}

SVM::~SVM()
{
}
#endif // CMAKE_USE_DLIB

#ifdef CMAKE_USE_SHARK
// ID of the commit with Andres' implementation: a8410b05.
auto train_forest_model( const shark::ClassificationDataset & dataset
                       , unsigned num_trees
                       )
{
    shark::RFClassifier< label::Num > model;
    shark::RFTrainer< label::Num > trainer;

    if( dataset.empty() )
    {
        return model;
    }

    trainer.setNTrees( num_trees );
    trainer.train( model, dataset );

    return model;
}

Forest::Forest( const dat::Dataset & d )
    : _model{ train_forest_model( to_shark_dataset( d ), _num_trees ) }
{
}

Forest::Forest( const shark::ClassificationDataset & d )
    : _model{ train_forest_model( d, _num_trees ) }
{

}

label::Num Forest::predict( const dat::Spectrum & s ) const
{
    return predict( to_shark_vector( s ) );
}

label::Num Forest::predict( const shark::RealVector & v ) const
{
    label::Num p;
    _model.eval( v, p );
    return p;
}
#endif  // CMAKE_USE_SHARK

const std::vector< std::string > ALL_MODELS{ "chance"
#ifdef CMAKE_USE_DLIB
                                           , "cor"
                                           , "svm"
#endif
#ifdef CMAKE_USE_SHARK
                                           , "forest"
#endif
                                           };

}  // namespace model


/*
 * srkin.cpp
 *
 *  Created on: Jul 9, 2014
 *      Author: Shirong
 */

#ifndef __SRKIN_CPP_
#define __SRKIN_CPP_

#include "../../include/srkin/srkin.h"

#include <boost/property_tree/ptree.hpp> //for property_tree
#include <boost/property_tree/json_parser.hpp> //for json_reader
#include <boost/optional/optional.hpp> //for optional

namespace srkin_sr {

	srkin::srkin(std::string cwd_sr_in, rsp::temperature_t t, rsp::pressure_t p, rsp::volume_t v) :RU_sk(8.3144621E7), RUC_sk(1.9872041), PA_sk(1.01325E6)
	{
		set_cwd(cwd_sr_in);
		set_temperature(t);
		set_pressure(p);
		set_volume(v);

		rsp::relationshipParser::read_chem_out_ele_spe(this->element_v_sk, this->species_v_sk, this->spe_name_index_map_sk, this->cwd_sk + "/input/chem.out");
		rsp::relationshipParser::read_chem_out_reaction(this->species_v_sk, this->reaction_v_sk, this->spe_name_index_map_sk, this->cwd_sk + "/input/chem.out");

		rsp::relationshipParser::set_reaction_net_reactant_product(this->reaction_v_sk);
		rsp::relationshipParser::set_spe_sk_info(this->species_v_sk, this->reaction_v_sk);

		read_initial_spe_concentration(this->species_v_sk.size(), "/input/setting.json");

		//set prefactor_A
		for (std::size_t i = 0; i < this->reaction_v_sk.size(); ++i) {
			this->reaction_v_sk[i].rate_constant = cal_reaction_rate_coef(i, this->temperature_sk, this->pressure_sk);
		}

	}

	srkin::~srkin()
	{
	}

	void srkin::read_initial_spe_concentration(const std::size_t nkk, std::string filename)
	{
		//configurations, read configuration file named "setting.json"
		boost::property_tree::ptree pt;
		//read configuration file "setting.cfg"
		boost::property_tree::read_json(this->cwd_sk + std::string("/input/setting.json"), pt, std::locale());

		this->species_conc_v_sk.resize(nkk);

		//read with json_parser as property_tree
		//nice and easy
		for (auto &key1 : pt.get_child("chem_init.species_index_concentration")) {
			species_conc_v_sk[boost::lexical_cast<std::size_t>(key1.first)] = key1.second.get_value<double>();
		}

	}

	std::string srkin::get_cwd()
	{
		return this->cwd_sk;
	}

	void srkin::set_cwd(std::string cwd_in)
	{
		this->cwd_sk = cwd_in;
	}

	rsp::temperature_t srkin::get_temperature()
	{
		return this->temperature_sk;
	}

	void srkin::set_temperature(rsp::temperature_t t_in)
	{
		this->temperature_sk = t_in;
	}

	rsp::volume_t srkin::get_volume()
	{
		return this->volume_sk;
	}

	void srkin::set_volume(rsp::volume_t volume_in)
	{
		this->volume_sk = volume_in;
	}

	rsp::pressure_t srkin::get_pressure()
	{
		return this->pressure_sk;
	}

	void srkin::set_pressure(rsp::pressure_t pressure_in)
	{
		this->pressure_sk = pressure_in;
	}

	const std::vector<rsp::concentration_t>& srkin::get_species_conc_v() const
	{
		// TODO: insert return statement here
		return this->species_conc_v_sk;
	}

	void srkin::set_species_conc_v(const std::size_t spe_ind, const double conc)
	{
		this->species_conc_v_sk[spe_ind] = conc;
	}

	void srkin::set_species_conc_v(const double speConcV[])
	{
		for (std::size_t i = 0; i < this->species_conc_v_sk.size(); ++i) {
			this->species_conc_v_sk[i] = speConcV[i];
		}
	}

	double srkin::cal_reaction_rate_coef(std::size_t reaction_ind, rsp::temperature_t T, rsp::pressure_t P)
	{
		if (T == 0)
			return this->reaction_v_sk[reaction_ind].prefactor_A;
		else if (this->reaction_v_sk[reaction_ind].delta == 0)
			return this->reaction_v_sk[reaction_ind].prefactor_A;
		else
			return this->reaction_v_sk[reaction_ind].prefactor_A* pow(T, this->reaction_v_sk[reaction_ind].delta) * exp(-this->reaction_v_sk[reaction_ind].barrier_E / T / this->RUC_sk);
	}

	void srkin::set_reaction_rate_coef(std::size_t reaction_ind, rsp::temperature_t T, rsp::pressure_t P)
	{
		this->reaction_v_sk[reaction_ind].rate_constant = this->cal_reaction_rate_coef(reaction_ind, T, P);
	}

	void srkin::cal_reaction_rate(const std::size_t reaction_ind)
	{

		this->reaction_v_sk[reaction_ind].reaction_rate = this->reaction_v_sk[reaction_ind].rate_constant;
		for (std::size_t i = 0; i < reaction_v_sk[reaction_ind].reactant.size(); ++i) {
			this->reaction_v_sk[reaction_ind].reaction_rate *= pow(this->species_conc_v_sk[reaction_v_sk[reaction_ind].reactant[i].first], reaction_v_sk[reaction_ind].reactant[i].second);
		}
	}

	void srkin::cal_reaction_rate()
	{
		for (std::size_t i = 0; i < this->reaction_v_sk.size(); ++i)
			cal_reaction_rate(i);
	}

	void srkin::cal_reaction_rate(const double * T, const double * C, double * FWDK, double * REVK)
	{
		set_temperature(*T);
		set_species_conc_v(C);
		for (std::size_t i = 0; i < this->reaction_v_sk.size(); ++i) {
			cal_reaction_rate(i);
			FWDK[i] = this->reaction_v_sk[i].reaction_rate;
			REVK[i] = 0.0;
		}
	}

	void srkin_sr::srkin::cal_reaction_rate_discrete(const std::size_t reaction_ind)
	{

		this->reaction_v_sk[reaction_ind].reaction_rate = this->reaction_v_sk[reaction_ind].rate_constant;
		for (std::size_t i = 0; i < reaction_v_sk[reaction_ind].reactant.size(); ++i) {
			//check, number of species must be not less than the stoichoimetric coefficient
			if (this->species_conc_v_sk[reaction_v_sk[reaction_ind].reactant[i].first] < reaction_v_sk[reaction_ind].reactant[i].second) {
				this->reaction_v_sk[reaction_ind].reaction_rate = 0.0;
				break;
			}
			else
				this->reaction_v_sk[reaction_ind].reaction_rate *= pow(this->species_conc_v_sk[reaction_v_sk[reaction_ind].reactant[i].first], reaction_v_sk[reaction_ind].reactant[i].second);
		}
	}

	void srkin_sr::srkin::cal_reaction_rate_discrete()
	{
		for (std::size_t i = 0; i < this->reaction_v_sk.size(); ++i)
			cal_reaction_rate_discrete(i);
	}

	rsp::reaction_rate_t srkin::get_reaction_rate(const std::size_t reaction_ind)
	{
		return this->reaction_v_sk[reaction_ind].reaction_rate;
	}

	rsp::reaction_rate_t srkin::cal_spe_destruction_rate(const std::size_t spe_ind)
	{
		double spe_dr_tp = 0.0;
		for (std::size_t i = 0; i < species_v_sk[spe_ind].reaction_k_index_s_coef_v.size(); ++i) {
			spe_dr_tp += reaction_v_sk[species_v_sk[spe_ind].reaction_k_index_s_coef_v[i].first].reaction_rate *species_v_sk[spe_ind].reaction_k_index_s_coef_v[i].second;
		}
		return spe_dr_tp;
	}

	void srkin::cal_spe_destruction_rate(const double * T, const double * C, double * CDOT, double * DDOT)
	{
		set_temperature(*T);
		set_species_conc_v(C);
		cal_reaction_rate();

		for (std::size_t i = 0; i < this->species_v_sk.size(); ++i) {
			CDOT[i] = 0.0;
			DDOT[i] = cal_spe_destruction_rate(i);
		}
	}

} /*namespace srkin_sr */

#endif /* __SRKIN_CPP_ */


#include <iostream> 	// std::cout
#include <tpl/regex/Match.h> // minimization of including tpl

using namespace tpl;

// -------------------------------------------------------------------------

void validation_example_1(const char* str)
{
	if (str == integer())
		std::cout << "  " << str << " is an integer value.\n";
	else if (str == real())
		std::cout << "  " << str << " is a real value.\n";
	else
		std::cout << "  " << str << " isn't a numeric value.\n";
}

// -------------------------------------------------------------------------

void validation_example_2(const char* str)
{
	int val;
	if (str == integer()/assign(val) && val >= 1 && val <= 12)
		std::cout << "  " << str << " is an integer value between 1 to 12.\n";
	else
		std::cout << "  " << str << " is not valid data.\n";
}

// -------------------------------------------------------------------------

#include <boost/spirit/phoenix.hpp>

using namespace phoenix;

void validation_example_3(const char* str)
{
	if (str == integer()/meet(arg1 >= 1 && arg1 <= 12))
		std::cout << "  " << str << " is an integer value between 1 to 12.\n";
	else
		std::cout << "  " << str << " is not valid data.\n";
}

// -------------------------------------------------------------------------

void validation_example_4(const char* str)
{
	if (str == integer()/(ge(1) && le(12)))
		std::cout << "  " << str << " is an integer value between 1 to 12.\n";
	else
		std::cout << "  " << str << " is not valid data.\n";
}

// -------------------------------------------------------------------------

int main()
{
	validation_example_1("-135");
	validation_example_1("+.23e-23");
	validation_example_1("-.1.e23");
	std::cout << "\n";

	validation_example_2("3");
	validation_example_2("13");
	validation_example_2("-135");
	validation_example_2("+.23e-23");
	std::cout << "\n";

	validation_example_3("3");
	validation_example_3("13");
	validation_example_3("-135");
	validation_example_3("+.23e-23");
	std::cout << "\n";

	validation_example_4("3");
	validation_example_4("13");
	validation_example_4("-135");
	validation_example_4("+.23e-23");
	std::cout << "\n";
	return 0;
}

// -------------------------------------------------------------------------


#ifndef PYTHONIC_TYPES_SLICE_HPP
#define PYTHONIC_TYPES_SLICE_HPP

#include "pythonic/include/types/slice.hpp"

#include "pythonic/__builtin__/None.hpp"

#ifndef NDEBUG
#include <boost/simd/function/bitofsign.hpp>
#endif

#include <cassert>
#include <stdexcept>
#include <ostream>
#include <algorithm>

PYTHONIC_NS_BEGIN

namespace types
{

  struct contiguous_slice;

  namespace details
  {

    long roundup_divide(long a, long b)
    {
      if (b > 0)
        return (a + b - 1) / b;
      else
        return (a + b + 1) / b;
    }
  }

  normalized_slice::normalized_slice()
  {
  }
  normalized_slice::normalized_slice(long lower, long upper, long step)
      : lower(lower), upper(upper), step(step)
  {
  }

  normalized_slice normalized_slice::
  operator*(normalized_slice const &other) const
  {
    return {lower + step * other.lower, lower + step * other.upper,
            step * other.step};
  }
  normalized_slice normalized_slice::
  operator*(contiguous_normalized_slice const &other) const
  {
    return {lower + step * other.lower, lower + step * other.upper,
            step * other.step};
  }
  normalized_slice normalized_slice::operator*(slice const &other) const
  {
    return (*this) * other.normalize(size());
  }
  normalized_slice normalized_slice::
  operator*(contiguous_slice const &other) const
  {
    return (*this) * other.normalize(size());
  }

  long normalized_slice::size() const
  {
    return std::max(0L, details::roundup_divide(upper - lower, step));
  }

  long normalized_slice::get(long i) const
  {
    return lower + i * step;
  }

  slice::slice(none<long> lower, none<long> upper, none<long> step)
      : lower(lower), upper(upper), step(step.is_none ? 1 : (long)step)
  {
  }

  slice::slice() : lower(__builtin__::None), upper(__builtin__::None), step(1)
  {
  }

  slice slice::operator*(slice const &other) const
  {
    // We do ! implement these because it requires to know the "end"
    // value of the slice which is ! possible if it is ! "step == 1" slice
    // TODO: We can skip these constraints if we know begin, end && step.
    assert(!((other.step < 0 || static_cast<long>(other.upper) < 0 ||
              static_cast<long>(other.lower) < 0) &&
             step != 1 && step != -1) &&
           "! implemented");
    bound<long> new_lower;
    if (other.lower.is_none() || (long)other.lower == 0) {
      if (other.step > 0)
        new_lower = lower;
      else if (step > 0) {
        if (upper.is_none() || (long)upper == 0)
          // 0 means the first value && ! the last value
          new_lower = none_type{};
        else
          new_lower = (long)upper - 1;
      } else {
        if (upper.is_none() || (long)upper == -1)
          new_lower = none_type{};
        else
          new_lower = (long)upper + 1;
      }
    } else {
      none<long> ref = ((long)other.lower > 0) ? lower : upper;
      if (ref.is_none) {
        if (step > 0)
          new_lower = (long)other.lower * step;
        else
          new_lower = (long)other.lower * step - 1;
      } else
        new_lower = (long)ref + (long)other.lower * step;
    }

    long new_step = step * other.step;

    bound<long> new_upper;
    if (other.upper.is_none()) {
      if (other.step > 0)
        new_upper = upper;
      else if (step > 0) {
        if (lower.is_none() || (long)lower == 0)
          new_upper = none_type{};
        else
          new_upper = (long)lower - 1;
      } else {
        if (lower.is_none() || (long)lower == -1)
          // 0 means the first value && ! the last value
          new_upper = none_type{};
        else
          new_upper = (long)lower + 1;
      }
    } else {
      none<long> ref = ((long)other.upper > 0) ? lower : upper;
      if (ref.is_none) {
        if (step > 0)
          new_upper = (long)other.upper * step;
        else
          new_upper = (long)other.upper * step - 1;
      } else
        new_upper = (long)ref + (long)other.upper * step;
    }
    return {new_lower, new_upper, new_step};
  }

  /*
     Normalize change a[:-1] to a[:len(a)-1] to have positif index.
     It also check for value bigger than len(a) to fit the size of the
     container
     */
  normalized_slice slice::normalize(long max_size) const
  {
    long normalized_upper;
    if (upper.is_none()) {
      if (step > 0L)
        normalized_upper = max_size;
      else
        normalized_upper = -1L;
    } else {
      if (upper < 0L)
        normalized_upper = std::max(-1L, max_size + upper);
      else if (upper > max_size)
        normalized_upper = max_size;
      else
        normalized_upper = (long)upper;
    }

    long normalized_lower;
    if (lower.is_none() && step > 0L)
      normalized_lower = 0L;
    else if (lower.is_none() && step < 0L)
      normalized_lower = max_size - 1L;
    else if (lower < 0L)
      normalized_lower = std::max(0L, max_size + lower);
    else if (lower > max_size)
      normalized_lower = max_size - 1L;
    else
      normalized_lower = (long)lower;

    return {normalized_lower, normalized_upper, step};
  }

  /*
   * An assert is raised when we can't compute the size without more
   * informations.
   */
  long slice::size() const
  {
    assert(!(upper.is_none() && lower.is_none()));
    long len;
    if (upper.is_none()) {
      assert(boost::simd::bitofsign((long)step) !=
             boost::simd::bitofsign((long)lower));
      len = -(long)lower;
    } else if (lower.is_none()) {
      assert(boost::simd::bitofsign((long)step) ==
             boost::simd::bitofsign((long)upper));
      len = upper;
    } else
      len = upper - lower;
    return std::max(0L, details::roundup_divide(len, step));
  }

  long slice::get(long i) const
  {
    assert(!upper.is_none() && !lower.is_none());
    return (long)lower + i * (long)step;
  }

  contiguous_normalized_slice::contiguous_normalized_slice()
  {
  }

  contiguous_normalized_slice::contiguous_normalized_slice(long lower,
                                                           long upper)
      : lower(lower), upper(upper)
  {
  }

  contiguous_normalized_slice contiguous_normalized_slice::
  operator*(contiguous_normalized_slice const &other) const
  {
    return contiguous_normalized_slice(lower + other.lower,
                                       lower + other.upper);
  }
  contiguous_normalized_slice contiguous_normalized_slice::
  operator*(contiguous_slice const &other) const
  {
    return (*this) * other.normalize(size());
  }

  normalized_slice contiguous_normalized_slice::
  operator*(normalized_slice const &other) const
  {
    return normalized_slice(lower + step * other.lower,
                            lower + step * other.upper, step * other.step);
  }

  normalized_slice contiguous_normalized_slice::
  operator*(slice const &other) const
  {
    return (*this) * other.normalize(size());
  }

  long contiguous_normalized_slice::size() const
  {
    return std::max(0L, upper - lower);
  }

  inline long contiguous_normalized_slice::get(long i) const
  {
    return lower + i;
  }

  contiguous_slice::contiguous_slice(none<long> lower, none<long> upper)
      : lower(lower.is_none ? 0 : (long)lower), upper(upper)
  {
  }

  contiguous_slice::contiguous_slice()
  {
  }

  contiguous_slice contiguous_slice::
  operator*(contiguous_slice const &other) const
  {
    long new_lower;
    if (other.lower < 0)
      new_lower = upper + other.lower * step;
    else
      new_lower = lower + other.lower * step;

    bound<long> new_upper;
    if (other.upper.is_none())
      new_upper = upper;
    else if ((long)other.upper < 0) {
      if (upper.is_none())
        new_upper = (long)other.upper * step;
      else
        new_upper = upper + (long)other.upper * step;
    } else
      new_upper = lower + (long)other.upper * step;

    return {new_lower, new_upper};
  }

  slice contiguous_slice::operator*(slice const &other) const
  {
    none<long> new_lower;
    if (other.lower.is_none() || (long)other.lower == 0) {
      if (other.step > 0)
        new_lower = lower;
      else if (upper.is_none() || (long)upper == 0)
        // 0 means the first value && ! the last value
        new_lower = none_type{};
      else
        new_lower = (long)upper - 1;
    } else {
      if ((long)other.lower > 0)
        new_lower = lower + (long)other.lower * step;
      else if (upper.is_none())
        new_lower = (long)other.lower * step;
      else
        new_lower = (long)upper + (long)other.lower * step;
    }

    long new_step = other.step;

    bound<long> new_upper;
    if (other.upper.is_none()) {
      if (other.step > 0)
        new_upper = upper;
      else if ((long)lower == 0)
        new_upper = none_type{};
      else
        new_upper = (long)lower - 1;
    } else {
      if ((long)other.upper > 0)
        new_upper = lower + (long)other.upper * step;
      else if (upper.is_none())
        new_upper = (long)other.upper * step;
      else
        new_upper = (long)upper + (long)other.upper * step;
    }
    return {new_lower, new_upper, new_step};
  }

  /*
     Normalize change a[:-1] to a[:len(a)-1] to have positif index.
     It also check for value bigger than len(a) to fit the size of the
     container
     */
  contiguous_normalized_slice contiguous_slice::normalize(long max_size) const
  {
    long normalized_upper;
    if (upper.is_none())
      normalized_upper = max_size;
    else if (upper < 0L)
      normalized_upper = std::max(-1L, max_size + upper);
    else if (upper > max_size)
      normalized_upper = max_size;
    else
      normalized_upper = (long)upper;

    long normalized_lower = (long)lower;
    if (lower < 0L)
      normalized_lower = std::max(0L, max_size + lower);
    else if (lower > max_size)
      normalized_lower = max_size - 1L;
    else
      normalized_lower = (long)lower;

    return contiguous_normalized_slice(normalized_lower, normalized_upper);
  }

  long contiguous_slice::size() const
  {
    long len;
    if (upper.is_none()) {
      assert(lower < 0);
      len = -lower;
    } else
      len = upper - lower;
    return std::max(0L, len);
  }

  inline long contiguous_slice::get(long i) const
  {
    return int(lower) + i;
  }

  slice slice::operator*(contiguous_slice const &other) const
  {
    // We do ! implement these because it requires to know the "end"
    // value of the slice which is ! possible if it is ! "step == 1" slice
    // TODO: We can skip these constraints if we know begin, end && step.
    assert(!((static_cast<long>(other.upper) < 0 ||
              static_cast<long>(other.lower) < 0) &&
             step != 1 && step != -1) &&
           "! implemented");

    bound<long> new_lower;
    if (other.lower == 0)
      new_lower = lower;
    else {
      bound<long> ref = ((long)other.lower > 0) ? lower : upper;
      if (ref.is_none()) {
        if (step > 0)
          new_lower = (long)other.lower * step;
        else
          new_lower = (long)other.lower * step - 1;
      } else
        new_lower = (long)ref + (long)other.lower * step;
    }

    long new_step = step;

    bound<long> new_upper;
    if (other.upper.is_none())
      new_upper = upper;
    else {
      bound<long> ref = ((long)other.upper > 0) ? lower : upper;
      if (ref.is_none()) {
        if (step > 0)
          new_upper = (long)other.upper * step;
        else
          new_upper = (long)other.upper * step - 1;
      } else
        new_upper = (long)ref + (long)other.upper * step;
    }
    return {new_lower, new_upper, new_step};
  }
}
PYTHONIC_NS_END

#endif