/*
    Copyright (c) 2011 Alin Marin Elena <alinm.elena@gmail.com>

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use,
    copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the
    Software is furnished to do so, subject to the following
    conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
    OTHER DEALINGS IN THE SOFTWARE.
*/

#include <cmath>
#include <iostream>

#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_01.hpp>

#include "solution.h"
#include "utils.h"
#include "reading.h"
#include "colours.h"
#include "gutenberg.h"
#include "collectivevariable.h"

using namespace std;
using namespace colours;

void solution::InitializeSimulation ( std::vector<atom>& a, const double& temp, const double& box, const int& seed, string const& filename , double const& gamma )
{
     InitializeSimulation ( a,temp,box,seed,filename );
     int k=0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).setGamma ( gamma );
          ( *i ).setID ( k );
          k++;
     }
}

void solution::InitializeSimulation ( std::vector<atom>& a, const double& temp, const double& box, const int& seed, string const& filename )
{
// equal spaced in the box
     boost::mt19937 igen ( seed );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >    rgauss ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     double x,y,z,fact;

     int k=reading::readXYZ ( filename.c_str(),a );
     for ( int i=0; i<a.size(); i++ ) {
          a[i].setMass ( 1.0 );
          a[i].setID ( i );
          fact=sqrt ( utils::Kb*temp/a[i].getMass() );
          a[i].setVelocities ( fact*rgauss(),fact*rgauss(),fact*rgauss() );
          a[i].setForces ( 0.0,0.0,0.0 );
     }

//remove the movement of the centre of mass
     centerOfMassV ( a );
     scaleVelocities ( a,temp );
}

void solution::RandomVelocities ( std::vector<atom>& a, const double& temp, const double& box, const int& seed )
{
// equal spaced in the box
     boost::mt19937 igen ( seed );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >    rgauss ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     double x,y,z,fact;

     for ( int i=0; i<a.size(); i++ ) {
          fact=sqrt ( utils::Kb*temp/a[i].getMass() );
          a[i].setVelocities ( fact*rgauss(),fact*rgauss(),fact*rgauss() );
          a[i].setForces ( 0.0,0.0,0.0 );
     }
//remove the movement of the centre of mass
     centerOfMassV ( a );
     scaleVelocities ( a,temp );
}

void solution::InitializeSimulation ( std::vector<atom>& a, string const& filename )
{
     int k=reading::readFull ( filename.c_str(),a );
     if ( k!=0 ) {
          utils::Print ( "Error!!!",cout,colours::red );
     }
}

void solution::GenerateLattice ( vector< atom >&a, const double&rho,
                                 const int&mass, const double&nAtoms, double& box, const string& el )
{

     double volume=nAtoms*mass/rho;
     box=pow ( volume,1.0/3.0 );
     int n=int ( pow ( nAtoms,1.0/3.0 ) );
     if ( n*n*n!=nAtoms ) n++;
     double h=box/ ( n );
     double x;
     double y;
     double z;
     int m=0;
     for ( int i=0; i<n; i++ ) {
          x=i*h;
          for ( int j=0; j<n; j++ ) {
               y=j*h;
               for ( int k=0; k<n; k++ ) {
                    z=k*h;
                    if ( m<nAtoms ) {
                         a.push_back ( atom() );
                         a[m].setElement ( el );
                         a[m].setPositions ( x,y,z );
                         a[m].setMass ( mass );
                         a[m].setID ( m );
                    }
                    m++;
               }
          }
     }

}

void solution::centerOfMassV ( vector< atom >& a )
{
     double vmx=0.0, vmy=0.0, vmz=0.0,mt=0.0;
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          vmx+= ( *it ).px();
          vmy+= ( *it ).py();
          vmz+= ( *it ).pz();
          mt+= ( *it ).getMass();
     }
     vmx=-vmx/mt;
     vmy=-vmy/mt;
     vmz=-vmz/mt;
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).addtoVelocities ( vmx,vmy,vmz );
     }
}

void solution::scaleVelocities ( vector< atom >& a, const double& T )
{
     double tx=0.0,ty=0.0, tz=0.0;
     if ( T>0.0 ) {
          for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
               tx+= ( *it ).getMass() * ( *it ).vx2();
               ty+= ( *it ).getMass() * ( *it ).vy2();
               tz+= ( *it ).getMass() * ( *it ).vz2();
          }
          tx=tx/ ( a.size() *utils::Kb ) ;
          ty=ty/ ( a.size() *utils::Kb ) ;
          tz=tz/ ( a.size() *utils::Kb ) ;
          tx=sqrt ( T/tx );
          ty=sqrt ( T/ty );
          tz=sqrt ( T/tz );
     }
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).scaleVelocity ( tx,ty,tz );
     }
}

double solution::computeForces ( vector< atom >& a, const double & rc, const double& box, double& vir )
{
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).setForces ( 0.0,0.0,0.0 );
     }

     double R,fR,fx,fy,fz,energy;
     vector<double> dR ( 3 );
     double ec=utils::LennardJones ( rc );
     energy=0.0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end()-1; i++ ) {
          for ( vector<atom>::iterator j=i+1; j<a.end(); j++ ) {
               R=rdr ( dR,*i,*j,box );
               if ( R<rc ) {
                    fR=utils::LennardJonesdR ( R );
                    fx=-fR*dR[0]/R;
                    fy=-fR*dR[1]/R;
                    fz=-fR*dR[2]/R;
                    ( *i ).addtoForces ( fx,fy,fz );
                    ( *j ).addtoForces ( -fx,-fy,-fz );
                    energy+=utils::LennardJones ( R )-ec;
                    vir+=fR*R;
               }
          }
     }
     return energy;
}

double solution::velocityVerlet ( vector< atom >& a, const double& rc, const double& box, const double& dt , double& kin, double &vir )
{
     kin=0.0;
     vir=0.0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updatePositionsVV ( dt );
          ( *i ).putInBox ( box );
     }
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVV ( dt );
     }
     double energy = solution::computeForces ( a,rc,box,vir );
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVV ( dt );
          kin+= ( *i ).v2() * ( *i ).getMass();
     }
     kin=kin/2.0;
     return energy;
}

void solution::MDDriverVV ( vector< atom >& a, generalInputs& in )
{

     double potEnergy, kinEnergy, vir,pressure;
     potEnergy = PotentialEnergy ( a,in.cut,in.box, in.shift,vir );
     kinEnergy = KineticEnergy ( a );
     double iT,tT;
     double consQ;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
     consQ=potEnergy+kinEnergy;
     iT=2.0/3.0*kinEnergy/a.size() /utils::Kb;
     tT=iT;
     gutenberg::mdHeader ( cout,in.fancy );
     gutenberg::mdRepLine ( 0.0,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );
     gutenberg::printAtoms ( a,in.debug );
     vector<double> avg ( 2 );
     int nsamp=0;
     for ( int k=0; k<avg.size(); k++ ) avg[k]=0.0;
     for ( int i=1; i<=in.nSteps; i++ ) {
          potEnergy = solution::velocityVerlet ( a,in.cut,in.box,in.dt,kinEnergy,vir );

          iT=2.0/3.0*kinEnergy/a.size() /utils::Kb;
          tT+= ( iT-tT ) / ( i+1.0 );

          if ( i%in.frequency==0 ) {
               gutenberg::printForces ( a,in.log );
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
               gutenberg::printAtoms ( a,in.debug );
          }
          consQ=potEnergy+kinEnergy;
          avg[0]+=consQ;
          avg[1]+=consQ*consQ;
          nsamp++;
          pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
          gutenberg::mdRepLine ( i*in.dt,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,cout );
     }
     for ( int k=0; k<avg.size(); k++ ) avg[k]=avg[k]/nsamp;
     utils::Print ( "CV:",cout,red );
     utils::Print ( ( avg[1]-avg[0]*avg[0] ) /utils::Kb/tT/tT,cout, green );
     cout<<endl;

}

