#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


/*
 * 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_ */


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_ANGLEAXIS_HPP_
#define KINDR_ROTATIONS_EIGEN_ANGLEAXIS_HPP_

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"
#include "kindr/rotations/eigen/RotationEigenFunctions.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class AngleAxis
 * \brief Implementation of an angle axis rotation based on Eigen::AngleAxis
 *
 *  The following two typedefs are provided for convenience:
 *   - \ref eigen_impl::AngleAxisAD "AngleAxisAD" for active rotation and primitive type double
 *   - \ref eigen_impl::AngleAxisAF "AngleAxisAF" for active rotation and primitive type float
 *   - \ref eigen_impl::AngleAxisPD "AngleAxisPD" for passive rotation and primitive type double
 *   - \ref eigen_impl::AngleAxisPF "AngleAxisPF" for passive rotation and primitive type float
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *  \tparam Usage_ the rotation usage which is either active or passive
 *
 *  \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class AngleAxis : public AngleAxisBase<AngleAxis<PrimType_, Usage_>, Usage_> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::AngleAxis<PrimType_> Base;

  /*! Data container
   */
  Base angleAxis_;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;
  /*! \brief The axis type is a 3D vector.
   */
  typedef Eigen::Matrix<PrimType_, 3, 1> Vector3;

  /*! \brief All four parameters stored in a vector [angle; axis]
   */
  typedef Eigen::Matrix<PrimType_, 4, 1> Vector4;

  /*! \brief Default constructor using identity rotation.
   */
  AngleAxis()
    : angleAxis_(Base::Identity()) {
  }

  /*! \brief Constructor using four scalars.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param angle     rotation angle
   *  \param v1      first entry of the rotation axis vector
   *  \param v2      second entry of the rotation axis vector
   *  \param v3      third entry of the rotation axis vector
   */
  AngleAxis(Scalar angle, Scalar v1, Scalar v2, Scalar v3)
    : angleAxis_(angle,Vector3(v1,v2,v3)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using angle and axis.
   * In debug mode, an assertion is thrown if the rotation vector has not unit length.
   * \param angle   rotation angle
   * \param axis     rotation vector with unit length (Eigen vector)
   */
  AngleAxis(Scalar angle, const Vector3& axis)
    : angleAxis_(angle,axis) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using a 4x1matrix.
   * In debug mode, an assertion is thrown if the rotation vector has not unit length.
   * \param vector     4x1-matrix with [angle; axis]
   */
  AngleAxis(const Vector4& vector)
    : angleAxis_(vector(0),vector.template block<3,1>(1,0)) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using Eigen::AngleAxis.
   *  In debug mode, an assertion is thrown if the rotation vector has not unit length.
   *  \param other   Eigen::AngleAxis<PrimType_>
   */
  explicit AngleAxis(const Base& other) // explicit on purpose
    : angleAxis_(other) {
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit AngleAxis(const RotationBase<OtherDerived_, Usage_>& other)
    : angleAxis_(internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  AngleAxis& operator =(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  AngleAxis& operator ()(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  AngleAxis inverted() const {
    Base inverse = this->toImplementation().inverse();
    inverse.angle() = -inverse.angle();
    inverse.axis() = -inverse.axis();
    return AngleAxis(inverse);
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  AngleAxis& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return static_cast<Implementation&>(angleAxis_);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return static_cast<const Implementation&>(angleAxis_);
  }

  /*! \brief Returns the rotation angle.
   *  \returns rotation angle (scalar)
   */
  inline Scalar angle() const {
    return angleAxis_.angle();
  }

  /*! \brief Sets the rotation angle.
   */
  inline void setAngle(Scalar angle) {
    angleAxis_.angle() = angle;
  }

  /*! \brief Returns the rotation axis.
   *  \returns rotation axis (vector)
   */
  inline const Vector3& axis() const {
    return angleAxis_.axis();
  }

  /*! \brief Sets the rotation axis.
   */
  inline void setAxis(const Vector3& axis) {
    angleAxis_.axis() = axis;
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Sets the rotation axis.
   */
  inline void setAxis(Scalar v1, Scalar v2, Scalar v3) {
    angleAxis_.axis() = Vector3(v1,v2,v3);
    KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
  }

  /*! \brief Sets angle-axis from a 4x1-matrix
   */
  inline void setVector(const Vector4& vector) {
    this->setAngle(vector(0));
    this->setAxis(vector.template block<3,1>(1,0));
  }

  /*! \returns the angle and axis in a 4x1 vector [angle; axis].
   */
  inline Vector4 vector() const {
    Vector4 vector;
    vector(0) = angle();
    vector.template block<3,1>(1,0) = axis();
    return vector;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  AngleAxis& setIdentity() {
    this->setAngle(static_cast<Scalar>(0));
    this->setAxis(static_cast<Scalar>(1), static_cast<Scalar>(0), static_cast<Scalar>(0));
    return *this;
  }

  /*! \brief Returns a unique angle axis rotation with angle in [0,pi].
   *  This function is used to compare different rotations.
   *  \returns copy of the angle axis rotation which is unique
   */
  AngleAxis getUnique() const {
    AngleAxis aa(kindr::common::floatingPointModulo(angle()+M_PI,2*M_PI)-M_PI, axis()); // first wraps angle into [-pi,pi)
    if(aa.angle() > 0)  {
      return aa;
    } else if(aa.angle() < 0) {
      if(aa.angle() != -M_PI) {
        return AngleAxis(-aa.angle(),-aa.axis());
      } else { // angle == -pi, so axis must be viewed further, because -pi,axis does the same as -pi,-axis

        if(aa.axis()[0] < 0) {
          return AngleAxis(-aa.angle(),-aa.axis());
        } else if(aa.axis()[0] > 0) {
          return AngleAxis(-aa.angle(),aa.axis());
        } else { // v1 == 0

          if(aa.axis()[1] < 0) {
            return AngleAxis(-aa.angle(),-aa.axis());
          } else if(aa.axis()[1] > 0) {
            return AngleAxis(-aa.angle(),aa.axis());
          } else { // v2 == 0

            if(aa.axis()[2] < 0) { // v3 must be -1 or 1
              return AngleAxis(-aa.angle(),-aa.axis());
            } else  {
              return AngleAxis(-aa.angle(),aa.axis());
            }
          }
        }
      }
    } else { // angle == 0
      return AngleAxis();
    }
  }

  /*! \brief Modifies the angle axis rotation such that the lies angle in [0,pi).
   *  \returns reference
   */
  AngleAxis& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using AngleAxisBase<AngleAxis<PrimType_, Usage_>, Usage_>::operator*;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const AngleAxis& a) {
    out << a.angle() << ", " << a.axis().transpose();
    return out;
  }
};

//! \brief Active angle axis rotation with double primitive type
typedef AngleAxis<double, RotationUsage::ACTIVE>  AngleAxisAD;
//! \brief Active angle axis rotation with float primitive type
typedef AngleAxis<float,  RotationUsage::ACTIVE>  AngleAxisAF;
//! \brief Passive angle axis rotation with double primitive type
typedef AngleAxis<double, RotationUsage::PASSIVE> AngleAxisPD;
//! \brief Passive angle axis rotation with float primitive type
typedef AngleAxis<float,  RotationUsage::PASSIVE> AngleAxisPF;

} // namespace eigen_impl

namespace internal {

template<typename PrimType_, enum RotationUsage Usage_>
class get_scalar<eigen_impl::AngleAxis<PrimType_, Usage_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::AngleAxis<PrimType_, Usage_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::AngleAxis<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::AngleAxis<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::AngleAxis<PrimType_, RotationUsage::PASSIVE>> {
 public:
  typedef eigen_impl::AngleAxis<PrimType_, RotationUsage::ACTIVE> OtherUsage;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::AngleAxis<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::AngleAxis<SourcePrimType_, Usage_>& a) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(a.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    typedef typename eigen_impl::RotationVector<SourcePrimType_, Usage_>::Scalar Scalar;

    const eigen_impl::RotationVector<DestPrimType_, Usage_> rv(rotationVector);

    if (rv.toImplementation().norm() < common::internal::NumTraits<Scalar>::dummy_precision()) {
      return eigen_impl::AngleAxis<DestPrimType_, Usage_>();
    }
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(rv.toImplementation().norm(), rv.toImplementation().normalized());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>& q) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromQuaternion<SourcePrimType_, DestPrimType_>(q.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::RotationMatrix<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::RotationMatrix<SourcePrimType_, Usage_>& rotationMatrix) {
//    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));
//    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));

//   if (Usage_ == RotationUsage::ACTIVE) {
//     return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.matrix()));
//
//   }
//   if (Usage_ == RotationUsage::PASSIVE) {
//     return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));
//
//   }
//
   return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRotationMatrix<SourcePrimType_, DestPrimType_>(rotationMatrix.toImplementation()));

  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>& xyz) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromRpy<SourcePrimType_, DestPrimType_>(xyz.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::AngleAxis<DestPrimType_, Usage_>, eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::AngleAxis<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>& zyx) {
    return eigen_impl::AngleAxis<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getAngleAxisFromYpr<SourcePrimType_, DestPrimType_>(zyx.toImplementation()));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class ComparisonTraits<eigen_impl::AngleAxis<PrimType_, Usage_>, eigen_impl::AngleAxis<PrimType_, Usage_>> {
 public:
  inline static bool isEqual(const eigen_impl::AngleAxis<PrimType_, Usage_>& a, const eigen_impl::AngleAxis<PrimType_, Usage_>& b){
    const double tolPercent = 0.01;
    return common::eigen::compareRelative(a.angle(), b.angle(), tolPercent) &&
        common::eigen::compareRelative(a.axis().x(), b.axis().x(), tolPercent) &&
        common::eigen::compareRelative(a.axis().y(), b.axis().y(), tolPercent) &&
        common::eigen::compareRelative(a.axis().z(), b.axis().z(), tolPercent);
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Fixing Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, enum RotationUsage Usage_>
class FixingTraits<eigen_impl::AngleAxis<PrimType_, Usage_>> {
 public:
  inline static void fix(eigen_impl::AngleAxis<PrimType_, Usage_>& aa) {
    aa.setAxis(aa.axis().normalized());
  }
};

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_ANGLEAXIS_HPP_ */


/*
 * Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org)
 *
 * 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.
 *
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <math.h>
#include <string>
#include <boost/lexical_cast.hpp>
#include "analysistool.h"
#include <votca/tools/histogram.h>
#include <votca/csg/version.h>
#include "bondedstatistics.h"
#include "tabulatedpotential.h"

using namespace std;
using namespace boost;

TabulatedPotential::TabulatedPotential()
{
    _tab_smooth1 = _tab_smooth2 = 0;
    _T = 300;
}

void TabulatedPotential::Register(map<string, AnalysisTool *> &lib)
{
    lib["tab"] = this;
    lib["hist"] = this;
}

void TabulatedPotential::Command(BondedStatistics &bs, string cmd, vector<string> &args)
{
    if(args[0] == "set") {
        if(cmd == "hist") SetOption(_hist_options, args);
        else if(cmd == "tab") {
            if(!SetOption(_tab_options, args)) {
                if(args.size() >2) {
                    if(args[1] == "smooth_pdf")
                        _tab_smooth1 = lexical_cast<int>(args[2]);
                    else if(args[1] == "smooth_pot")
                        _tab_smooth2 = lexical_cast<int>(args[2]);
                    else if(args[1] == "T")
                        _T = lexical_cast<double>(args[2]);
                    else {
                        cout << "unknown option " << args[2] << endl;
                        return;
                    }
                }
            }
            if(args.size() <=2) {
                cout << "smooth_pdf: " << _tab_smooth1 << endl;
                cout << "smooth_pot: " << _tab_smooth2 << endl;
                cout << "T: " << _T << endl;
            }
        }
    }
    else if(args.size() >= 2) {
        if(cmd == "hist") WriteHistogram(bs, args);
        else if(cmd == "tab") WritePotential(bs, args);
    }
    else cout << "wrong number of arguments" << endl;
}

void TabulatedPotential::Help(string cmd, vector<string> &args)
{
    if(args.size() == 0) {
        if(cmd == "tab") {
            cout << "tab <file> <selection>\n"
                 << "Calculate tabulated potential by inverting the distribution function. "
                    "Statistics is calculated using all interactions in selection.\n"
                    "see also: help tab set\n\n"
                    "example:\ntab set scale bond\ntab U_bond.txt *:bond:*\n";
        }
        if(cmd == "hist") {
            cout << "hist <file> <selection>\n"
                 << "Calculate distribution function for selection. "
                    "Statistics is calculated using all interactions in selection.\n"
                    "see also: help hist set\n\n"
                    "example:hist U_bond.txt *:bond:*\n";
        }
        return;
    }
    if(args[0] == "set") {
        if(args.size() == 1) {
            cout << cmd << " set <option> <value>\n"
                 << "set option for this command. Use \"" << cmd << " set\""
                    " for a list of available options. To get help on a specific option use e.g.\n"
                    << cmd << " set periodic\n";
            return;
        }
        if(args[1] == "n") {
            cout << cmd << "set n <integer>\n"
                 << "set number of bins for table\n";
            return;
        }
        if(args[1] == "min") {
            cout << cmd << "set min <value>\n"
                 << "minimum value of interval for histogram (see also periodic, extend)\n";
            return;
        }
        if(args[1] == "max") {
            cout << cmd << "set max <value>\n"
                 << "maximum value of interval for histogram (see also periodic, extend)\n";
            return;
        }
        if(args[1] == "periodic") {
            cout << cmd << "set periodic <value>\n"
                << "can be 1 for periodic interval (e.g. dihedral) or 0 for "
                    "non-periodic (e.g. bond)\n";
            return;
        }
        if(args[1] == "auto") {
            cout << cmd << "set auto <value>\n"
                 "can be 1 for automatically determine the interval for the "
                 "table (min, max, extend will be ignored) or 0 to use min/max as specified\n";
            return;
        }
        if(args[1] == "extend") {
            cout << cmd << "set extend <value>\n"
                 "should only be used with auto=0. Can be 1 for extend the interval "
                 "if values are out of bounds (min/max) "
                 "or 0 to ignore values which are out of the interal\n";
            return;
        }
        if(args[1] == "scale") {
            cout << cmd << "set scale <value>\n"
                "volume normalization of pdf. Can be no (no scaling), bond "
                "(1/r^2) or angle ( 1/sin(phi) ). See VOTCA manual, section "
                "theoretical background for details\n";
            return;
        }
        if(args[1] == "normalize") {
            cout << cmd << "set normalize <value>\n"
                 "can be 1 for a normalized histogram or 0 to skip normalization\n";
            return;
        }

        if(cmd == "tab") {
            if(args[1] == "smooth_pdf") {
                cout << "tab set smooth_pdf <value>\n"
                 "Perform so many smoothing iterations on the distribution function before inverting the potential\n";
                return;
            }
            if(args[1] == "smooth_pot") {
                cout << "tab set smooth_pot <value>\n"
                 "Perform so many smoothing iterations on tabulated potential after inverting the potential\n";
                return;
            }
            if(args[1] == "T") {
                cout << "tab set T <value>\n"
                 "Temperature in Kelvin the simulation was performed\n";
                return;
            }
        }
    }

    cout << "no help text available" << endl;
}

bool TabulatedPotential::SetOption(Histogram::options_t &op, const vector<string> &args)
{
    if(args.size() >2) {
        if(args[1] == "n")
            op._n = lexical_cast<int>(args[2]);
        else if(args[1] == "min") {
            op._min = lexical_cast<double>(args[2]);
        }
        else if(args[1] == "max")
            op._max = lexical_cast<double>(args[2]);
        else if(args[1] == "periodic")
            op._periodic = lexical_cast<bool>(args[2]);
        else if(args[1] == "auto")
            op._auto_interval = lexical_cast<bool>(args[2]);
        else if(args[1] == "extend")
            op._extend_interval = lexical_cast<bool>(args[2]);
        else if(args[1] == "normalize")
            op._normalize = lexical_cast<bool>(args[2]);
        else if(args[1] == "scale") {
            if(args[2]=="no" || args[2]=="bond" || args[2]=="angle")
                op._scale = args[2];
            else {
                cout << "scale can be: no, bond or angle\n";
            }
        }
        else {
            return false;
        }
    }
    else {
        cout << "n: " << op._n << endl;
        cout << "min: " << op._min << endl;
        cout << "max: " << op._max << endl;
        cout << "periodic: " << op._periodic << endl;
        cout << "auto: " << op._auto_interval << endl;
        cout << "extend: " << op._extend_interval << endl;
        cout << "scale: " << op._scale << endl;
        cout << "normalize: " << op._normalize << endl;
    }
    return true;
}

void TabulatedPotential::WriteHistogram(BondedStatistics &bs, vector<string> &args)
{
    ofstream out;
    DataCollection<double>::selection *sel = NULL;

    for(size_t i=1; i<args.size(); i++)
        sel = bs.BondedValues().select(args[i], sel);
    Histogram h(_hist_options);
    h.ProcessData(sel);
    out.open(args[0].c_str());
/*    out << "# histogram, created csg version " <<  VERSION_STR  << endl;
    out << "# n = " << _hist_options._n << endl;
    out << "# min = " << _hist_options._min << endl;
    out << "# max = " << _hist_options._max << endl;
    out << "# periodic = " << _hist_options._periodic << endl;
    out << "# auto = " << _hist_options._auto_interval << endl;
    out << "# extend = " << _hist_options._extend_interval << endl;
    out << "# scale = " << _hist_options._scale << endl;*/
    out << h ;
    out.close();
    cout << "histogram created using " << sel->size() << " data-rows, written to " << args[0] << endl;
    delete sel;
}

void TabulatedPotential::CalcForce(vector<double> &U, vector<double> &F, double dx, bool bPeriodic)
{
    size_t n=U.size();
    double f = 0.5/dx;
    F.resize(n);
    if(bPeriodic)
        F[n-1] = F[0] = -(U[1] - U[n-2])*f;
    else {
        F[0] = -(U[1] - U[0])*2*f;
        F[n-1] = -(U[n-1] - U[n-2])*2*f;
    }
    for(size_t i=1; i<n-1; i++)
        F[i] = -(U[i+1] - U[i-1])*f;
}

void TabulatedPotential::WritePotential(BondedStatistics &bs, vector<string> &args)
{
   ofstream out;
    DataCollection<double>::selection *sel = NULL;

    for(size_t i=1; i<args.size(); i++)
        sel = bs.BondedValues().select(args[i], sel);
    Histogram h(_tab_options);
    h.ProcessData(sel);
    for(int i=0; i<_tab_smooth1; ++i)
        Smooth(h.getPdf(), _tab_options._periodic);
    BoltzmannInvert(h.getPdf(), _T);
    for(int i=0; i<_tab_smooth2; ++i)
        Smooth(h.getPdf(), _tab_options._periodic);
    out.open(args[0].c_str());

/*       out << "# tabulated potential, created csg version " VERSION_STR  << endl;
    out << "# n = " << _tab_options._n << endl;
    out << "# min = " << _tab_options._min << endl;
    out << "# max = " << _tab_options._max << endl;
    out << "# periodic = " << _tab_options._periodic << endl;
    out << "# auto = " << _tab_options._auto_interval << endl;
    out << "# extend = " << _tab_options._extend_interval << endl;
    out << "# scale = " << _tab_options._scale << endl;
    out << "# smooth_pdf = " << _tab_smooth1 << endl;
    out << "# smooth_pot = " << _tab_smooth2 << endl;
    out << "# T = " << _T << endl;*/

    vector<double> F;

    CalcForce(h.getPdf(), F, h.getInterval(), _tab_options._periodic);
    for(int i=0; i<h.getN(); i++) {
        out << h.getMin() + h.getInterval()*((double)i) << " " << h.getPdf()[i] << " " << F[i] << endl;
    }
    out.close();
    cout << "histogram created using " << sel->size() << " data-rows, written to " << args[0] << endl;
    delete sel;
}

void TabulatedPotential::Smooth(vector<double> &data, bool bPeriodic)
{
    double old[3];
    int n=data.size();
    if(bPeriodic) {
        old[0] = data[n-3];
        old[1] = data[n-2];
    }
    else {
        old[0] = data[0];
        old[1] = data[0];
    }
    size_t i;
    for(i=0; i<data.size()-2; i++) {
        old[2] = data[i];
        data[i] = (old[0] + 2.*old[1] + 3.*data[i] + 2.*data[i+1] + data[i+2])/9.;
        old[0]=old[1];
        old[1]=old[2];;
    }
    if(bPeriodic) {
        data[i] = (old[0] + 2.*old[1] + 3.*data[i] + 2.*data[i+1] + data[0])/9.;
        old[0]=old[1];old[1]=data[i];
        data[n-1] = data[0];
    }
    else {
        data[i] = (old[0] + 2.*old[1] + 3.*data[i] + 3.*data[i+1])/9.;
        old[0]=old[1];old[1]=data[i];
        i++;
        data[i] = (old[0] + 2.*old[1] + 6.*data[i])/9.;
    }
}

void TabulatedPotential::BoltzmannInvert(vector<double> &data, double T)
{
    double _min, _max;

    _min = numeric_limits<double>::max();
    _max = numeric_limits<double>::min();

    for(size_t i=0; i<data.size(); i++) {
        _max = max(data[i], _max);
        if(data[i] > 0) _min = min(data[i], _min);
    }
    _max = -8.3109*T*log(_max)*0.001;
    _min = -8.3109*T*log(_min)*0.001-_max;

    for(size_t i=0; i<data.size(); i++) {
        if(data[i] == 0) data[i] = _min;
        else
            data[i] = -8.3109*T*log(data[i])*0.001 - _max;
    }
}