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

// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>
#include <votca/xtp/aomatrix.h>
#include <votca/xtp/bsecoupling.h>
#include <votca/tools/constants.h>
#include <boost/format.hpp>

namespace votca { namespace xtp {

namespace ub = boost::numeric::ublas;
using boost::format;

void BSECoupling::Initialize(Property* options){

    #if (GWBSE_DOUBLE)
        CTP_LOG(ctp::logDEBUG, *_pLog) <<  " Compiled with full double support" << flush;
    #else
        CTP_LOG(ctp::logDEBUG, *_pLog) <<  " Compiled with float/double mixture (standard)" << flush;
    #endif

    std::string key = Identify();
    _doSinglets=false;
    _doTriplets=false;
   _output_perturbation=false;

    _openmp_threads = 0;

    if ( options->exists( key + ".openmp") ) {
                 _openmp_threads = options->get(key + ".openmp").as<int> ();
            }

    string spintype   = options->get(key + ".spin").as<string> ();
        if(spintype=="all"){
            _doSinglets=true;
            _doTriplets=true;
        }
        else if(spintype=="triplet"){
            _doTriplets=true;
        }
        else if(spintype=="singlet"){
            _doSinglets=true;
        }
        else{
            throw std::runtime_error((boost::format("Choice % for type not known. Available singlet,triplet,all") % spintype).str());
        }
    _degeneracy = options->get(key + ".degeneracy").as<double> ();

   if ( options->exists( key + ".algorithm") ) {
                string algorithm = options->get(key + ".algorithm").as<string> ();
                 if(algorithm=="perturbation"){
                    _output_perturbation=true;
                 }
   }

        _levA  = options->get(key + ".moleculeA.states").as<int> ();
        _levB  = options->get(key + ".moleculeB.states").as<int> ();
        _occA  = options->get(key + ".moleculeA.occLevels").as<int> ();
        _occB  = options->get(key + ".moleculeB.occLevels").as<int> ();
        _unoccA  = options->get(key + ".moleculeA.unoccLevels").as<int> ();
        _unoccB  = options->get(key + ".moleculeB.unoccLevels").as<int> ();

}

void BSECoupling::addoutput(Property *_type_summary,Orbitals* _orbitalsA,
                               Orbitals* _orbitalsB){

    string algorithm="full_diag";
    int methodindex=1;
    if (_output_perturbation){
        algorithm="perturbation";
        methodindex=0;
    }
    _type_summary->setAttribute("algorithm",algorithm);
    if (_doSinglets){
        Property *_singlet_summary = &_type_summary->add("singlets","");
        for (int stateA = 0; stateA < _levA ; ++stateA ) {
           for (int stateB = 0; stateB <_levB ; ++stateB ) {
               double JAB = getSingletCouplingElement( stateA , stateB, methodindex);

               Property *_coupling_summary = &_singlet_summary->add("coupling", (format("%1$1.6e") % JAB).str());
               double energyA = _orbitalsA->BSESingletEnergies()(stateA)*conv::hrt2ev;
               double energyB = _orbitalsB->BSESingletEnergies()(stateB)*conv::hrt2ev;
               _coupling_summary->setAttribute("excitonA", stateA);
               _coupling_summary->setAttribute("excitonB", stateB);
               _coupling_summary->setAttribute("energyA", (format("%1$1.6e") % energyA).str());
               _coupling_summary->setAttribute("energyB", (format("%1$1.6e") % energyB).str());
               _coupling_summary->setAttribute("pert", (format("%1$1.6e") % getSingletCouplingElement( stateA , stateB, 0)).str());
               _coupling_summary->setAttribute("diag", (format("%1$1.6e") % getSingletCouplingElement( stateA , stateB, 1)).str());

           }
        }
    }

    if ( _doTriplets){
        Property *_triplet_summary = &_type_summary->add("triplets","");
        for (int stateA = 0; stateA < _levA ; ++stateA ) {
           for (int stateB = 0; stateB < _levA ; ++stateB ) {
               double JAB = getTripletCouplingElement( stateA , stateB,methodindex );
               //real_gwbse energyAD = getTripletDimerEnergy( stateA  );
               //real_gwbse energyBD = getTripletDimerEnergy( stateB  );
               Property *_coupling_summary = &_triplet_summary->add("coupling", (format("%1$1.6e") % JAB).str());
               double energyA = _orbitalsA->BSETripletEnergies()(stateA)*conv::hrt2ev;
               double energyB = _orbitalsB->BSETripletEnergies()(stateB)*conv::hrt2ev;
               _coupling_summary->setAttribute("excitonA", stateA);
               _coupling_summary->setAttribute("excitonB", stateB);
               _coupling_summary->setAttribute("energyA", (format("%1$1.6e") % energyA).str());
               _coupling_summary->setAttribute("energyB", (format("%1$1.6e") % energyB).str());
               _coupling_summary->setAttribute("pert", (format("%1$1.6e") % getTripletCouplingElement( stateA , stateB, 0)).str());
               _coupling_summary->setAttribute("diag", (format("%1$1.6e") % getTripletCouplingElement( stateA , stateB, 1)).str());

           }
        }
    }
}

double BSECoupling::getSingletCouplingElement( int levelA, int levelB, int methodindex) {
    return JAB_singlet[methodindex]( levelA  , levelB +  _levA ) * votca::tools::conv::hrt2ev;
}

double BSECoupling::getTripletCouplingElement( int levelA, int levelB, int methodindex) {

    return JAB_triplet[methodindex]( levelA  , levelB + _levA ) * votca::tools::conv::hrt2ev;
}

/**
 * \brief evaluates electronic couplings
 *
 * @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 BSECoupling::CalculateCouplings(Orbitals* _orbitalsA, Orbitals* _orbitalsB, Orbitals* _orbitalsAB) {
       CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Calculating exciton couplings" << flush;
     // set the parallelization
    #ifdef _OPENMP

    if ( _openmp_threads > 0 ) omp_set_num_threads(_openmp_threads);
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << " Using "<< omp_get_max_threads()<<" threads" << flush;
    #endif

    _orbitalsAB->setCoupledExcitonsA(_levA);
    _orbitalsAB->setCoupledExcitonsB(_levB);
    //check to see if ordering of atoms agrees
    const std::vector<ctp::QMAtom*> atomsA=_orbitalsA->QMAtoms();
    const std::vector<ctp::QMAtom*> atomsB=_orbitalsB->QMAtoms();
    const std::vector<ctp::QMAtom*> atomsAB=_orbitalsAB->QMAtoms();

    for (unsigned i=0;i<atomsAB.size();i++){
        ctp::QMAtom* dimer=atomsAB[i];
        ctp::QMAtom* monomer=NULL;
        if (i<atomsA.size()){
            monomer=atomsA[i];
        }
        else if (i<atomsB.size()+atomsA.size() ){
            monomer=atomsB[i-atomsA.size()];
        }
        else{
            throw runtime_error((boost::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 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(ctp::logERROR,*_pLog) << "======WARNING=======\n Coordinates of monomers and dimer atoms do not agree, do you know what you are doing?\n " << flush;
            break;
        }

    }

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

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

    // number of levels stored in monomers
    int _levelsA = _orbitalsA->getNumberOfLevels();
    int _levelsB = _orbitalsB->getNumberOfLevels();

    // get exciton information of molecule A
    int _bseA_cmax        = _orbitalsA->getBSEcmax();
    int _bseA_cmin        = _orbitalsA->getBSEcmin();
    int _bseA_vmax        = _orbitalsA->getBSEvmax();
    int _bseA_vmin        = _orbitalsA->getBSEvmin();
    int _bseA_vtotal      = _bseA_vmax - _bseA_vmin +1 ;
    int _bseA_ctotal      = _bseA_cmax - _bseA_cmin +1 ;
    int _bseA_size        = _bseA_vtotal * _bseA_ctotal;
    int _bseA_singlet_exc = _orbitalsA->BSESingletCoefficients().size2();
    int _bseA_triplet_exc = _orbitalsA->BSETripletCoefficients().size2();

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   molecule A has " << _bseA_singlet_exc << " singlet excitons with dimension " << _bseA_size << flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   molecule A has " << _bseA_triplet_exc << " triplet excitons with dimension " << _bseA_size << flush;

    // now, two storage assignment matrices for two-particle functions
    ub::matrix<int> _combA;
    _combA.resize(_bseA_size,2);
    int _cnt = 0;
    for ( int _v = 0; _v < _bseA_vtotal; _v++){
        for ( int _c = 0; _c < _bseA_ctotal; _c++){
            _combA(_cnt,0) = _v;
            _combA(_cnt,1) = _bseA_vtotal + _c;
            _cnt++;
        }
    }

    // get exciton information of molecule B
    int _bseB_cmax        = _orbitalsB->getBSEcmax();
    int _bseB_cmin        = _orbitalsB->getBSEcmin();
    int _bseB_vmax        = _orbitalsB->getBSEvmax();
    int _bseB_vmin        = _orbitalsB->getBSEvmin();
    int _bseB_vtotal      = _bseB_vmax - _bseB_vmin +1 ;
    int _bseB_ctotal      = _bseB_cmax - _bseB_cmin +1 ;
    int _bseB_size        = _bseB_vtotal * _bseB_ctotal;
    int _bseB_singlet_exc = _orbitalsB->BSESingletCoefficients().size2();
    int _bseB_triplet_exc = _orbitalsB->BSETripletCoefficients().size2();

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   molecule B has " << _bseB_singlet_exc << " singlet excitons with dimension " << _bseB_size << flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   molecule B has " << _bseB_triplet_exc << " triplet excitons with dimension " << _bseB_size << flush;

    // now, two storage assignment matrices for two-particle functions
    ub::matrix<int> _combB;
    _combB.resize(_bseB_size,2);
    _cnt = 0;
    for ( int _v = 0; _v < _bseB_vtotal; _v++){
        for ( int _c = 0; _c < _bseB_ctotal; _c++){
            _combB(_cnt,0) = _bseA_vtotal + _bseA_ctotal + _v;
            _combB(_cnt,1) = _bseA_vtotal + _bseA_ctotal + _bseB_vtotal + _c;
            _cnt++;
        }
    }

    if(_levA>_bseA_singlet_exc){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of excitons you want is greater than stored for molecule A. Setting to max number available" << flush;
        _levA=_bseA_singlet_exc;
    }
    if(_levB>_bseB_singlet_exc){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of excitons you want is greater than stored for molecule B. Setting to max number available" << flush;
        _levB=_bseB_singlet_exc;
    }

    if(_levA>_bseA_singlet_exc){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of Frenkel states you want is greater than stored for molecule A. Setting to max number available" << flush;
        _levA=_bseA_singlet_exc;
    }
    if(_levB>_bseB_singlet_exc){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of Frenkel states you want is greater than stored for molecule B. Setting to max number available" << flush;
        _levB=_bseB_singlet_exc;
    }

    if(_unoccA>_bseA_ctotal){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of occupied orbitals in molecule A for CT creation exceeds number of KS-orbitals in BSE" << flush;
        _unoccA=_bseA_ctotal;
    }
    else if (_unoccA<0){
        _unoccA=_bseA_ctotal;
    }
    if(_unoccB>_bseB_ctotal){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of occupied orbitals in molecule B for CT creation exceeds number of KS-orbitals in BSE" << flush;
        _unoccB=_bseB_ctotal;
    }
    else if (_unoccB<0){
        _unoccB=_bseB_ctotal;
    }

    if(_occA>_bseA_vtotal){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of unoccupied orbitals in molecule A for CT creation exceeds number of KS-orbitals in BSE" << flush;
        _occA=_bseA_vtotal;
    }
    else if (_occA<0){
        _occA=_bseA_vtotal;
    }
    if(_occB>_bseB_vtotal){
        CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Number of unoccupied orbitals in molecule B for CT creation exceeds number of KS-orbitals in BSE" << flush;
        _occB=_bseB_vtotal;
    }else if (_occB<0){
        _occB=_bseB_vtotal;
    }

    // get exciton information of pair AB
    int _bseAB_cmax = _orbitalsAB->getBSEcmax();
    int _bseAB_cmin = _orbitalsAB->getBSEcmin();
    int _bseAB_vmax = _orbitalsAB->getBSEvmax();
    int _bseAB_vmin = _orbitalsAB->getBSEvmin();
    int _bseAB_vtotal = _bseAB_vmax - _bseAB_vmin +1 ;
    int _bseAB_ctotal = _bseAB_cmax - _bseAB_cmin +1 ;
    int _bseAB_size   = _bseAB_vtotal * _bseAB_ctotal;
    // check if electron-hole interaction matrices are stored
    if ( ! _orbitalsAB->hasEHinteraction() ){
        CTP_LOG(ctp::logERROR,*_pLog) << "BSE EH int not stored in dimer " << flush;
        return false;
    }
    const ub::matrix<real_gwbse>&    _eh_d = _orbitalsAB->eh_d();
    const ub::matrix<real_gwbse>&    _eh_x = _orbitalsAB->eh_x();

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   dimer AB has BSE EH interaction (direct)   with dimension " << _eh_d.size1() << " x " <<  _eh_d.size2() << flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   dimer AB has BSE EH interaction (exchange) with dimension " << _eh_x.size1() << " x " <<  _eh_x.size2() << flush;
    // now, two storage assignment matrices for two-particle functions
    ub::matrix<int> _combAB;
    _combAB.resize(_bseAB_size,2);
    _cnt = 0;
    for ( int _v = 0; _v < _bseAB_vtotal; _v++){
        for ( int _c = 0; _c < _bseAB_ctotal; _c++){
            //_combAB(_cnt,0) = _v;
            //_combAB(_cnt,1) = _bseAB_vtotal + _c;

            _combAB(_cnt,0) = _bseAB_vmin + _v;
            _combAB(_cnt,1) = _bseAB_vmin + _bseAB_vtotal + _c;

            _cnt++;
        }
    }

    // DFT levels of monomers can be reduced to those used in BSE
    _levelsA = _bseA_vtotal + _bseA_ctotal;
    _levelsB = _bseB_vtotal + _bseB_ctotal;
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   levels used in BSE of molA: " << _bseA_vmin << " to " << _bseA_cmax << " total: " << _bseA_vtotal + _bseA_ctotal <<  flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   levels used in BSE of molB: " << _bseB_vmin << " to " << _bseB_cmax << " total: " << _bseB_vtotal + _bseB_ctotal <<  flush;

    if ( ( _levelsA == 0 ) || (_levelsB == 0) ) {
        CTP_LOG(ctp::logERROR,*_pLog) << "No information about number of occupied/unoccupied levels is stored" << 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  );

    // constructing merged orbitals
    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 ) ) = ub::project( _orbitalsA->MOCoefficients() , ub::range(_bseA_vmin, _bseA_cmax+1) , ub::range ( 0, _basisA ));
    ub::project( _psi_AxB, ub::range (_levelsA, _levelsA + _levelsB ), ub::range ( _basisA, _basisA + _basisB ) ) = ub::project( _orbitalsB->MOCoefficients(), ub::range(_bseB_vmin, _bseB_cmax+1) , ub::range ( 0, _basisB ));

    // psi_AxB * S_AB * psi_AB
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   projecting monomer onto dimer orbitals" << flush;

     ub::matrix<double> _overlapAB;
    if ( !_orbitalsAB->hasAOOverlap() ) {
            CTP_LOG(ctp::logDEBUG,*_pLog) << "Reading overlap matrix from orbitals" << flush;
           _overlapAB= _orbitalsAB->AOOverlap();
    }else{
        CTP_LOG(ctp::logDEBUG,*_pLog) << "Calculating overlap matrix for basisset: "<< _orbitalsAB->getDFTbasis()<< flush;
        BasisSet _dftbasisset;
        AOBasis _dftbasis;
        _dftbasisset.LoadBasisSet(_orbitalsAB->getDFTbasis());

        _dftbasis.AOBasisFill(&_dftbasisset, _orbitalsAB->QMAtoms());
        AOOverlap _dftAOoverlap;
        _dftAOoverlap.Fill(_dftbasis);
        _overlapAB=_dftAOoverlap.Matrix();
    }

    ub::matrix<double> _psi_AB = ub::prod( _overlapAB,ub::trans(_orbitalsAB->MOCoefficients()) );
    ub::matrix<double> _psi_AxB_dimer_basis = ub::prod( _psi_AxB, _psi_AB );
    _psi_AB.resize(0,0);
    _overlapAB.resize(0,0);
    //cout<< "_psi_AxB_dimer"<<endl;
    unsigned int LevelsA = _levelsA;
    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){
            int monomer = 0;
            int level = 0;
            if ( i < LevelsA ) {
                monomer = 1;
                level   = _bseA_vmin + i;
            } else {
                monomer = 2;
                level   = _bseB_vmin + i -_levelsA;

            }
            CTP_LOG(ctp::logERROR,*_pLog) << "\nERROR: " << i << " Projection of orbital " << level << " of monomer " << monomer << " on dimer is insufficient,mag="<<mag<<" maybe the orbital order is screwed up, otherwise increase dimer basis.\n"<<flush;
        }
    }

    //notation AB is CT states with A+B-, BA is the counterpart
    //Setting up CT-states:
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   Setting up CT-states" << flush;
    //Number of A+B- states
    int noAB=_occA*_unoccB;
    //Number of A-B+ states
    int noBA=_unoccA*_occB;

    ub::matrix<int> comb_CTAB;
    comb_CTAB.resize(noAB,2);
    _cnt = 0;

    // iterate A over occupied, B over unoccupied
    int v_start=_bseA_vtotal-_occA;
    for ( int _v = v_start; _v < _bseA_vtotal; _v++){
        for ( int _c = 0; _c <_unoccB; _c++){
            comb_CTAB(_cnt,0) =_v;
            comb_CTAB(_cnt,1) = _bseA_vtotal+_bseA_ctotal+_bseB_vtotal + _c;

            _cnt++;
        }
    }
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  <<"  "<<noAB <<" CT states A+B- created" << flush;

    ub::matrix<int> comb_CTBA;
    comb_CTBA.resize(noBA,2);
    _cnt = 0;
    // iterate A over unoccupied, B over occupied
    v_start=_bseB_vtotal-_occB;
    for ( int _v = v_start; _v < _bseB_vtotal; _v++){
        for ( int _c = 0; _c <_unoccA; _c++){
            comb_CTBA(_cnt,0) =_bseA_vtotal+_bseA_ctotal+_v;
            comb_CTBA(_cnt,1) = _bseA_vtotal+ _c;

            _cnt++;
        }
    }
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  <<"  "<<noBA <<" CT states B+A- created" << flush;

    // these 4 matrixes, matrix(i,j) contains the j-th dimer MO component of the i-th excitation

    ctAB.resize(noAB,_bseAB_size);
    #pragma omp parallel for
    for ( int _i_CT = 0 ; _i_CT < noAB ; _i_CT++){
    for ( int _i_bseAB = 0 ; _i_bseAB < _bseAB_size ; _i_bseAB++){
        ctAB(_i_CT,_i_bseAB)=_psi_AxB_dimer_basis( comb_CTAB(_i_CT,0), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( comb_CTAB(_i_CT,1), _combAB( _i_bseAB,1) );
        }
    }

    ctBA.resize(noBA,_bseAB_size);
    #pragma omp parallel for
    for ( int _i_CT = 0 ; _i_CT < noBA ; _i_CT++){
    for ( int _i_bseAB = 0 ; _i_bseAB < _bseAB_size ; _i_bseAB++){
        ctBA(_i_CT,_i_bseAB)=_psi_AxB_dimer_basis( comb_CTBA(_i_CT,0), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( comb_CTBA(_i_CT,1), _combAB( _i_bseAB,1) );
        }
    }

    // some more convenient storage

    _kap.resize(_bseA_size,_bseAB_size);
    #pragma omp parallel for
    for ( int _i_bseA = 0 ; _i_bseA < _bseA_size ; _i_bseA++){
        for ( int _i_bseAB = 0 ; _i_bseAB < _bseAB_size ; _i_bseAB++){
            _kap(_i_bseA,_i_bseAB) = _psi_AxB_dimer_basis( _combA(_i_bseA,0), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( _combA(_i_bseA,1), _combAB( _i_bseAB,1) );

        }
    }

    _kbp.resize(_bseB_size,_bseAB_size);
    #pragma omp parallel for
    for ( int _i_bseB = 0 ; _i_bseB < _bseB_size ; _i_bseB++){
        for ( int _i_bseAB = 0 ; _i_bseAB < _bseAB_size ; _i_bseAB++){
            _kbp(_i_bseB,_i_bseAB) = _psi_AxB_dimer_basis( _combB(_i_bseB,0), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( _combB(_i_bseB,1), _combAB( _i_bseAB,1) );
        }
    }

    // Same routines but also take <v|c'> <c|v'> projections into account
    /*

    _kap.resize(_bseA_size,_bseAB_size);
    for ( int _i_bseA = 0 ; _i_bseA < _bseA_size ; _i_bseA++){
        for ( int _i_bseAB = 0 ; _i_bseAB < _bseAB_size ; _i_bseAB++){
            _kap(_i_bseA,_i_bseAB) = _psi_AxB_dimer_basis( _combA(_i_bseA,0), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( _combA(_i_bseA,1), _combAB( _i_bseAB,1) )+
              _psi_AxB_dimer_basis( _combA(_i_bseA,1), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( _combA(_i_bseA,0), _combAB( _i_bseAB,1) )     ;

        }
    }

    _kbp.resize(_bseB_size,_bseAB_size);
    for ( int _i_bseB = 0 ; _i_bseB < _bseB_size ; _i_bseB++){
        for ( int _i_bseAB = 0 ; _i_bseAB < _bseAB_size ; _i_bseAB++){
            _kbp(_i_bseB,_i_bseAB) = _psi_AxB_dimer_basis( _combB(_i_bseB,0), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( _combB(_i_bseB,1), _combAB( _i_bseAB,1) )+
                    _psi_AxB_dimer_basis( _combB(_i_bseB,1), _combAB( _i_bseAB,0) ) * _psi_AxB_dimer_basis( _combB(_i_bseB,0), _combAB( _i_bseAB,1) );
        }
    }
    */

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()   << "   construct projection of product functions " << flush;