double solution::VECIntegrator ( vector< atom >& a, const double&rc , const double& box , const double& dt, const double& s1, const double& s2, double& kin, double& vir )
{

     kin=0.0;
     vir = 0.0;
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
     }

     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updatePositionsVEC ( dt,s1 );
     }
     double energy = solution::computeForces ( a,rc,box, vir );
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
          kin+= ( *i ).v2() * ( *i ).getMass();
     }
     kin=kin/2.0;
     return energy;
}

void solution::MDDriverVEC ( vector< atom >& a, generalInputs & in )
{
     //set the random generators...
     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     vector<double> b ( 6 );
     double potEnergy, kinEnergy, vir;

     potEnergy = PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     kinEnergy = KineticEnergy ( a );

     double iT,tT;
     double consQ;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
     gutenberg::logLine ( "Volume: ",volume,in.log );
     gutenberg::logLine ( "Density: ",rho,in.log );
     consQ=potEnergy+kinEnergy;
     iT=2.0/3.0*kinEnergy/a.size() /utils::Kb;
     tT=iT;
     gutenberg::mdHeader ( cout,in.fancy );
     gutenberg::mdRepLine ( 0.0,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,in.log );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );
     gutenberg::printAtoms ( a,in.debug );

     double s1=sqrt ( utils::Kb*in.TBath*in.dt/6.0 ) *in.dt;
     double s2=sqrt ( 2.0*utils::Kb*in.TBath );
     double v2=0.0;

     vector<double> K,E,vx,vy,vz,P,v;
//     K.push_back ( kinEnergy/a.size() );
//     E.push_back ( consQ/a.size() );
//     P.push_back ( potEnergy/a.size() );
//     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
//         vx.push_back ( ( *it ).gvx() );
//         vy.push_back ( ( *it ).gvy() );
//         vz.push_back ( ( *it ).gvz() );
//         v.push_back ( sqrt ( ( *it ).v2() ) );
//         }
     double aene=0.0, aene2=0.0,cv;
     int nsamp=0;
     for ( int i=1; i<=in.nSteps; i++ ) {
          for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               b[2]=xi();
               b[3]=xi();
               b[4]=xi();
               b[5]=xi();
               ( *it ).setNoise ( b );
          }
          potEnergy = solution::VECIntegrator ( a,in.cut,in.box,in.dt,s1,s2,kinEnergy,vir );

//         pressure=rho* ( utils::Kb*tT ) +vir/volume/3.0;
          gutenberg::mdRepLine ( i*in.dt,potEnergy/a.size(),kinEnergy/a.size(),iT,tT,pressure,consQ/a.size(),VCM ( a ),in.fancy,in.log );

          if ( i%in.frequency==0 )  {
//             gutenberg::printForces ( a,in.log );
//             gutenberg::printXYZ ( in.xyz,in.name,a,true );
//             gutenberg::printAtoms ( a,in.debug );
               iT=2.0*kinEnergy/ ( 3.0*a.size() *utils::Kb );

               consQ=potEnergy+kinEnergy;
               if ( i == in.nEquil ) {
                    nsamp=0;
                    E.erase ( E.begin(),E.end() );
               }
               if ( nsamp==0 ) {

                    aene=consQ;
                    tT=iT;
                    nsamp=0;
               } else {
                    nsamp++;
                    aene+= ( aene-consQ ) / ( nsamp+1.0 );
                    tT+= ( iT-tT ) / ( nsamp+1.0 );
               }
               E.push_back ( consQ/a.size() );

               cv= utils::sigma2 ( E ) / ( utils::Kb*tT*tT );
               if ( i>in.nEquil ) {
                    gutenberg::logInfo ( "step ",i-in.nEquil,cout );

               } else {
                    gutenberg::logInfo ( "step ",i,cout );
               }
               gutenberg::logInfo ( "T ",tT,cout );
               gutenberg::logInfo ( "<E> ",aene,cout );
               gutenberg::logLine ( "Cv: ", cv*a.size(),cout );
               gutenberg::logLine ( "Cv/N: ", cv,cout );

               if ( i>in.nEquil ) {

                    P.push_back ( potEnergy/a.size() );
                    for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
                         vx.push_back ( ( *it ).gvx() );
                         vy.push_back ( ( *it ).gvy() );
                         vz.push_back ( ( *it ).gvz() );
                         v2= ( *it ).v2();
                         v.push_back ( sqrt ( v2 ) );
                         K.push_back ( v2* ( *it ).getMass() /2.0 );
                    }
               }
          }
     }

     cv=utils::sigma2 ( E );
     utils::Print ( "CV:",cout,red );
     utils::Print ( cv/ ( utils::Kb*tT*tT ) *a.size(),cout, green );
     cout<<endl;
     utils::Print ( "sigma^2 K",cout, red );
     utils::Print ( utils::sigma2 ( K ),cout,green );
     cout<<endl;
     utils::Print ( "sigma^2 E",cout, red );
     utils::Print ( cv,cout,green );
     cout<<endl;
     solution::histogramWrapper ( K,in.bin,"Kvec.hist" );
     solution::histogramWrapper ( E,in.bin,"Evec.hist" );
     solution::histogramWrapper ( P,in.bin,"Pvec.hist" );
     solution::histogramWrapper ( vx,in.bin,"vxvec.hist" );
     solution::histogramWrapper ( vy,in.bin,"vyvec.hist" );
     solution::histogramWrapper ( vz,in.bin,"vzvec.hist" );
     solution::histogramWrapper ( v,in.bin,"vvec.hist" );
}

double solution::computeForcesTAMD ( vector< atom >& a, vector<colVar>& cv,const double & rc, const double& box, double& vir )
{
     for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
          ( *it ).setForces ( 0.0,0.0,0.0 );
     }

     double R,fR,fx,fy,fz,energy;
     vector<double> dR ( 3 );
     double ec=utils::LennardJones ( rc );
     int k=0;
     energy=0.0;
     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).force ( a,box );
     }
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          fx=0.0;
          fy=0.0;
          fz=0.0;
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               ( *it ).dtheta ( dR,k,a,box );
               fx=fx+ ( *it ).f*dR[0];
               fy=fy+ ( *it ).f*dR[1];
               fz=fz+ ( *it ).f*dR[2];
          }
          k++;
          ( *i ).addtoForces ( -fx,-fy,-fz );
          for ( vector<atom>::iterator j=i+1; j<a.end(); j++ ) {
               R=rdr ( dR,*i,*j,box );
               if ( R<rc ) {
                    fR=utils::LennardJonesdR ( R );
                    fx=-fR*dR[0]/R;
                    fy=-fR*dR[1]/R;
                    fz=-fR*dR[2]/R;
                    ( *i ).addtoForces ( fx,fy,fz );
                    ( *j ).addtoForces ( -fx,-fy,-fz );
                    energy+=utils::LennardJones ( R )-ec;
                    vir+=fR*R;
               }
          }
     }
     return energy;
}

