#include <iostream>
#include <Eigen/Dense>
#include <Eigen/MatrixFunctions>
#include "DIIS.hh"

void DIIS::step(Eigen::MatrixXd& F, const Eigen::MatrixXd& D,
	const Eigen::MatrixXd& S, const Eigen::MatrixXd& X, double Etot)
{
	if (_err_vecs_used >= _err_vecs.cols())
	{
		if (_err_vecs_used == 0)
		{
			_size = F.rows();
			_err_vecs.resize((_size-1)*_size/2, 10);
		}
		else
		{
			Eigen::MatrixXd new_err_vecs(_err_vecs.rows(), 2*_err_vecs.cols());
			new_err_vecs.leftCols(_err_vecs_used) = _err_vecs;
			_err_vecs.swap(new_err_vecs);
		}
	}

        // The error matrix is X(FDS - SDF)X = XFDX^-1 - h.c, which since S, P,
        // F and X are all symmetric, is antisymmetric. Therefore we only store
        // the upper triangle in vector err.
	Eigen::MatrixXd FDS = X*F*D*S*X;
	int idx = 0;
	for (int i = 1; i < _size; ++i)
		for (int j = 0; j < i; ++j, ++idx)
			_err_vecs(idx, _err_vecs_used) = FDS(i,j) - FDS(j,i);

	_max_err = _err_vecs.col(_err_vecs_used).lpNorm<Eigen::Infinity>();
	if (_max_err > std::abs(0.5*Etot))
		// This Fock matrix will not have a meaningful contribution
		// to convergence. Ignore it.
		return;

	++_err_vecs_used;
	_values.push_back(F);
	if (!_started)
	{
		if (_max_err >= std::abs(0.1*Etot))
			return;
		std::cout << "Starting DIIS\n";
		_started = true;
	}

	auto e = _err_vecs.leftCols(_err_vecs_used);
	Eigen::MatrixXd B(_err_vecs_used+1, _err_vecs_used+1);
	B.topLeftCorner(_err_vecs_used, _err_vecs_used) = e.transpose() * e * 2;
	B.rightCols(1).fill(-1);
	B.bottomRows(1).fill(-1);

	Eigen::VectorXd y = Eigen::VectorXd::Zero(_err_vecs_used+1);
	y[_err_vecs_used] = -1;

	Eigen::VectorXd coefs = B.colPivHouseholderQr().solve(y);
	F *= coefs[_err_vecs_used-1];
	for (int i = 0; i < _err_vecs_used-1; ++i)
		F += coefs[i] * _values[i];
}

void DIIS::step(Eigen::MatrixXd& Fa, const Eigen::MatrixXd& Da,
	Eigen::MatrixXd& Fb, const Eigen::MatrixXd& Db,
	const Eigen::MatrixXd& S, const Eigen::MatrixXd& X, double Etot)
{
	if (_err_vecs_used >= _err_vecs.cols())
	{
		if (_err_vecs_used == 0)
		{
			_size = Fa.rows();
			_err_vecs.resize((_size-1)*_size, 10);
		}
		else
		{
			Eigen::MatrixXd new_err_vecs(_err_vecs.rows(), 2*_err_vecs.cols());
			new_err_vecs.leftCols(_err_vecs_used) = _err_vecs;
			_err_vecs.swap(new_err_vecs);
		}
	}

	Eigen::MatrixXd FDS = X*Fa*Da*S*X;
	int idx = 0;
	for (int i = 1; i < _size; ++i)
		for (int j = 0; j < i; ++j, ++idx)
			_err_vecs(idx, _err_vecs_used) = FDS(i,j) - FDS(j,i);
	FDS = X*Fb*Db*S*X;
	for (int i = 1; i < _size; ++i)
		for (int j = 0; j < i; ++j, ++idx)
			_err_vecs(idx, _err_vecs_used) = FDS(i,j) - FDS(j,i);

	_max_err = _err_vecs.col(_err_vecs_used).lpNorm<Eigen::Infinity>();
	if (_max_err > std::abs(0.5*Etot))
		// This Fock matrix will not have a meaningful contribution
		// to convergence. Ignore it.
		return;

	++_err_vecs_used;
	Eigen::MatrixXd F(_size, 2*_size);
	F.leftCols(_size) = Fa;
	F.rightCols(_size) = Fb;
	_values.push_back(F);
	if (!_started)
	{
		if (_max_err >= std::abs(0.1*Etot))
			return;
		std::cout << "Starting DIIS\n";
		_started = true;
	}

	auto e = _err_vecs.leftCols(_err_vecs_used);
	Eigen::MatrixXd B(_err_vecs_used+1, _err_vecs_used+1);
	B.topLeftCorner(_err_vecs_used, _err_vecs_used) = e.transpose() * e * 2;
	B.rightCols(1).fill(-1);
	B.bottomRows(1).fill(-1);

	Eigen::VectorXd y = Eigen::VectorXd::Zero(_err_vecs_used+1);
	y[_err_vecs_used] = -1;

	Eigen::VectorXd coefs = B.colPivHouseholderQr().solve(y);
	F *= coefs[_err_vecs_used-1];
	for (int i = 0; i < _err_vecs_used-1; ++i)
		F += coefs[i] * _values[i];

	Fa = F.leftCols(_size);
	Fb = F.rightCols(_size);
}


/*
 *            Copyright 2009-2019 The VOTCA-MPIP Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * author: Denis Andrienko
 */

#include <votca/ctp/overlap.h>
#include <votca/ctp/linalg.h>

#include <votca/tools/constants.h>
#include <boost/format.hpp>
#include <boost/progress.hpp>