    //     cout << "Size of _kap " << _kap.size1() << " : " <<  _kap.size2() << "\n" << flush;
    //     cout << "Size of _kbp " << _kbp.size1() << " : " <<  _kbp.size2() << "\n" << flush;
    _psi_AxB_dimer_basis.resize(0,0);
    _combAB.resize(0,0);
    _combA.resize(0,0);
    _combB.resize(0,0);
    // now the different spin types
            if (_doSinglets) {
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Evaluating singlets" << flush;
                // get singlet BSE Hamiltonian from _orbitalsAB
                ub::matrix<double> _Hamiltonian_AB = _eh_d + 2.0 * _eh_x;
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Setup Hamiltonian" << flush;
                ub::matrix<real_gwbse> temp = ub::project(_orbitalsA->BSESingletCoefficients(),
                        ub::range(0, _orbitalsA->BSESingletCoefficients().size1()), ub::range(0, _levA));

                const ub::matrix<double> _bseA_T = ub::trans(temp);
                temp = ub::project(_orbitalsB->BSESingletCoefficients(),
                        ub::range(0, _orbitalsB->BSESingletCoefficients().size1()), ub::range(0, _levB));
                const ub::matrix<double> _bseB_T = ub::trans(temp);
                temp.resize(0, 0);

                JAB_singlet = ProjectExcitons(_bseA_T, _bseB_T, _Hamiltonian_AB);
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   calculated singlet couplings " << flush;
            }

