/*
 *            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;
}

}}


#include "CS_Retro_NavigatorGadget.h"

#include <armadillo>
#include <algorithm>
#include <cmath>

#include "hoNDArray_math_util.cpp"

using namespace Gadgetron;

// class constructor
CS_Retro_NavigatorGadget::CS_Retro_NavigatorGadget()
{
}

// class destructor - delete temporal buffer/memory
CS_Retro_NavigatorGadget::~CS_Retro_NavigatorGadget()
{
}

// read flexible data header
int CS_Retro_NavigatorGadget::process_config(ACE_Message_Block *mb)
{
	// get gadget property
#ifdef __GADGETRON_VERSION_HIGHER_3_6__
	min_card_freq_	= MinCardFreq.value();
	max_card_freq_	= MaxCardFreq.value();
	min_resp_freq_	= MinRespFreq.value();
	max_resp_freq_	= MaxRespFreq.value();
	iNavMethod_		= NavigationMethod.value();
#else
	min_card_freq_	= *(get_int_value("MinCardFreq").get());
	max_card_freq_	= *(get_int_value("MaxCardFreq").get());
	min_resp_freq_	= *(get_int_value("MinRespFreq").get());
	max_resp_freq_	= *(get_int_value("MaxRespFreq").get());
	iNavMethod_		= *(get_int_value("NavigationMethod").get());
#endif

	// some basic error checking for parameters
	if (min_card_freq_ < 0 || max_card_freq_ < 0 || min_resp_freq_ < 0 || max_resp_freq_ < 0) {
		GERROR("Given parameters min_card_freq_, max_card_freq_, min_resp_freq_, max_resp_freq_ must not be negative!\n");
		return GADGET_FAIL;
	}

	if (min_card_freq_ >= max_card_freq_) {
		GERROR("max_card_freq_ must be greater than min_card_freq_!\n");
		return GADGET_FAIL;
	}

	if (min_resp_freq_ >= max_resp_freq_) {
		GERROR("max_resp_freq_ must be greater than min_resp_freq_!\n");
		return GADGET_FAIL;
	}

	return GADGET_OK;
}

int CS_Retro_NavigatorGadget::process(GadgetContainerMessage<ISMRMRD::ImageHeader> *m1,GadgetContainerMessage<hoNDArray<std::complex<float> > > *m2, GadgetContainerMessage<hoNDArray<std::complex<float> > > *m3)
{
	// fetch attribute values from header
	iNoChannels_ = m1->getObjectPtr()->channels;
	iNoNav_		 = m1->getObjectPtr()->user_int[5];
	lNoScans_	 = m3->getObjectPtr()->get_size(1);

	field_of_view_[0] = m1->getObjectPtr()->field_of_view[0];
	field_of_view_[1] = m1->getObjectPtr()->field_of_view[1];
	field_of_view_[2] = m1->getObjectPtr()->field_of_view[2];

	// get navigator signal according to selected method
	// 0: classical
	// 1: PCA
	switch (iNavMethod_) {
	case 0:
		try {
			getNav2D(*m2->getObjectPtr());
		} catch ( ... ) {
			GERROR("An exception occurred\n");
			return GADGET_FAIL;
		}

		break;

	case 1:
		try {
			getNav2DPCA(*m2->getObjectPtr());
		} catch ( ... ) {
			GERROR("An exception occurred\n");
			return GADGET_FAIL;
		}

		break;

	default:
		GERROR("Navigation method %d unknown! Please specify one via gadget property.\n", iNavMethod_);
		return GADGET_FAIL;
	}

	// update cardiac gates to 1 if no signal is present
	if (navigator_card_interpolated_.size() == 0) {
		set_number_of_gates(m1->getObjectPtr()->user_int[0], 1, 1);
	}

	// equalize vector lengths (zero-padding)
	while (navigator_card_interpolated_.size() < navigator_resp_interpolated_.size()) {
		navigator_card_interpolated_.push_back(0);
	}
	while (navigator_resp_interpolated_.size() < navigator_card_interpolated_.size()) {
		navigator_resp_interpolated_.push_back(0);
	}

	// achieve message format [respiratory navigator, cardiac navigator]
	GadgetContainerMessage<hoNDArray<float> > *tmp_m2 = new GadgetContainerMessage<hoNDArray<float> >();
	tmp_m2->getObjectPtr()->create(navigator_resp_interpolated_.size(), 2);

	// copy data to array (resp, card, resp, card,... - therefore Indices 2*iI [+1]
	float *fPtr = tmp_m2->getObjectPtr()->get_data_ptr();
	for (size_t iI = 0; iI < navigator_resp_interpolated_.size(); iI++) {
		fPtr[2*iI] = navigator_resp_interpolated_.at(iI);
		fPtr[2*iI+1] = navigator_card_interpolated_.at(iI);
	}

	m1->cont(tmp_m2);
	tmp_m2->cont(m3);

	if (this->next()->putq(m1) < 0) {
		return GADGET_FAIL;
	}

	// free memory
	m2->cont(NULL);
	m2->release();

	return GADGET_OK;
}

// get interpolated navigator signal
void CS_Retro_NavigatorGadget::getNav2D(hoNDArray<std::complex<float> > &aNav)
{
	GDEBUG("\n\n**************************************\n********** get navigator 2D **********\n**************************************\n\n");

	// reconstruct the 1-D projections for all measurements and all channels
	GINFO("domain transformation - k-space to image\n");

	/* MATLAB
	% Reconstruct the 1-D projections for all measurements and all channels
	dImg = fftshift(ifft(ifftshift(dKSpace)));
	dImg = fftshift(ifft(ifftshift(dImg, 3), [], 3), 3);
	dImg = flipdim(dImg, 1); % Invert the RO direction: 1-N -> H-F
	dImg = dImg(iNSamples/4:iNSamples.*3/4 - 1, :, :, :); % RO x t x PE x CH
	*/
	hoNDArray<std::complex<float> > aImg = aNav;
	hoNDFFT_CS<float>::instance()->ifftshift3D(aImg);
	hoNDFFT_CS<float>::instance()->ifft1(aImg);
	hoNDFFT_CS<float>::instance()->fftshift3D(aImg);
	hoNDFFT_CS<float>::instance()->ifft(&aImg, 2, true);
	flip_array(aImg, 0);

	// crop center part due to twofold oversampling
	GINFO("crop center part of two-fold oversampled data..\n");

	std::vector<size_t> vStart, vSize;

	vStart.push_back(static_cast<size_t>(std::floor(0.25*aNav.get_size(0)))-1);
	vStart.push_back(0);
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.push_back(std::floor(0.5*aNav.get_size(0)));
	vSize.push_back(aNav.get_size(1));
	vSize.push_back(aNav.get_size(2));
	vSize.push_back(aNav.get_size(3));

	get_subarray(aImg, vStart, vSize, aImg);

	// get channel power and normalize channel images
	GINFO("calculate channel power and normalize channel images..\n");

	std::vector<float> fPower(iNoChannels_);
	hoNDArray<std::complex<float> > aPower = aImg;
	aPower.fill(std::complex<float>(0,0));

	for (int c = 0; c < iNoChannels_; c++) {
		size_t offset = aImg.get_size(0)*aImg.get_size(1)*aImg.get_size(2)*c;
		hoNDArray<std::complex<float> > SubArray(aImg.get_size(0), aImg.get_size(1), aImg.get_size(2), aImg.get_data_ptr()+offset, false);
		fPower.at(c) = asum(SubArray);

		// fill part of the 3D array
		#pragma omp parallel for
		for (size_t i = 0; i < aImg.get_size(0)*aImg.get_size(1)*aImg.get_size(2); i++) {
			aPower.at(i+offset) = std::complex<float>(fPower.at(c), fPower.at(c));
		}
	}

	divide(aImg, aPower, aImg);

	GINFO("channel images normalized..\n");

	aPower.clear();

	// get x range of respiratory motion & FFT without scrambling
	hoNDArray<std::complex<float> > aFreq = aImg;
	hoNDFFT_CS<float>::instance()->fft(&aFreq, 1, false);
	multiplyConj(aFreq,aFreq,aPower);

	// conversion from complex float to float
	hoNDArray<float> afPower(aPower.get_dimensions());
	for (size_t i = 0; i < afPower.get_number_of_elements(); i++) {
		afPower[i] = aPower[i].real();
	}

	/*
	dIMGres = 1./(double(dNavPeriod)./1000.*double(iNMeasurements)); % The frequency resolution of dIMG in Hz
	dPower = squeeze(sum(dPower(:, round(1./(5.*dIMGres)):round(1./(3.*dIMGres)), :, :), 2)); % RO x PE x CH
	*/
	float fIMGRes = 1.0/((static_cast<float>(GlobalVar::instance()->iNavPeriod_)/1000.0)*static_cast<float>(iNoNav_)); // frequency resolution of aImg in Hz
	hoNDArray<float> aPowerInChan, aPowerAcrossChan;

	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(std::floor(1.0/(5*fIMGRes)-.5));
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(afPower.get_size(0));
	vSize.push_back(std::ceil(1.0/(3*fIMGRes)-.5)-std::ceil(1.0/(5*fIMGRes)-.5)+1);
	vSize.push_back(afPower.get_size(2));
	vSize.push_back(afPower.get_size(3));

	get_subarray(afPower, vStart, vSize, afPower);

	sum_dim(afPower, 1, afPower); // RO x PE x CH
	sum_dim(afPower, 1, aPowerInChan); // RO x CH
	sum_dim(aPowerInChan, 1, aPowerAcrossChan); // RO x 1

	// Prevent detection of regions in the abdomen
	for (long i = aPowerAcrossChan.get_number_of_elements(); i > std::floor(aPowerAcrossChan.get_number_of_elements()*.75); i--) {
		aPowerAcrossChan.at(i) = 0;
	}

	// get Gaussian filter kernel
	std::vector<float> vGaussian;
	filter1DGaussian(vGaussian, 20);
	arrayConv(aPowerAcrossChan, vGaussian, 0);

	// find index of maximum
	int iMaxIndex = amax(aPowerAcrossChan);

	GINFO("data filtered and maximum determined.. iMaxIndex: %i\n", iMaxIndex);

	if (iMaxIndex < 20) {
		GWARN("iMaxIndex=%d < 20! It is set to 20 to be able to perform further computing. Errors may occur.\n", iMaxIndex);
		iMaxIndex = 20;
	}

	if ((iMaxIndex < 20) || (iMaxIndex > static_cast<int>(aPowerInChan.get_size(0))-20)) {
		GERROR("Error: iMaxIndex out of bounds..\n");

		throw std::range_error("iMaxIndex out of bounds\n");
	}

	//-------------------------------------------------------------------------
	// sort out channels with no relevant information in target area
	GINFO("get channels which contain most information..\n");

	vStart.clear();
	vStart.push_back(iMaxIndex-20);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(41);
	vSize.push_back(iNoChannels_);

	get_subarray(aPowerInChan, vStart, vSize,aPowerInChan);

	GINFO("41 elements around maximum cropped..\n");

	std::vector<float> vGoodChannels;

	// loop over channels
	for (int c = 0; c < iNoChannels_; c++) {
		hoNDArray<float> aTmp;
		size_t offset = aPowerInChan.get_size(0)*c;
		aTmp.create(aPowerInChan.get_size(0), aPowerInChan.get_data_ptr()+offset, false);
		int iTmpInd = amax(aTmp);
		vGoodChannels.push_back(aTmp.at(iTmpInd));
	}

	int iIndex = std::max_element(vGoodChannels.begin(), vGoodChannels.end())- vGoodChannels.begin();
	float fMax = vGoodChannels.at(iIndex);
	int iNumGood = 0;

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		if (vGoodChannels.at(i) > .2*fMax) {
			vGoodChannels.at(i) = 1;
			iNumGood++;
		} else {
			vGoodChannels.at(i) = 0;
		}
	}

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		GDEBUG("vGoodChannels[%i]: %f\n", i,vGoodChannels.at(i));
	}

	//-------------------------------------------------------------------------
	// get the best PE line
	// get sub array of good channels
	GDEBUG("get best PE line..\n");

	hoNDArray<float> aPowerInPE(41,afPower.get_size(1), iNumGood);

	vStart.clear();
	vStart.push_back(iMaxIndex-20);
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(41);
	vSize.push_back(afPower.get_size(1));
	vSize.push_back(1);

	GDEBUG("vStart: %i, %i, %i, vSize: %i, %i, %i, afPower size: %i, %i, %i\n", vStart.at(0), vStart.at(1), vStart.at(2), vSize.at(0), vSize.at(1), vSize.at(2), afPower.get_size(0), afPower.get_size(1), afPower.get_size(2));

	size_t o = 0; //helper - only non-zero entries of vGoodChannels are of interest
	for (int c = 0; c < iNoChannels_; c++) {
		if (vGoodChannels.at(c) == 1) {
			hoNDArray<float> aTmp;
			aTmp.create(&vSize);
			vStart.at(2) = c;
			get_subarray(afPower, vStart, vSize, aTmp);

			// fill array
			size_t offset = aPowerInPE.get_size(0)*aPowerInPE.get_size(1)*o;
			for (size_t i = 0; i < aTmp.get_number_of_elements(); i++) {
				aPowerInPE.at(i + offset) = aTmp.at(i);
			}

			o++;
		}
	}

	sum_dim(aPowerInPE, 2, aPowerInPE);
	sum_dim(aPowerInPE, 0, aPowerInPE);

	GINFO("\n aPowerInPE\n");
	aPowerInPE.print(std::cout);

	iMaxIndex = amax(aPowerInPE);
	int iMaxChan = iMaxIndex;

	GINFO("found at %i\n", iMaxIndex);

	//-------------------------------------------------------------------------
	// find best corresponding channels according to best phase encoding position
	GINFO("searching for channels according to best PE line..\n");

	// get sub array
	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(0);
	vStart.push_back(iMaxIndex);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(aImg.get_size(0));
	vSize.push_back(aImg.get_size(1));
	vSize.push_back(1);
	vSize.push_back(aImg.get_size(3));

	aFreq.clear();

	get_subarray(aImg, vStart, vSize, aFreq);

	hoNDFFT_CS<float>::instance()->fft(&aFreq, 1, false);
	multiplyConj(aFreq,aFreq,aPower);

	// conversion from complex float to float
	afPower.clear();
	afPower.create(*aPower.get_dimensions());
	for (size_t i = 0; i < afPower.get_number_of_elements(); i++) {
		afPower[i] = aPower[i].real();
	}

	fIMGRes = 1.0/((static_cast<float>(GlobalVar::instance()->iNavPeriod_)/1000.0)*static_cast<float>(iNoNav_)); // frequency resolution of aImg in Hz

	aPowerInChan.clear();
	aPowerAcrossChan.clear();

	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(std::ceil(1.0/(5*fIMGRes)-.5));
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(afPower.get_size(0));
	vSize.push_back(std::ceil(1.0/(3*fIMGRes)-.5)-std::ceil(1.0/(5*fIMGRes)-.5)+1);
	vSize.push_back(afPower.get_size(2));
	vSize.push_back(afPower.get_size(3));

	GDEBUG("vSize: %i, %i, %i, %i - fIMGRes: %f\n", vSize.at(0), vSize.at(1), vSize.at(2), vSize.at(3), fIMGRes);

	get_subarray(afPower, vStart, vSize, afPower);
	sum_dim(afPower, 1, afPower); // RO x PE x CH
	sum_dim(afPower, 1, aPowerInChan); // RO x CH
	sum_dim(aPowerInChan, 1, aPowerAcrossChan); // RO x 1

	// Prevent detection of regions in the abdomen
	for (long i = aPowerAcrossChan.get_number_of_elements(); i > std::floor(aPowerAcrossChan.get_number_of_elements()*.75); i--) {
		aPowerAcrossChan.at(i) = 0;
	}

	GINFO("filter data with Gaussian kernel..\n");

	// get Gaussian filter kernel
	vGaussian.clear();
	filter1DGaussian(vGaussian, 20);
	arrayConv(aPowerAcrossChan, vGaussian);

	// find index of maximum
	iMaxIndex = amax(aPowerAcrossChan);
	int dX = iMaxIndex;

	GINFO("found at %i\n", iMaxIndex);

	// get good channels
	vStart.clear();
	vStart.push_back(iMaxIndex-20);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(41);
	vSize.push_back(aPowerInChan.get_size(1));

	get_subarray(aPowerInChan, vStart, vSize,aPowerInChan);

	vGoodChannels.clear();

	// loop over channels
	for (int c = 0; c < iNoChannels_; c++) {
		size_t offset = aPowerInChan.get_size(0)*c;

		hoNDArray<float> aTmp;
		aTmp.create(aPowerInChan.get_size(0), aPowerInChan.get_data_ptr()+offset, false);

		int iTmpInd = amax(aTmp);
		vGoodChannels.push_back(aTmp.at(iTmpInd));
	}

	iIndex = std::max_element(vGoodChannels.begin(), vGoodChannels.end())-vGoodChannels.begin();
	fMax =vGoodChannels.at(iIndex);
	iNumGood = 0;

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		if (vGoodChannels.at(i) > .2*fMax) {
			vGoodChannels.at(i) = 1;
			iNumGood++;
		} else {
			vGoodChannels.at(i) = 0;
		}
	}

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		GDEBUG("vGoodChannels[%i]: %f\n", i,vGoodChannels.at(i));
	}

	// get relevant image - dRelevantImg = squeeze(dImg(:, :, dPos, lGoodChannels));
	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(0);
	vStart.push_back(iMaxChan);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(aImg.get_size(0));
	vSize.push_back(aImg.get_size(1));
	vSize.push_back(1);
	vSize.push_back(1);

	hoNDArray<std::complex<float> > acRelevantImg(aImg.get_size(0), aImg.get_size(1), iNumGood);

	o = 0; //helper - only non-zero entries of vGoodChannels are of interest
	hoNDArray<std::complex<float> > afTmp;
	for (int c = 0; c < iNoChannels_; c++) {
		if (vGoodChannels.at(c) == 1) {
			vStart.at(3) = c;
			get_subarray(aImg, vStart, vSize, afTmp);

			// fill array
			size_t offset = acRelevantImg.get_size(0)*acRelevantImg.get_size(1)*o;
			for (size_t i = 0; i < afTmp.get_number_of_elements(); i++) {
				acRelevantImg.at(i + offset) = afTmp.at(i);
			}

			o++;
		}
	}

	//-------------------------------------------------------------------------
	GINFO("get SOSImg\n");

	hoNDArray<float> aRelevantImg(*acRelevantImg.get_dimensions());
	multiplyConj(acRelevantImg,acRelevantImg,acRelevantImg);
	// complex float to float datatype
	for (size_t i = 0; i < aRelevantImg.get_number_of_elements(); i++) {
		aRelevantImg[i] = acRelevantImg[i].real();
	}

	// loop over channels
	size_t tOffset = aRelevantImg.get_size(0)*aRelevantImg.get_size(1);
	hoNDArray<float> hafMax(aRelevantImg.get_dimensions());
	float *fPtr = hafMax.get_data_ptr();
	for (size_t iI = 0; iI < aRelevantImg.get_size(2); iI++) {
		hoNDArray<float> tmp(aRelevantImg.get_size(0), aRelevantImg.get_size(1), aRelevantImg.get_data_ptr()+tOffset*iI);
		iMaxIndex = amax(tmp);
		fMax = tmp.at(iMaxIndex);

		for (size_t iL = 0; iL < tOffset; iL++) {
			fPtr[iL+tOffset*iI] = fMax;
		}
	}

	divide(aRelevantImg, hafMax, aRelevantImg);

	hoNDArray<float> aSOSImg;
	sum_dim(aRelevantImg, 2, aSOSImg);
	sqrt(aSOSImg, aSOSImg);

	// convert array
	hoNDArray<std::complex<float> > cfaSOSImgTest(aSOSImg.get_dimensions());
	cfaSOSImgTest.fill(std::complex<float>(0.0, 0.0));
	std::complex<float> *cfPointer = cfaSOSImgTest.get_data_ptr();
	for (size_t iI = 0; iI < cfaSOSImgTest.get_number_of_elements(); iI++) {
		cfPointer[iI] = std::complex<float>(aSOSImg.at(iI), 0.0);
	}

	//-------------------------------------------------------------------------
	// get navigator
	int iDisplacementMax = 80; // [mm] diaphragm displacement (max. +/- 80mm)
	int iDisplacement = static_cast<int>(std::ceil(static_cast<float>(iDisplacementMax)/(static_cast<float>(field_of_view_[0])/static_cast<float>(aImg.get_size(0))) - 0.5));

	// fill last line in ref image
	hoNDArray<float> aRefImg = aSOSImg;
	aRefImg.fill(0.0);
	for (size_t iI = 0; iI < aSOSImg.get_size(0); iI++) {
		aRefImg.at(iI + aSOSImg.get_size(0)*(aSOSImg.get_size(1)-1)) = aSOSImg.at(iI + aSOSImg.get_size(0)*(aSOSImg.get_size(1)-1));
	}

	// create index vector and navigator vector (filled with zeros)
	std::vector<int> vIdx;
	for (size_t i = 0; i < aRefImg.get_size(1); i++) {
		vIdx.push_back(i);
	}

	std::vector<float> navigator_resp;
	for (size_t i = 0; i < aRefImg.get_size(1); i++) {
		navigator_resp.push_back(0);
	}

	hoNDArray<float> aRMSImg;
	for (int i = aRefImg.get_size(1)-2; i > 1; i--) {// -1 ; i--){
		aRMSImg.create(aRefImg.get_size(0), 2*iDisplacement+1);
		aRMSImg.fill(0.0);

		//MATLAB: tmp = dRefImg(:,idx(i+1:end))
		hoNDArray<float> tmp;

		vStart.clear();
		vStart.push_back(0);
		vStart.push_back(i+1);

		vSize.clear();
		vSize.push_back(aRefImg.get_size(0));
		vSize.push_back(aRefImg.get_size(1)-i-1);

		get_subarray(aRefImg, vStart, vSize, tmp);

		hoNDArray<float> repTmp(tmp.get_dimensions());

		//MATLAB: dSOSImg(:,idx(i)
		hoNDArray<float> aTmp;

		vStart.clear();
		vStart.push_back(0);
		vStart.push_back(vIdx.at(i));

		vSize.clear();
		vSize.push_back(aSOSImg.get_size(0));
		vSize.push_back(1);

		get_subarray(aSOSImg, vStart, vSize, aTmp);

		hoNDArray<float> aTmp2 = aTmp;
		circshift(aTmp2, -iDisplacement-1, 0);

		for (int l = -iDisplacement; l <= iDisplacement; l++) {
			//MATLAB: circshift(dSOSImg(:,idx(i)), iD)
			circshift(aTmp2, 1, 0);

			//MATLAB: (tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)]))
			hoNDArray<float> tmp2(tmp.get_dimensions());
			tmp2.fill(0.0); // result of subtraction

			int N = tmp.get_size(0), LE = tmp.get_size(1);
			float *pA = tmp.begin(), *pB = aTmp2.begin(), *pR = tmp2.begin();

			#pragma omp parallel for default(none) schedule(static) shared(N, pA, pB, pR, LE)
			for (int iL = 0; iL < LE; iL++) {
				for (int iE = 0; iE < N; iE++) {
					pR[iE + N*iL] = pA[iE + N*iL] - pB[iE];
				}
			}

			//MATLAB: (tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)])).^2
			multiply(tmp2, tmp2, tmp2);

			//MATLAB: sum((tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)])).^2,2)
			hoNDArray<float> hTmp2;
			if (tmp2.get_number_of_dimensions() > 1) {
				std::vector<size_t> vD = *tmp2.get_dimensions();
				vD.pop_back();
				hTmp2.create(&vD);
				hTmp2.fill(0.0);

				float *pNewArray = hTmp2.get_data_ptr();
				float *pOldArray = tmp2.get_data_ptr();
				int N = tmp2.get_size(1);
				int L = hTmp2.get_number_of_elements();

				#pragma omp parallel for default(none) schedule(static) shared(N, L, pNewArray, pOldArray)
				for (int sum_dim = 0; sum_dim < N; sum_dim++) {
					for (int i = 0; i < L; i++) {
						pNewArray[i] += pOldArray[i + L*sum_dim];
					}
				}
			}

			//MATLAB: dRMSImg(:,dDisplacement-iD+1) = sum((tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)])).^2,2)
			int iOffset = (iDisplacement-l)*hTmp2.get_size(0);
			memcpy(aRMSImg.begin() + iOffset, hTmp2.begin(), sizeof(float)*hTmp2.get_size(0));
		}

		//MATLAB: sum(dRMSImg(dX-round(dDisplacement/2):dX+round(dDisplacement/2),:))
		vStart.clear();
		vStart.push_back(dX-static_cast<int>((static_cast<float>(iDisplacement)/2)+.5));
		vStart.push_back(0);

		vSize.clear();
		vSize.push_back(iDisplacement+1);
		vSize.push_back(aRMSImg.get_size(1));

		get_subarray(aRMSImg, vStart, vSize, aRMSImg);

		hoNDArray<std::complex<float>> cfaRMSImgTest(aRMSImg.get_dimensions());
		cfaRMSImgTest.fill(std::complex<float>(0.0, 0.0));
		std::complex<float> *cfPointer = cfaRMSImgTest.get_data_ptr();
		for (size_t iI = 0; iI < cfaRMSImgTest.get_number_of_elements(); iI++) {
			cfPointer[iI] = std::complex<float>(aRMSImg.at(iI), 0.0);
		}

		sum_dim(aRMSImg, 0, aRMSImg);

		cfaRMSImgTest.clear();
		cfaRMSImgTest.create(aRMSImg.get_dimensions());
		cfaRMSImgTest.fill(std::complex<float>(0.0, 0.0));
		cfPointer = cfaRMSImgTest.get_data_ptr();
		for (size_t iI = 0; iI < cfaRMSImgTest.get_number_of_elements(); iI++) {
			cfPointer[iI] = std::complex<float>(aRMSImg.at(iI), 0.0);
		}

		//MATLAB: min(sum(dRMSImg(dX-round(dDisplacement/2):dX+round(dDisplacement/2),:)))
		// TODO: change back to amin (Gadgetron project seems to introduce the error)
// 		int iMinVal = amin(&aRMSImg);
		int iMinVal = aRMSImg.at(0);
		for (size_t i = 0; i < aRMSImg.get_number_of_elements(); i++) {
			if (aRMSImg.at(i) < iMinVal) {
				iMinVal = aRMSImg.at(i);
			}
		}

		//MATLAB: dDisplacement + 1 - dNav(i)
		navigator_resp.at(i) = iDisplacement - iMinVal;

		//MATLAB: circshift(dSOSImg(:,idx(i)), dNav(i))
		hoNDArray<float> aTmp3 = aTmp;
		circshift(aTmp3, navigator_resp.at(i), 0);

		if (i%20 == 0) {
			GINFO("Getting Navigator - %.1f %%\n", static_cast<float>(aRefImg.get_size(1)-2-i)/static_cast<float>(aRefImg.get_size(1)-2)*100);
		}

		//MATLAB: dRefImg(:,idx(i)) = circshift(dSOSImg(:,idx(i)), dNav(i))
		memcpy(aRefImg.get_data_ptr()+vIdx.at(i)*aRefImg.get_size(0), aTmp3.get_data_ptr(), sizeof(float)*aTmp3.get_size(0));
	}

	for (size_t i = 0; i < navigator_resp.size(); i++) {
		navigator_resp.at(i) *= -1;
	}

	// get Gaussian filter kernel and calculate convolution with navigator data
	vGaussian.clear();
	filter1DGaussian(vGaussian, 5);
	vectorConv(navigator_resp, vGaussian, 0);

	//-------------------------------------------------------------------------
	// interpolate navigator data signal to TR intervals
	GINFO("interpolation of navigator data to TR intervals..\n");

	for (size_t i = 0; i < navigator_resp.size(); i++) {
		navigator_resp.at(i) = -navigator_resp.at(i);
	}

	int iMin = std::min_element(navigator_resp.begin(), navigator_resp.end())-navigator_resp.begin();
	float fMin = navigator_resp.at(iMin);
	for (size_t i = 0; i < navigator_resp.size(); i++) {
		navigator_resp.at(i) -= fMin;
	}

	// build vector with elements 0..lNoScans_ to interpolate navigator_resp_interpolated_ below
	std::vector<float> vNavIndNew;
	for (long i = 0; i < lNoScans_; i++) {
		vNavIndNew.push_back(i);
	}

	GDEBUG("vNavInd size: %i, navigator_resp size: %i, vNavIndNew size: %i\n", GlobalVar::instance()->vNavInd_.size(), navigator_resp.size(), vNavIndNew.size());

	std::vector<float> vNavInd = GlobalVar::instance()->vNavInd_;
	navigator_resp_interpolated_ = interp1<float>(vNavInd, navigator_resp, vNavIndNew);

	return;
}