namespace votca { namespace ctp {

namespace ub = boost::numeric::ublas;

double inv_sqrt(double x) { return 1./sqrt(x); }
using boost::format;

/*
 * Calculates S^{-1/2}
 */
void Overlap::SQRTOverlap(ub::symmetric_matrix<double> &S,
                          ub::matrix<double> &S2 ) {

    double (*_inv_sqrt)(double);
    _inv_sqrt = &inv_sqrt;

    ub::vector<double>                  _eigenvalues;
    ub::matrix<double>                  _eigenvectors;

    int _size = S.size1();

    _eigenvalues.resize( _size );
    _eigenvectors.resize( _size, _size );

    //convert from symmetric to matrix for faster evaluation
    ub::matrix<double> temp=S;

    // TEST CASE FOR EIGEN
       ub::matrix<double> m(3,3);
       /*
       m(0,0) = -2;
       m(0,1) = -4;
       m(0,2) =  2;
       m(1,0) = -2;
       m(1,1) =  1;
       m(1,2) =  2;
       m(2,0) =  4;
       m(2,1) =  2;
       m(2,2) =  5;
       */

       /* 4 -2 -2
       m(0,0) =  1;
       m(0,1) = -3;
       m(0,2) =  3;
       m(1,0) =  3;
       m(1,1) = -5;
       m(1,2) =  3;
       m(2,0) =  6;
       m(2,1) = -6;
       m(2,2) =  4;
       */

       /*symmetric, -1, -1, 8
       m(0,0) =    2;
       m(0,1) =    1;
       m(0,2) =    -2;
       m(1,0) =    1;
       m(1,1) =    0;
       m(1,2) =    1;
       m(2,0) =    -2;
       m(2,1) =    1;
       m(2,2) =    0;

    _eigenvalues.resize( 3 );
    _eigenvectors.resize( 3, 3 );

    linalg::eigenvalues_symm(m, _eigenvalues, _eigenvectors);

    std::cout << "\nEigenvalues" << std::endl << _eigenvalues;
    std::cout << "\nEigenvectors" << std::endl << _eigenvectors << std::endl;
    exit(0);
    */

    linalg::eigenvalues_symm(temp, _eigenvalues, _eigenvectors);

    // compute inverse sqrt of all eigenvalues
    std::transform(_eigenvalues.begin(), _eigenvalues.end(), _eigenvalues.begin(),  _inv_sqrt );

    // form a diagonal matrix S^{-1/2}
    ub::diagonal_matrix<double> _diagS2( _eigenvalues.size(), _eigenvalues.data() );

    // multiply from the left on the U
    ub::matrix<double> _temp = ub::prod( _eigenvectors, _diagS2 );

    // multiply from the right on the transpose U
    S2 = ub::prod( _temp, ub::trans( _eigenvectors ) );
    return;
 }

double Overlap::getCouplingElement( int levelA, int levelB,  Orbitals* _orbitalsA,
    Orbitals* _orbitalsB, ub::matrix<double>* _JAB, double  _energy_difference ) {

    int _levelsA = _orbitalsA->getNumberOfLevels();

    if ( _energy_difference != 0 ) {
        std::vector<int> list_levelsA = *_orbitalsA->getDegeneracy( levelA, _energy_difference );
        std::vector<int> list_levelsB = *_orbitalsA->getDegeneracy( levelB, _energy_difference );

        double _JAB_sq = 0; double _JAB_one_level;

        for (std::vector<int>::iterator iA = list_levelsA.begin()++; iA != list_levelsA.end(); iA++) {
                for (std::vector<int>::iterator iB = list_levelsB.begin()++; iB != list_levelsB.end(); iB++) {
                    //cout << *iA << ':' << *iB << endl;
                    _JAB_one_level = _JAB->at_element( *iA - 1  , *iB -1 + _levelsA );
                    _JAB_sq +=  _JAB_one_level*_JAB_one_level ;
                }
        }

        return sqrt(_JAB_sq / ( list_levelsA.size() * list_levelsB.size() ) ) * tools::conv::hrt2ev ;

    } else {

        return _JAB->at_element( levelA - 1  , levelB -1 + _levelsA ) * tools::conv::hrt2ev;

    }
    // the  matrix should be symmetric, could also return this element
    // _JAB.at_element( _levelsA + levelB - 1  , levelA - 1 );
}

/**
 * \brief evaluates electronic couplings
 *
 * This is a slow version with a rather large block matrix AxB
 *
 * @param _orbitalsA molecular orbitals of molecule A
 * @param _orbitalsB molecular orbitals of molecule B
 * @param _orbitalsAB molecular orbitals of the dimer AB
 * @param _JAB matrix with electronic couplings
 * @return false if failed
 */
bool Overlap::CalculateIntegrals(Orbitals* _orbitalsA, Orbitals* _orbitalsB,
    Orbitals* _orbitalsAB, ub::matrix<double>* _JAB) {

    CTP_LOG(logDEBUG,*_pLog) << "Calculating electronic couplings" << std::flush;

    const std::vector<QMAtom*> atomsA=_orbitalsA->QMAtoms();
    const std::vector<QMAtom*> atomsB=_orbitalsB->QMAtoms();
    const std::vector<QMAtom*> atomsAB=_orbitalsAB->QMAtoms();

  for (unsigned i=0;i<atomsAB.size();i++){
        QMAtom* dimer=atomsAB[i];
        QMAtom* monomer=NULL;
        if (i<atomsA.size()){
            monomer=atomsA[i];
        }
        else if (i<atomsB.size()+atomsA.size() ){
            monomer=atomsB[i-atomsA.size()];
        }
        else{
	  throw std::runtime_error((format("Number of Atoms in dimer %3i and the two monomers A:%3i B:%3i does not agree") %atomsAB.size() %atomsA.size() %atomsB.size()).str());
        }

        if(monomer->type != dimer->type){
	  throw std::runtime_error("\nERROR: Atom types do not agree in dimer and monomers\n");
        }
        if(std::abs(monomer->x-dimer->x)>0.001 || std::abs(monomer->y-dimer->y)>0.001 || std::abs(monomer->z-dimer->z)>0.001){
            CTP_LOG(logERROR,*_pLog) << "======WARNING=======\n Coordinates of monomers and dimer atoms do not agree, do you know what you are doing?\n " << std::flush;
            break;
        }

    }

    // constructing the direct product orbA x orbB
    int _basisA = _orbitalsA->getBasisSetSize();
    int _basisB = _orbitalsB->getBasisSetSize();

    if ( ( _basisA == 0 ) || ( _basisB == 0 ) ) {
        CTP_LOG(logERROR,*_pLog) << "Basis set size is not stored in monomers" << std::flush;
        return false;
    }

    int _levelsA = _orbitalsA->getNumberOfLevels();
    int _levelsB = _orbitalsB->getNumberOfLevels();

    //boost::timer t; // start timing
    //double _st = t.elapsed();

    CTP_LOG(logDEBUG,*_pLog) << "Levels:Basis A[" << _levelsA << ":" << _basisA << "]"
                                     << " B[" << _levelsB << ":" << _basisB << "]" << std::flush;

    if ( ( _levelsA == 0 ) || (_levelsB == 0) ) {
        CTP_LOG(logERROR,*_pLog) << "No information about number of occupied/unoccupied levels is stored" << std::flush;
        return false;
    }

    //       | Orbitals_A          0 |      | Overlap_A |
    //       | 0          Orbitals_B |  X   | Overlap_B |  X  Transpose( Orbitals_AB )
    ub::zero_matrix<double> zeroB( _levelsA, _basisB ) ;
    ub::zero_matrix<double> zeroA( _levelsB, _basisA ) ;
    ub::matrix<double> _psi_AxB ( _levelsA + _levelsB, _basisA + _basisB  );

    CTP_LOG(logDEBUG,*_pLog) << "BOOST: Constructing direct product AxB ["
            << _psi_AxB.size1() << "x"
            << _psi_AxB.size2() << "]" << std::flush;

/*
    // TEST CASE FOR EIGEN
       ub::matrix<double> m1_(2,3);
       m1_(0,0) = 1;
       m1_(0,1) = 2;
       m1_(0,2) = 3;
       m1_(1,0) = 4;
       m1_(1,1) = 5;
       m1_(1,2) = 6;
       //std::cout << std::endl << m1_ << std::endl ;

       ub::matrix<double> m2_(3,2);
       m2_(0,0) = 7;
       m2_(0,1) = 8;
       m2_(1,0) = 9;
       m2_(1,1) = 10;
       m2_(2,0) = 11;
       m2_(2,1) = 12;
       //std::cout << std::endl << m2_ << std::endl ;

       ub::prod(m1_, m2_);
 */

    ub::project( _psi_AxB, ub::range (0, _levelsA ), ub::range ( _basisA, _basisA +_basisB ) ) = zeroB;
    ub::project( _psi_AxB, ub::range (_levelsA, _levelsA + _levelsB ), ub::range ( 0, _basisA ) ) = zeroA;
    ub::project( _psi_AxB, ub::range (0, _levelsA ), ub::range ( 0, _basisA ) ) = *_orbitalsA->getOrbitals();
    ub::project( _psi_AxB, ub::range (_levelsA, _levelsA + _levelsB ), ub::range ( _basisA, _basisA + _basisB ) ) = *_orbitalsB->getOrbitals();

    // psi_AxB * S_AB * psi_AB
    CTP_LOG(logDEBUG,*_pLog) << "Projecting dimer onto monomer orbitals" << std::flush;
    ub::matrix<double> _orbitalsAB_Transposed = ub::trans( *_orbitalsAB->getOrbitals() );
    if ( (*_orbitalsAB->getOverlap()).size1() == 0 ) {
            CTP_LOG(logERROR,*_pLog) << "Overlap matrix is not stored";
            return false;
    }

    ub::matrix<double> _psi_AB = ub::prod( *_orbitalsAB->getOverlap(), _orbitalsAB_Transposed );
    ub::matrix<double> _psi_AxB_dimer_basis = ub::prod( _psi_AxB, _psi_AB );
    _psi_AB.clear();

    //check to see if projection quality is sufficient
    for (unsigned i=0;i<_psi_AxB_dimer_basis.size1();i++){
        double mag=0.0;
        for (unsigned j=0;j<_psi_AxB_dimer_basis.size2();j++){
            mag+=_psi_AxB_dimer_basis(i,j)*_psi_AxB_dimer_basis(i,j);

    }
        if (mag<0.95){
	  throw std::runtime_error("\nERROR: Projection of monomer orbitals on dimer is insufficient, increase dimer basis.\n");
        }
    }

    // J = psi_AxB_dimer_basis * FAB * psi_AxB_dimer_basis^T
    CTP_LOG(logDEBUG,*_pLog) << "Projecting the Fock matrix onto the dimer basis" << std::flush;
    ub::diagonal_matrix<double> _fock_AB( _orbitalsAB->getNumberOfLevels(), (*_orbitalsAB->getEnergies()).data() );
    ub::matrix<double> _temp = ub::prod( _fock_AB, ub::trans( _psi_AxB_dimer_basis ) ) ;
    ub::matrix<double> JAB_dimer = ub::prod( _psi_AxB_dimer_basis, _temp);

    // S = psi_AxB_dimer_basis * psi_AxB_dimer_basis^T
    CTP_LOG(logDEBUG,*_pLog) << "Constructing Overlap matrix" << std::flush;
    ub::symmetric_matrix<double> _S_AxB = ub::prod( _psi_AxB_dimer_basis, ub::trans( _psi_AxB_dimer_basis ));
    ub::matrix<double> _S_AxB_2(_S_AxB.size1(), _S_AxB.size1() );
    ub::trans( _S_AxB );

    // Square root of the overlap matrix
    CTP_LOG(logDEBUG,*_pLog) << "Calculating square root of the overlap matrix" << std::flush;
    SQRTOverlap( _S_AxB , _S_AxB_2 );

   CTP_LOG(logDEBUG,*_pLog) << "Calculating the effective overlap JAB ["
              << JAB_dimer.size1() << "x"
              << JAB_dimer.size2() << "]" << std::flush;

    ub::matrix<double> JAB_temp( _levelsA + _levelsB, _levelsA + _levelsB );
    ub::noalias(JAB_temp) = ub::prod( JAB_dimer, _S_AxB_2 );
    (*_JAB) = ub::prod( _S_AxB_2, JAB_temp );

    CTP_LOG(logDEBUG,*_pLog) << "Done with electronic couplings" << std::flush;
    return true;

}

/**
 * \brief evaluates electronic couplings
 *
 * This is a different version with block matrices [slower]
 *
 * @param _orbitalsA molecular orbitals of molecule A
 * @param _orbitalsB molecular orbitals of molecule B
 * @param _orbitalsAB molecular orbitals of the dimer AB
 * @param _JAB matrix with electronic couplings
 * @return false if failed
 */
bool Overlap::CalculateIntegralsOptimized(Orbitals* _orbitalsA, Orbitals* _orbitalsB,
    Orbitals* _orbitalsAB, ub::matrix<double>* _JAB) {

    CTP_LOG(logDEBUG,*_pLog) << "Calculating electronic couplings" << std::flush;

    // constructing the direct product orbA x orbB
    int _basisA = _orbitalsA->getBasisSetSize();
    int _basisB = _orbitalsB->getBasisSetSize();

    if ( ( _basisA == 0 ) || ( _basisB == 0 ) ) {
        CTP_LOG(logERROR,*_pLog) << "Basis set size is not stored in monomers" << std::flush;
        return false;
    }

    int _levelsA = _orbitalsA->getNumberOfLevels();
    int _levelsB = _orbitalsB->getNumberOfLevels();

    boost::timer t; // start timing
    double _st = t.elapsed();

    CTP_LOG(logDEBUG,*_pLog) << "Levels:Basis A[" << _levelsA << ":" << _basisA << "]"
                                     << " B[" << _levelsB << ":" << _basisB << "]" << std::flush;

    if ( ( _levelsA == 0 ) || (_levelsB == 0) ) {
        CTP_LOG(logERROR,*_pLog) << "No information about number of occupied/unoccupied levels is stored" << std::flush;
        return false;
    }

    // these flags should be set before any ublas header is called
    // #define NDEBUG
    // otherwise the code is very inefficient

    //       | Orbitals_A          0 |      | Overlap_A |
    //       | 0          Orbitals_B |  X   | Overlap_B |  X  Transpose( Orbitals_AB )
    //constructing a slice of the Overlap matrix
    ub::matrix_range< ub::symmetric_matrix<double> > Overlap_A = ub::project( *_orbitalsAB->getOverlap(), ub::range ( 0, _basisA), ub::range (0, _basisA +_basisB) );
    ub::matrix_range< ub::symmetric_matrix<double> > Overlap_B = ub::project( *_orbitalsAB->getOverlap(), ub::range ( _basisA, _basisA +_basisB ), ub::range (0, _basisA +_basisB) );

    CTP_LOG(logDEBUG,*_pLog) << "Projecting the monomer onto dimer orbitals [" << _levelsA + _levelsB << "x" << _basisA + _basisB << "]";
    ub::matrix<double> _psi_AB ( _levelsA + _levelsB, _basisA + _basisB  );

    ub::matrix_range< ub::matrix<double> > _psi_AB_A = ub::project( _psi_AB, ub::range (0, _levelsA ), ub::range ( 0, _basisA +_basisB ) ) ;
    ub::noalias(_psi_AB_A) = ub::prod(*_orbitalsA->getOrbitals(), Overlap_A);

    ub::matrix_range< ub::matrix<double> > _psi_AB_B = ub::project( _psi_AB, ub::range (_levelsA, _levelsA + _levelsB ), ub::range ( 0, _basisA +_basisB ) ) ;
    ub::noalias(_psi_AB_B) = ub::prod(*_orbitalsB->getOrbitals(), Overlap_B );
    CTP_LOG(logDEBUG,*_pLog)  << " (" << t.elapsed() - _st << "s) " << std::flush; _st = t.elapsed();

    ub::matrix<double> _psi_AxB_dimer_basis (_levelsA + _levelsB, _basisA + _basisB );
    ub::matrix<double> OrbAB_Transp = ub::trans( *_orbitalsAB->getOrbitals() );
    CTP_LOG(logDEBUG,*_pLog)  << "Transposing OrbitalsAB (" << t.elapsed() - _st << "s)" << "\x1b[0;39m" << std::flush; _st = t.elapsed();
    ub::noalias(_psi_AxB_dimer_basis) = ub::prod( _psi_AB,  OrbAB_Transp );

    CTP_LOG(logDEBUG,*_pLog)  << "Multiplying PsiAB x OrbitalsAB (" << t.elapsed() - _st << "s)" << "\x1b[0;39m" << std::flush; _st = t.elapsed();

    _psi_AB.resize(0,0,false); OrbAB_Transp.resize(0,0,false);

    //   _psi_AxB_dimer_basis * F  * _psi_AxB_dimer_basis^T
    CTP_LOG(logDEBUG,*_pLog) << "Projecting the Fock matrix onto the dimer basis";
    ub::zero_matrix<double> _zero ( _levelsA + _levelsB, _levelsA + _levelsB );
    ub::matrix<double> JAB_dimer( _zero ) ;
    ub::vector<double> energies = (*_orbitalsAB->getEnergies());

    for ( int i1 = 0; i1 < _levelsA + _levelsB ; i1++ ) {
    for ( int i2 = i1; i2 < _levelsA + _levelsB; i2++ ) {
        for ( int k = 0; k < _basisA + _basisB; k++  ) {
                JAB_dimer(i1,i2) += _psi_AxB_dimer_basis.at_element(i1, k) * _psi_AxB_dimer_basis.at_element(i2, k) * energies(k);
        }
        JAB_dimer(i2,i1) = JAB_dimer(i1,i2);
    }}
    energies.clear();
    CTP_LOG(logDEBUG,*_pLog)  << " (" << t.elapsed() - _st << "s)" << "\x1b[0;39m" << std::flush; _st = t.elapsed();

    // S = psi_AxB_dimer_basis * psi_AxB_dimer_basis^T
    ub::symmetric_matrix<double> _S_AxB = ub::prod( _psi_AxB_dimer_basis, ub::trans( _psi_AxB_dimer_basis ));
    _psi_AxB_dimer_basis.resize(0,0,false);

    ub::matrix<double> _S_AxB_2(_S_AxB.size1(), _S_AxB.size1() );

     ub::trans( _S_AxB );
     CTP_LOG(logDEBUG,*_pLog) << "Calculating square root of the overlap matrix ["
             << _S_AxB.size1() << "x"
             << _S_AxB.size2() << "]";
     SQRTOverlap( _S_AxB , _S_AxB_2 );
     _S_AxB.resize(0,0,false);
     CTP_LOG(logDEBUG,*_pLog)  << " (" << t.elapsed() - _st << "s)" << "\x1b[0;39m" << std::flush; _st = t.elapsed();

     CTP_LOG(logDEBUG,*_pLog) << "Calculating the effective overlap JAB ["
             << JAB_dimer.size1() << "x"
             << JAB_dimer.size2() << "]";

    ub::matrix<double> JAB_temp( _levelsA + _levelsB, _levelsA + _levelsB );

    ub::noalias(JAB_temp) = ub::prod( JAB_dimer, _S_AxB_2 );
    (*_JAB) = ub::prod( _S_AxB_2, JAB_temp );

    // cleanup
    JAB_dimer.resize(0,0,false); JAB_temp.resize(0,0,false); _S_AxB_2.resize(0,0,false);
    CTP_LOG(logDEBUG,*_pLog)  << " (" << t.elapsed() - _st << "s)" << "\x1b[0;39m" << std::flush; _st = t.elapsed();

    CTP_LOG(logDEBUG,*_pLog) << "Done with electronic couplings" << std::flush;
    return true;
};

}}