            if (_doTriplets) {
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Evaluating triplets" << flush;
                // get triplet BSE Hamiltonian from _orbitalsAB
                ub::matrix<double> _Hamiltonian_AB = _eh_d;
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "  Converted Hamiltonian to double" << flush;
                ub::matrix<real_gwbse> temp = ub::project(_orbitalsA->BSETripletCoefficients(),
                        ub::range(0, _orbitalsA->BSETripletCoefficients().size1()), ub::range(0, _levA));
                const ub::matrix<double> _bseA_T = ub::trans(temp);
                temp = ub::project(_orbitalsB->BSETripletCoefficients(), ub::range(0, _orbitalsB->BSETripletCoefficients().size1()), ub::range(0, _levB));
                const ub::matrix<double> _bseB_T = ub::trans(temp);
                temp.resize(0, 0);

                JAB_triplet = ProjectExcitons(_bseA_T, _bseB_T, _Hamiltonian_AB);
                CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   calculated triplet couplings " << flush;
            }

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "  Done with exciton couplings" << flush;
    return true;
};

std::vector< ub::matrix<double> > BSECoupling::ProjectExcitons(const ub::matrix<double>& _bseA_T, const ub::matrix<double>& _bseB_T,
                                  ub::matrix<double>& _H){

     // get projection of monomer excitons on dimer product functions
     ub::matrix<double> _proj_excA = ub::prod( _bseA_T, _kap);
     ub::matrix<double> _proj_excB = ub::prod( _bseB_T, _kbp);

     _bseA_exc = _proj_excA.size1();
     _bseB_exc = _proj_excB.size1();
     _bse_exc=_bseA_exc+_bseB_exc;

     unsigned _ctAB=ctAB.size1();

     unsigned _ctBA=ctBA.size1();
     _ct=_ctAB+_ctBA;
     unsigned nobasisfunc=_H.size1();

     ub::matrix<double> fe_states=ub::matrix<double>(_bse_exc,nobasisfunc);
     ub::project(fe_states, ub::range ( 0, _bseA_exc ),ub::range (0, nobasisfunc )  )=_proj_excA;
     ub::project(fe_states, ub::range ( _bseA_exc, _bse_exc ),ub::range (0, nobasisfunc )  )=_proj_excB;

     ub::matrix<double> ct_states=ub::matrix<double>(_ct,nobasisfunc);

     //cout<< _ct<< "ct states"<<endl;
    if(_ct>0){
     //orthogonalize ct-states with respect to the FE states.
       CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << " Orthogonalizing CT-states with respect to FE-states" << flush;

     if(_ctAB>0){
     ub::project(ct_states, ub::range ( 0 , _ctAB ) ,ub::range (0,nobasisfunc ) )=ctAB;
    }
    if(_ctBA>0){
     ub::project(ct_states, ub::range ( _ctAB, _ct ),ub::range (0, nobasisfunc )  )=ctBA;
     }

        //orthogonalize ct-states with respect to FE states
     ub::matrix<double> overlaps=ub::prod(fe_states,ub::trans(ct_states));

     //cout << "overlap"<< overlaps.size1()<< "x"<<overlaps.size2()<<endl;
     //cout << overlaps<<endl;
     ub::matrix<double> correction=ub::prod(ub::trans(overlaps),fe_states);
     //cout << "correction"<< correction.size1()<< "x"<<correction.size2()<<endl;
    // cout << "ct_states"<< ct_states.size1()<< "x"<<ct_states.size2()<<endl;

    ct_states=ct_states-correction;

     overlaps.resize(0,0);
     correction.resize(0,0);
     //normalize

     for (unsigned i=0;i<_ct;i++){
         double norm=0.0;
         for (unsigned j=0;j<nobasisfunc;j++){
         norm+=ct_states(i,j)*ct_states(i,j);
         }
         //cout << "norm ["<<i<<"]:" <<norm<<endl;
         norm=1/std::sqrt(norm);
         if(norm<0.95){
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << " WARNING: CT-state "<< i<< " norm is only"<< norm << flush;
         }
         for (unsigned j=0;j<nobasisfunc;j++){
            ct_states(i,j)=norm*ct_states(i,j);
         }

     }
    //cout <<ub::prod(fe_states,ub::trans(ct_states))<<endl;

    }

     ub::matrix<double> projection =ub::zero_matrix<double>(_bse_exc+_ct,nobasisfunc);

     CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << " merging projections into one vector  " << flush;

  ub::project(projection, ub::range (0 , _bse_exc) ,ub::range (0,nobasisfunc ) )=fe_states;

     if(_ct>0){
    ub::project(projection, ub::range ( _bse_exc , _bse_exc+_ct ) ,ub::range (0,nobasisfunc ) )=ct_states;
     }
      CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   Setting up coupling matrix size "<< _bse_exc +_ct<<"x"<<_bse_exc +_ct << flush;
     // matrix _J

    //  E_A         J_AB        J_A_ABCT        J_A_BACT
    //  J_BA        E_B         J_B_ABCT        J_B_BACT
    //  J_ABCT_A    J_ABCT_B    E_ABCT          J_ABCT_BACT
    //  J_BACT_A   J_BACT_B    J_BACT_ABCT     E_BACT

     // I think this only works for hermitian/symmetric H so only in TDA
     // setup J

     ub::matrix<double> _temp=ub::prod(_H,ub::trans(projection));
     _H.resize(0,0);
     ub::matrix<double> _J_dimer=ub::prod(projection,_temp);
     _temp.resize(0,0);

    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   Setting up overlap matrix size "<< _bse_exc +_ct<<"x"<<_bse_exc +_ct << flush;
     // setup S

    ub::matrix<double> _S_dimer=ub::prod(projection,ub::trans(projection));

    projection.resize(0,0);
    if(tools::globals::verbose &&  _bse_exc+_ct<100){
         CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------"<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << "_J_dimer[Ryd]"<<flush;

     CTP_LOG(ctp::logDEBUG, *_pLog) << _J_dimer<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << "_S_dimer"<<flush;

     CTP_LOG(ctp::logDEBUG, *_pLog) << _S_dimer<<flush;
      CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------"<<flush;
    }

    double small=linalg_loewdin(_J_dimer,_S_dimer);

    if(tools::globals::verbose && _bse_exc+_ct<100){
         CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------"<<flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << "_J_ortho[Ryd]"<<flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << _J_dimer<<flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << "_S-1/2"<<flush;
    CTP_LOG(ctp::logDEBUG, *_pLog) << _S_dimer<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------"<<flush;
    }
     CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   Smallest value of dimer overlapmatrix is "<< small<< flush;

    std::vector< ub::matrix<double> >_J;

     CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "   Running Perturbation algorithm"<< flush;
    _J.push_back( Perturbation(_J_dimer));
    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp()  << "    Running Projection algorithm"<< flush;
    _J.push_back( Fulldiag(_J_dimer));

       if(tools::globals::verbose){
     CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------"<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << "Jeff_pert[Hrt]"<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << _J[0]<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << "Jeff_diag[Hrt]"<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << _J[1]<<flush;
     CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------"<<flush;
     }

     return _J;
}

ub::matrix<double> BSECoupling::Perturbation(const ub::matrix<double>& _J_dimer){

    ub::matrix<double> _J = ub::zero_matrix<double>(_bse_exc, _bse_exc);
    bool _diag_ct = true;
    ub::matrix<double> _J_result=_J_dimer;
    if (_ct > 0 && _diag_ct) {

        ub::matrix<double> transformation = ub::identity_matrix<double>(_bse_exc + _ct, _bse_exc + _ct);
        ub::vector<double> eigenvalues_ct;

        ub::matrix<double> Ct = ub::project(_J_dimer, ub::range(_bse_exc, _bse_exc + _ct), ub::range(_bse_exc, _bse_exc + _ct));
        linalg_eigenvalues(eigenvalues_ct, Ct);
        ub::project(transformation, ub::range(_bse_exc, _bse_exc + _ct), ub::range(_bse_exc, _bse_exc + _ct)) = Ct;

        Ct.resize(0, 0);

        if (tools::globals::verbose) {

            CTP_LOG(ctp::logDEBUG, *_pLog) << "FE state hamiltonian" << flush;
            CTP_LOG(ctp::logDEBUG, *_pLog) << ub::project(_J_dimer, ub::range(0, _bse_exc), ub::range(0, _bse_exc)) << flush;
            if (_ct > 0) {
                CTP_LOG(ctp::logDEBUG, *_pLog) << "eigenvalues of CT states" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << eigenvalues_ct << flush;
            }

        }

        ub::matrix<double> _temp = ub::prod(_J_dimer, transformation);
        _J_result = ub::prod(ub::trans(transformation), _temp);
        if (tools::globals::verbose && _bse_exc + _ct < 100) {
            CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------" << flush;
            CTP_LOG(ctp::logDEBUG, *_pLog) << "_J_ortho[Hrt] CT-state diag" << flush;
            CTP_LOG(ctp::logDEBUG, *_pLog) << _J_result << flush;
            CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------" << flush;
        }
    }
    for (int stateA = 0; stateA < _levA; stateA++) {
        double Ea = _J_result(stateA, stateA);
        for (int stateB = 0; stateB < _levB; stateB++) {
            int stateBd = stateB + _bseA_exc;
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Calculating coupling between exciton A" << stateA + 1 << " and exciton B" << stateB + 1 << flush;
            double J = _J_result(stateA, stateBd);

            double Eb = _J_result(stateBd, stateBd);
            for (unsigned k = _bse_exc; k < (_bse_exc + _ct); k++) {
                double Eab = _J_result(k, k);
                if (std::abs(Eab - Ea) < 0.001) {
                    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "Energydifference between state A " << stateA + 1 << "and CT state " << k + 1 << " is " << Eab - Ea << "[Hrt]" << flush;
                }
                if (std::abs(Eab - Eb) < 0.001) {
                    CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "Energydifference between state B " << stateB + 1 << "and CT state " << k + 1 << " is " << Eab - Eb << "[Hrt]" << flush;

                }
                J += 0.5 * _J_result(k, stateA) * _J_result(k, stateBd)*(1 / (Ea - Eab) + 1 / (Eb - Eab)); // Have no clue why 0.5
            }
            _J(stateA, stateBd) = J;
            _J(stateBd, stateA) = J;

        }
    }

    return _J;
}

ub::matrix<double> BSECoupling::Fulldiag(const ub::matrix<double>& _J_dimer){
    ub::matrix<double> _J = ub::zero_matrix<double>(_bse_exc, _bse_exc);

    ub::vector<double> _J_eigenvalues;
    ub::matrix<double> J_eigenvectors;

    linalg_eigenvalues(_J_dimer,_J_eigenvalues, J_eigenvectors);
    if (tools::globals::verbose && _bse_exc + _ct < 10) {
        CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------" << flush;
        CTP_LOG(ctp::logDEBUG, *_pLog) << "Eigenvectors of J" << flush;

        CTP_LOG(ctp::logDEBUG, *_pLog) << J_eigenvectors << flush;
        CTP_LOG(ctp::logDEBUG, *_pLog) << "J_eigenvalues[Hrt]" << flush;
        CTP_LOG(ctp::logDEBUG, *_pLog) << _J_eigenvalues << flush;
        CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------" << flush;
    }
    //Calculate projection on subspace for every pair of excitons separately
    for (int stateA = 0; stateA < _levA; stateA++) {
        for (int stateB = 0; stateB < _levB; stateB++) {
            int stateBd = stateB + _bseA_exc;
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Calculating coupling between exciton A" << stateA + 1 << " and exciton B" << stateB + 1 << flush;
            std::vector<unsigned> index;
            std::vector<int> signvec;
            for (unsigned i = 0; i < _bse_exc + _ct; i++) {
                if (i == unsigned(stateA) || i == unsigned(stateBd)) {

                    double close = 0.0;
                    unsigned ind = 0;
                    int sign = 0;
                    //row
                    for (unsigned j = 0; j < _bse_exc + _ct; j++) {
                        bool check = true;
                        // if index i is already in index
                        // should not happen but if one vector was similar to two others.
                        for (unsigned l = 0; l < index.size(); l++) {
                            if (j == index[l]) {
                                check = false;
                                break;
                            }
                        }

                        if (check && std::abs(J_eigenvectors(i, j)) > close) {
                            ind = j;
                            close = std::abs(J_eigenvectors(i, j));
                            if (J_eigenvectors(i, j) >= 0) {
                                sign = 1;
                            } else {
                                sign = -1;
                            }
                        }
                    }
                    index.push_back(ind);
                    signvec.push_back(sign);
                }
            }

            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Order is: [Initial state n->nth eigenvalue]" << flush;
            CTP_LOG(ctp::logDEBUG, *_pLog) << "    A" << stateA + 1 << ":" << stateA + 1 << "->" << index[0] + 1 << " ";
            CTP_LOG(ctp::logDEBUG, *_pLog) << "    B" << stateB + 1 << ":" << stateBd + 1 << "->" << index[1] + 1 << " " << flush;

            //setting up transformation matrix _T and diagonal matrix _E for the eigenvalues;

            ub::matrix<double> _E = ub::zero_matrix<double>(2, 2);
            ub::matrix<double> _T = ub::zero_matrix<double>(2, 2);
            //find the eigenvectors which are most similar to the initial states

            //row
            for (unsigned i = 0; i < 2; i++) {
                unsigned k = index[i];
                double sign = signvec[i];
                double normr = 1 / std::sqrt(J_eigenvectors(stateA, k) * J_eigenvectors(stateA, k) + J_eigenvectors(stateBd, k) * J_eigenvectors(stateBd, k));
                _T(0, i) = sign * J_eigenvectors(stateA, k) * normr;
                _T(1, i) = sign * J_eigenvectors(stateBd, k) * normr;
                _E(i, i) = _J_eigenvalues(k);
            }

            if ((_T(1, 1) * _T(0, 0) - _T(1, 0) * _T(0, 1)) < 0) {
                CTP_LOG(ctp::logDEBUG, *_pLog) << " Reduced state matrix is not in a right handed basis, multiplying second eigenvector by -1 " << flush;
                _T(0, 1) = -_T(0, 1);
                _T(1, 1) = -_T(1, 1);
            }

            if (tools::globals::verbose) {
                CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << "_T" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << _T << flush;

            }

            ub::matrix<double> S_small = ub::prod(_T, ub::trans(_T));
            if (tools::globals::verbose) {

                CTP_LOG(ctp::logDEBUG, *_pLog) << "S_small" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << S_small << flush;

            }
            //orthogonalize that matrix
            double small = linalg_loewdin(_E, S_small);
            CTP_LOG(ctp::logDEBUG, *_pLog) << ctp::TimeStamp() << "   Smallest value of dimer overlapmatrix is " << small << flush;
            if (tools::globals::verbose) {

                CTP_LOG(ctp::logDEBUG, *_pLog) << "S-1/2" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << S_small << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << "E_ortho" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << _E << flush;
            }
            _T = ub::prod(_T, S_small);
            //cout <<  ub::prod(_T,ub::trans(_T))<<endl;
            if (tools::globals::verbose) {

                CTP_LOG(ctp::logDEBUG, *_pLog) << "T_ortho" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << _T << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << "---------------------------------------" << flush;
            }

            ub::matrix<double> temp = ub::prod(_T, _E);

            ub::matrix<double> _J_small = ub::prod(temp, ub::trans(_T));
            if (tools::globals::verbose) {

                CTP_LOG(ctp::logDEBUG, *_pLog) << "T_ortho*E_ortho" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << temp << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << "T_ortho*E_ortho*T_ortho^T" << flush;
                CTP_LOG(ctp::logDEBUG, *_pLog) << _J_small << flush;
            }

            _J(stateA, stateBd) = _J_small(0, 1);
            _J(stateBd, stateA) = _J_small(1, 0);

        }
    }

    return _J;
}

}}