double solution::TAMDIntegrator ( vector< atom >& a, vector<colVar>&cv, const double&rc , const double& box , const double& dt, const double& s1,const double& c1, const double& s2, double& kin, double&kinz, double&vir )
{

     kin=0.0;
     kinz=0.0;
     vir =0.0;

     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
     }
     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).updatezvVEC ( dt );
     }

     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updatePositionsVEC ( dt,s1 );
     }
     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).updatezpVEC ( dt,c1 );
     }
     double energy = solution::computeForcesTAMD ( a,cv,rc,box, vir );
     for ( vector<atom>::iterator i=a.begin(); i<a.end(); i++ ) {
          ( *i ).updateVelocitiesVEC ( dt,s2 );
          kin+= ( *i ).v2() * ( *i ).getMass();
     }

     for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
          ( *i ).updatezvVEC ( dt );
          kinz+= ( *i ).v* ( *i ).v* ( *i ).mu;
     }

     kin=kin/2,0;
     kinz=kinz/2.0;
     return energy;
}

///
///  @brief the driver for TAMD
///  @details http://dx.doi.org/10.1016/j.cplett.2006.05.062
///   it uses the VEC integrator for EOM
///  @param a a vector of atoms
///  @param cv the vector of collective variables
///  @param in contains the input parameters
///
void solution::MDDriverTAMD ( vector< atom >& a, vector<colVar>& cv, generalInputs & in )
{
     //set the random generators...

     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     vector<double> b ( 6 );
     double potEnergy, kinEnergy, vir;

     potEnergy = PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     kinEnergy = KineticEnergy ( a );

     double iT,tT;
     double iTz,tTz;
     double consQ;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;

     consQ=potEnergy+kinEnergy;
     iT=2.0*kinEnergy/ ( 3.0*a.size() *utils::Kb );
     tT=iT;
     iTz=0.0;
     tTz=iTz;
     double kinz=0.0;
     gutenberg::mdTAMDHeader ( cv,cout,in.fancy );
     gutenberg::mdTAMDRepLine ( 0.0,potEnergy/a.size(),kinEnergy/a.size(),tT,tTz,pressure,kinz/cv.size(),consQ/a.size(),VCM ( a ),cv,in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );
     gutenberg::printAtoms ( a,in.debug );

     double s1=sqrt ( utils::Kb*in.TBath*in.dt/6.0 ) *in.dt;
     double c1=sqrt ( utils::Kb*in.Tcv*in.dt/6.0 ) *in.dt;
     double s2=sqrt ( 2.0*utils::Kb*in.TBath );

     vector<double> KX,KZ,vxX,vyX,vyZ,vX,vzX,vZ;
     double v2;
     for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
          ( *it ).s=sqrt ( 2.0*utils::Kb*in.Tcv* ( *it ).gamma/ ( *it ).mu );
     }
     for ( int i=1; i<=in.nSteps; i++ ) {
          for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               b[2]=xi();
               b[3]=xi();
               b[4]=xi();
               b[5]=xi();
               ( *it ).setNoise ( b );
          }
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               ( *it ).setNoise ( b );
          }
          potEnergy = solution::TAMDIntegrator ( a,cv,in.cut,in.box,in.dt,s1,c1,s2,kinEnergy, kinz,vir );

          iT=2.0*kinEnergy/ ( 3.0*a.size() *utils::Kb );
          tT+= ( iT-tT ) / ( i+1.0 );

          iTz=2.0*kinz/ ( cv.size() *utils::Kb );
          tTz+= ( iTz-tTz ) / ( i+1.0 );
          if ( i%in.frequency==0 && i>in.nEquil ) {
               gutenberg::printForces ( a,in.log );
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
               gutenberg::printAtoms ( a,in.debug );
               for ( vector<atom>::iterator it=a.begin(); it<a.end(); it++ ) {
                    vxX.push_back ( ( *it ).gvx() );
                    vyX.push_back ( ( *it ).gvy() );
                    vzX.push_back ( ( *it ).gvz() );
                    v2= ( *it ).v2();
                    vX.push_back ( sqrt ( v2 ) );
                    KX.push_back ( v2* ( *it ).getMass() /2.0 );
               }
               for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
                    vZ.push_back ( ( *it ).v );
                    KZ.push_back ( ( *it ).mu* ( *it ).v* ( *it ).v/2.0 );
               }
          }
          consQ=potEnergy+kinEnergy;
          pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
          gutenberg::mdTAMDRepLine ( i*in.dt,potEnergy/a.size(),kinEnergy/a.size(),tT,tTz,pressure,kinz/cv.size(),consQ/a.size(),VCM ( a ),cv,in.fancy,cout );
     }
     solution::histogramWrapper ( vxX,in.bin,"vxX.hist" );
     solution::histogramWrapper ( vyX,in.bin,"vyX.hist" );
     solution::histogramWrapper ( vzX,in.bin,"vzX.hist" );
     solution::histogramWrapper ( vX,in.bin,"vX.hist" );
     solution::histogramWrapper ( KX,in.bin,"KX.hist" );
     solution::histogramWrapper ( vZ,in.bin,"vZ.hist" );
     solution::histogramWrapper ( KZ,in.bin,"KZ.hist" );
}

void solution::optimalDr ( double& d , int& otry, int& ogood, const int& itry, const int& igood, const double& box )
{
     if ( itry!=otry ) {
          double f= ( double ) ( igood-ogood ) / ( double ) ( itry-otry );
          double od=d;
          d=d*f/0.5; //0.5 is the target accteptance ratio;
          if ( d/od>1.5 ) d=od*1.5;
          if ( d/od<0.5 ) d=od*0.5;
          if ( d>box/2 ) d=box/2;
          otry=itry;
          ogood=igood;
     }

}