#include <boost/math/constants/constants.hpp>
#include "TypesFunctions.hh"

#include <TMath.h>

#include "ReactorNorm.hh"

const double pi = boost::math::constants::pi<double>();

ReactorNormAbsolute::ReactorNormAbsolute(const std::vector<std::string> &isonames)
{
  variable_(&m_norm, "Norm");
  auto norm = transformation_("isotopes")
    .types(TypesFunctions::ifSame, [](TypesFunctionArgs fargs) {
        for (size_t i = 0; i < fargs.rets.size(); ++i) {
          fargs.rets[i] = DataType().points().shape(1);
        }
      })
    .func([](ReactorNormAbsolute *obj, FunctionArgs fargs) {
        auto& args=fargs.args;
        auto& rets=fargs.rets;
        for (size_t i = 0; i < rets.size(); ++i) {
          rets[i].x[0] = obj->m_norm*args[i].x.sum();
        }
      })
  ;
  for (const std::string &isoname: isonames) {
    norm.input("fission_fraction_"+isoname);
    norm.output("norm_"+isoname);
  }
}

ReactorNorm::ReactorNorm(const std::vector<std::string> &isonames)
  : m_ePerFission(isonames.size())
{
  variable_(&m_thermalPower, "ThermalPower");
  for (size_t i = 0; i < isonames.size(); ++i) {
    variable_(&m_ePerFission[i], "EnergyPerFission_"+isonames[i]);
  }
  variable_(&m_targetProtons, "TargetProtons");
  variable_(&m_L, "L");
  auto norm = transformation_("isotopes")
    .types(TypesFunctions::ifSame, [](TypesFunctionArgs fargs) {
        for (size_t i = 0; i < fargs.rets.size(); ++i) {
          fargs.rets[i] = DataType().points().shape(1);
        }
      })
    .func(&ReactorNorm::calcIsotopeNorms)
  ;
  for (const std::string &isoname: isonames) {
    norm.input("fission_fraction_"+isoname);
    norm.output("norm_"+isoname);
  }
  norm.input("livetime");
  norm.input("power_rate");
}

void ReactorNorm::calcIsotopeNorms(FunctionArgs fargs) {
  auto& args=fargs.args;
  auto& rets=fargs.rets;
  const auto &livetime = args[rets.size()+0].x;
  const auto &power_rate = args[rets.size()+1].x;
  static double conversionFactor = 1.0e-7/TMath::Qe();
  auto distanceWeight = (conversionFactor / (4*pi*std::pow(m_L, 2)));
  auto coeff = m_targetProtons*m_thermalPower*distanceWeight;

  double ePerFission_avg=0.0;
  for (size_t i = 0; i < rets.size(); ++i) {
    ePerFission_avg+=args[i].x(0)*m_ePerFission[i].value();
  }
  for (size_t i = 0; i < rets.size(); ++i) {
    rets[i].x[0] = (coeff*livetime*power_rate*args[i].x/ePerFission_avg).sum();
  }
}


#ifndef BART_SRC_CALCULATOR_TWO_GRID_SPECTRAL_SHAPE_I_HPP_
#define BART_SRC_CALCULATOR_TWO_GRID_SPECTRAL_SHAPE_I_HPP_

#include <deal.II/lac/full_matrix.h>

#include "utility/has_description.h"

//! Classes for calculating the two-grid spectral shape function
namespace bart::acceleration::two_grid::spectral_shape {

/*! \brief Interface for classes that calculate the spectral shape function for the two-grid acceleration method.
 *
 * The spectral shape function is defined for the two-grid method of Adams and Morel (2017).
 * This function must be calculated in each homogenous material region, and is calculated by solving
 * for the eigenvector \f$\xi \in \mathbb{R}^{(G \times 1)}\f$ in the following equation:
 * \f[
 * \left(\bf{T} - \bf{S}_{D0}\right)^{-1}\bf{S}_{U0}\vec{\xi} = \rho\vec{\xi}\;,
 * \f]
 * where \f$\bf{T}\f$ is the total cross-section matrix, \f$\bf{S}_{D0}\f$ is the isotropic downscatter matrix,
 * \f$\bf{S}_{U0}\f$ is the isotropic upscatter matrix, and \f$\rho\f$ is the spectral radius. The spectral shape
 * function is normalized such that
 * \f[
 * \sum_{g = 0}^{G - 1}\xi_g = 1\;.
 * \f]
 *
 */
class SpectralShapeI : public utility::HasDescription {
 public:
  using DealiiMatrix = dealii::FullMatrix<double>;
  virtual ~SpectralShapeI() = default;
  /*! \brief Calculate the spectral shape.
   *
   * @param sigma_t total cross-section matrix.
   * @param sigma_s full scattering matrix.
   * @return spectral shape function
   */
  virtual auto CalculateSpectralShape(const DealiiMatrix& sigma_t,
                                      const DealiiMatrix& sigma_s) -> std::vector<double> = 0;
};

} // namespace bart::acceleration::two_grid::spectral_shape

#endif //BART_SRC_CALCULATOR_TWO_GRID_SPECTRAL_SHAPE_I_HPP_


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

#include <GraphMol/RDKitBase.h>

#include "WHIM.h"
#include "MolData3Ddescriptors.h"

#include <math.h>
#include <Eigen/Dense>
#include <Eigen/SVD>

using namespace Eigen;

namespace RDKit {
namespace Descriptors {
namespace {

MolData3Ddescriptors moldata3D;

double roundn(double in, int factor) {
  return round(in * pow(10., factor)) / pow(10., factor);
}

MatrixXd GetCenterMatrix(MatrixXd &Mat) {
  VectorXd v = Mat.colwise().mean();
  MatrixXd X = Mat.rowwise() - v.transpose();
  return X;
}

MatrixXd GetCovMatrix(MatrixXd &X, MatrixXd &Weigth, double weigth) {
  return X.transpose() * Weigth * X / weigth;
}

JacobiSVD<MatrixXd> *getSVD(MatrixXd &Mat) {
  JacobiSVD<MatrixXd> *svd =
      new JacobiSVD<MatrixXd>(Mat, ComputeThinU | ComputeThinV);
  return svd;
}

std::vector<double> getWhimD(std::vector<double> weigthvector,
                             MatrixXd MatOrigin, int numAtoms, double th) {
  double *weigtharray = &weigthvector[0];

  Map<VectorXd> Weigth(weigtharray, numAtoms);

  MatrixXd WeigthMat = Weigth.asDiagonal();

  double weigth = WeigthMat.diagonal().sum();

  MatrixXd Xmean = GetCenterMatrix(MatOrigin);

  MatrixXd covmat = GetCovMatrix(Xmean, WeigthMat, weigth);

  JacobiSVD<MatrixXd> *svd = getSVD(covmat);

  std::vector<double> w(18);
  // prepare data for Whim parameter computation

  const double *SingVal = svd->singularValues().data();
  MatrixXd Scores = Xmean * svd->matrixV();  //  V is similar

  // compute parameters
  w[0] = SingVal[0];
  w[1] = SingVal[1];
  w[2] = SingVal[2];
  w[3] = SingVal[0] + SingVal[1] + SingVal[2];  // T
  w[4] = SingVal[0] * SingVal[1] + SingVal[0] * SingVal[2] +
         SingVal[1] * SingVal[2];                              // A
  w[5] = w[3] + w[4] + SingVal[0] * SingVal[1] * SingVal[2];   // V
  w[6] = SingVal[0] / (SingVal[0] + SingVal[1] + SingVal[2]);  // P1
  w[7] = SingVal[1] / (SingVal[0] + SingVal[1] + SingVal[2]);  // p2
  w[8] = SingVal[2] / (SingVal[0] + SingVal[1] + SingVal[2]);  // P3

  double res = 0.0;
  for (int i = 0; i < 3; i++) {
    res += std::abs(w[i] / w[3] - 1.0 / 3.0);
  }

  w[9] = 3.0 / 4.0 * res;  // K

  // center original matrix
  VectorXd v1 = Scores.col(0);
  VectorXd v2 = Scores.col(1);
  VectorXd v3 = Scores.col(2);

  //  inverse of the kurtosis
  if (v1.array().pow(4).sum() > 0) {
    w[10] = numAtoms * pow(w[0], 2) / v1.array().pow(4).sum();  // E1
  } else {
    w[10] = 0.0;
  }

  if (v2.array().pow(4).sum() > 0) {
    w[11] = numAtoms * pow(w[1], 2) / v2.array().pow(4).sum();  // E2
  } else {
    w[11] = 0.0;
  }

  if (v3.array().pow(4).sum() > 0) {
    w[12] = numAtoms * pow(w[2], 2) / v3.array().pow(4).sum();  // E3
  } else {
    w[12] = 0.0;
  }

  w[13] = (w[10] + w[11] + w[12]) / 3.0;  // mean total density of the atoms
                                          // called D is used on Dragon 6 not
                                          // just the sum!

  // check if the molecule is fully symmetrical "like CH4" using Canonical Rank
  // Index and/or Sphericity !

  double gamma[3];  // Gamma values
  double nAT = (double)numAtoms;

  // check if two atoms are symetric versus the new axis ie newx,newy,newz a
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < numAtoms; j++) {
      Scores(j, i) = roundn(Scores(j, i),
                            3);  // round the matrix! same as eigen tolerance !
    }
  }

// we should take into account atoms that are in the axis too!!! which is not trivial
  for (int i = 0; i < 3; i++) {
    std::vector<double> Symetric(2*numAtoms, 0.0);
    double ns = 0.0;
    double na = 0.0;
    for (int j = 0; j < numAtoms; j++) {
      bool amatch = false;
      for (int k = 0; k < numAtoms; k++) {
	      if (j==k) {
          continue;
        }
	      if (std::abs(Scores(j, i) + Scores(k, i)) <= th) {
		      // those that are close opposite & not close to the axis!
	        ns += 1;  // check only once the symetric none null we need to add +2!
		      // (reduce the loop duration)
		      amatch = true;
		      Symetric[j]=1.0;
		      Symetric[j+numAtoms]=2.0;
		      Symetric[k]=1.0;
		      Symetric[k+numAtoms]=2.0;
		      break;
        }
      }
      if (!amatch) {
        na +=1;
        Symetric[j]=0.0;
        Symetric[j+numAtoms]=std::abs(Scores(j, i));
      }
    }
    // take into account the atoms close to the axis
    for (int aj = 0; aj < numAtoms; aj++) {
	    if (Symetric[aj+numAtoms]<th && Symetric[aj]<1.0) {
		    ns +=1;
        na -=1;
      }
    }
    gamma[i] = 0.0;
    double gammainv=1.0;
    if (ns == 0) {
      gammainv = 1.0 - (na / nAT) * log(1.0 / nAT) / log(2.);
    }
    if (ns > 0) {
      gammainv = 1.0 - ((ns / nAT) * log(ns / nAT) / log(2.) +
                       (na / nAT) * log(1.0 / nAT) / log(2.));
    }
    gamma[i]=1.0/gammainv;
  }
  w[14] = gamma[0];  // G1
  w[15] = gamma[1];  // G2
  w[16] = gamma[2];  // G3
  w[17] = pow(gamma[0] * gamma[1] * gamma[2], 1.0 / 3.0);
  delete svd;