#ifndef PYTHONIC_INCLUDE_TYPES_LONG_HPP
#define PYTHONIC_INCLUDE_TYPES_LONG_HPP

#ifndef USE_GMP
using pythran_long_t = long long;
#define pythran_long(a) pythran_long_t(a)
#else
#include <gmpxx.h>
using pythran_long_t = mpz_class;
#define pythran_long(a) pythran_long_t(#a)

namespace pythonic
{

  /* some math overloads { */

  namespace operator_
  {
    template <class T, class U, class T1>
    auto mod(__gmp_expr<T, U> const &t0, T1 const &t1) -> decltype(t0 % t1);

    template <class T, class U>
    auto floordiv(__gmp_expr<T, U> const &a, long b) -> decltype(a / b);

    template <class T, class U, class Tp, class Up>
    auto floordiv(__gmp_expr<T, U> const &a, __gmp_expr<Tp, Up> const &b)
        -> decltype(a / b);
  }
  /* } */

  template <class T, class U>
  struct gmp_compo;

  template <class T>
  struct gmp_type;

  template <class T, class U>
  struct assignable<__gmp_expr<T, U>> {
    using type = typename gmp_compo<T, U>::type;
    static_assert(!std::is_same<type, double>::value,
                  "Cannot combine long and float.");
  };

  template <class T>
  struct assignable<__gmp_expr<T, T>> {
    using type = typename gmp_type<T>::type;
  };

  template <class T, class U>
  struct lazy<__gmp_expr<T, U>> : assignable<__gmp_expr<T, U>> {
  };

  // GMP_COMPO

  template <class T, class U>
  struct gmp_compo {
    using type = typename gmp_compo<typename gmp_type<T>::type,
                                    typename gmp_type<U>::type>::type;
  };

  template <>
  struct gmp_compo<long, double> {
    using type = double;
  };

  template <>
  struct gmp_compo<double, long> {
    using type = double;
  };

  template <>
  struct gmp_compo<mpz_class, long> {
    using type = mpz_class;
  };

  template <>
  struct gmp_compo<long, mpz_class> {
    using type = mpz_class;
  };

  template <>
  struct gmp_compo<double, mpz_class> {
    using type = double;
  };

  template <>
  struct gmp_compo<mpz_class, double> {
    using type = double;
  };

  template <class T>
  struct gmp_compo<double, T> {
    using type = double;
  };

  template <class T>
  struct gmp_compo<T, double> {
    using type = double;
  };

  template <class T>
  struct gmp_compo<long, T> {
    using type = typename gmp_type<T>::type;
  };

  template <class T>
  struct gmp_compo<T, long> {
    using type = typename gmp_type<T>::type;
  };

  template <class T>
  struct gmp_compo<mpz_class, T> {
    using type = typename gmp_type<T>::type;
  };

  template <class T>
  struct gmp_compo<T, mpz_class> {
    using type = typename gmp_type<T>::type;
  };

  template <class T>
  struct gmp_compo<T, T> {
    using type = typename gmp_type<T>::type;
  };

  template <>
  struct gmp_compo<mpz_class, mpz_class> {
    using type = mpz_class;
  };

  template <>
  struct gmp_compo<long, long> {
    using type = long;
  };

  template <>
  struct gmp_compo<double, double> {
    using type = double;
  };

  // GMP_TYPE

  template <class T>
  struct gmp_type {
    using type = T;
  };

  template <>
  struct gmp_type<__mpz_struct[1]> {
    using type = mpz_class;
  };

  template <class T, class U, class V>
  struct gmp_type<__gmp_binary_expr<T, U, V>> {
    using type = typename gmp_compo<T, U>::type;
  };

  template <class T, class V>
  struct gmp_type<__gmp_unary_expr<T, V>> {
    using type = typename gmp_type<T>::type;
  };

  template <class T, class U, class V>
  struct gmp_type<__gmp_unary_expr<__gmp_expr<T, U>, V>> {
    using type = typename gmp_compo<T, U>::type;
  };

  template <class T, class U>
  struct gmp_type<__gmp_expr<T, U>> {
    using type = typename gmp_compo<T, U>::type;
  };

  template <class T>
  struct gmp_type<__gmp_expr<T, T>> {
    using type = typename gmp_type<T>::type;
  };
}
/* compute hash of a gmp { */
namespace std
{
  template <class T, class U>
  struct hash<__gmp_expr<T, U>> {
    size_t operator()(__gmp_expr<T, U> const &x) const;
  };
}

#include <boost/functional/hash_fwd.hpp>
template <class T, class U>
std::size_t hash_value(__gmp_expr<T, U> const &x);

/* } */
#ifdef ENABLE_PYTHON_MODULE
#include "pythonic/python/register_once.hpp"

namespace pythonic
{

  template <>
  struct python_to_pythran<mpz_class> {
    python_to_pythran();
    static void *convertible(PyObject *obj_ptr);
    static void
    construct(PyObject *obj_ptr,
              boost::python::converter::rvalue_from_python_stage1_data *data);
  };

  struct custom_mpz_to_long {
    static PyObject *convert(const mpz_class &v);
  };

  template <>
  struct pythran_to_python<mpz_class> {
    pythran_to_python();
  };
}

#endif

#endif

#endif


/**
 * \ file GainMaxExpanderFilter.cpp
 */

#include <iostream>

#include <ATK/Dynamic/GainMaxExpanderFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (64)

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainMaxExpanderFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_0_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainMaxExpanderFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(outdata[i], 0.f, 1.f);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_threshold_2_ratio_2_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainMaxExpanderFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.set_max_reduction(0.001);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.700553358, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_threshold_2_ratio_4_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainMaxExpanderFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.389224231, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainMaxExpanderFilter_const_1_threshold_2_ratio_4_max_reduction_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainMaxExpanderFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(0.0001);
  filter.set_max_reduction(.1);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_GE(outdata[i], .1);
  }
}


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <string.h>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <vector>
#include <cmath>
#include <map>
#include <ctime>
#include <sstream>
#include <algorithm>
#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include "colormod.h" // namespace Color
#include "keyboard.h"
#ifndef Included_MATH_HELPER_H
#define Included_MATH_HELPER_H
#include "math_helper.h"
#endif
#ifndef Included_STRING_CONVERTOR_H
#define Included_STRING_CONVERTOR_H
#include "string_convertor.h"
#endif

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