// get interpolated navigator signal by Principal Component Analysis
void CS_Retro_NavigatorGadget::getNav2DPCA(hoNDArray<std::complex<float> > &aNav)
{
	GDEBUG("\n\n**************************************\n********** get navigator 2D **********\n**************************************\n\n");

	// reconstruct the 1-D projections for all measurements and all channels
	GINFO("domain transformation - k-space to image\n");

	size_t iNSamples		= aNav.get_size(0);
	size_t iNMeasurement	= aNav.get_size(1);
	size_t iNavRes	 		= aNav.get_size(2);
	size_t iNChannels		= aNav.get_size(3);

	/* MATLAB
	% Reconstruct the 1-D projections for all measurements and all channels
	dImg = fftshift(ifft(ifftshift(dKSpace)));
	*/
	hoNDArray<std::complex<float> > aImg = aNav;
	hoNDFFT_CS<float>::instance()->ifftshift3D(aImg);
	hoNDFFT_CS<float>::instance()->ifft1(aImg);
	hoNDFFT_CS<float>::instance()->fftshift3D(aImg);

	// ATTENTION: Work with [s] values
	// dNavPeriod = dNavPeriod/1000; dTR = dTR/1000;
	min_card_freq_ /= 60;
	max_card_freq_ /= 60;
	min_resp_freq_ /= 60;
	max_resp_freq_ /= 60;

	// 1. Step: Restack
	// is already done in C++ code

	// 2. Step: Compute PCA based on KLT principal components are saved in coeff in descending order
	std::vector<size_t> coeff_dims;
	coeff_dims.push_back(iNMeasurement);
	coeff_dims.push_back(iNMeasurement);

	hoNDArray<std::complex<float> > coeff;
	coeff.create(&coeff_dims);
	coeff.delete_data_on_destruct(true);

	// prepare aImg for KLT
	// first: permute
	std::vector<size_t> aImg_new_order;
	aImg_new_order.push_back(0);
	aImg_new_order.push_back(2);
	aImg_new_order.push_back(3);
	aImg_new_order.push_back(1);
	aImg = permute(aImg, aImg_new_order);

	// then reshape
	std::vector<size_t> new_aImg_dims;
	new_aImg_dims.push_back(iNSamples*iNavRes*iNChannels);
	new_aImg_dims.push_back(iNMeasurement);
	aImg.reshape(&new_aImg_dims);

	GINFO("Performing KLT (may take a while)\n");

	hoNDKLT<std::complex<float> > VT;
	VT.prepare(aImg, static_cast<size_t>(1), static_cast<size_t>(0), true);
	VT.eigen_vector(coeff);

	GINFO("Continuing with search for motion\n");

	// 3. Step: search for respiratory motion
	// calculate frequency
	//%dFs = 1/((length(iLC)*dTR/1000)/(length(coeff)));
	double f_s = iNMeasurement/(lNoScans_*(GlobalVar::instance()->fTR_/1000.0)); // Get the sampling frequency (/1000 because fTR_ is in ms, not in s)

	// get next base of 2
	//%dFactorfft = 2.^nextpow2(size(dCoeff,1));
	unsigned int factor_fft = std::pow(2, std::ceil(log(coeff.get_size(0))/log(2)));

	// first zero padd coeff
	hoNDArray<std::complex<float> > coeff_padded;
	std::vector<size_t> coeff_padded_dims;
	coeff_padded_dims.push_back(factor_fft);
	coeff_padded_dims.push_back(coeff.get_size(1));
	coeff_padded.create(&coeff_padded_dims);
	for (size_t col = 0; col < coeff.get_size(1); col++) {
		size_t offset_old = col * coeff.get_size(0);
		size_t offset_new = col * coeff_padded.get_size(0);
		memcpy(coeff_padded.get_data_ptr()+offset_new, coeff.get_data_ptr()+offset_old, coeff.get_size(0)*sizeof(coeff.at(0)));
	}

	// and continue with fft
	// note: ifftshift2D is done before zero padding, thus none here
	hoNDFFT_CS<float>::instance()->fft1(coeff_padded);

	//%dCoeffF = abs(dCoeffF);
	hoNDArray<float> coeff_abs;
	coeff_abs.create(&coeff_padded_dims);
	for (size_t i = 0; i < coeff_padded.get_number_of_elements(); i++) {
		coeff_abs.at(i) = abs(coeff_padded.at(i));
	}

	// filtering
	// calculate lower and upper boundary frequency
	// Note: we only need the floor() value, so we build it directly instead of later on.
	//%Fl = dFactorfft/dFs * dCutOffResp(1);
	const unsigned int resp_f_l = std::floor(factor_fft/f_s * min_resp_freq_);
	//%Fu = dFactorfft/dFs * dCutOffResp(2);
	const unsigned int resp_f_u = std::floor(factor_fft/f_s * max_resp_freq_);

	if (resp_f_u <= resp_f_l) {
		GERROR("resp_f_u (=%d) must be greater than resp_f_l (=%d). Please set parameters correct!\n", resp_f_u, resp_f_l);
		throw runtime_error("Illegal parameters min_resp_freq_ or max_resp_freq_!");
	}

	// now get iPeak (dVal can be omitted). iPeak is the column number where the maximum value occures.
	// we could either implement it the Matlab way (crop hoNDArray, search max value per line and max col position)
	// or we do some intelligent data handling to get the position directly:
	float max_val = std::numeric_limits<float>::min();		// initialize as minimum (it could also be 0 because coeff_abs only contains abs values)
	size_t peak_position = 0;		// note: -1 is insufficient because size_t is unsigned, so pos is max. But we will rewrite max_val either.
	for (size_t f = static_cast<size_t>(resp_f_l); f <= static_cast<size_t>(resp_f_u); f++) {
		for (size_t component_number = search_range_min_-1; component_number < search_range_max_; component_number++) {		// be aware: search_range counts MATLAB like (from 1 to length)
			size_t pos = component_number * coeff_abs.get_size(0) + f;

			if (max_val < coeff_abs.at(pos)) {
				max_val = coeff_abs.at(pos);
				peak_position = component_number;
			}
		}
	}

	// now extract navi signal
	//%dRespNavi = real(dCoeff(:,iPeak)) - imag(dCoeff(:,iPeak));
	std::vector<float> resp_navi;

	for (size_t f = 0; f < coeff.get_size(0); f++) {
		size_t pos = peak_position * coeff.get_size(0) + f;
		resp_navi.push_back(coeff.at(pos).real() - coeff.at(pos).imag());
	}

	// and convolute
	//%dRespNavi = conv(dRespNavi, fGaussianLP(5), 'same');
	// first build gaussian low pass
	std::vector<float> gaussian_lowpass = {
		0.0269,
		0.2334,
		0.4794,
		0.2334,
		0.0269,
	};
	// convolute
	resp_navi = arma::conv_to<std::vector<float> >::from(arma::conv(arma::Col<float>(resp_navi), arma::Col<float>(gaussian_lowpass), "same"));

	// Note: Matlab implementation: %dRespNavi = -dRespNavi;
	// is implicitly done during KLT and can be omitted here

	// build vector with elements 0..lNoScans_ to interpolate navigator_resp_interpolated_ below
	std::vector<float> nav_ind_new;
	for (long i = 0; i < lNoScans_; i++) {
		nav_ind_new.push_back(i);
	}

	// get navigator indices
	std::vector<float> nav_ind = GlobalVar::instance()->vNavInd_;

	GDEBUG("nav_ind size: %i, resp_navi size: %i, nav_ind_new size: %i\n", nav_ind.size(), resp_navi.size(), nav_ind_new.size());

	// interpolate to output vector
	navigator_resp_interpolated_ = interp1<float>(nav_ind, resp_navi, nav_ind_new);

	// 4. Step: search for cardiac motion
	//%Fl = dFactorfft/dFs * dCutOffCard(1);
	const unsigned int card_f_l = std::floor(factor_fft/f_s * min_card_freq_);
	//%Fu = dFactorfft/dFs * dCutOffCard(2);
	const unsigned int card_f_u = std::floor(factor_fft/f_s * max_card_freq_);

	if (card_f_u <= card_f_l) {
		GERROR("card_f_u (=%d) must be greater than card_f_l (=%d). Please set parameters correct!\n", card_f_u, card_f_l);
		throw runtime_error("Illegal parameters min_resp_freq_ or max_resp_freq_!");
	}

	// now get iPeak (dVal can be omitted). iPeak is the column number where the maximum value occures.
	// we could either implement it the Matlab way (crop hoNDArray, search max value per line and max col position)
	// or we do some intelligent data handling to get the position directly:
	max_val = std::numeric_limits<float>::min();		// initialize as minimum (it could also be 0 because coeff_abs only contains abs values)
	peak_position = 0;		// note: -1 is insufficient because size_t is unsigned, so pos is max. But we will rewrite max_val either.
	for (size_t f = static_cast<size_t>(card_f_l); f <= static_cast<size_t>(card_f_u); f++) {
		for (size_t component_number = search_range_min_-1; component_number < search_range_max_; component_number++) {		// be aware: search_range counts MATLAB like (from 1 to length)
			size_t pos = component_number * coeff_abs.get_size(0) + f;

			if (max_val < coeff_abs.at(pos)) {
				max_val = coeff_abs.at(pos);
				peak_position = component_number;
			}
		}
	}

	// now extract navi signal
	//%dCardNavi = real(dCoeff(:,iPeak)) - imag(dCoeff(:,iPeak));
	for (size_t f = 0; f < coeff.get_size(0); f++) {
		size_t pos = peak_position * coeff.get_size(0) + f;
		navigator_card_interpolated_.push_back(coeff.at(pos).real() - coeff.at(pos).imag());
	}

	// interpolate signal
	//%dCardNavi = interp1(0:dNavPeriod:dNavPeriod*(size(dCoeff,1)-1), dCardNavi, 0:dTR:dTR*(length(iLC)-1), 'pchip');
	// first build new interpolation indices
	nav_ind.clear();
	nav_ind_new.clear();
	for (unsigned int i = 0; i < GlobalVar::instance()->iNavPeriod_*(coeff.get_size(0)-1); i += GlobalVar::instance()->iNavPeriod_) {
		nav_ind.push_back(i);
	}
	for (int counter = 0; counter < lNoScans_; counter++) {
		nav_ind_new.push_back(counter*GlobalVar::instance()->fTR_);
	}
	navigator_card_interpolated_ = interp1<float>(nav_ind, navigator_card_interpolated_, nav_ind_new);

	// change f_s
	//%dFs = 1./(dTR/1000); % now interpolated to TR level
	f_s = 1.0f/(GlobalVar::instance()->fTR_/1000);

	// butterworth filtering
	// % smoothing and R-peak detection
	//%[b,a] = fButterself(dCutOffCard(1)/(dFs/2), dCutOffCard(2)/(dFs/2));
	//%dCardNavi = FiltFiltSelf(b,a,double(dCardNavi));
	butterworth_filtering(min_card_freq_/(f_s/2.0f), max_card_freq_/(f_s/2.0f), navigator_card_interpolated_);

	//%dCardNavi = diff(dCardNavi); % search R-peak and differential signal
	navigator_card_interpolated_ = arma::conv_to<std::vector<float> >::from(arma::diff(arma::Col<float>(navigator_card_interpolated_)));

	//%dECGInt_ms = (dCardNavi-min(dCardNavi(floor(length(dCardNavi)/10):floor(length(dCardNavi)*8/10))))/(max(dCardNavi(floor(length(dCardNavi)/10):floor(length(dCardNavi)*8/10)))-min(dCardNavi(floor(length(dCardNavi)/10):floor(length(dCardNavi)*8/10))));
	// first get correction factors
	float factor_subtract	= *std::min_element(std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()/10), std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()*8/10));	// note: integer divisions ensure std::floor()
	float factor_divide		= *std::max_element(std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()/10), std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()*8/10)) - factor_subtract;

	// apply factors
	for (size_t i = 0; i < navigator_card_interpolated_.size(); i++) {
		navigator_card_interpolated_.at(i) = (navigator_card_interpolated_.at(i)-factor_subtract) / factor_divide;
	}
}