  return w;
}

void GetWHIMs(const Conformer &conf, std::vector<double> &result,
              double *Vpoints, double th) {
  std::vector<double> wu(18);
  std::vector<double> wm(18);
  std::vector<double> wv(18);
  std::vector<double> we(18);
  std::vector<double> wp(18);
  std::vector<double> wi(18);
  std::vector<double> ws(18);

  int numAtoms = conf.getNumAtoms();
  Map<MatrixXd> matorigin(Vpoints, 3, numAtoms);
  MatrixXd MatOrigin = matorigin.transpose();
  std::vector<double> weigthvector;

  // intermediate 18 values stored in this order per weighted vector :
  // "L1","L2","L3","T","A","V","P1","P2","P3","K","E1","E2","E3","D","G1","G2","G3","G"
  weigthvector = moldata3D.GetUn(numAtoms);
  wu = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeMW(conf.getOwningMol());
  wm = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeVdW(conf.getOwningMol());
  wv = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeENeg(conf.getOwningMol());
  we = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativePol(conf.getOwningMol());
  wp = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetRelativeIonPol(conf.getOwningMol());
  wi = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  weigthvector = moldata3D.GetIState(conf.getOwningMol());
  ws = getWhimD(weigthvector, MatOrigin, numAtoms, th);

  result.clear();
  result.resize(126);

  for (int i = 0; i < 18; i++) {
    result[i + 18 * 0] = wu[i];
    result[i + 18 * 1] = wm[i];
    result[i + 18 * 2] = wv[i];
    result[i + 18 * 3] = we[i];
    result[i + 18 * 4] = wp[i];
    result[i + 18 * 5] = wi[i];
    result[i + 18 * 6] = ws[i];
  }
  wu.clear();
  wm.clear();
  wv.clear();
  we.clear();
  wp.clear();
  wi.clear();
  ws.clear();
}

void getWHIM(const ROMol &mol, std::vector<double> &res, int confId,
             double th) {
  int numAtoms = mol.getNumAtoms();
  const Conformer &conf = mol.getConformer(confId);
  double *Vpoints = new double[3 * numAtoms];

  for (int i = 0; i < numAtoms; ++i) {
    Vpoints[3 * i] = conf.getAtomPos(i).x;
    Vpoints[3 * i + 1] = conf.getAtomPos(i).y;
    Vpoints[3 * i + 2] = conf.getAtomPos(i).z;
  }

  std::vector<double> w(126);
  GetWHIMs(conf, w, Vpoints, th);
  delete [] Vpoints;

  // Dragon extract only this list in this order : L1 L2 L3 P1 P2 G1 G2 G3 E1 E2
  // E3
  int map1[11] = {0, 1, 2, 6, 7, 14, 15, 16, 10, 11, 12};

  for (int k = 0; k < 7; k++) {
    for (int i = 0; i < 11; i++) {
      res[i + 11 * k] = roundn(w[map1[i] + 18 * k], 3);
    }
  }

  for (int i = 0; i < 2; i++) {
    res[i + 13 * 7] = roundn(w[17 + 18 * i], 3);  // 92  93 for Gu  Gm
  }

  for (int i = 0; i < 7; i++) {
    res[i + 11 * 7] =
        roundn(w[3 + 18 * i],
               3);  // 78  79  80  81  82  83  84  for Tu  Tm  Tv  Te  Tp  Ti Ts
    res[i + 12 * 7] =
        roundn(w[4 + 18 * i],
               3);  // 85  86  87  88  89  90  91  for Tu  Am  Av  Ae  Ap  Ai As
    res[i + 13 * 7 + 2] =
        roundn(w[9 + 18 * i],
               3);  // 94  95  96  97  98  99  100 for Ku  Km  Kv  Ke  Kp  Ki Ks
    res[i + 14 * 7 + 2] =
        roundn(w[13 + 18 * i],
               3);  // 101 102 103 104 105 106 107 for Du  Dm  Dv  De  Dp  Di Ds
    res[i + 15 * 7 + 2] =
        roundn(w[5 + 18 * i],
               3);  // 108 109 110 111 112 113 114 for Vu  Vm  Vv  Ve  Vp  Vi Vs
  }
}

}  // end of anonymous namespace

void WHIM(const ROMol &mol, std::vector<double> &res, int confId, double th) {
  PRECONDITION(mol.getNumConformers() >= 1, "molecule has no conformers")
  // Dragon final list is: L1u L2u L3u P1u P2u G1u G2u G3u E1u E2u E3u L1m L2m
  // L3m P1m P2m G1m G2m G3m E1m E2m E3m L1v L2v L3v P1v P2v G1v G2v G3v E1v E2v
  // E3v L1e L2e L3e P1e P2e G1e G2e G3e E1e E2e E3e L1p L2p L3p P1p P2p G1p G2p
  // G3p E1p E2p E3p L1i L2i L3i P1i P2i G1i G2i G3i E1i E2i E3i L1s L2s L3s P1s
  // P2s G1s G2s G3s E1s E2s E3s Tu  Tm  Tv  Te  Tp  Ti  Ts  Au  Am  Av  Ae  Ap
  // Ai  As  Gu  Gm  Ku  Km  Kv  Ke  Kp  Ki  Ks  Du  Dm  Dv  De  Dp  Di  Ds  Vu
  // Vm  Vv  Ve  Vp  Vi  Vs

  res.clear();
  res.resize(114);
  getWHIM(mol, res, confId, th);
}
}  // end of Descriptors namespace
}  // end of RDKit namespace


////////////////////////////////////////////////////////////////////////////////////////////////
////                     Population Dynamics single cell simulations                       /////
////                          (For predtermined SNAIL levels)				               /////
////                Codes adopted from Tripathi et. al. 2020 PLOS Comp Bio                 /////
////                Modification done by Jain et al., Date: 19th Jan 2022                  /////
////////////////////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <cmath>
#include <ctime>
#include <random>
#include <string>
#include <fstream>
#include <boost/array.hpp>
#include <boost/unordered_map.hpp>
#include <boost/numeric/odeint.hpp>

#define NUMNODES 4

typedef boost::array<double, NUMNODES> cell_state;
typedef boost::unordered_map<int, boost::array<double, NUMNODES>> population;

std::mt19937 generator; // for random number generation

double temp_var[13];  // temp_var hold the value for initialization parameters provided at start

//////////////////////// k combination out of n //////////////////////////////
double nchoosek(int n, int k)
{
	double result = 1.0;
	double result0 = 1.0;

	for(int i = 0; i < k; i++)
	{
		result *= (n - i);
	}
	for(int i = 1; i <= k; i++)
	{
		result0 *= i;
	}

	return(result / result0);
}
///////////////////////////////////////////////////////////////////////////////////

// function to break a string and separate numeral character based on the delimiter
void tokenize(std::string s, std::string del = " ")
{
	int start = 0;
	int end = s.find(del);
	int i = 0;
	while (end != -1) {
		//std::cout << s.substr(start, end - start) << std::endl;
		temp_var[i] = std::stod(s.substr(start, end - start));
		//std::cout << temp_var[i] << std::endl;
		start = end + del.size();
		end = s.find(del, start);
		i++;
	}
	//std::cout << s.substr(start, end - start);
	temp_var[i] = std::stod(s.substr(start, end - start));
	//std::cout << temp_var[i];
}
///////////////////////////////////////////////////////////////////////////////////