///
///  @brief the driver for a metropolis montecarlo simulation
///  @param a a vector of atoms
///  @param in contains the input parameters
///
void solution::MMCDriver ( vector< atom >& a, generalInputs& in )
{
     boost::mt19937 igen ( in.seeds[0] );
     boost::uniform_01<boost::mt19937> rn ( igen );

     double ei,ef;
     int rAtom;
     double rx,ry,rz;

     int itry=0;
     int igood=0;
     int otry, ogood;
     otry=itry;
     ogood=igood;
     int nsamp=0;
     double fact;
     double et,vt;
     double aene=0.0, ap=0.0,aene2=0.0;
     double rho=density ( a,in.box );
     double volume=in.box*in.box*in.box;
     double vir, viri,virf;
     double etot=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     gutenberg::logLine ( "Volume: ",volume,in.log );
     gutenberg::logLine ( "Density: ",rho,in.log );
     utils::Print ( etot,cout,32,16 );
     cout<<endl;

     vector<double>ene;

     double pressure;
     int cycles;
     int startsamp;
     pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
     gutenberg::MMCHeader ( cout, in.fancy );
     gutenberg::MMCRepLine ( 0,etot/a.size(),pressure,in.fancy,cout );
     gutenberg::logLine ( "Initial etot ", etot,cout );
     gutenberg::logLine ( "Initial etot/N ", etot/a.size(),cout );
     gutenberg::logLine ( "Initial virial ", vir,cout );
     solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
     for ( int stage=0; stage<2; stage++ ) {
          if ( stage==0 ) {
               cycles=in.nEquil;
               startsamp=cycles/10;
               utils::Print ( "Equilibration stage:", cout, blue );
               cout<<endl;
          } else {
               startsamp=0;
               cycles=in.mccycles;
               utils::Print ( "Production stage:", cout, blue );
               cout <<endl;
               etot=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
               ene.push_back ( etot/a.size() );
               gutenberg::MMCHeader ( cout, in.fancy );
               gutenberg::MMCRepLine ( 0,etot/a.size(),pressure,in.fancy,cout );
          }
          gutenberg::logInfo ( "old dr:",in.dr,cout );
          solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
          gutenberg::logLine ( "new dr:",in.dr,cout );
          itry=0;
          igood=0;
          aene=0.0;
          aene2=0.0;
          ap=0.0;
          nsamp=0;
          for ( int i=0; i<cycles; i++ ) {
               // one should compute the optimal dr here...
               for ( int j=0; j<in.mcsteps; j++ ) {
                    itry++;
                    rAtom=int ( rn() *a.size() );
                    ei=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,viri );
                    a[rAtom].saveOldPositions ( rx,ry,rz );
                    a[rAtom].randomMove ( in.dr,rn(),rn(),rn() );
                    ef=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,virf );
                    fact=- ( ef-ei ) / ( utils::Kb*in.TBath );
                    if ( rn() <exp ( fact ) ) {
                         igood++;
                         etot=etot+ef-ei;
                         vir=vir+virf-viri;
                         a[rAtom].putInBox ( in.box );
                    } else { //if rejected
                         a[rAtom].setPositions ( rx,ry,rz );
                    }
               }
               if ( ( ( i+1 ) %in.frequency==0 ) && ( i+1>startsamp ) ) {
                    //sampling point...
                    pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
                    gutenberg::MMCRepLine ( ( i+1 ) *in.mcsteps,etot/a.size(),pressure,in.fancy,in.log );
//                 gutenberg::printXYZ ( in.xyz,in.name,a,true );
                    ene.push_back ( etot/a.size() );
                    aene+=etot;
                    aene2+=etot*etot;
                    ap+=pressure;
                    nsamp++;

                    gutenberg::logInfo ( "|cycle: ",i+1,in.log );
                    gutenberg::logInfo ( "|attempts: ",itry, in.log );
                    gutenberg::logInfo ( "|accepted: ",igood, in.log );
                    gutenberg::logLine ( "|ratio", ( double ) ( igood ) / ( double ) ( itry ),in.log );
               }
               if ( ( i+1 ) % ( cycles/5 ) ==0 ) {
                    gutenberg::logInfo ( "|cycle: ",i+1,cout );
                    gutenberg::logInfo ( "|attempts: ",itry-otry, cout );
                    gutenberg::logInfo ( "|accepted: ",igood-ogood, cout );
                    gutenberg::logLine ( "|ratio", ( double ) ( igood-ogood ) / ( double ) ( itry-otry ),cout );
                    gutenberg::logInfo ( "old dr:",in.dr,cout );
                    solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
                    gutenberg::logInfo ( "new dr:",in.dr,cout );
                    cout<<endl;
               }

          }
          gutenberg::logLine ( "Etot: ",etot,cout );
          gutenberg::logLine ( "Erot/N",etot/a.size(),cout );
          pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
          gutenberg::logLine ( "virial: ",vir,cout );
          gutenberg::logLine ( "pressure: ",pressure,cout );
          et=PotentialEnergy ( a,in.cut,in.box,in.shift,vt );
          gutenberg::logLine ( "diff energy: ",et-etot,cout );
          gutenberg::logLine ( "diff virial: ", vt-vir,cout );
          gutenberg::logInfo ( "|cycle: ",cycles,cout );
          gutenberg::logInfo ( "|attempts: ",itry, cout );
          gutenberg::logInfo ( "|accepted: ",igood, cout );
          gutenberg::logLine ( "|ratio", ( double ) ( igood ) / ( double ) ( itry ),cout );
          aene2=aene2/nsamp;
          aene=aene/nsamp;
          gutenberg::logLine ( "Avg energy:", aene,cout );
          gutenberg::logLine ( "Avg energy/N:", aene/a.size(),cout );
          gutenberg::logLine ( "Avg pressure:", ap/nsamp,cout );
          gutenberg::logLine ( "CV: ",1.5*utils::Kb*a.size() + ( aene2-aene*aene ) / ( utils::Kb*in.TBath*in.TBath ),cout );
     }

     solution::histogramWrapper ( ene,in.bin,"Pmmc.hist" );

}

///
///  @brief the driver for TAMC
///  @details TAMC paper
///   it uses the VEC integrator for EOM
///  @param a a vector of atoms
///  @param cv the vector of collective variables
///  @param in contains the input parameters
///
void solution::MDDriverTAMC ( vector< atom >& a, vector<colVar>& cv, generalInputs & in )
{
     //set the random generators...

     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     boost::uniform_01<boost::mt19937> rn ( igen );
     vector<double> b ( 2 );
     double potEnergy, vir,fact,etot;

     potEnergy = PotentialEnergy ( a,in.cut,in.box,in.shift,vir ) +zpot ( a,cv,in.box );

     double iT=0.0,tT=0.0;
     double consQ,kin;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;

     int itry=0, otry=0,igood=0, ogood=0;
     double ei,ef, viri,virf;
     int rAtom;
     double rx,ry,rz;
     double kinz=0.0;

     gutenberg::mdTAMCHeader ( cv,cout,in.fancy );
     gutenberg::mdTAMCRepLine ( 0.0,potEnergy/a.size(),tT,pressure,kinz,cv,in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );

     double c1=sqrt ( utils::Kb*in.Tcv*in.dt/6.0 ) *in.dt;

     double aene=0.0, ap=0.0;
     int nsamp=0;
     vector<double> avgfz ( cv.size() ), KZ,vZ;
     for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
          ( *it ).s=sqrt ( 2.0*utils::Kb*in.Tcv* ( *it ).gamma/ ( *it ).mu );
     }
     for ( int step=1; step<=in.nSteps; step++ ) {
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               ( *it ).setNoise ( b );
          }
          vir =0.0;
          aene=0.0;
          ap=0.0;
          for ( int k=0; k<cv.size(); k++ ) avgfz[k]=0.0;
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dt );
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezpVEC ( in.dt,c1 );
          }
          //steps MC

          for ( int i=0; i<in.mccycles; i++ ) {
               for ( int j=0; j<in.mcsteps; j++ ) {
                    itry++;
                    rAtom=int ( rn() *a.size() );
                    ei=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,viri ) +zpot ( a,cv,in.box );
                    a[rAtom].saveOldPositions ( rx,ry,rz );
                    a[rAtom].randomMove ( in.dr,rn(),rn(),rn() );
                    a[rAtom].putInBox ( in.box );
                    ef=a[rAtom].myPotentialEnergy ( a,in.cut,in.box,in.shift,virf ) +zpot ( a,cv,in.box );
                    fact=- ( ef-ei ) / ( utils::Kb*in.TBath );
                    if ( rn() <exp ( fact ) ) {
                         igood++;
                         etot=etot+ef-ei;
                         vir=vir+virf-viri;
                    } else { //if rejected
                         a[rAtom].setPositions ( rx,ry,rz );
                    }
               }
               //sampling point...

               pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;
               aene+=etot/a.size();
               ap+=pressure;
               nsamp++;
               for ( int k=0; k<cv.size(); k++ ) {
                    cv[k].force ( a,in.box );
                    avgfz[k]+=cv[k].f;
               }
               solution::optimalDr ( in.dr,otry,ogood,itry,igood,in.box );
          }
          for ( int k=0; k<cv.size(); k++ ) {
               cv[k].f=avgfz[k]/nsamp;
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dt );
          }

          if ( step%in.frequency==0 ) {
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
          }
          kinz=zKin ( cv );

          iT=2.0*kinz/ ( cv.size() *utils::Kb );
          if ( step>=in.nEquil ) {
               tT+= ( iT-tT ) / ( step-in.nEquil+1.0 );
               if ( step%in.frequency==0 ) {
                    for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
                         vZ.push_back ( ( *it ).v );
                         KZ.push_back ( ( *it ).mu* ( *it ).v* ( *it ).v/2.0 );
                    }
               }
          }
          gutenberg::mdTAMCRepLine ( step*in.dt,aene/nsamp,tT,pressure/nsamp,kinz,cv,in.fancy,cout );
          nsamp=0;
     }

     solution::histogramWrapper ( vZ,in.bin,"vZtdmc.hist" );
     solution::histogramWrapper ( KZ,in.bin,"KZtdmc.hist" );

}