void CS_Retro_NavigatorGadget::butterworth_filtering(const double fl, const double fh, std::vector<float> &signal)
{
	// Filter the Signal with a first order butterworth filter

	//===============================================================
	//calculate the numerator and denominator of a first order butterworth filter. (End of calulation is indicated by =======)
	//===============================================================

	//ul = 4*tan(pi*fl/2);
	//uh = 4*tan(pi*fh/2);
	float ul = 4*tan(M_PI*fl/2);
	float uh = 4*tan(M_PI*fh/2);

	//Bandwidth and center frequency
	float Bw = uh - ul;
	float Wn = std::sqrt(ul*uh);

	// Matlab:
	//t1 = [1+(Wn*(-Bw/Wn)/4) Wn/4; -Wn/4 1];
	//t2 = [1-(Wn*(-Bw/Wn)/4) -Wn/4; Wn/4 1];
	//ad = inv(t2)*t1;
	// Note: indexing follows mathematical convention: t1[row number][column number]
	float t1[2][2], t2[2][2];
	std::complex<float> ad[2][2];	// note: only real values in ad, but calculation of e later on must be complex

	t1[0][0] = 1+(Wn*(-Bw/Wn)/4);
	t1[0][1] = Wn/4;
	t1[1][0] = -Wn/4;
	t1[1][1] = 1;

	// also transpose
	t2[0][0]= 1-(Wn*(-Bw/Wn)/4);
	t2[0][1]= -Wn/4;
	t2[1][0]= Wn/4;
	t2[1][1]= 1;

	// matlab: ad = inv(t2)*t1;
	float det_t2 = t2[0][0]*t2[1][1]-t2[0][1]*t2[1][0];
	ad[0][0] = (+t2[1][1]*t1[0][0]-t2[0][1]*t1[1][0])/det_t2;
	ad[0][1] = (+t2[1][1]*t1[0][1]-t2[0][1]*t1[1][1])/det_t2;
	ad[1][0] = (-t2[1][0]*t1[0][0]+t2[0][0]*t1[1][0])/det_t2;
	ad[1][1] = (-t2[1][0]*t1[0][1]+t2[0][0]*t1[1][1])/det_t2;

	//%den = poly(ad);
	//e = eig(ad);
	std::complex<float> e[2];

	// computate eigenvalues
	e[0] = (ad[0][0]+ad[1][1]+std::sqrt(std::pow(ad[0][0]+ad[1][1], 2) - std::complex<float>(4)*(ad[0][0]*ad[1][1]-ad[0][1]*ad[1][0])))/std::complex<float>(2);
	e[1] = (ad[0][0]+ad[1][1]-std::sqrt(std::pow(ad[0][0]+ad[1][1], 2) - std::complex<float>(4)*(ad[0][0]*ad[1][1]-ad[0][1]*ad[1][0])))/std::complex<float>(2);

	std::complex<float> kern[3];

	//den = [1 0 0];
	float den[] = {1, 0, 0};

	// In Matlab:
	//% Expand recursion formula
	//den(2) = den(2) - e(1)*den(1);
	//den(3) = den(3) - e(2)*den(2);
	//den(2) = den(2) - e(2)*den(1);
	//
	// Some thoughts:
	//	- e is either real or conjugate complex (see above)
	//	- in complex case: calculation of den becomes real (because e conjugate complex)
	//	- then: imaginary part can always be ignored and the matlab calculation reduces to:
	den[1] = -e[0].real() - e[1].real();
	den[2] = e[0].real() * e[1].real() + std::pow(e[0].imag(), 2);

	Wn = 2*atan(Wn/4);

	//%  normalize so |H(w)| == 1:
	//%kern = exp(-1i*Wn*(0:2));
	for (size_t k = 0; k < ARRAYSIZE(kern); k++) {
		kern[k] = std::exp(std::complex<float>(0.0, -1.0)*std::complex<float>(Wn)*std::complex<float>(k));
	}

	//f = (kern(1)*den(1)+kern(2)*den(2)+kern(3)*den(3))/(kern(1)-kern(3));
	std::complex<float> f = (kern[0]*den[0]+kern[1]*den[1]+kern[2]*den[2])/(kern[0]-kern[2]);

	float num[3] = {f.real(), 0, -f.real()};

	//===========================================================
	//end of calculating the numerator and denominator of the first order butterworth filter
	//===========================================================

	//filtfilt() equivalent function. b = num and a = den

	//============================================================
	//start of zero phase digital filter function
	//============================================================

	//n    = length(den); always 3 in first order case
	//z(n) = 0;
	//num = num / den(1);
	//den = den / den(1);
	float z[3] = {0};

	//Y    = zeros(size(X));
	//for m = 1:length(Y)
	//  Y(m) = num(1) * X(m) + z(1);
	//   for i = 2:n
	//      z(i - 1) = num(i) * X(m) + z(i) - den(i) * Y(m);
	//   end
	//end
	std::vector<float> Y;
	for (size_t m = 0; m < signal.size(); m++) {
		Y.push_back(num[0] * signal.at(m) + z[0]);

		for (size_t i = 1; i < ARRAYSIZE(den); i++) {
			z[i-1] = num[i] * signal.at(m) + z[i] - den[i] * Y.at(m);
		}
	}

	//clear z
	//z(n) = 0;
	z[0] = 0;
	z[1] = 0;
	z[2] = 0;

	//flip vector
	std::reverse(Y.begin(),Y.end());
	signal.clear();
	signal = Y;

	//Y    = zeros(size(X));
	// second round filtering (backward)
	//for m = 1:length(Y)
	//   Y(m) = b(1) * X(m) + z(1);
	//   for i = 2:n
	//      z(i - 1) = b(i) * X(m) + z(i) - a(i) * Y(m);
	//   end
	//end
	Y.clear();
	for (size_t m = 0; m < signal.size(); m++) {
		Y.push_back(num[0] * signal.at(m) + z[0]);

		for (size_t i = 1; i < ARRAYSIZE(den); i++) {
			z[i-1] = num[i] * signal.at(m) + z[i] - den[i] * Y.at(m);
		}
	}

	//flip again
	std::reverse(Y.begin(),Y.end());
	signal = Y;
	Y.clear();

	//============================================================
	//end of zero phase digital filter function
	//============================================================
}