/////////////////// EMT core circuit ODEs (updates a cells' state) ////////////////
void EMT_system(const cell_state &x, cell_state &dxdt, double t)
{
	double ku200 = 0.05, kmz = 0.5, kz = 0.1;

	// Transcription rate:
	double gu200 = 2100, gmz = 11, gz = 100;

	// Hills function threshold :
	double z0u200 = 220000, z0mz = 25000, s0u200 = 180000, s0mz = 180000, u2000 = 10000;

	// Cooperativity:
	double nzu200 = 3, nsu200 = 2, nzmz = 2, nsmz = 2, nu200 = 6;

	// fold change
	double lamdazu200 = 0.1, lamdasu200 = 0.1, lamdazmz = 7.5, lamdasmz = 10;

	double Mu0=1/std::pow((1+x[0]/u2000),nu200);
	double Mu1=std::pow((x[0]/u2000),1)/std::pow((1+x[0]/u2000),nu200);
	double Mu2=std::pow((x[0]/u2000),2)/std::pow((1+x[0]/u2000),nu200);
	double Mu3=std::pow((x[0]/u2000),3)/std::pow((1+x[0]/u2000),nu200);
	double Mu4=std::pow((x[0]/u2000),4)/std::pow((1+x[0]/u2000),nu200);
	double Mu5=std::pow((x[0]/u2000),5)/std::pow((1+x[0]/u2000),nu200);
	double Mu6=std::pow((x[0]/u2000),6)/std::pow((1+x[0]/u2000),nu200);

	double Hillszu200=(1+lamdazu200*std::pow((x[2]/z0u200),nzu200))/(1+std::pow((x[2]/z0u200),nzu200));
	double Hillssu200=(1+lamdasu200*std::pow((x[3]/s0u200),nsu200))/(1+std::pow((x[3]/s0u200),nsu200));
	double Hillszmz=(1+lamdazmz*std::pow((x[2]/z0mz),nzmz))/(1+std::pow((x[2]/z0mz),nzmz));
	double Hillssmz=(1+lamdasmz*std::pow((x[3]/s0mz),nsmz))/(1+std::pow((x[3]/s0mz),nsmz));

	dxdt[0] = gu200*Hillszu200*Hillssu200-x[1]*(0.005*6*Mu1+2*0.05*15*Mu2+3*0.5*20*Mu3+4*0.5*15*Mu4+5*0.5*6*Mu5+6*0.5*Mu6)-ku200*x[0];
	dxdt[1] = gmz*Hillszmz*Hillssmz-x[1]*(0.04*6*Mu1+0.2*15*Mu2+20*Mu3+15*Mu4+6*Mu5+Mu6)-kmz*x[1];
	dxdt[2] = gz*x[1]*(Mu0+0.6*6*Mu1+0.3*15*Mu2+0.1*20*Mu3+0.05*15*Mu4+0.05*6*Mu5+0.05*Mu6)-kz*x[2];
	dxdt[3] = 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////// to assign phenotypes to cells /////////////////////////
void get_phenotypes(population &P, boost::unordered_map <int, int> &phenotype)
{
	double x1 = 193.2, x2 = 208.7, y1 = 243.09, y2 = 90.93;
	double a1 = y2 - y1, b1 = x1 - x2, c1 = x2*y1 - y2*x1;

	double u1 = 185.12, u2 = 224.67, v1 = 698.395, v2 = 495.802;
	double a2 = v2 - v1, b2 = u1 - u2, c2 = u2*v1 - v2*u1;

	double x, y, fac1, fac2;
	int state = -1;

	for(int i = 0; i < P.size(); i++)
	{
		x = P[i][3] / 1e3;
		y = P[i][1];
		state = -1;

		if(x < u1)
		{
			state = 0;
		}
		else if(x > u2)
		{
			state = 2;
		}

		else
		{
			fac1 = (a1*x + b1*y + c1) / b1;
			fac2 = (a2*x + b2*y + c2) / b2;
			if(fac2 >= 0)
			{
				state = 2;
			}
			else if(fac1 < 0)
			{
				state = 0;
			}
			else
				state = 1;

		}

		if(state == -1)
		{
			std::cout << "Error in phenotype assignment." << "\n";
		}
		else
		{
			phenotype[i] = state;
		}
	}
}

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

//// To initialize cellular states of the initial population
void initialize_Signal_lognormal(population& P, double* pop_fraction, const int pop_size, const double SNAIL)
{

	//std::normal_distribution <> dist{ 0.0, 1.0 };

	//std::vector<int> V[3];

	int flag = 0;
	//const unsigned int N = 100000;
	double I = SNAIL;

	/*double CV2 = 1.0;
	double SD = std::sqrt(std::log(CV2 * CV2 + 1.0));
	double M = 200e3;
	*/

	boost::unordered_map <int, int> phenotype;
	boost::array<int, 4> count = { 0,0,0,0 };

	population temp_P, temp_P_vector;
	std::ifstream steady_states, eigen_values;

	steady_states.open("steady_states.csv");
	eigen_values.open("eigen_values.csv");
	std::string line;

// load steady states matrix (save steady_states.csv in working directory)////

	int num_states = 0;

	while(getline(steady_states,line)){

		//std::cout<<line;
		tokenize(line,",");

		for(int i = 0; i < NUMNODES; i++){
			temp_P[num_states][i] = temp_var[i];
		}
			num_states++;
	}

// to load eigen values matrix (save steady_states.csv in working directory) ////

	num_states = 0;

	while(getline(eigen_values,line)){

		//std::cout<<line;
		tokenize(line,",");

		for(int i = 0; i < NUMNODES; i++){
			temp_P_vector[num_states][i] = temp_var[i];
		}
			num_states++;
	}
	///////////////////////////////////////////////////////////////

	steady_states.close();
	eigen_values.close();

	//////// to initialize cells based on random generated SNAIL levels and population fraction.///
	///// The logic is to search for nearest SNAIL value, compared to sampled I, in steady states and eigen values matrices
	/// and initialize cells with miR200, mZEB, and ZEB variable vector at that SNAIL value

	int first_time = 1, min_indx = 0, selected_P_indx = 0 ;
	double abs_diff = 0, diff = 0;

	while (count[3] < (pop_size))
	{

		if(I <= temp_P[temp_P.size()-1][3]){
			first_time = 1;
			min_indx = 0;
			selected_P_indx = 0 ;
			abs_diff = 0;
			diff = 0;

			population selected_P;

			for(int g = 0; g < temp_P.size(); g++){

				if(temp_P[g][3] - I >= 0){
					abs_diff = temp_P[g][3] - I;
				}
				else
				{
					abs_diff = -(temp_P[g][3] - I);
				}

				if(abs_diff<= 100 && temp_P_vector[g][0] < 0 && temp_P_vector[g][1] < 0 && temp_P_vector[g][2] < 0){

					if(first_time==1){
						diff = abs_diff;
						min_indx = g;
						first_time = 2;
					}
					else{
						if(abs_diff < diff)
							min_indx = g;
					}

				}
				else if(first_time == 2)
				{

				for(int i = 0; i < NUMNODES; i++){
					selected_P[selected_P_indx][i] = temp_P[g][i];

					/*std::cout<< selected_P[selected_P_indx][i] << " ";

					if(i ==  NUMNODES-1)
						std::cout<<"\n";*/

				}

				selected_P_indx++;
				first_time = 1;

				}
			}

			get_phenotypes(selected_P,phenotype);
			/*std::cout << selected_P.size() << "\n";

			for(int i = 0; i < phenotype.size(); i++){
				std::cout<<phenotype[i] << "\n";
			}*/

			for(int i = 0; i < selected_P.size();i++){

				if (phenotype[i] == 0 && count[0] < pop_size * pop_fraction[0]) {
					for(int j = 0; j < NUMNODES; j++){
						P[count[3]][j] = selected_P[i][j];

						/*std::cout<< selected_P[i][j] << " ";

						if(j ==  NUMNODES-1)
							std::cout<<"\n";*/
					}
					count[0]++;

				}
				else if (phenotype[i] == 1 && count[1] < pop_size * pop_fraction[1]){
					for(int j = 0; j < NUMNODES; j++){
						P[count[3]][j] = selected_P[i][j];

						/*std::cout<< selected_P[i][j] << " ";

						if(j ==  NUMNODES-1)
							std::cout<<"\n";*/

					}
					count[1]++;

				}
				else if (phenotype[i] == 2 && count[2] < pop_size * pop_fraction[2]){
					for(int j = 0; j < NUMNODES; j++){
						P[count[3]][j] = selected_P[i][j];
						/*std::cout<< selected_P[i][j] << " ";

						if(j ==  NUMNODES-1)
							std::cout<<"\n";*/
					}
					count[2]++;

				}

				count[3] = count[0] + count[1] + count[2];

			}

			selected_P.clear();
			phenotype.clear();
		}
	}
	/////////////////////////////////////////////////////////////////////////////////////////////////////

}

///// uniform sampling a cell from a subpopulation////////////////
int get_sample(boost::unordered_map <int, int> &phenotype, int pid)
{
	std::vector<int> V;
	int count = 0;
	for(int i = 0; i < phenotype.size(); i++)
	{
		if(phenotype[i] == pid)
		{
			V.push_back(i);
			count += 1;
		}
	}

	std::uniform_int_distribution<int> distribution(0, count - 1);

	return(V[distribution(generator)]);
}
//////////////////////////////////////////////////////////////////////

////////////////// adding duplication and partitioning noise/fluctuation on cell division //////////
void add_sym_noise_normal(population &P, int rep_id, int new_id, double * eta, int eta_id)
{
	std::normal_distribution <> distribution{0, 1};

	std::uniform_real_distribution <> distribution_unifrom{0.2, 0.4};

	double temp_snail[2];

	double rand_num[2];

	rand_num[0] =  distribution(generator);
	rand_num[1] =  distribution(generator);

	//do{
	if(eta_id == 0){
	        temp_snail[0] = P[rep_id][3] + distribution(generator)*distribution_unifrom(generator)*P[rep_id][3];
	        temp_snail[1] = P[rep_id][3] + distribution(generator)*distribution_unifrom(generator)*P[rep_id][3];
        }
        else{
        	temp_snail[0] = P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]/2 + rand_num[1]*eta[1]*(2*P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]) ;
	        temp_snail[1] = P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]/2 - rand_num[1]*eta[1]*(2*P[rep_id][3] + rand_num[0]*eta[0]*P[rep_id][3]) ;
        }

        //std::cout << temp_snail[0] << " " << temp_snail[1];

	//}while(temp_snail[0] <= 0.0 || temp_snail[1] <=0.0);

	P[rep_id][3] = temp_snail[0];
	P[new_id][3] = temp_snail[1];

        if(P[rep_id][3] < 0.0)
        {
                P[rep_id][3] = 0.0;
        }
        if(P[new_id][3] < 0.0)
        {
                P[new_id][3] = 0.0;
        }

}

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