// Robot state publishing
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayRobotState.h>
// Kinematics
#include <moveit_msgs/GetPositionIK.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
//wam
#include "wam_msgs/MatrixMN.h"
#include "wam_srvs/JointMove.h"
#include "sensor_msgs/JointState.h"

#include "pose_helper.h"
using namespace std;
bool commandLock = false;
Color::Modifier c_red(Color::FG_RED);
Color::Modifier c_yellow(Color::FG_YELLOW);
Color::Modifier c_green(Color::FG_GREEN);
Color::Modifier c_default(Color::FG_DEFAULT);
int dofNum = 7;
bool lock = false;
bool ready_signal1 = false;
bool ready_signal2 = false;
bool ready_signal3 = false;
double step_size = 0.02;//m
double current_pos[]={9999, 9999, 9999, 9999, 9999, 9999, 9999};//initialize as an invalid  value set, stores the current pose
std::vector<double> joint_values; //IK solutions
moveit::core::RobotModelPtr kinematic_model;
wam_srvs::JointMove mv_srv;
ros::ServiceClient Joint_move_client ;
geometry_msgs::Pose thisPose;
ros::Publisher pubCommand;

cv::Mat readTransformation()
{
  std::ifstream transform_file;
  std::string transform_file_path = "/home/chris/catkin_ws/src/long_range_teleoperation/robot-side/3_Calibration_Solver/result.txt";
  transform_file.open(transform_file_path.c_str(), std::ios_base::in | std::ios_base::binary);

  if(!transform_file) {
      std::cerr << "Can't open transform file" << std::endl;
      std::exit(-1);
  }
  cv::Mat transformation(4,4,cv::DataType<double>::type);

  std::string line;
  int i = 0;
  while(getline(transform_file, line) && i < 4) {
      std::istringstream in(line);
      double c1, c2, c3, c4;
      in >> c1 >> c2 >> c3 >> c4;
      transformation.at<double>(i,0) = c1;
      transformation.at<double>(i,1) = c2;
      transformation.at<double>(i,2) = c3;
      transformation.at<double>(i,3) = c4;
      ++i;
  }

  std::cout <<"calibration solver result: "<<endl<< transformation <<std::endl;
  return transformation;
}

std::vector<double> targetTransformation(double x, double y, double z)
{
  cv::Mat transformationMatrix = readTransformation();
  cv::Mat t(4,1,cv::DataType<double>::type);
  t.at<double>(0,0) = x;
  t.at<double>(1,0) = y;
  t.at<double>(2,0) = z;
  t.at<double>(3,0) = 1;
  cv::Mat result = transformationMatrix * t;
  std::vector<double> rv;
  rv.push_back(result.at<double>(0,0)/result.at<double>(3,0));
  rv.push_back(result.at<double>(1,0)/result.at<double>(3,0));
  rv.push_back(result.at<double>(2,0)/result.at<double>(3,0));
  return rv;
}

void wamPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
 {
   //cout<<"wam msgs"<<endl;
    thisPose = msg->pose;
    ready_signal1 = true;
 }

void wamJointsCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
   //cout<<"joint pose obtained:"<<endl;//<<initial_Joint_pose[1]<<endl;
    //cout<<initial_Joint_pose[0]<<"  "<<initial_Joint_pose[1]<<"  "<<initial_Joint_pose[2]<<" "<<initial_Joint_pose[3]<<" "<< initial_Joint_pose[4]
   // <<" "<<initial_Joint_pose[5]<<" "<<initial_Joint_pose[6]<<endl;
   current_pos[0]=msg->position[0];
   current_pos[1]=msg->position[1];
   current_pos[2]=msg->position[2];
   current_pos[3]=msg->position[3];
   current_pos[4]=msg->position[4];
   current_pos[5]=msg->position[5];
   current_pos[6]=msg->position[6];
   ready_signal2 = true;
}

void moveRobotJointValue(std::vector<double> joint_values_t)
{
  lock=true;
  std::vector<float> jnts;
     for(size_t i=0;i<dofNum;i++)
     {
       float newJ = (float)(joint_values_t[i]);
       jnts.push_back(newJ);
     }
  mv_srv.request.joints = jnts;
  cout<<"send to robot to the  position "<<endl;
  string_convertor::printOutStdVector_f(jnts);
  cout << "Press any key to move..." << endl;
  getchar();
  Joint_move_client.call(mv_srv);///////////////////////////////////////////////////
  //boost::this_thread::sleep( boost::posix_time::milliseconds(1000) );
  lock=false;
}

geometry_msgs::Pose comput_fk(double currentJoints[]) //get current end  effector  pose
{
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
  //fill in the seed
  kinematic_state->setVariablePositions(currentJoints);
  cout<<"compute fk current joints "<<currentJoints[0]<<", "<<currentJoints[1]<<", "<<currentJoints[2]<<endl;
  kinematic_state->update();
  const Eigen::Affine3d end_effector_state = kinematic_state->getGlobalLinkTransform("wam/wrist_palm_link");//table_base
  ROS_INFO_STREAM("FK Translation: " << end_effector_state.translation());
  Eigen::Quaterniond rot_q(end_effector_state.rotation());
  ROS_INFO_STREAM("FK Rotation q x: " << rot_q.x());
  ROS_INFO_STREAM("FK Rotation q y: " << rot_q.y());
  ROS_INFO_STREAM("FK Rotation q z: " << rot_q.z());
  ROS_INFO_STREAM("FK Rotation q w: " << rot_q.w());
  geometry_msgs::Pose p;
  p.position.x = end_effector_state.translation()(0);
  p.position.y = end_effector_state.translation()(1);
  p.position.z = end_effector_state.translation()(2);
  p.orientation.x = rot_q.x();
  p.orientation.y = rot_q.y();
  p.orientation.z = rot_q.z();
  p.orientation.w = rot_q.w();
  return p;
}

bool comput_IK(geometry_msgs::Pose p, double referenceJoints[])
{
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
  const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm");//All, it's in the planning group name
  // const std::vector<std::string> &joint_names2 = joint_model_group->getJointModelNames();
  // joint_names2=string_convertor::safeRemoveStringVec(joint_names2,0);//first  item of  joint name  is  caused by  mistake
  // cout<<"joint names count  "<<joint_names2.size()<<endl;//8
  //fill in the seed
  kinematic_state->setVariablePositions(referenceJoints);
  cout<<"compute IK current joints "<<referenceJoints[0]<<", "<<referenceJoints[1]<<", "<<referenceJoints[2]<<endl;
  kinematic_state->update();
  bool found_ik = kinematic_state->setFromIK(joint_model_group, p, 10, 0.1);
  // Now, we can print out the IK solution (if found):
  if (found_ik)
  {
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    cout<<c_green<<"IK solution found: ";
    for(std::size_t i=0; i < joint_values.size(); ++i)
    {
      ROS_INFO("IK solution Joint %s: %f", to_string(i+1).c_str(), joint_values[i]);
      cout<<joint_values[i]<<",";
    }
    cout<<c_default<<endl;
  }
  else
  {
    ROS_INFO("Did not find IK solution");
  }
  return found_ik;
}

void test_IK_FK()
{
  geometry_msgs::Pose pose = comput_fk(current_pos);
  cout<<"using pose to test "<<endl;
  cout<<"x "<<pose.position.x <<" y "<<pose.position.y<<" z "<<pose.position.z<<endl;
  bool ik_found = comput_IK(pose, current_pos);
  if(ik_found)
  {
    ROS_INFO("asdaf");
  }
}

void printPose(geometry_msgs::Pose pose)
{
  cout<<"x "<<pose.position.x <<" y "<<pose.position.y<<" z "<<pose.position.z<<endl;
  cout<<"qx: "<<pose.orientation.x <<" qy: "<<pose.orientation.y<<" qz: "<<pose.orientation.z<<" qw: "<< pose.orientation.w<<endl;
}

void execute2target(geometry_msgs::Pose targetRobotPose)
{
  cout<<c_red<<"New target: "<<endl;
  printPose(targetRobotPose);
  cout<<c_red<<"press key to continue"<<c_default<<endl;
  ready_signal1=false;ready_signal2=false;
  while(!ready_signal1&&!ready_signal2)
  {
    ros::spinOnce();
    boost::this_thread::sleep( boost::posix_time::milliseconds(100) );
    cout<<"wait for ready_signal1 and ready_signal2"<<endl;
  }
  cout<<"robot joints and poses values updated!"<<endl;
  geometry_msgs::Pose initial_robot_pose = thisPose;
  geometry_msgs::Pose initial_FK_pose = comput_fk(current_pos);

  cout<<c_green<<" corr FK target is: "<<endl;
  printPose(pose_helper::convert2FKtarget(initial_FK_pose, initial_robot_pose, targetRobotPose));
  cout<<c_default;

  cout<<"initial robot pose"<<endl;
  printPose(initial_robot_pose);
  cout<<"initial FK pose"<<endl;
  printPose(initial_FK_pose);
  int steps_num = pose_helper::get_steps_num(initial_robot_pose, targetRobotPose, step_size);
  cout<<"steps Number for this target: "<<steps_num<<endl;

  getchar();
  for(int i=0; i< steps_num; i++)
  {
    geometry_msgs::Pose step_pose = pose_helper::next_step_pose(initial_FK_pose, initial_robot_pose, targetRobotPose, i+1, steps_num);
    cout<<c_red;
    cout<<"step "<<(i+1)<<" target, out of "<<steps_num<<endl;
    printPose(step_pose);
    cout<<c_default;
    //this step_pose is represented in IK sovler's frame now.
    bool ik_found = comput_IK(step_pose, current_pos);
    if(ik_found)
    {
      moveRobotJointValue(joint_values);
      cout<<c_yellow;
      ROS_INFO("robot moved ");
      cout<<c_default;
      //update joint values
      current_pos[0]=joint_values[0];
      current_pos[1]=joint_values[1];
      current_pos[2]=joint_values[2];
      current_pos[3]=joint_values[3];
      current_pos[4]=joint_values[4];
      current_pos[5]=joint_values[5];
      current_pos[6]=joint_values[6];
      // ready_signal2 = false;
      // while(!ready_signal2)
      // {
      //   ros::spinOnce();
      //   boost::this_thread::sleep( boost::posix_time::milliseconds(100) );
      //   cout<<"wait for ready_signal2"<<endl;
      // }
    }
    else
      i = i -1;
    cout<<"loop index : "<<i<<endl;
  }
  cout<<"haha, task accomplished!"<<endl;
}