///
///  @brief the driver for TAHMC
///  @details TAMC paper and HMC paper
///   it uses the VEC integrator for EOM and velocity verlet for atoms
///  @param a a vector of atoms
///  @param cv the vector of collective variables
///  @param in contains the input parameters
///
void solution::MDDriverTAHMC ( vector< atom >& a, vector<colVar>& cv, generalInputs & in )
{
     //set the random generators...

     boost::mt19937 igen ( in.seeds[0] );
     boost::variate_generator<boost::mt19937, boost::normal_distribution<> >  xi ( igen, boost::normal_distribution<> ( 0.0,1.0 ) );
     boost::uniform_01<boost::mt19937> rn ( igen );
     vector<double> b ( 2 );
     double aPot, vir,fact,etot;
     double etoti,etotf;

     aPot = PotentialEnergy ( a,in.cut,in.box,in.shift,vir ) + zpot ( a,cv,in.box );

     double iT=0.0,tT=0.0;
     double consQ,kin;
     double rho=density ( a, in.box );
     double volume = in.box*in.box*in.box;
     double pressure=rho* ( utils::Kb*in.TBath ) +vir/volume/3.0;

     int itry=0, otry=0,igood=0, ogood=0;
     double zpoti,zpotf, viri,virf;
     int irseed;

     double kinz=0.0;
     double kini,kinf;
     vector<double> pos ( 3* a.size() );
     cout << pos.size() <<" "<<a.size() <<endl;
     for ( int i=0; i<a.size(); i++ ) {
          a[i].saveOldPositions ( pos[3*i],pos[3*i+1],pos[3*i+2] );
     }
     gutenberg::mdTAMCHeader ( cv,cout,in.fancy );
     gutenberg::mdTAMCRepLine ( 0.0,aPot/a.size(),tT,pressure,kinz,cv,in.fancy,cout );
     gutenberg::printXYZ ( in.xyz,in.name,a,true );

     double c1=sqrt ( utils::Kb*in.Tcv*in.dtz/6.0 ) *in.dt;

     double aene=0.0, ap=0.0,acc;
     int nsamp;
     vector<double> avgfz ( cv.size() ), KZ,vZ;
     irseed=rn();
     solution::zHMCSampler ( a, cv, in, irseed,pos, avgfz );
     for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
          ( *it ).s=sqrt ( 2.0*utils::Kb*in.Tcv* ( *it ).gamma/ ( *it ).mu );
     }
     for ( int step=1; step<=in.Nz; step++ ) {
          for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
               b[0]=xi();
               b[1]=xi();
               ( *it ).setNoise ( b );
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dtz );
          }
          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezpVEC ( in.dtz,c1 );
          }
          //steps HMC
          irseed=rn();
          solution::zHMCSampler ( a, cv, in, irseed,pos, avgfz );
//         solution::zSample

          for ( vector<colVar>::iterator i=cv.begin(); i<cv.end(); i++ ) {
               ( *i ).updatezvVEC ( in.dtz );
          }
          if ( step%in.frequency==0 ) {
               gutenberg::printXYZ ( in.xyz,in.name,a,true );
          }
          kinz=zKin ( cv );

          iT=2.0*kinz/ ( cv.size() *utils::Kb );

          tT+= ( iT-tT ) / ( step+1.0 );
          if ( step%in.frequency==0 ) {
               for ( vector<colVar>::iterator it=cv.begin(); it<cv.end(); it++ ) {
                    vZ.push_back ( ( *it ).v );
                    KZ.push_back ( ( *it ).mu* ( *it ).v* ( *it ).v/2.0 );
               }
          }

          gutenberg::mdTAMCRepLine ( step*in.dt,aene/nsamp,tT,pressure/nsamp,kinz,cv,in.fancy,cout );
          nsamp=0;
     }

//     solution::histogramWrapper ( vZ,in.bin,"vZtdmc.hist" );
//     solution::histogramWrapper ( KZ,in.bin,"KZtdmc.hist" );

}

void solution::zHMCSampler ( vector< atom >& a, vector<colVar>& cv, generalInputs & in, const int& irseed,
                             vector<double>& pos, vector<double>& avgfz )
{

     boost::mt19937 igen ( irseed );
     boost::uniform_01<boost::mt19937> rn ( igen );

     double epoti,epotf,zpoti,zpotf,kini,kinf;
     int igood,iseed,nsamp;
     double aHg,aH,dHg,fact,vir,acc;

     aHg=0.0;
     aH=0.0;
     nsamp=0;
     for ( int k=0; k<cv.size(); k++ ) avgfz[k]=0.0;
     epoti=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
     zpoti=zpot ( a,cv,in.box );
     igood=0;
     for ( int i=0; i<in.mccycles; i++ ) {
          iseed=int ( rn() *10000 );
          utils::Print ( "Start Mini-Trajectory ",cout,colours::green );
          utils::Print ( i,cout,colours::green );
          cout <<endl;

          solution::RandomVelocities ( a, in.TBath, in.box,iseed );
          kini=KineticEnergy ( a );
          solution::MDDriverVV ( a,in );
          epotf=PotentialEnergy ( a,in.cut,in.box,in.shift,vir );
          zpotf=zpot ( a,cv,in.box );
          kinf=KineticEnergy ( a );
          fact=- ( epotf-epoti+zpotf-zpoti+kinf-kini ) / ( utils::Kb*in.TBath );
          dHg=- ( epotf-epoti+kinf-kini ) / ( utils::Kb*in.TBath );
          if ( rn() <exp ( fact ) ) {
               igood++;
               epoti=epotf;
               zpoti=zpotf;
               for ( int j=0; j<a.size(); j++ ) {
                    a[j].saveOldPositions ( pos[3*j],pos[3*j+1],pos[3*j+2] );
               }
          } else { //if rejected
               for ( int j=0; j<a.size(); j++ ) {
                    a[j].setPositions ( pos[3*j],pos[3*j+1],pos[3*j+2] );
               }
          }
          utils::Print ( "End Mini-Trajectory ",cout,colours::green );
          utils::Print ( i,cout,colours::green );
          utils::Print ( fact,cout,colours::green );
          cout <<endl;
          if ( ( i%in.frequency==0 ) && ( i>=in.nEquil ) ) {
               nsamp++;
               for ( int k=0; k<cv.size(); k++ ) {
                    cv[k].force ( a,in.box );
                    avgfz[k]+=cv[k].f;
               }
               aHg+= ( exp ( dHg )-aHg ) / ( i-in.nEquil+1.0 );
               aH+= ( exp ( fact )-aH ) / ( i-in.nEquil+1.0 );
               utils::Print ( "runing averages ",cout,colours::red );
               utils::Print ( aHg,cout,colours::red );
               utils::Print ( aH,cout,colours::red );
               cout<<endl;
          }
     }
     utils::Print ( "runing averages ",cout,colours::red );
     utils::Print ( aHg,cout,colours::red );
     utils::Print ( aH,cout,colours::red );
     acc= ( double ) igood/ ( double ) in.mccycles;
     utils::Print ( "acceptance ",cout,colours::green );
     utils::Print ( acc,cout,colours::green );
     cout <<endl;

     for ( int k=0; k<cv.size(); k++ ) {
          cv[k].f=avgfz[k]/nsamp;
     }

}