////////// sampling a fraction of cells from a subpopulation when overall population size increase 80% of carrying capacity K
int FACS(population &P, int N, int type_start, std::vector <int> &cell_index)
{
	boost::unordered_map <int, int> phenotype;
	get_phenotypes(P, phenotype);
	std::vector<int> usefulIndex;
	int count = 0, flag = 1;

	for(int i = 0; i < phenotype.size(); i++)
	{
		if(phenotype[i] == type_start)
		{
			usefulIndex.push_back(i);
			count += 1;
		}
	}
	if(count < N)
	{
		std::cout << "Not enough cells of type" << std::to_string(type_start) << " present in the culture." << "\n";
		flag = 0;
		return(flag);
	}

	std::uniform_int_distribution<int> distribution(0, usefulIndex.size() - 1);

	int size = 0;
	int index = 0;
	while(size < N)
	{
		//index = usefulIndex[distribution(generator)];

		index = usefulIndex[size];
		cell_index.push_back(index);

		/*sorted_P[size+cell_count] = {0.0, 0.0, 0.0, 0.0};
		for(int i = 0; i < NUMNODES; i++)
		{
			sorted_P[size+cell_count][i] = P[index][i];
		}*/
		size += 1;
	}

	return(flag);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////

////////////////////////// population dynamics ////////////////////////////////////////////////////////////
int simulate_normal(population& P, double end_time, double * eta, int eta_id,int frac_id, int sim_type, boost::array <double, 3> GR, int GR_ratio_id, int pop_size, int sim_num, int SNAIL_id)
{
	boost::unordered_map <int, double> update_time;
	boost::unordered_map <int, int> phenotype;
	boost::array<double, 3> count;
	//boost::array<int, 3> cell_count;
	std::vector <int> cell_index;

	double temp_count[3];
	double day_indx = 1;

	for(int i = 0; i < P.size(); i++)
	{
		update_time[i] = 0.0;
	}

	int last_index = P.size();

	get_phenotypes(P, phenotype);

	//std::cout << GR[0] << " " << GR[1] << " " << GR[2] << "\n";

	boost::array <double, 3> growth;
	growth[0] = std::log(2) / GR[0];
	growth[1] = std::log(2) / GR[1];
	growth[2] = std::log(2) / GR[2];
	boost::array<double, 3> r0 = {growth[0], growth[1], growth[2]};
	boost::array<double, 3> d0 = {growth[0] / 10.0, growth[1] / 10.0, growth[2] / 10.0};
	boost::array<double, 6> rates = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

	int K = 5000;

	if(sim_type == 1)
	{
		K = 2000;
	}

	for(int i = 0; i < P.size(); i++)
	{
		if(phenotype[i] == 0)
		{
			if(P.size() < K)
			{
				rates[0] += r0[0]*(1 - float(P.size()) / K);
			}
			rates[3] += d0[0];
		}
		else if(phenotype[i] == 1)
		{
			if(P.size() < K)
			{
				rates[1] += r0[1]*(1 - float(P.size()) / K);
			}
			rates[4] += d0[1];
		}
		else
		{
			if(P.size() < K)
			{
				rates[2] += r0[2]*(1 - float(P.size()) / K);
			}
			rates[5] += d0[2];
		}
	}

	std::uniform_real_distribution<double> distribution(0.0, 1.0);

	double t = 0.0, a0, dt, p0, p1;
	double sum_prev, sum_new;
	double last_updated = 0.0;
	int event_id, rep_id, death_id;

	std::ofstream end_of_day, SNAIL_distribution, mZEB_distribution;

	if(sim_type == 1){

		count = {0.0, 0.0, 0.0};
		for(int i = 0; i < P.size(); i++)
		{
			count[phenotype[i]] += 1.0;
		}
		for(int i = 0; i < 3; i++)
		{
			count[i] = count[i] / P.size();
		}

		// Below, 'E_frac_spread_Data_20hrs_DT_4_weeks' is the folder name in which output data files will be stored.
		// Create it in current working directory before running the simulation.
		end_of_day.open("E_frac_spread_Data_20hrs_DT_4_weeks/end_of_day_counts_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(SNAIL_id) + ".csv", std::ios::app);

		//SNAIL_distribution.open("E_frac_spread_SNAIL_dist_20hrs_DT_4_weeks/snail_dist_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(SNAIL_id) + "_" + std::to_string(sim_num) + ".csv");

		//mZEB_distribution.open("E_frac_spread_ZEB_dist_20hrs_DT_4_weeks/mZEB_dist_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(SNAIL_id) + "_" + std::to_string(sim_num) + ".csv");

		/*for(int i = 0; i < P.size(); i++){
			if(i < P.size() -1){
				SNAIL_distribution << P[i][3] << ",";
				mZEB_distribution << P[i][1] << ",";
			}
			else{
				SNAIL_distribution << P[i][3] << "\n";
				mZEB_distribution << P[i][1] << "\n";
			}
		}*/
		//phenotype_switch.open("phenotype_switch/end_of_day_counts_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" + std::to_string(GR_ratio_id) + "_" + std::to_string(sim_num) + ".csv", std::ios::app);
	}

	while(t <= end_time)
	{
		a0 = 0.0;

		for(int i = 0; i < 6; i++)
		{
			a0 += rates[i];
		}

		p0 = distribution(generator);
		dt = (1.0 / a0)*std::log(1 / p0);
		t += dt;

		p1 = distribution(generator);
		sum_prev = 0.0;
		sum_new = 0.0;
		event_id = -1;

		for(int i = 0; i < 6; i++)
		{
			sum_new = sum_prev + rates[i] / a0;
			if(p1 >= sum_prev && p1 < sum_new)
			{
				event_id = i;
				break;
			}
			sum_prev = sum_new;
		}

		if(event_id == 0)
		{
			rep_id = get_sample(phenotype, 0);
		}
		else if(event_id == 1)
		{
			rep_id = get_sample(phenotype, 1);
		}
		else if(event_id == 2)
		{
			rep_id = get_sample(phenotype, 2);
		}
		else if(event_id == 3)
		{
			death_id = get_sample(phenotype, 0);
		}
		else if(event_id == 4)
		{
			death_id = get_sample(phenotype, 1);
		}
		else if(event_id == 5)
		{
			death_id = get_sample(phenotype, 2);
		}

		if(event_id < 3)
		{

			boost::numeric::odeint::integrate(EMT_system, P[rep_id], 0.0, t - update_time[rep_id], 0.1);

			P[last_index] = {0.0, 0.0, 0.0, 0.0};  // last index holds the length of Population Array vector
			for(int j = 0; j < NUMNODES; j++)
			{
				P[last_index][j] = P[rep_id][j];
			}

			add_sym_noise_normal(P, rep_id, last_index, eta, eta_id);

			update_time[rep_id] = t;
			update_time[last_index] = t;
			last_index += 1;
		}
		else
		{
			for(int j = 0; j < NUMNODES; j++)
			{
				P[death_id][j] = P[last_index - 1][j];
			}
			update_time[death_id] = update_time[last_index - 1];
			phenotype[death_id] = phenotype[last_index - 1];
			P.erase(last_index - 1);
			update_time.erase(last_index - 1);
			phenotype.erase(last_index - 1);
			last_index -= 1;
		}

		if(P.size()==0)
		{
			if(sim_type == 1)
			{
				end_of_day.close();
				//SNAIL_distribution.close();
				//mZEB_distribution.close();
			}

			return(1);
		}

		if(true)
		{
			for(int i = 0; i < P.size(); i++)
			{
				if(t - update_time[i] > 0.0)
				{
					boost::numeric::odeint::integrate(EMT_system, P[i], 0.0, t - update_time[i], 0.1);
				}
				update_time[i] = t;
			}
		}

		get_phenotypes(P, phenotype);

		/// reducing population size by sampling different subpopulations as per their last distribution

		if(P.size() > 0.8 * K && sim_type == 1){

			count = {0.0, 0.0, 0.0};

			for(int i = 0; i < P.size(); i++)
			{
				count[phenotype[i]] += 1.0;
			}

			for(int i = 0; i < 3; i++)
			{
				count[i] = count[i] / P.size();

				double b = 0; // to truncate the double to two decimal places

				for (double k = 0; k<=100; k++)
				{
					if(k > 100*count[i])
					{
						b = 100*count[i] - (k-1);
						count[i] = count[i] - b/100;
						break;
					}
				}

				//std::cout << count[i] << "\n";

				int flag = FACS(P,pop_size*count[i],i,cell_index);
				if (flag == 0){
					std::cout <<" stopped simulation due to non-availability of cells\n";
					return(-1);
				}
			}

			std::sort(cell_index.begin(),cell_index.end());

			//for (int i = 0; i< cell_index.size(); i++) std::cout << cell_index[i] << "\n";
			//std::cout<< cell_index.size()<<"\n";

			for (int i = 0; i < cell_index.size(); i++){
					if(i != cell_index[i]){
						for(int j = 0; j < NUMNODES; j++){
							P[i][j] = P[cell_index[i]][j];
							//std::cout << P[i][j] << " ";
						}
						phenotype[i] = phenotype[cell_index[i]];
					}
					update_time[i] = t;

					//std::cout<<"\n";
			}
			for (int i = P.size()-1; i >= cell_index.size(); i--)
			{
				P.erase(i);
				update_time.erase(i);
				phenotype.erase(i);
			}
			last_index = P.size();

			/*std::cout << "\nsize of the sampled pop is " << last_index <<"\n";

			for (int i = 0; i < P.size(); i++)
			{
				std::cout << P[i][1] << " " << P[i][3] << "\n";
			}*/

			cell_index.clear();

		}
		///////////////////////////////////////////////////////////////////////////////////////

		count = {0.0, 0.0, 0.0};

		for(int i = 0; i < P.size(); i++)
		{
			count[phenotype[i]] += 1.0;
		}

		/*for(int i = 0; i < 3; i++)
		{
			count[i] = count[i] / P.size();
		}*/

		rates = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

		for(int i = 0; i < P.size(); i++)
		{
			if(phenotype[i] == 0)
			{
				if(P.size() < K)
				{
					rates[0] += r0[0]*(1 - float(P.size()) / K);
				}
				rates[3] += d0[0];
			}
			else if(phenotype[i] == 1)
			{
				if(P.size() < K)
				{
					rates[1] += r0[1]*(1 - float(P.size()) / K);
				}
				rates[4] += d0[1];
			}
			else
			{
				if(P.size() < K)
				{
					rates[2] += r0[2]*(1 - float(P.size()) / K);
				}
				rates[5] += d0[2];
			}
		}

		if(sim_type == 1)
		{

			if (t < 24* day_indx) {
				temp_count[0] = count[0];
				temp_count[1] = count[1];
				temp_count[2] = count[2];

			}
			else {

				//std::cout << day_indx << "," << temp_count[0] << "," << temp_count[1] << "," << temp_count[2] << "," << P.size() << "\n";

				//// the phenotypic distribution is stored each day in absolute cell counts ////

				end_of_day << day_indx << "," << temp_count[0] << "," << temp_count[1] << "," << temp_count[2] << "," << (P.size()-1) << "\n";

				/*if((int(day_indx)%14) == 0){ // save distribution data every four weeks
					for(int i = 0; i < P.size(); i++){
						if(i < P.size() -1){
							SNAIL_distribution << P[i][3] << ",";
							mZEB_distribution << P[i][1] << ",";
						}
						else{
							SNAIL_distribution << P[i][3] << "\n";
							mZEB_distribution << P[i][1] << "\n";
						}
					}
				}*/

				day_indx++;

				temp_count[0] = count[0];
				temp_count[1] = count[1];
				temp_count[2] = count[2];

			}
		}
	}

	if(sim_type == 1)
	{
		end_of_day.close();
	//	SNAIL_distribution.close();
	//	mZEB_distribution.close();
	}

	return(0);
}

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

	tokenize(argv[1], "_"); /// to separate parameters from input string

	boost::unordered_map <int, int> phenotype;
	boost::array <double, 3> GR; // doubling time of the three population
	std::vector <int> cell_index; // used with FACS
	int aborted;

	int count[3];

	// order the input parameter values delimited by '_' as follows:
// e.g. 1_0.2_0.1_0_1_0_0_0_20_20_20_0_50000
	int eta_id = temp_var[0];
	double eta[2] = { temp_var[1], temp_var[2] };
	int frac_id = temp_var[3];
	double pop_fraction[3] = { temp_var[4], temp_var[5], temp_var[6] };
	int GR_ratio_id = temp_var[7];
	GR[0] = temp_var[8];
	GR[1] = temp_var[9];
	GR[2] = temp_var[10];
	int SNAIL_id = temp_var[11];
	double SNAIL = temp_var[12];

	int sim_num = 1;
	int pop_size = 200;
	int total_inde_runs = 50;
	int time_in_days = 4 * 7 ; // 4 respresent 4 weeks
	int file_rows = 0;

	std::ifstream csv_file;

	std::string line;

	while(true){

		//// check how many runs of data is stored in output file
		csv_file.open("E_frac_spread_Data_20hrs_DT_32_weeks/end_of_day_counts_" + std::to_string(eta_id) + "_" + std::to_string(frac_id) + "_" +
			std::to_string(GR_ratio_id) + "_"  + std::to_string(SNAIL_id) + ".csv");

		/*if(csv_file.is_open())
		{
			std::cout<<"file is correctly opened\n";
		}*/

		file_rows = 0;
		while(getline(csv_file,line)){
			file_rows++;
		}

		csv_file.close();
		//std::cout << file_rows << "\n";

		/////////// don't proceed if file has data of required number of runs or incomplete previous simulation data
		if(file_rows >= (total_inde_runs * time_in_days) || (file_rows%time_in_days) != 0){

			if(file_rows%time_in_days != 0)
				std::cout << "stopped bacause of already missing data \n";
			else if(file_rows > (total_inde_runs * time_in_days))
				std::cout << total_inde_runs * time_in_days << " enough data already present\n";
			break;
		}
		//////////////////////////////////////////////////////////////////////////////////////

		generator = std::mt19937(std::time(NULL) + file_rows);

		population P;

		initialize_Signal_lognormal(P, pop_fraction, 1, SNAIL); // initial pop size as 1 for single cell simualtion

		/*for(int i = 0; i < P.size(); i++){

			for(int j = 0; j < NUMNODES; j++){
				std::cout << P[i][j] << " ";
			}
			std::cout<<"\n";
		}*/

		std::cout << "initialization complete for eta id " << eta_id << " frac_id " << frac_id << " GR ratio id " << GR_ratio_id << " SNAIL_id " << SNAIL_id <<" and sim num is " << sim_num << std::endl;

		aborted = simulate_normal(P, 24 * time_in_days, eta, eta_id, frac_id, 1, GR, GR_ratio_id, pop_size, sim_num, SNAIL_id);

		if(aborted == 1)
		{
			std::cout<<"the simulation is aborted due to extinction of population\n";
		}else{

			std::cout << "simulation complete for eta id " << eta_id << " frac_id " << frac_id << " GR ratio id " << GR_ratio_id << " SNAIL_id " << SNAIL_id <<" and sim num is " << sim_num << std::endl;
			sim_num++;
		}

		P.clear();
	}

	return(0);

}


/* ------------------------------------------------------------------------- */
/* UMFPACK Version 4.1 (Apr. 30, 2003), Copyright (c) 2003 by Timothy A.     */
/* Davis.  All Rights Reserved.  See ../README for License.                  */
/* email: davis@cise.ufl.edu    CISE Department, Univ. of Florida.           */
/* web: http://www.cise.ufl.edu/research/sparse/umfpack                      */
/* ------------------------------------------------------------------------- */

/***********************************************************************/
/*         UMFPACK Copyright, License and Availability                 */
/***********************************************************************/
/*
 *
 * UMFPACK Version 4.1 (Apr. 30, 2003),  Copyright (c) 2003 by Timothy A.
 * Davis.  All Rights Reserved.
 *
 * UMFPACK License:
 *
 *   Your use or distribution of UMFPACK or any modified version of
 *   UMFPACK implies that you agree to this License.
 *
 *   THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
 *   EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
 *
 *   Permission is hereby granted to use or copy this program, provided
 *   that the Copyright, this License, and the Availability of the original
 *   version is retained on all copies.  User documentation of any code that
 *   uses UMFPACK or any modified version of UMFPACK code must cite the
 *   Copyright, this License, the Availability note, and "Used by permission."
 *   Permission to modify the code and to distribute modified code is granted,
 *   provided the Copyright, this License, and the Availability note are
 *   retained, and a notice that the code was modified is included.  This
 *   software was developed with support from the National Science Foundation,
 *   and is provided to you free of charge.
 *
 * Availability:
 *
 *   http://www.cise.ufl.edu/research/sparse/umfpack
 *
 */

/* Used by permission. */

/* Simple demo program for UMFPACK               */
/* from UMFPACK Version 4.1 Quick Start Guide    */

/* modified by Kresimir Fresl, 2003              */
/* UMFPACK bindings & ublas::compressed_matrix<> */

#include <iostream>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_sparse.hpp>
#include <boost/numeric/bindings/umfpack/umfpack.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
namespace umf = boost::numeric::bindings::umfpack;

int main() {

  ublas::compressed_matrix<double, ublas::column_major, 0,
    ublas::unbounded_array<int>, ublas::unbounded_array<double> > A (5,5,12);
  ublas::vector<double> B (5), X (5);

  A(0,0) = 2.; A(0,1) = 3;
  A(1,0) = 3.; A(1,2) = 4.; A(1,4) = 6;
  A(2,1) = -1.; A(2,2) = -3.; A(2,3) = 2.;
  A(3,2) = 1.;
  A(4,1) = 4.; A(4,2) = 2.; A(4,4) = 1.;

  B(0) = 8.; B(1) = 45.; B(2) = -3.; B(3) = 3.; B(4) = 19.;

  umf::symbolic_type<double> Symbolic;
  umf::numeric_type<double> Numeric;

  umf::symbolic (A, Symbolic);
  umf::numeric (A, Symbolic, Numeric);
  umf::solve (A, X, B, Numeric);

  std::cout << X << std::endl;  // output: [5](1,2,3,4,5)
}


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
///
/// Code related to the phonon density fo states (DOS).

#include <qpoint_grid.hpp>
#include <boost/math/distributions/normal.hpp>

namespace alma {
/// Objects of this class handle the contribution of a mode to the
/// phonon DOS. We use an adaptive Gaussian smearing algorithm
/// to broaden the isolated modes.
class Gaussian_for_DOS {
public:
    /// Average energy.
    const double mu;
    /// Standard deviation.
    const double sigma;
    /// True if the usual lower bound would be negative.
    const bool truncated;
    /// Lower bound to the values that can be considered compatible
    /// with the average energy.
    const double lbound;
    /// Upper bound to the values that can be considered compatible
    /// with the average energy.
    const double ubound;
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] iq - q point index
    /// @param[in] im - branch index
    /// @param[in] scalebroad - prefactor for the standard
    /// deviation.
    Gaussian_for_DOS(const Gamma_grid& grid,
                     std::size_t iq,
                     std::size_t im,
                     double scalebroad)
        : mu(grid.get_spectrum_at_q(iq).omega(im)),
          sigma(scalebroad *
                grid.base_sigma(grid.get_spectrum_at_q(iq).vg.col(im))),
          truncated(mu - constants::nsigma * sigma < 0.),
          lbound(truncated ? 0. : mu - constants::nsigma * sigma),
          ubound(mu + constants::nsigma * sigma) {
        if ((this->mu == 0.) || (this->sigma == 0.))
            this->dist = nullptr;
        else
            this->dist = std::make_unique<boost::math::normal>(mu, sigma);
    }

    /// Get the amplitude of this contribution at a given frequency.
    ///
    /// @param[in] omega - an angular frequency in rad / ps
    /// @return the value of the Gaussian, normalized to 1 with
    /// respect to integration over omega.
    double get_contribution(double omega) const {
        if (this->dist)
            return boost::math::pdf(*(this->dist), omega);
        else
            return 0.;
    }

private:
    /// Underlying Gaussian distribution object.
    std::unique_ptr<boost::math::normal> dist;
};
} // namespace alma


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__MPC__CLTV_OCP_LIE_HPP_
#define CBR_CONTROL__MPC__CLTV_OCP_LIE_HPP_