void udpCommandCallback(const std_msgs::String::ConstPtr& msg)
{
  if(commandLock)
  {
    cout<<c_red<<"command locked!"<<endl<<c_default;
    return;
  }
  //send the command to remote robot
  cout<<"recved: "<<msg->data<<endl;//<<msg->data<<endl;
  if(msg->data.find("coarse_target_move") != std::string::npos && !commandLock) //fine our command
  {
    //execute command
    commandLock = true;
    std::vector<string> straa = string_convertor::split(msg->data, ':');
    string datastr = straa[2];
    std::vector<double> dataValues = string_convertor::split2double(datastr, ',');
    std::vector<double> transformedResult = targetTransformation(dataValues[0], dataValues[1], dataValues[2]);
    geometry_msgs::Pose target_robot_pose = pose_helper::construct_geometry_pose(transformedResult[0], transformedResult[1], transformedResult[2], (int)(dataValues[3]));
    cout<<c_green<<"go to execution phase"<<endl<<c_default;
    execute2target(target_robot_pose);
    // std_msgs::String msg;
    // msg.data = "";
    // pubCommand.publish(msg); //clear the command buffer.
    commandLock = false;
  }
  if(msg->data.find("way_point_move") != std::string::npos && !commandLock) //fine our command
  {
    //execute command
    //way_point_move:1:x,y,z,qx,qy,qz,qw,.........
    commandLock = true;
    std::vector<string> straa = string_convertor::split(msg->data, ':');
    string datastr = straa[2];
    std::vector<double> dataValues = string_convertor::split2double(datastr, ',');
    geometry_msgs::Pose target_robot_pose;
    target_robot_pose.position.x = dataValues[0]
    target_robot_pose.position.y = dataValues[1]
    target_robot_pose.position.z = dataValues[2]
    target_robot_pose.orientation.x = dataValues[3]
    target_robot_pose.orientation.y = dataValues[4]
    target_robot_pose.orientation.z = dataValues[5]
    target_robot_pose.orientation.w = dataValues[6]

    cout<<c_green<<"go to execution phase"<<endl<<c_default;
    execute2target(target_robot_pose);
    // std_msgs::String msg;
    // msg.data = "";
    // pubCommand.publish(msg); //clear the command buffer.
    commandLock = false;
  }
}
//===========================MAIN FUNCTION START===========================