void solution::histogramWrapper ( std::vector< double >& a, const double& bin, const string& filename )
{

     vector<double> aHist;
     double amin;
     utils::histogram ( a,bin,aHist,amin );
     utils::NormaliseHistogram ( aHist,bin );
     gutenberg::printHistogram ( aHist,amin,bin,filename );

}

// kate: indent-mode cstyle; indent-width 5; replace-tabs on;


//
// Created by Hamza El-Kebir on 6/18/21.
//

#ifndef LODESTAR_BUTCHERTABLEAU_HPP
#define LODESTAR_BUTCHERTABLEAU_HPP

#include <type_traits>
#include <tuple>
#include <array>
#include <string>
#include <string_view>
#include <functional>
#include "Lodestar/aux/Indices.hpp"
#include "Lodestar/aux/Conjunction.hpp"
#include <iostream>
#include <Eigen/Dense>

namespace ls {
    namespace primitives {
        namespace detail {
            /**
             * @brief A Butcher tableau row.
             *
             * @details There exist two partial specializations for this type: one for a regular row, which includes
             * a node and several Runge-Kutta coefficients (the exact number being depending on \c (TStage + 1)); the
             * other is a weight row, which only includes an array of \c TStages for the weights.
             *
             * @tparam TScalarType Type of the scalar coefficients.
             * @tparam TStages Number of stages in the integration scheme.
             * @tparam TStage Index of the current stage.
             * @tparam TIsWeights If true, the current row is a weight row.
             */
            template<typename TScalarType, size_t TStages, size_t TStage, bool TIsWeights>
            struct ButcherRowImpl {
            };

            /**
             * @brief Butcher tableau specialization for regular row.
             *
             * @sa ButcherRowImpl
             *
             * @tparam TScalarType Type of the scalar coefficients.
             * @tparam TStages Number of stages in the integration scheme.
             * @tparam TStage Index of the current stage.
             */
            template<typename TScalarType, size_t TStages, size_t TStage>
            struct ButcherRowImpl<TScalarType, TStages, TStage, false> {
                static_assert(TStage >= 0, "Butcher tableau row number must be non-negative.");
                static_assert(TStage < (TStages - 1),
                              "Butcher tableau row number must be smaller than the number of stages minus one.");

                TScalarType node;
                std::array<TScalarType, TStage + 1> rkCoefficients;
            };

            /**
             * @brief Butcher tableau specialization for weight row.
             *
             * @sa ButcherRowImpl
             *
             * @tparam TScalarType Type of the scalar coefficients.
             * @tparam TStages Number of stages in the integration scheme.
             * @tparam TStage Index of the current stage.
             */
            template<typename TScalarType, size_t TStages, size_t TStage>
            struct ButcherRowImpl<TScalarType, TStages, TStage, true> {
                std::array<TScalarType, TStages> weights;
            };
        }

        /**
         * @brief Implements a compile-time structure for simple/extended Butcher tableaus.
         *
         * @details Explicit methods in the Runge-Kutta family of numerical integrators can be described by the
         * following coefficient:
         * <ul>
         *      <li> Runge-Kutta coefficients \c a_ij;
         *      <li> Weights \c b_i;
         *      <li> Nodes \c c_i.
         * </ul>
         *
         *
         * @tparam TStages Number of stages in the integration scheme.
         * @tparam TExtended If true, an extended Butcher tableau is given.
         * @tparam TScalarType Type of the scalar coefficients.
         */
        template<size_t TStages, bool TExtended = true, typename TScalarType = double>
        class ButcherTableau {
            static_assert(TStages > 1, "Butcher tableau must have more than one stage.");
        };

        template<size_t TStages, typename TScalarType>
        class ButcherTableau<TStages, false, TScalarType> {
        public:
            static_assert(TStages > 1, "Butcher tableau must have more than one stage.");

            static const size_t stages = TStages; //! Number of stages.
            using type = TScalarType; //! Coefficient type.

            /**
             * @brief Alias template for \c ButcherRow.
             *
             * @tparam TStage Index of the current stage.
             * @tparam TIsWeights If true, the current row is a weight row.
             */
            template<size_t TStage, bool TIsWeights = (TStage >= (TStages - 1))>
            using ButcherRow = detail::ButcherRowImpl<TScalarType, TStages, TStage, TIsWeights>;

            /**
             * @brief Returns the Runge-Kutta coefficient for the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), TScalarType>::type
            inline getCoefficient()
            {
                return std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients);
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient getter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), TScalarType>::type
            inline getCoefficient()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");

                return TScalarType{};
            }

            /**
             * @brief Sets the Runge-Kutta coefficient at the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients) = coeff;
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient setter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");
            }

            /**
             * @brief Returns the node value for the given row number.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), TScalarType>::type
            inline getNode()
            {
                return std::get<TRow>(rows_).node;
            }

            /**
             * @brief Degenerate node getter.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), TScalarType>::type
            inline getNode()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");

                return TScalarType{};
            }

            /**
             * @brief Sets the node value for the given row number.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                std::get<TRow>(rows_).node = node;
            }

            /**
             * @brief Degenerate node setter.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
            }

            /**
             * @brief Returns the weight value at the given index.
             *
             * @tparam TIdx Weight index.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx < TStages, TScalarType>::type
            inline getWeight()
            {
                return std::get<TIdx>(std::get<TStages - 1>(rows_).weights);
            }

            /**
             * @brief Degenerate weight getter.
             *
             * @tparam TIdx Weight index.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx >= TStages, TScalarType>::type
            inline getWeight()
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");

                return TScalarType{};
            }

            /**
             * @brief Sets the weight value for the given index.
             *
             * @tparam TIdx Weight index.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx < TStages, void>::type
            inline setWeight(TScalarType weight)
            {
                std::get<TIdx>(std::get<TStages - 1>(rows_).weights) = weight;
            }

            /**
             * @brief Degenerate weight setter.
             *
             * @tparam TIdx Weight index.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx >= TStages, void>::type
            inline setWeight(TScalarType weight)
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function is the entry point to run a single iteration of the Runge-Kutta scheme
             * encoded in the Butcher tableau.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0>
            typename std::enable_if<TStage == 0, TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h)
            {
//                TType kCurr;
//                std::cout << "f(t,y) in Butcher tableau: " << f(t, y) << std::endl;
//                kCurr = f(t, y);
                auto kCurr = f(t,y);

                return execute<TType, TStage + 1>(f, y, t, h, kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the regular stages of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage < TStages) &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType kCurr{}, yCurr = y;

                yCurr += h * sumCoefficients<TType, TStage>(vars...);
                TScalarType tCurr = t + getNode<TStage - 1>() * h;

                kCurr = f(tCurr, yCurr);

                return execute<TType, TStage + 1>(f, y, t, h, vars..., kCurr);
//                return execute<TType, TStage + 1>(f, y, t, h, vars..., f(t + getNode<TStage - 1>() * h, y + h * sumCoefficients<TType, TStage>(vars...)));
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the final stage of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage == TStages) &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType yFinal = y;
                yFinal += h * sumWeights<TType>(vars...);

                return yFinal;
            }

        protected:
            /**
             * Helper struct that will define a tuple of specialized Butcher rows.
             *
             * @sa Repeat
             * @sa Indices
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices An \c IndexSequence type.
             */
            template<int TTimes, typename TIndices = typename Indices<TTimes>::type>
            struct Rows;