#include <Eigen/Dense>

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

#include <cbr_math/lie/common.hpp>
#include <cbr_utils/utils.hpp>

#include <limits>
#include <utility>

#include "ocp_common.hpp"

namespace cbr
{

/* ---------------------------------------------------------------------------------------------- */
/*                          Lie group Optimal Control Problem Linearizer                          */
/* ---------------------------------------------------------------------------------------------- */

/**
 * @brief Linearize a nonlinear control problem defined on a Lie group to a continuous linear time-varying problem
 * @tparam lie_pb_t nonlinear problem on Lie group satisfying interface conditions
 * The difference from linearized trajectory is the Rn state
 *  a(t) = log( Xl(t)^{-1} * X(t) )
 *             <=>
 *  X(t) = Xl(t) * exp (a(t))
 *
 * We have that
 *  \dot a = [d^r exp_a]^{-1} * f(Xl * exp(a), ul + ue) - [d^l exp_a]^{-1} d^r Xl_t
 * which is a nonlinear system on R^{DoF}. Thus the linearized system is
 *  \dot a = A(t) a + B(t) u + E(t) for
 *
 *  A(t) = (d/da) [d^r exp_a]^{-1} * f(Xl * exp(a), ul + ue)   at a = 0, ue = 0
 *  B(t) = (d/du) [d^r exp_a]^{-1} * f(Xl * exp(a), ul + ue)   at a = 0, ue = 0
 *  E(t) = [d^r exp_a]^{-1} f(Xl * exp(a), ul + ue) - [d^l exp_a]^{-1} d^r Xl_t  at a = 0, ue = 0
 *
 * Setting to zero:
 *
 *  A(t) = (d/da) [d^r exp_a]^{-1} * f(Xl * exp(a), ul)  at a = 0
 *  B(t) = (d/u) f(Xl, ul)                               at u = ul
 *  E(t) = f(Xl, ul) - d^r Xl_t
 */
template<typename lie_pb_t>
class CltvOcpLie
{
public:
  using lie_t = typename lie_pb_t::state_t;
  using input_t = typename lie_pb_t::input_t;
  using state_t = typename lie_pb_t::deriv_t;     // linearized problem is defined in tangent space

  static constexpr std::size_t nx = state_t::SizeAtCompileTime;
  static constexpr std::size_t nu = input_t::SizeAtCompileTime;

  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using E_t = Eigen::Matrix<double, nx, 1>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Get return type of problem functions
  using T_t = std::result_of_t<decltype(&lie_pb_t::get_T)(lie_pb_t)>;
  using x0_t = std::result_of_t<decltype(&lie_pb_t::get_x0)(lie_pb_t)>;
  using xlr_t = std::result_of_t<decltype(&lie_pb_t::get_xl)(lie_pb_t, double)>;
  using ulr_t = std::result_of_t<decltype(&lie_pb_t::get_ul)(lie_pb_t, double)>;
  using xdr_t = std::result_of_t<decltype(&lie_pb_t::get_xd)(lie_pb_t, double)>;
  using udr_t = std::result_of_t<decltype(&lie_pb_t::get_ud)(lie_pb_t, double)>;
  using Qr_t = std::result_of_t<decltype(&lie_pb_t::get_Q)(lie_pb_t, double)>;
  using QTr_t = std::result_of_t<decltype(&lie_pb_t::get_QT)(lie_pb_t)>;
  using Rr_t = std::result_of_t<decltype(&lie_pb_t::get_R)(lie_pb_t, double)>;