int main(int argc, char* argv[]){
  // Initialize the ROS system and become a node.
   // if(argc==2)
   //      numThres = atoi(argv[1]);
  ros::init(argc, argv, "tele_op");
  ros::NodeHandle n("~");
  ros::Subscriber subP = n.subscribe("/zeus/wam/pose", 1, wamPoseCallback);
  ros::Subscriber wam_joints_sub=n.subscribe("/zeus/wam/joint_states",1, wamJointsCallback);
  ros::Subscriber udp_tasks=n.subscribe("/udp/command",1, udpCommandCallback);
  //pubCommand = n.advertise<std_msgs::String>("/udp/command", 1, true);
  Joint_move_client = n.serviceClient<wam_srvs::JointMove>("/zeus/wam/joint_move");

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  kinematic_model = robot_model_loader.getModel();
  ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
  commandLock = true;
  test_IK_FK();
  //std::vector<double> testrestulst = targetTransformation(-0.0632346, 0.0754276, 0.728763);
  //cout<<"target transofrmation: "<<testrestulst[0]<<"  "<<testrestulst[1]<<"  "<<testrestulst[2]<<endl;
  cout<<"press key to continue"<<endl;
  getchar();

    while(!ready_signal1)
    {
      ros::spinOnce();
      boost::this_thread::sleep( boost::posix_time::milliseconds(100) );
      cout<<"wait for readySignal 1 in main"<<endl;
    }
    commandLock = false;
    cout<<"readySignal1 Received! goto main loop. CommandLock set to false."<<endl;
  // int presetMark = 3; //press button -0.08, -0.69, 0.17, 3./// handler  0.4, 0, 0.4, 1
  // geometry_msgs::Pose target_robot_pose = pose_helper::construct_geometry_pose(0.4, 0, 0.4, 1);
  // execute2target(target_robot_pose);
  ros::spin();
  ros::waitForShutdown();
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <cmath>
#include <iostream>
#include <iomanip>
#include <limits>

#include <Eigen/Core>

#include "green/DerivativeTypes.hpp"
#include "green/AnisotropicLiquid.hpp"
#include "cavity/Element.hpp"
#include "cavity/GePolCavity.hpp"
#include "green/IonicLiquid.hpp"
#include "bi_operators/NumericalIntegrator.hpp"
#include "green/UniformDielectric.hpp"
#include "TestingMolecules.hpp"
#include "green/Vacuum.hpp"
#include "utils/MathUtils.hpp"

SCENARIO("A collocation integrator with numerical integrator of the diagonal elements", "[bi_operators][bi_operators_numerical]")
{
    GIVEN("A GePol cavity for a single sphere in the origin")
    {
        Molecule molec = dummy<0>(1.44 / bohrToAngstrom());
        double area = 10.0;
        GePolCavity cavity = GePolCavity(molec, area, 0.0, 100.0);
        Eigen::MatrixXd results = Eigen::MatrixXd::Zero(cavity.size(), cavity.size());
        Eigen::MatrixXd reference = Eigen::MatrixXd::Zero(cavity.size(), cavity.size());

        /*! \class NumericalIntegrator
         *  \test \b NumericalIntegratorTest_vacuum tests the numerical evaluation of the vacuum matrix representations of S and D
         */
        WHEN("the vacuum Green's function is used")
        {
            Vacuum<AD_directional, NumericalIntegrator> gf;
            THEN("the matrix elements of S are")
            {
                results = gf.singleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("vacuum_S_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
            AND_THEN("the matrix elements of D are")
            {
                Eigen::MatrixXd results = gf.doubleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("vacuum_D_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                // Checks sum rule for D operator
                REQUIRE(D_sum == Approx(2 * M_PI));
                */
            }
        }

        /*! \class NumericalIntegrator
         *  \test \b NumericalIntegratorTest_uniformdielectric tests the numerical evaluation of the uniform dielectric matrix representations of S and D
         */
        WHEN("the uniform dielectric Green's function is used")
        {
            double eps = 80.0;
            UniformDielectric<AD_directional, NumericalIntegrator> gf(eps);
            THEN("the matrix elements of S are")
            {
                results = gf.singleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("uniformdielectric_S_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
            AND_THEN("the matrix elements of D are")
            {
                Eigen::MatrixXd results = gf.doubleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("uniformdielectric_D_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
        }

        /*! \class NumericalIntegrator
         *  \test \b NumericalIntegratorTest_ionic tests the numerical evaluation of the ionic liquid matrix representations of S and D
         */
        WHEN("the ionic liquid Green's function is used")
        {
            double eps = 80.0;
            double kappa = 1.5;
            IonicLiquid<AD_directional, NumericalIntegrator> gf(eps, kappa);
            THEN("the matrix elements of S are")
            {
                results = gf.singleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("ionicliquid_S_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
            AND_THEN("the matrix elements of D are")
            {
                results = gf.doubleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("ionicliquid_D_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
        }

        /*! \class NumericalIntegrator
         *  \test \b NumericalIntegratorTest_anisotropic tests the numerical evaluation of the anisotropic liquid matrix representations of S and D
         */
        WHEN("the ionic liquid Green's function is used")
        {
            Eigen::Vector3d epsilon = (Eigen::Vector3d() << 80.0, 80.0, 80.0).finished();
            Eigen::Vector3d euler = (Eigen::Vector3d() << 0.0, 0.0, 0.0).finished();
            AnisotropicLiquid<AD_directional, NumericalIntegrator> gf(epsilon, euler);
            THEN("the matrix elements of S are")
            {
                results = gf.singleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("anisotropicliquid_S_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
            AND_THEN("the matrix elements of D are")
            {
                results = gf.doubleLayer(cavity.elements());
                // Numerical integrator not really working now...
                /*
                reference = cnpy::custom::npy_load<double>("anisotropicliquid_D_numerical.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
                */
            }
        }
    }
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_INC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-arithmetic
    This function object returns  its parameter incremented by 1.

    @par Header <boost/simd/function/inc.hpp>

    @par Decorators

     - saturated_ ensures that @c saturated_(inc)(x) will never be strictly
     less than @c x, avoiding the wrap around from @ref Valmax to
     @ref Valmin with integer types.

    @see plus, dec, minus

    @par Example:

      @snippet inc.cpp inc

    @par Possible output:

      @snippet inc.txt inc
  **/
  Value inc(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/inc.hpp>
#include <boost/simd/function/simd/inc.hpp>

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SPLIT_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SPLIT_MULTIPLIES_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/split.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( split_multiplies_
                            , (typename A0, typename X)
                            , (bd::is_upgradable<A0>)
                            , bd::cpu_
                            , bs::pack_<bd::arithmetic_<A0>, X>
                            , bs::pack_<bd::arithmetic_<A0>, X>
                            )
  {
    using result_t = std::pair<bd::upgrade_t<A0>,bd::upgrade_t<A0>>;

    BOOST_FORCEINLINE result_t operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      auto s0 = split(a0);
      auto s1 = split(a1);

      return { s0[0]*s1[0], s0[1]*s1[1] };
    }
  };
} } }

#endif


/**
 * @file Data structures for estimation
 */

#ifndef WAVE_GEOMETRY_ESTIMATION_HPP
#define WAVE_GEOMETRY_ESTIMATION_HPP

#include <Eigen/Eigenvalues>

#include "geometry.hpp"

namespace wave {}  // namespace wave

#include "src/estimation/Uncertain.hpp"
#include "src/estimation/Noise.hpp"
#include "src/estimation/FactorVariableBase.hpp"
#include "src/estimation/FactorVariable.hpp"
#include "src/estimation/FactorBase.hpp"
#include "src/estimation/Factor.hpp"

#endif  // WAVE_GEOMETRY_ESTIMATION_HPP


/*
 * NormalVectorAverage.hpp
 *
 *  Created on: Sep 25, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 *
 */

#pragma once

// Eigen
#include <Eigen/Core>

// Kindr
#include <kindr/rotations/RotationEigen.hpp>

// STD
#include <iostream>

namespace foothold_finder {

class NormalVectorAverage
{
 public:
  NormalVectorAverage();
  virtual ~NormalVectorAverage();

  void add(const Eigen::Vector3d& vector);

  void getAverage(Eigen::Vector3d& average) const;

  void clear();

  friend std::ostream& operator <<(std::ostream& ostream, const NormalVectorAverage& normalVectorAverage);

 private:

  typedef kindr::rotations::eigen_impl::RotationVectorAD Rotation;

  //! Number of samples.
  unsigned int nSamples_;

  Rotation averageRotation_;

  const Eigen::Vector3d referenceVector_;
};

} /* namespace foothold_finder */


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_BITINTEGER_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_BITINTEGER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing bitinteger capabilities

    Transforms a floating point value in a pattern of bits
    stored in an integer with different formulas according to
    the floating point @ref bitofsign.

    This function is the converse of @ref bitfloating.

    @par Semantic:

    for any parameter @c x of floating type @c T

    @code
     as_integer_t<T> r = bitinteger(x);
    @endcode

    @par Note:
    This is an internally used utility function related to the computation of floating
    successors or predecessors

    @see next, prev, successor,  predecessor,  nextafter, bitfloating

  **/
  as_integer_t<T> bitinteger(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/bitinteger.hpp>
#include <boost/simd/function/simd/bitinteger.hpp>

#endif


#include <boost/numeric/odeint/stepper/extrapolation_stepper.hpp>


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file rename_test.cpp
/// \brief Add your file description here.

#define MCRL2_STATE_FORMULA_BUILDER_DEBUG

#define BOOST_TEST_MODULE state_formula
#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/lps/linearise.h"
#include "mcrl2/modal_formula/count_fixpoints.h"
#include "mcrl2/modal_formula/maximal_closed_subformula.h"
#include "mcrl2/modal_formula/parse.h"
#include "mcrl2/modal_formula/preprocess_state_formula.h"

using namespace mcrl2;
using namespace mcrl2::core;
using namespace mcrl2::lps;
using namespace mcrl2::lps::detail;
using namespace mcrl2::state_formulas;

BOOST_AUTO_TEST_CASE(test_rename)
{
  const std::string SPECIFICATION =
    "act a:Nat;                              \n"
    "                                        \n"
    "map smaller: Nat#Nat -> Bool;           \n"
    "                                        \n"
    "var x,y : Nat;                          \n"
    "                                        \n"
    "eqn smaller(x,y) = x < y;               \n"
    "                                        \n"
    "proc P(n:Nat) = sum m: Nat. a(m). P(m); \n"
    "                                        \n"
    "init P(0);                              \n"
    ;

  using mcrl2::state_formulas::pp;
  specification lpsspec = remove_stochastic_operators(linearise(SPECIFICATION));

  state_formula formula = parse_state_formula("(mu X. X) && (mu X. X)", lpsspec);

  data::set_identifier_generator generator;
  generator.add_identifiers(lps::find_identifiers(lpsspec));
  formula = rename_predicate_variables(formula, generator);

  std::cout << "pp(formula) == " << pp(formula) << std::endl;
  BOOST_CHECK(pp(formula) == "(mu X1. X1) && (mu X. X)" || pp(formula) == "(mu X. X) && (mu X1. X1)");

  generator = data::set_identifier_generator();
  generator.add_identifiers(lps::find_identifiers(lpsspec));
  parse_state_formula_options options;
  options.check_monotonicity = false;
  options.resolve_name_clashes = false;
  formula = parse_state_formula("mu X. mu X. X", lpsspec, options);
  std::cout << "formula: " << pp(formula) << std::endl;
  formula = rename_predicate_variables(formula, generator);
  std::cout << "formula: " << pp(formula) << std::endl;
  BOOST_CHECK_EQUAL(pp(formula), "mu X. mu X1. X1");
}

BOOST_AUTO_TEST_CASE(test_normalize)
{
  state_formula x = state_formulas::variable(identifier_string("X"), data::data_expression_list());
  state_formula y = state_formulas::variable(identifier_string("Y"), data::data_expression_list());
  state_formula f;
  state_formula f1;
  state_formula f2;

  f = imp(not_(x), y);
  f1 = normalize(f);
  f2 = or_(x, y);
  std::cout << "f  = " << state_formulas::pp(f) << std::endl;
  std::cout << "f1 = " << state_formulas::pp(f1) << std::endl;
  std::cout << "f2 = " << state_formulas::pp(f2) << std::endl;
  BOOST_CHECK_EQUAL(f1, f2);

  f  = not_(and_(not_(x), not_(y)));
  f1 = normalize(f);
  f2 = or_(x, y);
  std::cout << "f  = " << state_formulas::pp(f) << std::endl;
  std::cout << "f1 = " << state_formulas::pp(f1) << std::endl;
  std::cout << "f2 = " << state_formulas::pp(f2) << std::endl;
  BOOST_CHECK_EQUAL(f1, f2);
}

BOOST_AUTO_TEST_CASE(test_type_checking)
{
  specification lpsspec = remove_stochastic_operators(linearise(
                            "sort CPU = struct p1;"
                            "sort CPUs = Set(CPU);"
                            "init delta;"));

  state_formula formula = parse_state_formula("nu X (P : CPUs = {p1}) . val(P != {})", lpsspec);
}

BOOST_AUTO_TEST_CASE(test_type_checking_conversion_of_arguments)
{
  specification lpsspec = remove_stochastic_operators(linearise(
                            "sort B = struct d;"
                            "act a: List(B);"
                            "init a([d]);"
                          ));

  state_formula formula = parse_state_formula("true", lpsspec);

  BOOST_CHECK(is_may(formula));
  const state_formulas::may& f = atermpp::down_cast<state_formulas::may>(formula);
  BOOST_CHECK(is_regular_formula(f.formula()));
}

static inline
state_formula negate_variable(const variable& x)
{
  return state_formulas::not_(x);
}

BOOST_AUTO_TEST_CASE(test_not)
{
  data::data_expression_list args;
  variable v(core::identifier_string("v"), args);
  state_formula s = not_(v);
  BOOST_CHECK(is_not(s));

  state_formula t = negate_variable(v);
  BOOST_CHECK_EQUAL(s, t);

  // The following is expected to trigger an assertion failure
  // aterm_appl a = v;
  // state_formula t = not_(a);
}

// test case supplied by Jan Friso, 4-1-2011
BOOST_AUTO_TEST_CASE(test_parse)
{
  std::string spec_text =
    "act a:Nat; \n"
    "init a(1); \n"
    ;

  std::string formula_text = "true";

  lps::specification lpsspec = remove_stochastic_operators(lps::linearise(spec_text));
  state_formulas::state_formula f = state_formulas::parse_state_formula(formula_text, lpsspec);

  std::cerr << "--- f ---\n" << state_formulas::pp(f) << "\n\n" << f << std::endl;
  std::set<core::identifier_string> ids = state_formulas::find_identifiers(f);
  BOOST_CHECK(ids.find(core::identifier_string("1")) == ids.end());
  BOOST_CHECK(ids.find(core::identifier_string("@c1")) != ids.end());
}

BOOST_AUTO_TEST_CASE(test_count_fixpoints)
{
  state_formula formula;
  specification lpsspec;

  formula = parse_state_formula("(mu X. X) && (mu X. X)", lpsspec);
  BOOST_CHECK_EQUAL(count_fixpoints(formula), 2u);

  formula = parse_state_formula("exists b:Bool. (mu X. X) || forall b:Bool. (nu X. mu Y. (X || Y))", lpsspec);
  BOOST_CHECK_EQUAL(count_fixpoints(formula), 3u);
}

// Test case for bug #1094.
// This is expected to fail, due to the occurrence of an unbound variable Y
// in the formula.
BOOST_AUTO_TEST_CASE(test_1094)
{
  const std::string SPEC =
      "act  a,b,c,d;\n"
      "\n"
      "proc P(s3_X: Pos) =\n"
      "       (s3_X == 1) ->\n"
      "         a .\n"
      "         P(s3_X = 2)\n"
      "     + (s3_X == 2) ->\n"
      "         b .\n"
      "         P(s3_X = 3)\n"
      "     + (s3_X == 3) ->\n"
      "         a .\n"
      "         P(s3_X = 4)\n"
      "     + (s3_X == 3) ->\n"
      "         a .\n"
      "         P(s3_X = 5)\n"
      "     + (s3_X == 4) ->\n"
      "         c .\n"
      "         P(s3_X = 1)\n"
      "     + (s3_X == 5) ->\n"
      "         d .\n"
      "         P(s3_X = 1)\n"
      "     + delta;\n"
      "\n"
      "init P(1);\n"
    ;
  specification s(parse_linear_process_specification(SPEC));

  const std::string FORMULA = "[true*]([b] nu X. mu X.( [!c]X && [!c]Y))";

  BOOST_CHECK_THROW(parse_state_formula(FORMULA, s), mcrl2::runtime_error);
}

inline
state_formula sigma(const state_formula& x)
{
  variable X("X", data::data_expression_list());
  return x == X ? false_() : x;
}

BOOST_AUTO_TEST_CASE(test_replace_state_formulas)
{
  specification lpsspec;
  state_formula f = parse_state_formula("(mu X. X) && (mu X. X)", lpsspec);
  state_formula result = replace_state_formulas(f, sigma);
  state_formula expected_result = parse_state_formula("(mu X. false) && (mu X. false)", lpsspec);
  if (!(result == expected_result))
  {
    std::cout << "error: " << state_formulas::pp(result) << " != " << state_formulas::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

BOOST_AUTO_TEST_CASE(test_find_state_variables)
{
  specification lpsspec;

  state_formula f = parse_state_formula("(mu X. nu Y. true && mu Z. X && Z)", lpsspec);
  std::set<state_formulas::variable> v = state_formulas::find_state_variables(f);
  BOOST_CHECK(v.size() == 2);

  f = parse_state_formula("mu X. nu Y. (true && mu Z. (X && Y || Z))", lpsspec);
  v = find_state_variables(f);
  BOOST_CHECK(v.size() == 3);

  state_formulas::variable X("X", data::data_expression_list());
  state_formulas::variable Y("Y", data::data_expression_list());
  state_formulas::variable Z("Z", data::data_expression_list());
  state_formula phi = state_formulas::and_(X, Y);
  v = find_state_variables(phi);
  BOOST_CHECK(v.size() == 2);
  v = find_free_state_variables(phi);
  BOOST_CHECK(v.size() == 2);
  state_formula psi = state_formulas::mu("X", data::assignment_list(), phi);
  v = find_state_variables(psi);
  BOOST_CHECK(v.size() == 2);
  v = find_free_state_variables(psi);
  BOOST_CHECK(v.size() == 1);
}

inline
bool contains(const std::set<state_formulas::state_formula>& v, const std::string& s)
{
  for (const state_formulas::state_formula& f: v)
  {
    if (state_formulas::pp(f) == s)
    {
      return true;
    }
  }
  return false;
}

BOOST_AUTO_TEST_CASE(test_maximal_closed_subformulas)
{
  mcrl2::log::mcrl2_logger::set_reporting_level(mcrl2::log::debug, "state_formulas");

  specification lpsspec;
  state_formula f = parse_state_formula("(mu X. nu Y. true && mu Z. X && Z)", lpsspec);
  std::set<state_formulas::state_formula> v = maximal_closed_subformulas(f);
  BOOST_CHECK(v.size() == 1);

  f = parse_state_formula("exists b: Bool. forall c: Bool. val(b) && (val(c) || true) && false", lpsspec);
  v = maximal_closed_subformulas(f);
  BOOST_CHECK(v.size() == 1);

  state_formula g = exists(f).body();
  std::cout << "g = " << state_formulas::pp(g) << std::endl;
  v = maximal_closed_subformulas(g);
  for (const state_formula& f: v)
  {
    std::cout << "element " << f << std::endl;
  }
  BOOST_CHECK(v.size() == 2);
  BOOST_CHECK(contains(v, "true"));
  BOOST_CHECK(contains(v, "false"));

  state_formula h = forall(g).body();
  v = maximal_closed_subformulas(h);
  std::cout << "h = " << state_formulas::pp(h) << std::endl;
  for (const state_formula& f: v)
  {
    std::cout << "element " << f << std::endl;
  }
  BOOST_CHECK(v.size() == 2);
  BOOST_CHECK(contains(v, "true"));
  BOOST_CHECK(contains(v, "false"));
}

void test_has_unscoped_modal_formulas(const std::string& text, lps::specification& lpsspec, bool expected_result)
{
  state_formula x = parse_state_formula(text, lpsspec);
  bool result = state_formulas::detail::has_unscoped_modal_formulas(x);
  if (result != expected_result)
  {
    std::cout << "--- has_unscoped_modal_formulas failure ---" << std::endl;
    std::cout << "formula = " << text << std::endl;
    std::cout << "result  = " << result << std::endl;
  }
  BOOST_CHECK_EQUAL(result, expected_result);
}

BOOST_AUTO_TEST_CASE(has_unscoped_modal_formulas_test)
{
  const std::string text =
    "act a, b, c;                  \n"
    "                              \n"
    "proc P = delta;               \n"
    "                              \n"
    "init P;                       \n"
    ;
  lps::specification lpsspec = lps::parse_linear_process_specification(text);

  test_has_unscoped_modal_formulas("[a]true", lpsspec, true);
  test_has_unscoped_modal_formulas("mu X. X", lpsspec, false);
  test_has_unscoped_modal_formulas("true => [a]true", lpsspec, true);
  test_has_unscoped_modal_formulas("true => mu X. [a]true", lpsspec, false);
}

void test_preprocess_nested_modal_operators(const std::string& text, lps::specification& lpsspec, const std::string& expected_result_text)
{
  state_formula x = parse_state_formula(text, lpsspec);
  state_formula expected_result = parse_state_formula(expected_result_text, lpsspec);
  state_formula result = state_formulas::preprocess_nested_modal_operators(x);
  if (result != expected_result)
  {
    std::cout << "--- preprocess_nested_modal_operators failure ---" << std::endl;
    std::cout << "formula          = " << text << std::endl;
    std::cout << "result           = " << state_formulas::pp(result) << std::endl;
    std::cout << "expected result  = " << expected_result_text << std::endl;
  }
  BOOST_CHECK_EQUAL(result, expected_result);
}

BOOST_AUTO_TEST_CASE(preprocess_nested_modal_operators_test)
{
  const std::string text =
    "act a, b, c;                  \n"
    "                              \n"
    "proc P = delta;               \n"
    "                              \n"
    "init P;                       \n"
    ;
  lps::specification lpsspec = lps::parse_linear_process_specification(text);

  test_preprocess_nested_modal_operators("[a]true", lpsspec, "[a]true");
  test_preprocess_nested_modal_operators("[a]true", lpsspec, "[a]mu X. true");
  test_preprocess_nested_modal_operators("true && true", lpsspec, "true && mu X. true");
  test_preprocess_nested_modal_operators("true => mu X. [a]true", lpsspec, "true => mu X. [a]mu X1. true");
}

BOOST_AUTO_TEST_CASE(parse_modal_formula_test)
{
  const std::string text =
    "act a, b, c;                  \n"
    "                              \n"
    "proc P = delta;               \n"
    "                              \n"
    "init P;                       \n"
    ;
  lps::specification lpsspec = lps::parse_linear_process_specification(text);

  const std::string formula_text = "nu X(n: Nat = 0, b: Bool = false). X(n, b)";
  state_formula x = parse_state_formula(formula_text, lpsspec);
  std::string s = state_formulas::pp(x);
  BOOST_CHECK_EQUAL(s, formula_text);
}

BOOST_AUTO_TEST_CASE(check_parameter_name_clashes_test)
{
  const std::string text =
    "act a, b, c;                  \n"
    "                              \n"
    "proc P = delta;               \n"
    "                              \n"
    "init P;                       \n"
    ;
  lps::specification lpsspec = lps::parse_linear_process_specification(text);

  BOOST_CHECK(has_data_variable_name_clashes(parse_state_formula("nu X(n: Nat = 0). mu Y(n: Nat = 1). val(n == 0)", lpsspec)));
  BOOST_CHECK(!has_data_variable_name_clashes(parse_state_formula("nu X(n: Nat = 0). mu Y(m: Nat = 1). val(n == 0)", lpsspec)));
}

BOOST_AUTO_TEST_CASE(parse_state_formula_specification_test)
{
  const std::string text =
    "sort S;                       \n"
    "cons s0: S;                   \n"
    "act a: S;                     \n"
    "form forall s: S. [a(s)]true; \n"
    ;
  state_formula_specification x = parse_state_formula_specification(text);
  std::string text1 = state_formulas::pp(x);
  std::cout << text1 << std::endl;
  BOOST_CHECK_EQUAL(utilities::remove_whitespace(text), utilities::remove_whitespace(text1));
}


#ifndef MYSQL_ASIO_IMPL_DESERIALIZE_ROW_IPP
#define MYSQL_ASIO_IMPL_DESERIALIZE_ROW_IPP

#include <cstdlib>
#include <cmath>
#include <boost/lexical_cast/try_lexical_convert.hpp>
#include <date/date.h>

namespace mysql
{
namespace detail
{

inline Error deserialize_text_value_impl(
	std::string_view from,
	date& to
)
{
	constexpr std::size_t size = 4 + 2 + 2 + 2; // year, month, day, separators
	if (from.size() != size) return Error::protocol_value_error;
	unsigned year, month, day;
	char buffer [size + 1] {};
	memcpy(buffer, from.data(), from.size());
	int parsed = sscanf(buffer, "%4u-%2u-%2u", &year, &month, &day);
	if (parsed != 3) return Error::protocol_value_error;
	::date::year_month_day result (::date::year(year)/::date::month(month)/::date::day(day));
	if (!result.ok()) return Error::protocol_value_error;
	if (result > max_date || result < min_date) return Error::protocol_value_error;
	to = result;
	return Error::ok;
}

inline Error deserialize_text_value_impl(
	std::string_view from,
	time& to,
	unsigned decimals
)
{
	// size check
	constexpr std::size_t min_size = 2 + 2 + 2 + 2; // hours, mins, seconds, no micros
	constexpr std::size_t max_size = min_size + 1 + 1 + 7; // hour extra character, sign and micros
	decimals = std::min(decimals, 6u);
	if (from.size() < min_size || from.size() > max_size) return Error::protocol_value_error;

	// Parse it
	int hours;
	unsigned minutes, seconds, micros = 0;
	char buffer [max_size + 1] {};
	memcpy(buffer, from.data(), from.size());
	int parsed = decimals ? sscanf(buffer, "%4d:%2u:%2u.%6u", &hours, &minutes, &seconds, &micros) : // sign adds 1 char
			                sscanf(buffer, "%4d:%2u:%2u", &hours, &minutes, &seconds);
	if ((decimals && parsed != 4) || (!decimals && parsed != 3)) return Error::protocol_value_error;
	micros *= static_cast<unsigned>(std::pow(10, 6 - decimals));
	hours = std::abs(hours);
	bool is_negative = from[0] == '-';

	// Sum it
	auto res = std::chrono::hours(hours) + std::chrono::minutes(minutes) +
			   std::chrono::seconds(seconds) + std::chrono::microseconds(micros);
	if (is_negative)
	{
		res = -res;
	}

	// Range check
	if (res > max_time || res < min_time) return Error::protocol_value_error;

	to = res;
	return Error::ok;
}

inline Error deserialize_text_value_impl(
	std::string_view from,
	datetime& to,
	unsigned decimals
)
{
	// Length check
	constexpr std::size_t min_size = 4 + 5*2 + 5; // year, month, day, hour, minute, seconds, separators
	decimals = std::min(decimals, 6u);
	std::size_t expected_size = min_size + (decimals ? decimals + 1 : 0);
	if (from.size() != expected_size) return Error::protocol_value_error;

	// Date part
	date dt;
	auto err = deserialize_text_value_impl(from.substr(0, 10), dt);
	if (err != Error::ok) return err;

	// Time of day part
	time time_of_day;
	err = deserialize_text_value_impl(from.substr(11), time_of_day, decimals);
	if (err != Error::ok) return err;
	constexpr auto max_time_of_day = std::chrono::hours(24) - std::chrono::microseconds(1);
	if (time_of_day < std::chrono::seconds(0) || time_of_day > max_time_of_day) return Error::protocol_value_error;

	// Sum it up
	to = dt + time_of_day;
	return Error::ok;
}

template <typename T>
std::enable_if_t<std::is_arithmetic_v<T>, Error>
deserialize_text_value_impl(std::string_view from, T& to)
{
	bool ok = boost::conversion::try_lexical_convert(from.data(), from.size(), to);
	return ok ? Error::ok : Error::protocol_value_error;
}

inline Error deserialize_text_value_impl(std::string_view from, std::string_view& to)
{
	to = from;
	return Error::ok;
}

template <typename T, typename... Args>
Error deserialize_text_value_to_variant(std::string_view from, value& to, Args&&... args)
{
	T value;
	auto err = deserialize_text_value_impl(from, value, std::forward<Args>(args)...);
	if (err == Error::ok)
	{
		to = value;
	}
	return err;
}

inline bool is_next_field_null(
	DeserializationContext& ctx
)
{
	int1 type_byte;
	Error err = deserialize(type_byte, ctx);
	if (err == Error::ok)
	{
		if (type_byte.value == 0xfb)
		{
			return true; // it was null, do not rewind
		}
		ctx.rewind(1); // it was not null, rewind (this byte is part of the actual message)
	}
	return false;
}

}
}

inline mysql::Error mysql::detail::deserialize_text_value(
	std::string_view from,
	const field_metadata& meta,
	value& output
)
{
	switch (meta.protocol_type())
	{
    case protocol_field_type::tiny:
    case protocol_field_type::short_:
    case protocol_field_type::int24:
    case protocol_field_type::long_:
    case protocol_field_type::year:
    	return meta.is_unsigned() ?
    			deserialize_text_value_to_variant<std::uint32_t>(from, output) :
				deserialize_text_value_to_variant<std::int32_t>(from, output);
    case protocol_field_type::longlong:
    	return meta.is_unsigned() ?
    			deserialize_text_value_to_variant<std::uint64_t>(from, output) :
				deserialize_text_value_to_variant<std::int64_t>(from, output);
    case protocol_field_type::float_:
    	return deserialize_text_value_to_variant<float>(from, output);
    case protocol_field_type::double_:
    	return deserialize_text_value_to_variant<double>(from, output);
    case protocol_field_type::timestamp:
    case protocol_field_type::datetime:
    	return deserialize_text_value_to_variant<datetime>(from, output, meta.decimals());
    case protocol_field_type::date:
    	return deserialize_text_value_to_variant<date>(from, output);
    case protocol_field_type::time:
    	return deserialize_text_value_to_variant<time>(from, output, meta.decimals());
    // True string types
    case protocol_field_type::varchar:
    case protocol_field_type::var_string:
    case protocol_field_type::string:
    case protocol_field_type::tiny_blob:
    case protocol_field_type::medium_blob:
    case protocol_field_type::long_blob:
    case protocol_field_type::blob:
    case protocol_field_type::enum_:
    case protocol_field_type::set:
    // Anything else that we do not know how to interpret, we return as a binary string
    case protocol_field_type::decimal:
    case protocol_field_type::bit:
    case protocol_field_type::newdecimal:
    case protocol_field_type::geometry:
    default:
    	return deserialize_text_value_to_variant<std::string_view>(from, output);
	}
}

mysql::error_code mysql::detail::deserialize_text_row(
	DeserializationContext& ctx,
	const std::vector<field_metadata>& fields,
	std::vector<value>& output
)
{
	output.resize(fields.size());
	for (std::vector<value>::size_type i = 0; i < fields.size(); ++i)
	{
		bool is_null = is_next_field_null(ctx);
		if (is_null)
		{
			output[i] = nullptr;
		}
		else
		{
			string_lenenc value_str;
			Error err = deserialize(value_str, ctx);
			if (err != Error::ok) return make_error_code(err);
			err = deserialize_text_value(value_str.value, fields[i], output[i]);
			if (err != Error::ok) return make_error_code(err);
		}
	}
	if (!ctx.empty()) return make_error_code(Error::extra_bytes);
	return error_code();
}

#endif