            /**
             * @brief Partial specialization of the \c Rows struct, which defines a tuple of specialized Butcher rows.
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices Template pack of integers (from 0 to TTimes - 1 by default).
             */
            template<int TTimes, int... TIndices>
            struct Rows<TTimes, IndexSequence<TIndices...>> {
                using type = std::tuple<ButcherRow<TIndices>...>;
            };

            typename Rows<TStages>::type rows_; //! Rows of the Butcher tableau.

            /**
             * @brief Sums Runge-Kutta coefficients multiplied with results from previous stages.
             *
             * @details This function computes sum_(j=0)^i a_ij * k_i, where i is \c TStage.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<
                    (TStage > 0) && (TStage < TStages) &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumCoefficients(TArgs... vars)
            {
                return sumCoefficientsImpl<TType, TStage - 1>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumCoefficients.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, int... TIndices>
            TType
            inline sumCoefficientsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getCoefficient<TStage, (size_t) TIndices>() * vars)...);
            }

            /**
             * @brief Sums weight coefficients multiplied with results from previous stages.
             *
             * @tparam TType State type.
             * @tparam TArgs Parameter pack of argument (previous stage result) types
             * @tparam TIndex TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<(Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumWeights(TArgs... vars)
            {
                return sumWeightsImpl<TType>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumWeights.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, typename... TArgs, int... TIndices>
            TType
            inline sumWeightsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getWeight<(size_t) TIndices>() * vars)...);
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @details This particular function is the base case, which simply returns the value that it was given.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             *
             * @return Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                return var;
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @details This function checks convertibility of the argument type to \c TType.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<!std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                static_assert(std::is_convertible<TArg, TType>::value, "Summed values must be convertible.");
                return var;
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             *
             * @return Sum.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<std::is_convertible<TArg, TType>::value &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                return var + sum<TType>(vars...);
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<!(std::is_convertible<TArg, TType>::value &&
                                      (Conjunction<std::is_convertible<TArgs, TType>...>::value)), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                static_assert(std::is_convertible<TArg, TType>::value &&
                              (Conjunction<std::is_convertible<TArgs, TType>...>::value),
                              "Summed values must be convertible.");

                return var + sum<TType>(vars...);
            }
        };

// --------------- Extended Butcher tableau -----------------------

        /**
         * @brief Implements a compile-time structure for extended Butcher tableaus.
         *
         * @details Explicit methods in the Runge-Kutta family of numerical integrators can be described by the
         * following coefficient:
         * <ul>
         *      <li> Runge-Kutta coefficients \c a_ij;
         *      <li> Weights \c b_i;
         *      <li> Nodes \c c_i.
         * </ul>
         *
         *
         * @tparam TStages Number of stages in the integration scheme.
         * @tparam TScalarType Type of the scalar coefficients.
         */
        template<size_t TStages, typename TScalarType>
        class ButcherTableau<TStages, true, TScalarType> {
        public:
            static_assert(TStages > 1, "Butcher tableau must have more than one stage.");

            static const size_t stages = TStages; //! Number of stages.
            using type = TScalarType; //! Coefficient type.

            /**
             * @brief Alias template for \c ButcherRow.
             *
             * @tparam TStage Index of the current stage.
             * @tparam TIsWeights If true, the current row is a weight row.
             */
            template<size_t TStage, bool TIsWeights = (TStage >= (TStages - 1))>
            using ButcherRow = detail::ButcherRowImpl<TScalarType, TStages, TStage, TIsWeights>;

            /**
             * @brief Returns the Runge-Kutta coefficient for the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), TScalarType>::type
            inline getCoefficient()
            {
                return std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients);
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient getter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), TScalarType>::type
            inline getCoefficient()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");

                return TScalarType{};
            }

            /**
             * @brief Sets the Runge-Kutta coefficient at the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients) = coeff;
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient setter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");
            }

            /**
             * @brief Returns the node value for the given row number.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), TScalarType>::type
            inline getNode()
            {
                return std::get<TRow>(rows_).node;
            }

            /**
             * @brief Degenerate node getter.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), TScalarType>::type
            inline getNode()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");

                return TScalarType{};
            }

            /**
             * @brief Sets the node value for the given row number.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                std::get<TRow>(rows_).node = node;
            }

            /**
             * @brief Degenerate node setter.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
            }

            /**
             * @brief Returns the weight value at the given index.
             *
             * @details This function gets the higher order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are retrieved.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && THigherOrder, TScalarType>::type
            inline getWeight()
            {
                return std::get<TIdx>(std::get<TStages - 1>(rows_).weights);
            }

            /**
             * @brief Returns the weight value at the given index.
             *
             * @details This function gets the lower order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are retrieved.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && !THigherOrder, TScalarType>::type
            inline getWeight()
            {
                return std::get<TIdx>(std::get<TStages>(rows_).weights);
            }

            /**
             * @brief Degenerate weight getter.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are retrieved.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<TIdx >= TStages, TScalarType>::type
            inline getWeight()
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");

                return TScalarType{};
            }

            /**
             * @brief Sets the weight value for the given index.
             *
             * @details This function sets the higher order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are changed.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && THigherOrder, void>::type
            inline setWeight(TScalarType weight)
            {
                std::get<TIdx>(std::get<TStages - 1>(rows_).weights) = weight;
            }

            /**
             * @brief Sets the weight value for the given index.
             *
             * @details This function sets the lower order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are changed.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && !THigherOrder, void>::type
            inline setWeight(TScalarType weight)
            {
                std::get<TIdx>(std::get<TStages>(rows_).weights) = weight;
            }

            /**
             * @brief Degenerate weight setter.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are changed.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<TIdx >= TStages, void>::type
            inline setWeight(TScalarType weight)
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function is the entry point to run a single iteration of the Runge-Kutta scheme
             * encoded in the Butcher tableau.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers and embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false>
            typename std::enable_if<(TStage == 0) && !TIsEmbedded, TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h)
            {
                TType kCurr = f(t, y);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function is the entry point to run a single iteration of the Runge-Kutta scheme
             * encoded in the Butcher tableau.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Pair of integrated state (with higher-order scheme) and error.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false>
            typename std::enable_if<(TStage == 0) && TIsEmbedded, std::pair<TType, TType>>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h)
            {
                TType kCurr = f(t, y);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the regular stages of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage < TStages) && !TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h,
                    TArgs... vars)
            {
                TType kCurr{}, yCurr = y;

                yCurr += h * sumCoefficients<TType, TStage>(vars...);
                TScalarType tCurr = t + getNode<TStage - 1>() * h;

                kCurr = f(tCurr, yCurr);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, vars..., kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the regular stages of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Pair of integrated state (with higher-order scheme) and error.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage < TStages) && TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), std::pair<TType, TType>>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType kCurr{}, yCurr = y;

                yCurr += h * sumCoefficients<TType, TStage>(vars...);
                TScalarType tCurr = t + getNode<TStage - 1>() * h;

                kCurr = f(tCurr, yCurr);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, vars..., kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the final stage of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage == TStages) && !TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType yFinal = y;
                yFinal += h * sumWeights<TType>(vars...);

                return yFinal;
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the final stage of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Pair of integrated state (with higher-order scheme) and error.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage == TStages) && TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), std::pair<TType, TType>>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType yFinal = y;
                TType summedWeightsHigher = sumWeights<TType, true>(vars...);
                TType summedWeightsLower = sumWeights<TType, false>(vars...);
                yFinal += h * summedWeightsHigher;

                std::pair<TType, TType> pair{yFinal, h * (summedWeightsHigher - summedWeightsLower)};

                return pair;
            }

        protected:
            /**
             * Helper struct that will define a tuple of specialized Butcher rows.
             *
             * @sa Repeat
             * @sa Indices
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices An \c IndexSequence type.
             */
            template<int TTimes, typename TIndices = typename Indices<TTimes>::type>
            struct Rows;