  // Check return type of problem functions
  static_assert(
    std::is_same_v<std::decay_t<T_t>, double>,
    "The get_xl method of the problem must return a double.");
  static_assert(
    std::is_same_v<std::decay_t<x0_t>, lie_t>,
    "The get_x0 method of the problem must return the group type (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<xlr_t>, lie_t>,
    "The get_x0 method of the problem must return the group type (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<ulr_t>, input_t>,
    "The get_ul method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<xdr_t>, lie_t>,
    "The get_x0 method of the problem must return the group type (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<udr_t>, input_t>,
    "The get_ud method of the problem must return an nu*1 Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Qr_t>, Q_t>,
    "The get_Q method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<Rr_t>, R_t>,
    "The get_R method of the problem must return an nu*nu Eigen::Matrix (or a reference to one).");
  static_assert(
    std::is_same_v<std::decay_t<QTr_t>, Q_t>,
    "The get_QT method of the problem must return an nx*nx Eigen::Matrix (or a reference to one).");

  // Check problem dimensions
  static_assert(nx > 0, "Number of states must be > 0.");
  static_assert(nu > 0, "Number of inputs must be > 0.");

public:
  CltvOcpLie() = delete;
  CltvOcpLie(const CltvOcpLie &) = default;
  CltvOcpLie(CltvOcpLie &&) = default;
  CltvOcpLie & operator=(const CltvOcpLie &) = default;
  CltvOcpLie & operator=(CltvOcpLie &&) = default;

  explicit CltvOcpLie(const lie_pb_t & pb)
  : nl_pb_(pb)
  {}

  explicit CltvOcpLie(lie_pb_t && pb)
  : nl_pb_(std::move(pb))
  {}

  template<typename T1>
  CltvOcpLie(T1 && pb)
  : nl_pb_(std::forward<T1>(pb))
  {}

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    x0 = (nl_pb_.get_xl(0.).inverse() * nl_pb_.get_x0()).log();
  }

  void get_T(double & T) const
  {
    T = nl_pb_.get_T();
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    // state bounds not supported for now
    state_lb.setConstant(-std::numeric_limits<double>::infinity());
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    // state bounds not supported for now
    state_ub.setConstant(std::numeric_limits<double>::infinity());
  }

  void get_input_lb(double t, Eigen::Ref<input_t> input_lb) const
  {
    nl_pb_.get_input_lb(t, input_lb);
    input_lb -= nl_pb_.get_ul(t);
  }

  void get_input_ub(double t, Eigen::Ref<input_t> input_ub) const
  {
    nl_pb_.get_input_ub(t, input_ub);
    input_ub -= nl_pb_.get_ul(t);
  }

  A_t get_A(double t) const
  {
    using lie_ad_t = lie::detail::change_scalar_t<lie_t, autodiff::dual>;
    using tangent_ad_t = Eigen::Matrix<autodiff::dual, nx, 1>;

    const lie_ad_t xlin = nl_pb_.get_xl(t).template cast<autodiff::dual>();
    const input_t ulin = nl_pb_.get_ul(t);

    auto fx = [&](const tangent_ad_t & a) -> tangent_ad_t {
        return lie::dr_expinv<lie_ad_t>(a) * nl_pb_.get_f(xlin * lie_ad_t::exp(a), ulin);
      };

    tangent_ad_t a = tangent_ad_t::Zero();
    return autodiff::forward::jacobian(fx, autodiff::wrt(a), autodiff::forward::at(a));
  }

  B_t get_B(double t) const
  {
    using input_ad_t = Eigen::Matrix<autodiff::dual, nu, 1>;
    using tangent_ad_t = Eigen::Matrix<autodiff::dual, nx, 1>;

    const lie_t xlin = nl_pb_.get_xl(t);

    auto fu = [&](const input_ad_t & u) -> tangent_ad_t {
        return nl_pb_.get_f(xlin, u);
      };

    input_ad_t ulin = nl_pb_.get_ul(t);
    return autodiff::forward::jacobian(fu, autodiff::wrt(ulin), autodiff::forward::at(ulin));
  }

  E_t get_E(double t) const
  {
    const lie_t xlin = nl_pb_.get_xl(t);
    const input_t ulin = nl_pb_.get_ul(t);
    const typename lie_t::Tangent xlDot = nl_pb_.get_xldot(t);

    return nl_pb_.get_f(xlin, ulin) - xlDot;
  }

  Qr_t get_Q(double t) const
  {
    return nl_pb_.get_Q(t);
  }

  QTr_t get_QT() const
  {
    return nl_pb_.get_QT();
  }

  Rr_t get_R(double t) const
  {
    return nl_pb_.get_R(t);
  }

  state_t get_q(double t) const
  {
    const xlr_t xl = nl_pb_.get_xl(t);
    const xdr_t xd = nl_pb_.get_xd(t);
    const Qr_t Q = nl_pb_.get_Q(t);
    return (xd.inverse() * xl).log().transpose() * Q;
  }

  state_t get_qT() const
  {
    double T = nl_pb_.get_T();
    const xlr_t xl = nl_pb_.get_xl(T);
    const xdr_t xd = nl_pb_.get_xd(T);
    const QTr_t QT = nl_pb_.get_QT();
    return (xd.inverse() * xl).log().transpose() * QT;
  }

  input_t get_r(double t) const
  {
    const ulr_t ul = nl_pb_.get_ul(t);
    const udr_t ud = nl_pb_.get_ud(t);
    const Rr_t R = nl_pb_.get_R(t);
    return (ul - ud).transpose() * R;
  }

  lie_pb_t & problem()
  {
    return nl_pb_;
  }

protected:
  lie_pb_t nl_pb_{};
};

// Class template argument deduction guides
template<typename T>
CltvOcpLie(T)->CltvOcpLie<T>;

template<typename T1, typename T2>
CltvOcpLie(T1, T2)->CltvOcpLie<T1>;

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__CLTV_OCP_LIE_HPP_


/*
 * Math.cpp
 *
 *  Created on: Nov 3, 2010
 *      Author: dberrios
 */

#include "MathHelper.h"
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>
#include <cmath>
#include <algorithm>

namespace ccmc
{
	int Math::iminloc1d(int * array, long n, int * mask)
	{
	   long i,loc;
	   int min;
	   min = std::numeric_limits<int>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] < min) && (mask[i] > 0)) {
					 min=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::imaxloc1d(int * array, long n, int * mask)
	{
	   long i,loc;
	   int max;
	   max = boost::numeric::bounds<int>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] > max) && (mask[i] > 0)) {
					 max=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::fminloc1d(float * array, long n, int * mask)
	{
		long i,loc;
		float min;
		min = std::numeric_limits<float>::max( ); //if highest value possible in array is 99999999999
		for (i=0; i<n; i++)
		{
			if( (array[i] < min) && (mask[i] > 0))
			{
				min=array[i];
				loc=i;
			}
		}
		return loc;
	}

	int Math::fmaxloc1d(float * array, long n, int * mask)
	{
	   long i,loc;
	   float max;
	   max = boost::numeric::bounds<float>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] > max) && (mask[i] > 0)) {
					 max=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::dminloc1d(double * array, long n, int * mask)
	{
	   long i,loc;
	   double min;
	   min = std::numeric_limits<double>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] < min) && (mask[i] > 0)) {
					 min=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::dmaxloc1d(double * array, long n, int * mask)
	{
	   long i,loc;
	   double max;
	   max = boost::numeric::bounds<double>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if( (array[i] > max) && (mask[i] > 0)) {
					 max=array[i];
					 loc=i;
				  }
		   }
	   return loc;
	}

	int Math::ifindmin(int * array, long n)
	{
	   long i;
	   int min;
	   min = std::numeric_limits<int>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] < min)
					 min=array[i];
		   }
	   return min;
	}

	int Math::ifindmax(int * array, long n)
	{
	   long i;
	   int max;
	   max = boost::numeric::bounds<int>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] > max)
					 max=array[i];
		   }
	   return max;
	}

	long Math::lfindmin(long * array, long n)
	{
	   long i;
	   long min;
	   min = std::numeric_limits<long>::max( ); //if highest value possible in array is 99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] < min)
					 min=array[i];
		   }
	   return min;
	}

	long Math::lfindmax(long * array, long n)
	{
	   long i;
	   long max;
	   max = boost::numeric::bounds<long>::lowest(); //if lowest value possible in array is -99999999999
	   for (i=0; i<n; i++)
		   {
				  if(array[i] > max)
					 max=array[i];
		   }
	   return max;
	}

	float Math::ffindmin(float * array, long n)
	{
	   long i;
	   float min = std::numeric_limits<float>::max(); //if highest value possible in array is 1.e13
	   for (i=0; i<n; i++)
	   {
			  min = std::min(array[i], min);

	   }
	   return min;
	}

	float Math::ffindmax(float * array, long n)
	{
	   long i;
	   float max = -1.e20;
	   max = boost::numeric::bounds<float>::lowest(); //if lowest value possible in array is -1.e13
	   for (i=0; i<n; i++)
	   {
		   max = std::max((double)array[i] , (double)max);

	   }
	   return max;
	}

	double Math::dfindmin(double * array, long n)
	{
	   long i;
	   double min;
	   min = 1.e13; //if highest value possible in array is 1.e13
	   for (i=0; i<n; i++)
		   {
				  if(array[i] < min)
					 min=array[i];
		   }
	   return min;
	}

	double Math::dfindmax(double * array, long n)
	{
	   long i;
	   double max;
	   max = -1.e13; //if lowest value possible in array is -1.e13
	   for (i=0; i<n; i++)
		   {
				  if(array[i] > max)
					 max=array[i];
		   }
	   return max;
	}

	void Math::convert_xyz_to_rthetaphi(const float& x, const float& y, const float& z,
			float * r, float * t, float * p)
	{
	/*
	 Converts 3D cartesian coords (x,y,z) into spherical coords (r,theta,phi).
	*/

	      float  piov2,r2,zr,yx;

	      piov2 = std::acos(0.);

	      r2 = x*x+y*y+z*z;
	      if(r2 > 0.) {
	        *r  = std::sqrt(r2);
	        zr=z/ (*r);
	        *t = acos(zr);
	        if(x > 0.) {
	          yx=y/x;
	          *p = std::atan(yx);
	        } else if(x < 0.) {
	          yx=y/std::fabs(x);
	          *p = 2.*piov2 - atan(yx);
	        } else {
	          *p = piov2*sign(y);
	        }
	      } else {
	        *r = 0.;
	        *t = 0.;
	        *p = 0.;
	      }
	      if(*p < 0.) *p=*p+4.*piov2;               /* pmn testing */
	}

	void Math::convert_rthetaphi_to_xyz(const float& r, const float& t, const float& p,
			float * x, float * y, float * z)
	{
	/*
	  Converts spherical coords (r,theta,phi) into 3D cartesian coords (x,y,z)
	*/

	      float  sint,cost,sinp,cosp;

	      sint = std::sin(t);
	      cost = std::cos(t);
	      sinp = std::sin(p);
	      cosp = std::cos(p);

	      *x = r*sint*cosp;
	      *y = r*sint*sinp;
	      *z = r*cost;
	}

	int Math::sign(int v)
	{
	   return v > 0 ? 1 : (v < 0 ? -1 : 0);
	}

}