GADGET_FACTORY_DECLARE(CS_Retro_NavigatorGadget)


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef PNSGA_HPP_
#define PNSGA_HPP_

#include <algorithm>
#include <limits>

#include <boost/foreach.hpp>

#include <sferes/stc.hpp>
#include <sferes/parallel.hpp>
#include <sferes/ea/ea.hpp>
#include <sferes/fit/fitness.hpp>
#include <sferes/ea/dom_sort_basic.hpp>
#include <sferes/ea/common.hpp>
#include <sferes/ea/crowd.hpp>

#ifdef ENABLE_TIMING
#include "clock.hpp"
#endif

namespace sferes
{
namespace ea
{
namespace pnsga
{
SFERES_CLASS(prob_dom_f){
public:
    template<typename Indiv>
    inline bool operator() (const Indiv &ind, const std::vector<Indiv>&pop) const{
        dbg::trace trace("dom_sort_trace" , DBG_HERE);
        BOOST_FOREACH(Indiv i, pop){
            if (dominate_flag(i, ind) == 1)
                return false;
        }
        return true;
    }

    template<typename I1, typename I2>
    inline int dominate_flag(const I1& i1, const I2& i2) const
    {
        dbg::trace trace("dom_sort_trace" , DBG_HERE);
        assert(i1->fit().objs().size());
        assert(i2->fit().objs().size());
        assert(i1->fit().objs().size() == i2->fit().objs().size());

        size_t nb_objs = i1->fit().objs().size();
        assert(nb_objs);

        bool flag1 = false, flag2 = false;
        for (unsigned i = 0; i < nb_objs; ++i)
        {
            assert(i < Params::ea::obj_pressure_size());
            float pressure = Params::ea::obj_pressure(i);;
#ifdef GRADUAL
            if(i == 1) pressure = Params::ea::pressure;
#endif
            if (misc::rand<float>() > pressure)
                continue;
            float fi1 = i1->fit().obj(i);
            float fi2 = i2->fit().obj(i);
            if (fi1 > fi2)
                flag1 = true;
            else
                if (fi2 > fi1)
                    flag2 = true;
        }
        if (flag1 && !flag2)
            return 1;
        else
            if (!flag1 && flag2)
                return -1;
            else
                return 0;
    }
};
}
// Main class
SFERES_EA(Pnsga, Ea)
{
public:
    typedef boost::shared_ptr<crowd::Indiv<Phen> > indiv_t;
    typedef typename std::vector<indiv_t> pop_t;
    typedef typename pop_t::iterator it_t;
    typedef typename std::vector<std::vector<indiv_t> > front_t;

    void random_pop()
    {
        parallel::init();

        _parent_pop.resize(Params::pop::size);
        assert(Params::pop::size % 4 == 0);

        pop_t init_pop((size_t)(Params::pop::size * Params::pop::initial_aleat));
        parallel::p_for(parallel::range_t(0, init_pop.size()), random<crowd::Indiv<Phen> >(init_pop));
        _init_pop(init_pop);
    }

    void epoch()
    {
        //Clearing populations

        _mixed_pop.clear();
        _child_pop.clear();
        this->_pop.clear();
        _pareto_front.clear();

        //Parent selection
#ifdef ENABLE_TIMING
        _epoch_clock.resetAndStart();
        _parent_selection_clock.resetAndStart();
#endif
        _selection (_parent_pop, _child_pop);

#ifdef ENABLE_TIMING
        _parent_selection_clock.stop();
        _mutate_clock.resetAndStart();
#endif

        //Mutation (and crossover, if applicable)

        parallel::p_for(parallel::range_t(0, _child_pop.size()),
                mutate<crowd::Indiv<Phen> >(_child_pop));

#ifdef ENABLE_TIMING
        _mutate_clock.stop();
        _eval_clock.resetAndStart();
#endif

        //Evaluation
#ifndef EA_EVAL_ALL
        _eval_pop(_child_pop);
        _merge(_parent_pop, _child_pop, _mixed_pop);
#else
        _merge(_parent_pop, _child_pop, _mixed_pop);
        _eval_pop(_mixed_pop);
#endif

#ifdef ENABLE_TIMING
        _eval_clock.stop();
        _mod_clock.resetAndStart();
#endif

        //Apply modifiers (such as behavioral diversity)
        _apply_modifier(_mixed_pop);

#ifdef ENABLE_TIMING
        _mod_clock.stop();
        _survivor_selection_clock.resetAndStart();
#endif

#ifndef NDEBUG
        BOOST_FOREACH(indiv_t & ind, _mixed_pop)
        for (size_t i = 0; i < ind->fit().objs().size(); ++i)
        {
            assert(!std::isnan(ind->fit().objs()[i]));
        }
#endif
        _fill_nondominated_sort(_mixed_pop, _parent_pop);
        _convert_pop(_parent_pop, this->_pop);

#ifdef ENABLE_TIMING
        _survivor_selection_clock.stop();
        _epoch_clock.stop();
#endif
        assert(_parent_pop.size() == Params::pop::size);
        assert(_pareto_front.size() <= Params::pop::size * 2);
        assert(this->_pop.size() == Params::pop::size);
    }

    const std::vector<boost::shared_ptr<Phen> >& pareto_front() const {
        return _pareto_front;
    }

    const pop_t& mixed_pop() const{
        return _mixed_pop;
    }

    const pop_t& parent_pop() const{
        return _parent_pop;
    }

    const pop_t& child_pop() const{
        return _child_pop;
    }

    void init_pop(std::vector<boost::shared_ptr<Phen> > population){
        //        	std::cout << "Reading population: pnsga" << std::endl;
        _mixed_pop.clear();
        for (size_t i = 0; i < population.size(); ++i){
            indiv_t indiv (new crowd::Indiv<Phen>(*population[i]));
            _mixed_pop.push_back(indiv);
        }
        _init_pop(_mixed_pop);
        _convert_pop(_parent_pop, this->_pop);
    }

    void init_parent_pop(std::vector<boost::shared_ptr<Phen> > population){
        dbg::assertion(DBG_ASSERTION(population.size() <= Params::pop::size));
        _parent_pop.clear();
        for (size_t i = 0; i < Params::pop::size; ++i){
            indiv_t indiv (new crowd::Indiv<Phen>(*population[i]));
            _parent_pop.push_back(indiv);
        }
    }

    void set_gen(size_t gen){
        this->_gen = gen;
    }

    void best_pop(std::vector<boost::shared_ptr<Phen> >& population){
        //        	std::cout << "Reading population: pnsga" << std::endl;
        _mixed_pop.clear();
        for (size_t i = 0; i < Params::pop::size*2; ++i){
            indiv_t indiv (new crowd::Indiv<Phen>());
            indiv->gen() = population[0]->gen();
            _mixed_pop.push_back(indiv);
        }
        _init_pop(_mixed_pop);
        _convert_pop(_parent_pop, this->_pop);
    }

#ifdef ENABLE_TIMING
public:
    //Timing related functions
    double getEpochTime() const{
        return _epoch_clock.time();
    }

    double getParentSelectionTime() const{
        return _parent_selection_clock.time();
    }

    double getMutateTime() const{
        return _mutate_clock.time();
    }

    double getEvalTime() const{
        return _eval_clock.time();
    }

    double getModTime() const{
        return _mod_clock.time();
    }

    double getSurvivorSelectionTime() const{
        return _survivor_selection_clock.time();
    }

protected:
    //Timing related variables
    Clock _epoch_clock;
    Clock _parent_selection_clock;
    Clock _mutate_clock;
    Clock _eval_clock;
    Clock _mod_clock;
    Clock _survivor_selection_clock;
#endif

protected:

    std::vector<boost::shared_ptr<Phen> > _pareto_front;

    pop_t _parent_pop;
    pop_t _child_pop;
    pop_t _mixed_pop;

    void _init_pop(pop_t& init_pop)
    {
        _eval_pop(init_pop);
        _apply_modifier(init_pop);
        front_t fronts;
        _rank_crowd(init_pop, fronts);
        _fill_nondominated_sort(init_pop, _parent_pop);

    }

    void _update_pareto_front(const front_t& fronts)
    {
        _convert_pop(fronts.front(), _pareto_front);
    }

    void _convert_pop(const pop_t& pop1, std::vector<boost::shared_ptr<Phen> >& pop2)
    {
        pop2.resize(pop1.size());
        for (size_t i = 0; i < pop1.size(); ++i)
            pop2[i] = pop1[i];
    }

    void _eval_pop(pop_t& pop)
    {
        this->_eval.eval(pop, 0, pop.size(), this->_fit_proto);
    }

    void _apply_modifier(pop_t& pop)
    {
        _convert_pop(pop, this->_pop);
        this->apply_modifier();
    }

    void _fill_nondominated_sort(pop_t& mixed_pop, pop_t& new_pop)
    {
        assert(mixed_pop.size());
        front_t fronts;
#ifndef NDEBUG
        BOOST_FOREACH(indiv_t & ind, mixed_pop)
        for (size_t i = 0; i < ind->fit().objs().size(); ++i)
        {
            assert(!std::isnan(ind->fit().objs()[i]));
        }
#endif
        _rank_crowd(mixed_pop, fronts);
        new_pop.clear();

        // fill the i first layers
        size_t i;
        for (i = 0; i < fronts.size(); ++i)
            if (fronts[i].size() + new_pop.size() < Params::pop::size)
                new_pop.insert(new_pop.end(), fronts[i].begin(), fronts[i].end());
            else
                break;

        size_t size = Params::pop::size - new_pop.size();
        // sort the last layer
        if (new_pop.size() < Params::pop::size)
        {
            std::sort(fronts[i].begin(), fronts[i].end(), crowd::compare_crowd());
            for (size_t k = 0; k < size; ++k)
            {
                assert(i < fronts.size());
                new_pop.push_back(fronts[i][k]);
            }
        }
        assert(new_pop.size() == Params::pop::size);
    }

    //
    void _merge(const pop_t& pop1, const pop_t& pop2, pop_t& pop3)
    {
        assert(pop1.size());
        assert(pop2.size());
        pop3.clear();
        pop3.insert(pop3.end(), pop1.begin(), pop1.end());
        pop3.insert(pop3.end(), pop2.begin(), pop2.end());
        assert(pop3.size() == pop1.size() + pop2.size());
    }

    // --- tournament selection ---
    void _selection(pop_t& old_pop, pop_t& new_pop)
    {
        new_pop.resize(old_pop.size());
        std::vector<size_t> a1, a2;
        misc::rand_ind(a1, old_pop.size());
        misc::rand_ind(a2, old_pop.size());
        // todo : this loop could be parallelized
        for (size_t i = 0; i < old_pop.size(); i += 4)
        {
            const indiv_t& p1 = _tournament(old_pop[a1[i]], old_pop[a1[i + 1]]);
            const indiv_t& p2 = _tournament(old_pop[a1[i + 2]], old_pop[a1[i + 3]]);
            const indiv_t& p3 = _tournament(old_pop[a2[i]], old_pop[a2[i + 1]]);
            const indiv_t& p4 = _tournament(old_pop[a2[i + 2]], old_pop[a2[i + 3]]);
            assert(i + 3 < new_pop.size());
            p1->cross(p2, new_pop[i], new_pop[i + 1]);
            p3->cross(p4, new_pop[i + 2], new_pop[i + 3]);
        }
    }

    const indiv_t& _tournament(const indiv_t& i1, const indiv_t& i2)
    {
        pnsga::prob_dom_f<Params> p;
        int flag = p.dominate_flag(i1, i2);
        if (flag == 1)
            return i1;
        if (flag == -1)
            return i2;
        if (i1->crowd() > i2->crowd())
            return i1;
        if (i1->crowd() < i2->crowd())
            return i2;
        if (misc::flip_coin())
            return i1;
        else
            return i2;
    }

    // --- rank & crowd ---
    void _rank_crowd(pop_t& pop, front_t& fronts){
        std::vector<size_t> ranks;
#ifndef NDEBUG
        BOOST_FOREACH(indiv_t& ind, pop)
        for (size_t i = 0; i < ind->fit().objs().size(); ++i)
        { assert(!std::isnan(ind->fit().objs()[i])); }
#endif
        typename Params::ea::dom_sort_f()(pop, fronts, pnsga::prob_dom_f<Params>(), ranks);
        _update_pareto_front(fronts);
        parallel::p_for(parallel::range_t(0, fronts.size()),
                crowd::assign_crowd<indiv_t >(fronts));

        for (size_t i = 0; i < ranks.size(); ++i)
            pop[i]->set_rank(ranks[i]);
        parallel::sort(pop.begin(), pop.end(), crowd::compare_ranks());;
    }

};
}
}
#endif