            /**
             * @brief Partial specialization of the \c Rows struct, which defines a tuple of specialized Butcher rows.
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices Template pack of integers (from 0 to TTimes - 1 by default).
             */
            template<int TTimes, int... TIndices>
            struct Rows<TTimes, IndexSequence<TIndices...>> {
                using type = std::tuple<ButcherRow<TIndices>...>;
            };

            typename Rows<TStages + 1>::type rows_; //! Rows of the Butcher tableau.

            /**
             * @brief Sums Runge-Kutta coefficients multiplied with results from previous stages.
             *
             * @details This function computes sum_(j=0)^i a_ij * k_i, where i is \c TStage.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<
                    (TStage > 0) && (TStage < TStages) &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumCoefficients(TArgs... vars)
            {
                return sumCoefficientsImpl<TType, TStage - 1>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumCoefficients.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, int... TIndices>
            TType
            inline sumCoefficientsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getCoefficient<TStage, (size_t) TIndices>() * vars)...);
            }

            /**
             * @brief Sums weight coefficients multiplied with results from previous stages.
             *
             * @tparam TType State type.
             * @tparam THigherOrder If true, higher order weights are used.
             * @tparam TArgs Parameter pack of argument (previous stage result) types
             * @tparam TIndex TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, bool THigherOrder = true, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<(Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumWeights(TArgs... vars)
            {
                return sumWeightsImpl<TType, THigherOrder>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumWeights.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam THigherOrder If true, higher order weights are used.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, bool THigherOrder, typename... TArgs, int... TIndices>
            typename std::enable_if<THigherOrder, TType>::type
            inline sumWeightsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getWeight<(size_t) TIndices, true>() * vars)...);
            }

            /**
             * @brief Implementation of \c sumWeights.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam THigherOrder If true, higher order weights are used.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, bool THigherOrder, typename... TArgs, int... TIndices>
            typename std::enable_if<!THigherOrder, TType>::type
            inline sumWeightsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getWeight<(size_t) TIndices, false>() * vars)...);
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @details This particular function is the base case, which simply returns the value that it was given.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             *
             * @return Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                return var;
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @details This function checks convertibility of the argument type to \c TType.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<!std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                static_assert(std::is_convertible<TArg, TType>::value, "Summed values must be convertible.");
                return var;
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             *
             * @return Sum.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<std::is_convertible<TArg, TType>::value &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                return var + sum<TType>(vars...);
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<!(std::is_convertible<TArg, TType>::value &&
                                      (Conjunction<std::is_convertible<TArgs, TType>...>::value)), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                static_assert(std::is_convertible<TArg, TType>::value &&
                              (Conjunction<std::is_convertible<TArgs, TType>...>::value),
                              "Summed values must be convertible.");

                return var + sum<TType>(vars...);
            }
        };
    }
}

#endif //LODESTAR_BUTCHERTABLEAU_HPP


#include <functional>
#include <iostream>
#include <map>
#include <utility>

#include <boost/program_options.hpp>

#include "LinearProgram.h"
#include "Pivot.h"

namespace po = boost::program_options;

std::map<std::string, std::function<PivotFunctionReturnType(Tableau &tableau)>> pivotFunctions;
std::string allowedPivotFunctions;

/*
Take the pivot function callables from Pivot.h (or elsewhere), and register them with the command-line options parser
*/
void registerPivotFunctions()
{
	pivotFunctions = decltype(pivotFunctions){
		{"bland", Bland()},
		{"random", Random()},
		{"maxincrease", MaxIncrease()},
		{"maxcoef", MaxCoef()}};
	allowedPivotFunctions.clear();
	for (auto &it : pivotFunctions)
	{
		allowedPivotFunctions += it.first;
		allowedPivotFunctions += ',';
	}
	allowedPivotFunctions.pop_back();
}

/*
Parse the command-line options, and return them as a tuple.
*/
auto parseOptions(int argc, char **argv)
{
	po::options_description optionsDescription("Allowed options");
	std::string pivotHelpText = "the pivot rule that is used. Can be one of {" + allowedPivotFunctions + "}";
	optionsDescription.add_options()("verbose", po::value<bool>()->implicit_value(true)->default_value(false), "verbose output")("pivot", po::value<std::string>()->default_value("maxincrease"), pivotHelpText.c_str())("input", po::value<std::string>(), "input linear program");
	po::positional_options_description positionalOptionsDescription;
	positionalOptionsDescription.add("input", -1);

	po::variables_map variablesMap;
	po::store(
		po::command_line_parser(argc, argv)
			.options(optionsDescription)
			.positional(positionalOptionsDescription)
			.run(),
		variablesMap);
	po::notify(variablesMap);

	std::string inputPath;
	if (variablesMap.count("input"))
	{
		inputPath = variablesMap["input"].as<std::string>();
	}
	else
	{
		std::cout << "You must provide a valid path\n"
				  << optionsDescription;
		std::exit(1);
	}

	auto verboseOutput = variablesMap["verbose"].as<bool>();
	auto pivotAlgorithm = variablesMap["pivot"].as<std::string>();

	if (pivotFunctions.find(pivotAlgorithm) == pivotFunctions.end())
	{
		std::cout << "Invalid pivot algorithm \"" << pivotAlgorithm << "\"\n";
		std::cout << "Allowed values are {" << allowedPivotFunctions << "}\n";
		std::exit(1);
	}

	return std::make_tuple(inputPath, verboseOutput, pivotAlgorithm);
}

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

	std::string inputPath, pivotAlgorithm;
	bool verboseOutput;

	std::tie(inputPath, verboseOutput, pivotAlgorithm) = parseOptions(argc, argv);

	LinearProgram lp(inputPath, verboseOutput);
	lp.printFancyStatement();

	auto result = lp.solve(pivotFunctions[pivotAlgorithm]);

	switch (result)
	{
	case LinearProgram::Result::INFEASIBLE:
		std::cout << "The linear program is infeasible\n";
		break;
	case LinearProgram::Result::FEASIBLE_UNBOUNDED:
		std::cout << "The linear program is unbounded\n";
		break;
	case LinearProgram::Result::FEASIBLE_BOUNDED:
		std::cout << "An optimal solution is: ";
		lp.printFancySolution();
		std::cout << "\nThe value of the objective function is: " << lp.tableau.value();
		std::cout << "\nThe number of pivots is: " << lp.numPivots;
		std::cout << "\nThe pivot rule used: " << pivotAlgorithm;
	}
	return 0;
}