// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "sparse_optimizer.h"
#include <Eigen/LU>
#include <fstream>
#include <iomanip>
#include <chrono>

#include "../stuff/timeutil.h"
#include "../stuff/macros.h"
#include "../stuff/misc.h"

namespace g2o {

using namespace std;
using namespace Eigen;

template <typename Traits>
BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
  BlockSolverBase(),
  _linearSolver(linearSolver)
{
  // workspace
  _Hpp=0;
  _Hll=0;
  _Hpl=0;
  _HplCCS = 0;
  _HschurTransposedCCS = 0;
  _Hschur=0;
  _DInvSchur=0;
  _coefficients=0;
  _bschur = 0;
  _xSize=0;
  _numPoses=0;
  _numLandmarks=0;
  _sizePoses=0;
  _sizeLandmarks=0;
  _doSchur=true;
}

template <typename Traits>
void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks,
              int* blockLandmarkIndices, int numLandmarkBlocks,
              int s)
{
  deallocate();

  resizeVector(s);

  if (_doSchur) {
    // the following two are only used in schur
    assert(_sizePoses > 0 && "allocating with wrong size");
    _coefficients = new double [s];
    _bschur = new double[_sizePoses];
  }

  _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
  if (_doSchur) {
    _Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
    _Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
    _DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
    _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
    _HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
    _HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
#ifdef G2O_OPENMP
    _coefficientsMutex.resize(numPoseBlocks);
#endif
  }
}

template <typename Traits>
void BlockSolver<Traits>::deallocate()
{
  if (_Hpp){
    delete _Hpp;
    _Hpp=0;
  }
  if (_Hll){
    delete _Hll;
    _Hll=0;
  }
  if (_Hpl){
    delete _Hpl;
    _Hpl = 0;
  }
  if (_Hschur){
    delete _Hschur;
    _Hschur=0;
  }
  if (_DInvSchur){
    delete _DInvSchur;
    _DInvSchur=0;
  }
  if (_coefficients) {
    delete[] _coefficients;
    _coefficients = 0;
  }
  if (_bschur) {
    delete[] _bschur;
    _bschur = 0;
  }
  if (_HplCCS) {
    delete _HplCCS;
    _HplCCS = 0;
  }
  if (_HschurTransposedCCS) {
    delete _HschurTransposedCCS;
    _HschurTransposedCCS = 0;
  }
}

template <typename Traits>
BlockSolver<Traits>::~BlockSolver()
{
  delete _linearSolver;
  deallocate();
}

template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
  assert(_optimizer);

  size_t sparseDim = 0;
  _numPoses=0;
  _numLandmarks=0;
  _sizePoses=0;
  _sizeLandmarks=0;
  int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
  int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];

  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    int dim = v->dimension();
    if (! v->marginalized()){
      v->setColInHessian(_sizePoses);
      _sizePoses+=dim;
      blockPoseIndices[_numPoses]=_sizePoses;
      ++_numPoses;
    } else {
      v->setColInHessian(_sizeLandmarks);
      _sizeLandmarks+=dim;
      blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
      ++_numLandmarks;
    }
    sparseDim += dim;
  }
  resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
  delete[] blockLandmarkIndices;
  delete[] blockPoseIndices;

  // allocate the diagonal on Hpp and Hll
  int poseIdx = 0;
  int landmarkIdx = 0;
  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    if (! v->marginalized()){
      //assert(poseIdx == v->hessianIndex());
      PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
      if (zeroBlocks)
        m->setZero();
      v->mapHessianMemory(m->data());
      ++poseIdx;
    } else {
      LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
      if (zeroBlocks)
        m->setZero();
      v->mapHessianMemory(m->data());
      ++landmarkIdx;
    }
  }
  assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);

  // temporary structures for building the pattern of the Schur complement
  SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
  if (_doSchur) {
    schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
    schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
  }

  // here we assume that the landmark indices start after the pose ones
  // create the structure in Hpp, Hll and in Hpl
  for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
    OptimizableGraph::Edge* e = *it;

    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
      OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
      int ind1 = v1->hessianIndex();
      if (ind1 == -1)
        continue;
      int indexV1Bak = ind1;
      for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
        OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
        int ind2 = v2->hessianIndex();
        if (ind2 == -1)
          continue;
        ind1 = indexV1Bak;
        bool transposedBlock = ind1 > ind2;
        if (transposedBlock){ // make sure, we allocate the upper triangle block
          swap(ind1, ind2);
        }
        if (! v1->marginalized() && !v2->marginalized()){
          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
          if (_Hschur) {// assume this is only needed in case we solve with the schur complement
            schurMatrixLookup->addBlock(ind1, ind2);
          }
        } else if (v1->marginalized() && v2->marginalized()){
          // RAINER hmm.... should we ever reach this here????
          LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
        } else {
          if (v1->marginalized()){
            PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
            if (zeroBlocks)
              m->setZero();
            e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
          } else {
            PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
            if (zeroBlocks)
              m->setZero();
            e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
          }
        }
      }
    }
  }

  if (! _doSchur)
    return true;

  _DInvSchur->diagonal().resize(landmarkIdx);
  _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);

  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    if (v->marginalized()){
      const HyperGraph::EdgeSet& vedges=v->edges();
      for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
        for (size_t i=0; i<(*it1)->vertices().size(); ++i)
        {
          OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
          if (v1->hessianIndex()==-1 || v1==v)
            continue;
          for  (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
            for (size_t j=0; j<(*it2)->vertices().size(); ++j)
            {
              OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
              if (v2->hessianIndex()==-1 || v2==v)
                continue;
              int i1=v1->hessianIndex();
              int i2=v2->hessianIndex();
              if (i1<=i2) {
                schurMatrixLookup->addBlock(i1, i2);
              }
            }
          }
        }
      }
    }
  }

  _Hschur->takePatternFromHash(*schurMatrixLookup);
  delete schurMatrixLookup;
  _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);

  return true;
}

template <typename Traits>
bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
{
  for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
    int dim = v->dimension();
    if (! v->marginalized()){
      v->setColInHessian(_sizePoses);
      _sizePoses+=dim;
      _Hpp->rowBlockIndices().push_back(_sizePoses);
      _Hpp->colBlockIndices().push_back(_sizePoses);
      _Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
      ++_numPoses;
      int ind = v->hessianIndex();
      PoseMatrixType* m = _Hpp->block(ind, ind, true);
      v->mapHessianMemory(m->data());
    } else {
      std::cerr << "updateStructure(): Schur not supported" << std::endl;
      abort();
    }
  }
  resizeVector(_sizePoses + _sizeLandmarks);

  for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);

    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
      OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
      int ind1 = v1->hessianIndex();
      int indexV1Bak = ind1;
      if (ind1 == -1)
        continue;
      for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
        OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
        int ind2 = v2->hessianIndex();
        if (ind2 == -1)
          continue;
        ind1 = indexV1Bak;
        bool transposedBlock = ind1 > ind2;
        if (transposedBlock) // make sure, we allocate the upper triangular block
          swap(ind1, ind2);

        if (! v1->marginalized() && !v2->marginalized()) {
          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
        } else {
          std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
        }
      }
    }

  }

  return true;
}

template <typename Traits>
bool BlockSolver<Traits>::solve(){
  //cerr << __PRETTY_FUNCTION__ << endl;
  if (! _doSchur){
    double t=get_monotonic_time();
    bool ok = _linearSolver->solve(*_Hpp, _x, _b);
    G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
    if (globalStats) {
      globalStats->timeLinearSolver = get_monotonic_time() - t;
      globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
    }
    return ok;
  }

  // schur thing

  // backup the coefficient matrix
  double t=get_monotonic_time();

  // _Hschur = _Hpp, but keeping the pattern of _Hschur
  _Hschur->clear();
  _Hpp->add(_Hschur);

  //_DInvSchur->clear();
  memset (_coefficients, 0, _sizePoses*sizeof(double));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
  for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
    const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
    assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");

    // calculate inverse block for the landmark
    const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
    assert (D && D->rows()==D->cols() && "Error in landmark matrix");
    LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
    Dinv = D->inverse();

    LandmarkVectorType  db(D->rows());
    for (int j=0; j<D->rows(); ++j) {
      db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
    }
    db=Dinv*db;

    assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
    const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];

    for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
        it_outer != landmarkColumn.end(); ++it_outer) {
      int i1 = it_outer->row;

      const PoseLandmarkMatrixType* Bi = it_outer->block;
      assert(Bi);

      PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
      assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
      typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
#    ifdef G2O_OPENMP
      ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
#    endif
      Bb.noalias() += (*Bi)*db;

      assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
      typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();

      typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
      typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
      for (; it_inner != landmarkColumn.end(); ++it_inner) {
        int i2 = it_inner->row;
        const PoseLandmarkMatrixType* Bj = it_inner->block;
        assert(Bj);
        while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
          ++targetColumnIt;
        assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
        PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
        assert(Hi1i2);
        (*Hi1i2).noalias() -= BDinv*Bj->transpose();
      }
    }
  }
  //cerr << "Solve [marginalize] = " <<  get_monotonic_time()-t << endl;

  // _bschur = _b for calling solver, and not touching _b
  memcpy(_bschur, _b, _sizePoses * sizeof(double));
  for (int i=0; i<_sizePoses; ++i){
    _bschur[i]-=_coefficients[i];
  }

  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
  if (globalStats){
    globalStats->timeSchurComplement = get_monotonic_time() - t;
  }

  t=get_monotonic_time();
  bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
  if (globalStats) {
    globalStats->timeLinearSolver = get_monotonic_time() - t;
    globalStats->hessianPoseDimension = _Hpp->cols();
    globalStats->hessianLandmarkDimension = _Hll->cols();
    globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
  }
  //cerr << "Solve [decompose and solve] = " <<  get_monotonic_time()-t << endl;

  if (! solvedPoses)
    return false;

  // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
  // solution;
  double* xp = _x;
  double* cp = _coefficients;

  double* xl=_x+_sizePoses;
  double* cl=_coefficients + _sizePoses;
  double* bl=_b+_sizePoses;

  // cp = -xp
  for (int i=0; i<_sizePoses; ++i)
    cp[i]=-xp[i];

  // cl = bl
  memcpy(cl,bl,_sizeLandmarks*sizeof(double));

  // cl = bl - Bt * xp
  //Bt->multiply(cl, cp);
  _HplCCS->rightMultiply(cl, cp);

  // xl = Dinv * cl
  memset(xl,0, _sizeLandmarks*sizeof(double));
  _DInvSchur->multiply(xl,cl);
  //_DInvSchur->rightMultiply(xl,cl);
  //cerr << "Solve [landmark delta] = " <<  get_monotonic_time()-t << endl;

  return true;
}

template <typename Traits>
bool BlockSolver<Traits>::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
{
  double t = get_monotonic_time();
  bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
  if (globalStats) {
    globalStats->timeMarginals = get_monotonic_time() - t;
  }
  return ok;
}

template <typename Traits>
bool BlockSolver<Traits>::buildSystem()
{
  // clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
    OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
    assert(v);
    v->clearQuadraticForm();
  }
  _Hpp->clear();
  if (_doSchur) {
    _Hll->clear();
    _Hpl->clear();
  }

  // resetting the terms for the pairwise constraints
  // built up the current system by storing the Hessian blocks in the edges and vertices
# ifndef G2O_OPENMP
  // no threading, we do not need to copy the workspace
  JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
  // if running with threads need to produce copies of the workspace for each thread
  JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
# endif
  //printf("do linearize...........................................\n");
  //std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();

  for (int k = 0, m = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k, m++) {
    OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
    while(isnan(der_[6*m])) m++;
    e->set_j(der_[6*m],der_[6*m+1],der_[6*m+2],der_[6*m+3],der_[6*m+4],der_[6*m+5]);
    e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
    e->constructQuadraticForm();
#  ifndef NDEBUG
    for (size_t i = 0; i < e->vertices().size(); ++i) {
      const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
      if (! v->fixed()) {
        bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
        if (hasANan) {
          cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << endl;
          break;
        }
      }
    }
#  endif
  }
  //std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
  //std::chrono::duration<double, std::milli> time_span = t2 - t1;
  //printf("Costs %f ms to calculate the jacobian value, CPU \n", time_span.count());
  //printf("finish computing jacobian...........................................with edge numbre %d \n", counter);

  // flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
    OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
    int iBase = v->colInHessian();
    if (v->marginalized())
      iBase+=_sizePoses;
    v->copyB(_b+iBase);
  }

  return 0;
}

template <typename Traits>
bool BlockSolver<Traits>::setLambda(double lambda, bool backup)
{
  if (backup) {
    _diagonalBackupPose.resize(_numPoses);
    _diagonalBackupLandmark.resize(_numLandmarks);
  }
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numPoses > 100)
# endif
  for (int i = 0; i < _numPoses; ++i) {
    PoseMatrixType *b=_Hpp->block(i,i);
    if (backup)
      _diagonalBackupPose[i] = b->diagonal();
    b->diagonal().array() += lambda;
  }
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numLandmarks > 100)
# endif
  for (int i = 0; i < _numLandmarks; ++i) {
    LandmarkMatrixType *b=_Hll->block(i,i);
    if (backup)
      _diagonalBackupLandmark[i] = b->diagonal();
    b->diagonal().array() += lambda;
  }
  return true;
}

template <typename Traits>
void BlockSolver<Traits>::restoreDiagonal()
{
  assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions");
  assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions");
  for (int i = 0; i < _numPoses; ++i) {
    PoseMatrixType *b=_Hpp->block(i,i);
    b->diagonal() = _diagonalBackupPose[i];
  }
  for (int i = 0; i < _numLandmarks; ++i) {
    LandmarkMatrixType *b=_Hll->block(i,i);
    b->diagonal() = _diagonalBackupLandmark[i];
  }
}

template <typename Traits>
bool BlockSolver<Traits>::init(SparseOptimizer* optimizer, bool online)
{
  _optimizer = optimizer;
  if (! online) {
    if (_Hpp)
      _Hpp->clear();
    if (_Hpl)
      _Hpl->clear();
    if (_Hll)
      _Hll->clear();
  }
  _linearSolver->init();
  return true;
}

template <typename Traits>
void BlockSolver<Traits>::setWriteDebug(bool writeDebug)
{
  _linearSolver->setWriteDebug(writeDebug);
}

template <typename Traits>
bool BlockSolver<Traits>::saveHessian(const std::string& fileName) const
{
  return _Hpp->writeOctave(fileName.c_str(), true);
}

} // end namespace


/**
 *  This file is part of dvo.
 *
 *  Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  dvo is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dvo_core/rgbd_image.h>

#include <assert.h>

#include <boost/unordered_map.hpp>

#include <dvo_core/interpolation.h>

//#include "../util.h"
//#include "../stopwatch.h"

template<typename T>
static void pyrDownMeanSmooth(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      int x0 = x * 2;
      int x1 = x0 + 1;
      int y0 = y * 2;
      int y1 = y0 + 1;

      out.at<T>(y, x) = (T) ( (in.at<T>(y0, x0) + in.at<T>(y0, x1) + in.at<T>(y1, x0) + in.at<T>(y1, x1)) / 4.0f );
    }
  }
}

template<typename T>
static void pyrDownMeanSmoothIgnoreInvalid(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      int x0 = x * 2;
      int x1 = x0 + 1;
      int y0 = y * 2;
      int y1 = y0 + 1;

      T total = 0;
      int cnt = 0;

      if(std::isfinite(in.at<T>(y0, x0)))
      {
        total += in.at<T>(y0, x0);
        cnt++;
      }

      if(std::isfinite(in.at<T>(y0, x1)))
      {
        total += in.at<T>(y0, x1);
        cnt++;
      }

      if(std::isfinite(in.at<T>(y1, x0)))
      {
        total += in.at<T>(y1, x0);
        cnt++;
      }

      if(std::isfinite(in.at<T>(y1, x1)))
      {
        total += in.at<T>(y1, x1);
        cnt++;
      }

      if(cnt > 0)
      {
        out.at<T>(y, x) = (T) ( total / cnt );
      }
      else
      {
        out.at<T>(y, x) = InvalidDepth;
      }
    }
  }
}

template<typename T>
static void pyrDownMedianSmooth(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  cv::Mat in_smoothed;
  cv::medianBlur(in, in_smoothed, 3);

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      out.at<T>(y, x) = in_smoothed.at<T>(y * 2, x * 2);
    }
  }
}

RgbdImagePyramid::RgbdImagePyramid(cv::Mat intensity, cv::Mat depth) : levels(1)
{
  levels[0].intensity = intensity;
  levels[0].depth = depth;
  levels[0].initialize();
}

void RgbdImagePyramid::compute(const size_t num_levels)
{
  if(levels.size() >= num_levels) return;

  // if we already have some levels, we just need to compute the coarser levels
  size_t first = levels.size();

  levels.resize(num_levels);

  for(size_t idx = first; idx < num_levels; ++idx)
  {
    pyrDownMeanSmooth<IntensityType>(levels[idx - 1].intensity, levels[idx].intensity);
    //pyrDownMeanSmoothIgnoreInvalid<float>(levels[idx - 1].depth, levels[idx].depth);
    pyrDownMedianSmooth<float>(levels[idx - 1].depth, levels[idx].depth);
    levels[idx].initialize();
  }
}

RgbdImage& RgbdImagePyramid::level(size_t idx)
{
  assert(idx < levels.size());

  return levels[idx];
}

void RgbdImage::initialize()
{
  assert(hasIntensity() || hasDepth());

  if(hasIntensity() && hasDepth())
  {
    assert(intensity.size() == depth.size());
  }

  if(hasIntensity())
  {
    assert(intensity.type() == cv::DataType<IntensityType>::type && intensity.channels() == 1);
    width = intensity.cols;
    height = intensity.rows;
  }
  if(hasDepth())
  {
    assert(depth.type() == cv::DataType<DepthType>::type && depth.channels() == 1);
    width = depth.cols;
    height = depth.rows;
  }

  intensity_requires_calculation_ = true;
  depth_requires_calculation_ = true;
  pointcloud_requires_build_ = true;
}

bool RgbdImage::hasIntensity() const
{
  return !intensity.empty();
}

bool RgbdImage::hasRgb() const
{
  return !rgb.empty();
}

bool RgbdImage::hasDepth() const
{
  return !depth.empty();
}

void RgbdImage::calculateDerivatives()
{
  calculateIntensityDerivatives();
  calculateDepthDerivatives();
}

bool RgbdImage::calculateIntensityDerivatives()
{
  if(!intensity_requires_calculation_) return false;

  assert(hasIntensity());

  calculateDerivativeX<IntensityType>(intensity, intensity_dx);
  //calculateDerivativeY<IntensityType>(intensity, intensity_dy);
  calculateDerivativeYSseFloat(intensity, intensity_dy);
  /*
  cv::Mat dy_ref, diff;
  calculateDerivativeY<IntensityType>(intensity, dy_ref);
  cv::absdiff(dy_ref, intensity_dy, diff);
  tracker::util::show("diff", diff);
  cv::waitKey(0);
   */
  intensity_requires_calculation_ = false;
  return true;
}

void RgbdImage::calculateDepthDerivatives()
{
  if(!depth_requires_calculation_) return;

  assert(hasDepth());

  calculateDerivativeX<DepthType>(depth, depth_dx);
  calculateDerivativeY<DepthType>(depth, depth_dy);

  depth_requires_calculation_ = false;
}

template<typename T>
void RgbdImage::calculateDerivativeX(const cv::Mat& img, cv::Mat& result)
{
  result.create(img.size(), img.type());

  for(int y = 0; y < img.rows; ++y)
  {
    for(int x = 0; x < img.cols; ++x)
    {
      int prev = std::max(x - 1, 0);
      int next = std::min(x + 1, img.cols - 1);

      result.at<T>(y, x) = (T) (img.at<T>(y, next) - img.at<T>(y, prev)) * 0.5f;
    }
  }

  //cv::Sobel(img, result, -1, 1, 0, 3, 1.0f / 4.0f, 0, cv::BORDER_REPLICATE);

  // compiler auto-vectorization
  /*
  const float* img_ptr = img.ptr<float>();
  float* result_ptr = result.ptr<float>();

  for(int y = 0; y < img.rows; ++y)
  {
    *result_ptr++ = img_ptr[1] - img_ptr[0];

    for(int x = 1; x < img.cols - 1; ++x, ++img_ptr)
    {
      *result_ptr++ = img_ptr[2] - img_ptr[0];
    }

    *result_ptr++ = img_ptr[1] - img_ptr[0];

    img_ptr++;
  }
   */
}

template<typename T>
void RgbdImage::calculateDerivativeY(const cv::Mat& img, cv::Mat& result)
{
  result.create(img.size(), img.type());

  for(int y = 0; y < img.rows; ++y)
  {
    for(int x = 0; x < img.cols; ++x)
    {
      int prev = std::max(y - 1, 0);
      int next = std::min(y + 1, img.rows - 1);

      result.at<T>(y, x) = (T) (img.at<T>(next, x) - img.at<T>(prev, x)) * 0.5f;
    }
  }
  //cv::Sobel(img, result, -1, 0, 1, 3, 1.0f / 4.0f, 0, cv::BORDER_REPLICATE);

  // compiler auto-vectorization
  /*
  for(int y = 0; y < img.rows; ++y)
  {
    const float* prev_row = img.ptr<float>(std::max(y - 1, 0), 0);
    const float* next_row = img.ptr<float>(std::min(y + 1, img.rows - 1), 0);
    float* cur_row = result.ptr<float>(y, 0);

    for(int x = 0; x < img.cols; ++x)
    {
      *cur_row++ = *next_row++ - *prev_row++;
    }
  }
   */
}

void RgbdImage::buildPointCloud(const IntrinsicMatrix& intrinsics)
{
  if(!pointcloud_requires_build_) return;

  assert(hasDepth());

  static boost::unordered_map<IntrinsicMatrix, PointCloud, IntrinsicMatrix::Hash, IntrinsicMatrix::Equal> cache;

  if(cache.find(intrinsics) == cache.end())
  {
    PointCloud& def = cache[intrinsics];
    def.resize(Eigen::NoChange, width * height);

    int idx = 0;

    for(size_t y = 0; y < height; ++y)
    {
      for(size_t x = 0; x < width; ++x, ++idx)
      {
        def(0, idx) = (x - intrinsics.ox()) / intrinsics.fx();
        def(1, idx) = (y - intrinsics.oy()) / intrinsics.fy();
        def(2, idx) = 1.0;
        def(3, idx) = 0.0;
      }
    }
  }

  PointCloud& def = cache[intrinsics];
  pointcloud.resize(Eigen::NoChange, width * height);

  const float* depth_ptr = depth.ptr<float>();
  int idx = 0;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++depth_ptr, ++idx)
    {
      pointcloud.col(idx) = def.col(idx) * (*depth_ptr);
      pointcloud(3, idx) = 1.0;
    }
  }

  pointcloud_requires_build_ = false;
}

void RgbdImage::warpIntensity(const AffineTransform& transformationd, const PointCloud& reference_pointcloud, const IntrinsicMatrix& intrinsics, RgbdImage& result, PointCloud& transformed_pointcloud)
{
  Eigen::Affine3f transformation = transformationd.cast<float>();

  cv::Mat warped_image(intensity.size(), intensity.type());
  cv::Mat warped_depth(depth.size(), depth.type());

  float ox = intrinsics.ox();
  float oy = intrinsics.oy();

  float* warped_intensity_ptr = warped_image.ptr<IntensityType>();
  float* warped_depth_ptr = warped_depth.ptr<DepthType>();

  int outliers = 0;
  int total = 0;
  int idx = 0;

  transformed_pointcloud = transformation * reference_pointcloud;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++idx, ++warped_intensity_ptr, ++warped_depth_ptr)
    {

      const Eigen::Vector4f& p3d = transformed_pointcloud.col(idx);

      if(!std::isfinite(p3d(2)))
      {
        *warped_intensity_ptr = Invalid;
        *warped_depth_ptr = InvalidDepth;
        continue;
      }

      float x_projected = (float) (p3d(0) * intrinsics.fx() / p3d(2) + ox);
      float y_projected = (float) (p3d(1) * intrinsics.fy() / p3d(2) + oy);

      if(inImage(x_projected, y_projected))
      {
        float z = (float) p3d(2);

        *warped_intensity_ptr = Interpolation::bilinearWithDepthBuffer(this->intensity, this->depth, x_projected, y_projected, z);
        *warped_depth_ptr = z;
      }
      else
      {
        *warped_intensity_ptr = Invalid;
        *warped_depth_ptr = InvalidDepth;
        //outliers++;
      }

      //total++;
    }
  }

  result.intensity = warped_image;
  result.depth = warped_depth;
  result.initialize();
}

void RgbdImage::warpDepthForward(const AffineTransform& transformationx, const IntrinsicMatrix& intrinsics, RgbdImage& result, cv::Mat_<cv::Vec3d>& cloud)
{
  Eigen::Affine3d transformation = transformationx.cast<double>();

  cloud = cv::Mat_<cv::Vec3d>(depth.size(), cv::Vec3d(0, 0, 0));
  cv::Mat warped_depth = cv::Mat::zeros(depth.size(), depth.type());
  warped_depth.setTo(InvalidDepth);

  float ox = intrinsics.ox();
  float oy = intrinsics.oy();

  const float* depth_ptr = depth.ptr<float>();
  int outliers = 0;
  int total = 0;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++depth_ptr)
    {
      if(!std::isfinite(*depth_ptr))
      {
        continue;
      }

      float depth = *depth_ptr;
      Eigen::Vector3d p3d((x - ox) * depth / intrinsics.fx(), (y - oy) * depth / intrinsics.fy(), depth);
      Eigen::Vector3d p3d_transformed = transformation * p3d;

      float x_projected = (float) (p3d_transformed(0) * intrinsics.fx() / p3d_transformed(2) + ox);
      float y_projected = (float) (p3d_transformed(1) * intrinsics.fy() / p3d_transformed(2) + oy);

      if(inImage(x_projected, y_projected))
      {
        int yi = (int) y_projected, xi = (int) x_projected;

        if(!std::isfinite(warped_depth.at<DepthType>(yi, xi)) || (warped_depth.at<DepthType>(yi, xi) - 0.05) > depth)
          warped_depth.at<DepthType>(yi, xi) = depth;
      }

      p3d = p3d_transformed;

      total++;
      cloud(y, x) = cv::Vec3d(p3d(0), p3d(1), p3d(2));
    }
  }

  result.depth = warped_depth;
  result.initialize();
}

void RgbdImage::warpIntensityForward(const AffineTransform& transformationx, const IntrinsicMatrix& intrinsics, RgbdImage& result, cv::Mat_<cv::Vec3d>& cloud)
{
  Eigen::Affine3d transformation = transformationx.cast<double>();

  bool identity = transformation.affine().isIdentity(1e-6);

  cloud = cv::Mat_<cv::Vec3d>(intensity.size(), cv::Vec3d(0, 0, 0));
  cv::Mat warped_image = cv::Mat::zeros(intensity.size(), intensity.type());

  float ox = intrinsics.ox();
  float oy = intrinsics.oy();

  const float* depth_ptr = depth.ptr<float>();
  int outliers = 0;
  int total = 0;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++depth_ptr)
    {
      if(*depth_ptr <= 1e-6f) continue;

      float depth = *depth_ptr;
      Eigen::Vector3d p3d((x - ox) * depth / intrinsics.fx(), (y - oy) * depth / intrinsics.fy(), depth);

      if(!identity)
      {
        Eigen::Vector3d p3d_transformed = transformation * p3d;

        float x_projected = (float) (p3d_transformed(0) * intrinsics.fx() / p3d_transformed(2) + ox);
        float y_projected = (float) (p3d_transformed(1) * intrinsics.fy() / p3d_transformed(2) + oy);

        if(inImage(x_projected, y_projected))
        {
          int xp, yp;
          xp = (int) std::floor(x_projected);
          yp = (int) std::floor(y_projected);

          warped_image.at<IntensityType>(yp, xp) = intensity.at<IntensityType>(y, x);
        }
        else
        {
          outliers++;
        }

        p3d = p3d_transformed;
      }

      total++;
      cloud(y, x) = cv::Vec3d(p3d(0), p3d(1), p3d(2));
    }
  }

  //std::cerr << "warp out: " << outliers << " total: " << total << std::endl;

  if(identity)
  {
    warped_image = intensity;
  }
  else
  {
    //std::cerr << "did warp" << std::endl;
  }

  result.intensity = warped_image;
  result.depth = depth;
  result.initialize();
}

void RgbdImage::warpDepthForwardAdvanced(const AffineTransform& transformation, const IntrinsicMatrix& intrinsics, RgbdImage& result)
{
  assert(hasDepth());

  this->buildPointCloud(intrinsics);

  PointCloud transformed_pointcloud = transformation.cast<float>() * pointcloud;

  cv::Mat warped_depth(depth.size(), depth.type());
  warped_depth.setTo(InvalidDepth);

  float z_factor1 = transformation.rotation()(0, 0) + transformation.rotation()(0, 1) * (intrinsics.fx() / intrinsics.fy());
  float x_factor1 = -transformation.rotation()(2, 0) - transformation.rotation()(2, 1) * (intrinsics.fx() / intrinsics.fy());

  float z_factor2 = transformation.rotation()(1, 1) + transformation.rotation()(1, 0) * (intrinsics.fy() / intrinsics.fx());
  float y_factor2 = -transformation.rotation()(2, 1) - transformation.rotation()(2, 0) * (intrinsics.fy() / intrinsics.fx());

  for(int idx = 0; idx < height * width; ++idx)
  {
    Vector4 p3d = pointcloud.col(idx);
    NumType z = p3d(2);

    if(!std::isfinite(z)) continue;

    int x_length = (int) std::ceil(z_factor1 + x_factor1 * p3d(0) / z) + 1; // magic +1
    int y_length = (int) std::ceil(z_factor2 + y_factor2 * p3d(1) / z) + 1; // magic +1

    Vector4 p3d_transformed = transformed_pointcloud.col(idx);
    NumType z_transformed = p3d_transformed(2);

    int x_projected = (int) std::floor(p3d_transformed(0) * intrinsics.fx() / z_transformed + intrinsics.ox());
    int y_projected = (int) std::floor(p3d_transformed(1) * intrinsics.fy() / z_transformed + intrinsics.oy());

    // TODO: replace inImage(...) checks, with max(..., 0) on initial value of x_, y_ and  min(..., width/height) for their respective upper bound
    //for (int y_ = y_projected; y_ < y_projected + y_length; y_++)
    //  for (int x_ = x_projected; x_ < x_projected + x_length; x_++)

    int x_begin = std::max(x_projected, 0);
    int y_begin = std::max(y_projected, 0);
    int x_end = std::min(x_projected + x_length, (int) width);
    int y_end = std::min(y_projected + y_length, (int) height);

    for (int y = y_begin; y < y_end; ++y)
    {
      DepthType* v = warped_depth.ptr<DepthType>(y, x_begin);

      for (int x = x_begin; x < x_end; ++x, ++v)
      {
        if(!std::isfinite(*v) || (*v) > z_transformed)
        {
          (*v) = (DepthType) z_transformed;
        }
      }
    }
  }

  result.depth = warped_depth;
  result.initialize();
}

bool RgbdImage::inImage(const float& x, const float& y) const
{
  return x >= 0 && x < width && y >= 0 && y < height;
}


// Copyright (c) 2017, Lawrence Livermore National Security, LLC and
// UT-Battelle, LLC.
// Produced at the Lawrence Livermore National Laboratory and the Oak Ridge
// National Laboratory.
// LLNL-CODE-743438
// All rights reserved.
// This file is part of MGmol. For details, see https://github.com/llnl/mgmol.
// Please also read this link https://github.com/llnl/mgmol/LICENSE

//
//                  main.cc
//
//    Description:
//        Real grid, finite difference, molecular dynamics program
//        for with nonorthogonal localized orbitals.
//
//        Uses Mehrstellen operators, multigrid accelerations, and
//        non-local pseudopotentials.
//
//     Includes LDA and PBE exchange and correlation functionals.
//
// Units:
//   Potentials, eigenvalues and operators in Rydberg
//   Energies in Hartree
//
#include <cassert>
#include <iostream>
#include <iterator>
#include <vector>
using namespace std;

#ifdef _OPENMP
#include <omp.h>
#endif

#ifdef USE_CNR
#include <mkl.h>
#endif

#include <mpi.h>

#include "Control.h"
#include "DistMatrix.h"
#include "ExtendedGridOrbitals.h"
#include "LocGridOrbitals.h"
#include "MGmol.h"
#include "MGmol_MPI.h"
#include "MPIdata.h"
#include "MatricesBlacsContext.h"
#include "Mesh.h"
#include "PackedCommunicationBuffer.h"
#include "ReplicatedWorkSpace.h"
#include "SparseDistMatrix.h"
#include "magma_singleton.h"
#include "tools.h"

#include <fenv.h>
#include <sys/cdefs.h>
#include <time.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

//#include "MemTrack.h"

/*
void trapfpe () {
  feenableexcept(FE_INVALID|FE_DIVBYZERO|FE_OVERFLOW);
}
*/

// A helper function
template <class T>
ostream& operator<<(ostream& os, const vector<T>& v)
{
    copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
    return os;
}

int main(int argc, char** argv)
{
    // change handling of memory allocation errors
    set_new_handler(noMoreMemory);

    cout.sync_with_stdio();

    int mpirc = MPI_Init(&argc, &argv);
    if (mpirc != MPI_SUCCESS)
    {
        cerr << "MPI Initialization failed!!!" << endl;
        MPI_Abort(MPI_COMM_WORLD, 0);
    }
    MPI_Comm_rank(MPI_COMM_WORLD, &mype);
    assert(mype > -1);
    onpe0 = (mype == 0);

#ifdef HAVE_MAGMA
    magma_int_t magmalog;

    magmalog = magma_init();
    if (magmalog == MAGMA_SUCCESS)
    {
        std::cout << "MAGMA Initialization: success" << std::endl;
    }
    else
    {
        if (magmalog == MAGMA_ERR_UNKNOWN)
            std::cout << "MAGMA Initialization: unknown error" << std::endl;
        if (magmalog == MAGMA_ERR_HOST_ALLOC)
            std::cout << "MAGMA Initialization: fails host alloc" << std::endl;
        return 1;
    }
#endif

    string input_file("");
    string lrs_filename;
    string constraints_filename("");
    bool tcheck = false;

    float total_spin = 0.;
    bool with_spin   = false;

    po::variables_map vm;

    // use configure file if it can be found
    // std::string config_filename("mgmol.cfg");

    // read options from PE0 only
    if (onpe0) try
        {
            string config_file;

            // Declare a group of options that will be
            // allowed only on command line
            po::options_description generic("Generic options");
            generic.add_options()("version,v", "print version string")("help,h",
                "produce help message")("check", "check input")("config,c",
                po::value<string>(&config_file)->default_value("mgmol.cfg"),
                "name of a file of a configuration.")("atomicCoordinates,i",
                po::value<vector<string>>(),
                "coordinates filename")("LRsFilename,l",
                po::value<string>(&lrs_filename), "LRs filename");

            // Declare a group of options (with default when appropriate) that
            // will be allowed in config file
            po::options_description config("Configuration");
            config.add_options()(
                "spin", po::value<float>(), "system total spin")("verbosity",
                po::value<short>()->default_value(1), "verbosity level")(
                "constraintsFilename", po::value<string>(&constraints_filename),
                "Name of file with list of constraints")("xcFunctional",
                po::value<string>()->required(), "XC functional: LDA or PBE")(
                "FDtype", po::value<string>()->default_value("Mehrstellen"),
                "Finite Difference scheme")("charge",
                po::value<short>()->default_value(0), "system total charge")(
                "Domain.lx", po::value<float>()->required(),
                "domain dimension in x direction")("Domain.ly",
                po::value<float>()->required(),
                "domain dimension in y direction")("Domain.lz",
                po::value<float>()->required(),
                "domain dimension in z direction")("Domain.ox",
                po::value<float>()->required(), "domain origin in x direction")(
                "Domain.oy", po::value<float>()->required(),
                "domain origin in y direction")("Domain.oz",
                po::value<float>()->required(), "domain origin in z direction")(
                "Mesh.nx", po::value<short>()->required(),
                "mesh dimension in x direction")("Mesh.ny",
                po::value<short>()->required(),
                "mesh dimension in y direction")("Mesh.nz",
                po::value<short>()->required(),
                "mesh dimension in z direction")("Potentials.pseudopotential",
                po::value<vector<string>>()->multitoken(),
                "pseudopotentials list")("Potentials.external",
                po::value<vector<string>>()->multitoken(),
                "external potentials list")("Potentials.binExternal",
                po::value<bool>()->default_value(true),
                "binary external potential")("Restart.input_filename",
                po::value<string>()->default_value(""),
                "Read restart filename/directory")("Restart.input_level",
                po::value<short>()->default_value(0),
                "Read restart level")("Restart.input_type",
                po::value<string>()->default_value("distributed"),
                "Read restart type: distributed or single_file")(
                "Restart.output_filename",
                po::value<string>()->default_value("auto"),
                "Dump restart filename/directory")("Restart.output_level",
                po::value<short>()->default_value(3),
                "Write restart level")("Restart.output_type",
                po::value<string>()->default_value("distributed"),
                "Write restart type: distributed or single_file")(
                "Restart.interval", po::value<short>()->default_value(1000),
                "Restart frequency")("Restart.rescale_v",
                po::value<double>()->default_value(1.),
                "rescaling factor velocity of all atoms")("Poisson.bcx",
                po::value<string>()->default_value("periodic"),
                "boundary condition x")("Poisson.bcy",
                po::value<string>()->default_value("periodic"),
                "boundary condition y")("Poisson.bcz",
                po::value<string>()->default_value("periodic"),
                "boundary condition z")("Poisson.diel",
                po::value<string>()->default_value("off"),
                "continuum solvent: on/off")("Run.type",
                po::value<string>()->default_value("QUENCH"), "Run type")(
                "Quench.solver", po::value<string>()->default_value("ABPG"),
                "Iterative solver for quench")("Quench.max_steps",
                po::value<short>()->default_value(200),
                "Max. steps in loose quench")("Quench.max_steps_tight",
                po::value<short>()->default_value(1000),
                "Max. steps in tight quench")("Quench.atol",
                po::value<float>()->default_value(1.e-12),
                "Abs. tol. in quench convergence")("Quench.rtol",
                po::value<float>()->default_value(-1.),
                "Rel. tol. in quench convergence")("Quench.conv_criterion",
                po::value<string>()->default_value("deltaE"),
                "Convergence criterion")("Quench.MLWC", po::value<bool>(),
                "Compute MLWC in quench")("Quench.MLWF",
                po::value<bool>()->default_value(false),
                "Compute MLWF (apply rotation) in quench")(
                "Quench.num_lin_iterations",
                po::value<short>()->default_value(0),
                "Number of iterations without potential update in quench")(
                "Quench.preconditioner_num_levels",
                po::value<short>()->default_value(2),
                "Number of levels for MG preconditioner")(
                "Quench.spread_penalty_damping",
                po::value<float>()->default_value(0.),
                "Spread penalty damping factor")("Quench.spread_penalty_target",
                po::value<float>()->default_value(2.),
                "Spread penalty target")("SpreadPenalty.type",
                po::value<string>()->default_value("individual"),
                "Spread penalty type (individual,volume)")(
                "SpreadPenalty.damping", po::value<float>()->default_value(1.),
                "Spread penalty damping factor")("SpreadPenalty.target",
                po::value<float>()->default_value(-1.),
                "Spread penalty target")("SpreadPenalty.alpha",
                po::value<float>()->default_value(0.),
                "Spread penalty factor")("MD.num_steps",
                po::value<short>()->default_value(1), "number of MD steps")(
                "MD.dt", po::value<float>(), "time step for MD (a.u.)")(
                "MD.print_interval", po::value<short>()->default_value(1),
                "print intervale for MD data")("MD.print_directory",
                po::value<string>()->default_value("MD"),
                "print directory for MD data")("MD.thermostat",
                po::value<string>()->default_value("OFF"),
                "MD thermostat: ON or OFF")("MD.remove_mass_center_motion",
                po::value<bool>()->default_value(true),
                "Remove mass center motion")("MD.type",
                po::value<string>()->default_value("BOMD"), "MD type: BOMD")(
                "GeomOpt.type", po::value<string>()->default_value("LBFGS"),
                "Geometry optimization algorithm")("GeomOpt.tol",
                po::value<float>()->default_value(4.e-4),
                "Tolerance on forces for Geometry optimization")(
                "GeomOpt.max_steps", po::value<short>()->default_value(1),
                "max. number of Geometry optimization steps")("GeomOpt.dt",
                po::value<float>(), "Delta t for trial pseudo-time steps")(
                "atomicCoordinates", po::value<vector<string>>(),
                "coordinates filename")("Thermostat.type",
                po::value<string>()->default_value("Langevin"),
                "Thermostat type")("Thermostat.temperature",
                po::value<float>()->default_value(-1.),
                "Thermostat temperature")("Thermostat.relax_time",
                po::value<float>()->default_value(-1.),
                "Thermostat relaxation time")("Thermostat.width",
                po::value<float>()->default_value(-1.),
                "Thermostat width (for SCALING)")("Orbitals.nempty",
                po::value<short>()->default_value(0),
                "Number of empty orbitals")("Orbitals.initial_type",
                po::value<string>()->default_value("random"),
                "initial orbitals type")("Orbitals.initial_width",
                po::value<float>()->default_value(10000.),
                "initial orbitals radius")("Orbitals.temperature",
                po::value<float>()->default_value(0.),
                "electronic temperature [K]")("ProjectedMatrices.solver",
                po::value<string>()->default_value("exact"),
                "solver for projected matrices")("ProjectedMatrices.printMM",
                po::value<bool>()->default_value(false),
                "print projected matrices in MM format")(
                "LocalizationRegions.radius",
                po::value<float>()->default_value(1000.),
                "Localization regions radius")("LocalizationRegions.adaptive",
                po::value<bool>()->default_value(true),
                "Localization regions adaptivity")(
                "LocalizationRegions.move_tol",
                po::value<float>()->default_value(1000.),
                "Localization regions move tolerance")(
                "Parallel.atomic_info_radius",
                po::value<float>()->default_value(8.),
                "Max. distance for atomic data communication")(
                "LoadBalancing.alpha", po::value<float>()->default_value(0.0),
                "Parameter for computing bias for load balancing algo")(
                "LoadBalancing.damping_tol",
                po::value<float>()->default_value(0.9),
                "Damping parameter for computing bias for load balancing algo")(
                "LoadBalancing.max_iterations",
                po::value<short>()->default_value(50),
                "Maximum number of iterations for load balancing algo")(
                "LoadBalancing.modulo", po::value<short>()->default_value(1),
                "Modulos or parameter to control how often clusters are "
                "recomputed during md")("LoadBalancing.output_file",
                po::value<string>()->default_value(""),
                "Output file for dumping cluster information in vtk format");

            // Hidden options, will be allowed in config file, but will not be
            // shown to the user.
            po::options_description hidden("Hidden options");
            hidden.add_options()("Quench.interval_print_residual",
                po::value<short>()->default_value(0),
                "Print interval for residual in quench (0 for none)")(
                "Quench.required_tol", po::value<float>()->default_value(1000.),
                "required tolerance (code stops if not reached)")(
                "Quench.compute_cond_Gram",
                po::value<bool>()->default_value(false),
                "Compute condition number of S during quench")(
                "Quench.min_Gram_eigenvalue",
                po::value<float>()->default_value(0.),
                "min. eigenvalue for Gram matrix")("Quench.step_length",
                po::value<float>()->default_value(-1.),
                "Step length for corrections in quench")("Quench.ortho_freq",
                po::value<short>()->default_value(1000),
                "Orthonormalization frequency")(
                "Quench.pair_mlwf_distance_threshold",
                po::value<float>()->default_value(4.),
                "Max. distance between pairs for MLWF transform")(
                "AOMM.kernel_radius", po::value<float>()->default_value(-1.),
                "Radius of kernel functions in AOMM algorithm")(
                "AOMM.threshold_factor", po::value<float>()->default_value(-1.),
                "Multiplicative factor of kernel radius to set threshold in "
                "AOMM projectors dropping")("Orbitals.type",
                po::value<string>()->default_value("NO"),
                "orbital type")("Orbitals.dotProduct",
                po::value<string>()->default_value("diagonal"),
                "orbital dot product type")("Orbitals.overallocate_factor",
                po::value<float>()->default_value(1.2),
                "safety factor to use for static allocation of orbitals")(
                "Potentials.filterPseudo",
                po::value<char>()->default_value('f'),
                "filter")("Poisson.solver",
                po::value<string>()->default_value("CG"), "solver")(
                "Poisson.rho0", po::value<float>()->default_value(0.0004),
                "continuum solvent: rho0")("Poisson.beta",
                po::value<float>()->default_value(1.3),
                "continuum solvent: beta")("Poisson.FDtype",
                po::value<string>()->default_value("Mehrstellen"), "FDtype")(
                "Poisson.nu1", po::value<short>()->default_value(2), "nu_1")(
                "Poisson.nu2", po::value<short>()->default_value(2), "nu_2")(
                "Poisson.max_steps", po::value<short>()->default_value(20),
                "max. nb. steps Poisson solver")("Poisson.max_steps_initial",
                po::value<short>()->default_value(20),
                "max. nb. steps Poisson solver in first solve")(
                "Poisson.max_levels", po::value<short>()->default_value(10),
                "max. nb. MG levels Poisson solver")("Poisson.reset",
                po::value<bool>()->default_value(false),
                "reset Hartree potential at each MD step")("ABPG.m",
                po::value<short>()->default_value(1),
                "History length for Anderson extrapolation")("ABPG.beta",
                po::value<float>()->default_value(1.),
                "beta for Anderson extrapolation")("NLCG.parallel_transport",
                po::value<bool>()->default_value(true),
                "Turn ON/OFF parallel transport algorithm")(
                "MD.extrapolation_type", po::value<short>()->default_value(1),
                "MD extrapolation type")("MD.compute_cond_Gram",
                po::value<bool>()->default_value(false),
                "Compute condition number of S at end of quench")(
                "MD.min_Gram_eigenvalue", po::value<float>()->default_value(0.),
                "min. eigenvalue for Gram matrix")(
                "ShortSightedInverse.spread_factor",
                po::value<float>()->default_value(2.),
                "Shortsighted spread factor")("ShortSightedInverse.tol",
                po::value<float>()->default_value(1.e-10),
                "Shortsighted tolerance")("ShortSightedInverse.krylov_dim",
                po::value<short>()->default_value(10),
                "Shortsighted Krylov space max. dimension")(
                "ShortSightedInverse.max_iterations",
                po::value<short>()->default_value(10),
                "Shortsighted max. number of GMRES iterations")(
                "ShortSightedInverse.ilu_type",
                po::value<string>()->default_value("ILU"),
                "Shortsighted ILU type: ILU or ILUT")(
                "ShortSightedInverse.ilu_drop_tol",
                po::value<float>()->default_value(1.e-5),
                "Shortsighted ILU dropping tolerance")(
                "ShortSightedInverse.ilu_filling_level",
                po::value<short>()->default_value(0),
                "Shortsighted ILU filling level")(
                "ShortSightedInverse.ilut_max_fill",
                po::value<int>()->default_value(10000),
                "Shortsighted max. filling for ILUT")("Coloring.algo",
                po::value<string>()->default_value("RLF"),
                "Coloring algorithm: RLF or Greedy")("Coloring.scope",
                po::value<string>()->default_value("local"),
                "Coloring scope: local or global")(
                "LocalizationRegions.min_distance",
                po::value<float>()->default_value(0.),
                "min. distance between Localization centers")(
                "LocalizationRegions.extrapolation_scheme",
                po::value<string>()->default_value("linear"),
                "Extrapolation order for localization centers")(
                "LocalizationRegions.computation",
                po::value<short>()->default_value(0),
                "Flag for computing new centers from extrapolated orbitals.")(
                "DensityMatrix.mixing", po::value<float>()->default_value(1.),
                "Mixing coefficient for Density Matrix")("DensityMatrix.solver",
                po::value<string>()->default_value("Mixing"),
                "Algorithm for updating Density Matrix: Mixing, MVP, HMVP")(
                "DensityMatrix.nb_inner_it",
                po::value<short>()->default_value(3),
                "Max. number of inner iterations in DM optimization")(
                "DensityMatrix.algo",
                po::value<string>()->default_value("Diagonalization"),
                "Algorithm for computing Density Matrix. "
                "Diagonalization or SP2 or Chebyshev.")("DensityMatrix.use_old",
                po::value<bool>()->default_value(true),
                "Start DM optimization with matrix of previous WF step")(
                "DensityMatrix.approximation_order",
                po::value<short>()->default_value(500),
                "Polynomial order for Chebyshev approximation "
                "of density matrix. ")("DensityMatrix.approximation_ndigits",
                po::value<short>()->default_value(1),
                "Number of digits of precision for Chebyshev "
                "approximation of density matrix. ")(
                "DensityMatrix.approximation_power_maxits",
                po::value<short>()->default_value(100),
                "Maximum number of iterations for power method "
                "to compute interval for Chebyshev "
                "approximation of density matrix. ")("DensityMatrix.tol",
                po::value<float>()->default_value(1.e-7),
                "tolerance, used in iterative DM computation convergence "
                "criteria");

            po::options_description cmdline_options;
            cmdline_options.add(generic);

            po::options_description config_file_options;
            config_file_options.add(config).add(hidden);

            po::options_description visible("Allowed options");
            visible.add(generic).add(config);

            po::positional_options_description pd;
            pd.add("atomicCoordinates", -1);

            store(po::command_line_parser(argc, argv)
                      .options(cmdline_options)
                      .positional(pd)
                      .run(),
                vm);
            notify(vm);

            ifstream ifs(config_file.c_str());
            if (!ifs)
            {
                cout << "can not open config file: " << config_file << "\n";
                return 0;
            }
            else
            {
                store(parse_config_file(ifs, config_file_options), vm);
                notify(vm);
            }

            if (vm.count("help"))
            {
                if (onpe0) cout << visible << "\n";
                return 0;
            }

            if (vm.count("version"))
            {
                if (onpe0)
                {
#ifdef GITHASH
#define xstr(x) #x
#define LOG(x) cout << " MGmol: git_hash " << xstr(x) << endl;
                    LOG(GITHASH);
                    cout << endl;
#endif
                }
                return 0;
            }

            if (vm.count("check"))
            {
                tcheck = true;
            }
            if (vm.count("spin"))
            {
                total_spin = vm["spin"].as<float>();
                with_spin  = true;
                if (onpe0) cout << "Spin was set to " << total_spin << endl;
            }
            if (vm.count("atomicCoordinates"))
            {
                if (onpe0)
                    cout << "Input files is: "
                         << vm["atomicCoordinates"].as<vector<string>>()[0]
                         << "\n";
                input_file = vm["atomicCoordinates"].as<vector<string>>()[0];
            }
            else
            {
                if (vm["Restart.input_level"].as<short>() == 0)
                {
                    throw std::runtime_error(
                        "ERROR: No coordinates file provided!!!");
                }
            }

            if (onpe0) cout << "Spin: " << total_spin << "\n";

        } // try
        catch (exception& e)
        {
            cerr << e.what() << "\n";
            return 1;
        }

    MGmol_MPI::setup(MPI_COMM_WORLD, std::cout, with_spin);
    MGmol_MPI& mmpi      = *(MGmol_MPI::instance());
    MPI_Comm global_comm = mmpi.commGlobal();

    Control::setup(global_comm, with_spin, total_spin);
    Control& ct = *(Control::instance());

    ct.setOptions(vm);
    ct.sync();

    int ret = ct.checkOptions();
    if (ret < 0) return ret;

    mmpi.bcastGlobal(input_file);
    mmpi.bcastGlobal(lrs_filename);

#ifdef _OPENMP
    if (onpe0)
    {
        cout << " " << omp_get_max_threads() << " thread"
             << (omp_get_max_threads() > 1 ? "s " : " ");
        cout << "active" << endl << endl;
    }
    omp_set_nested(0);
    if (omp_get_nested())
    {
        cerr << "Nested parallelism not allowed" << endl;
        return 1;
    }
#endif

    // Enter main scope
    {
        // setup standard output and error
        sout = &std::cout;
        serr = &std::cerr;

        MGmolInterface* mgmol;
        if (ct.isLocMode())
            mgmol = new MGmol<LocGridOrbitals>(global_comm, *MPIdata::sout);
        else
            mgmol
                = new MGmol<ExtendedGridOrbitals>(global_comm, *MPIdata::sout);

        unsigned ngpts[3]    = { ct.ngpts_[0], ct.ngpts_[1], ct.ngpts_[2] };
        double origin[3]     = { ct.ox_, ct.oy_, ct.oz_ };
        const double cell[3] = { ct.lx_, ct.ly_, ct.lz_ };
        Mesh::setup(mmpi.commSpin(), ngpts, origin, cell, ct.lap_type);

        mgmol->setupFromInput(input_file);
        if (ct.restart_info < 3 || !ct.isLocMode())
            mgmol->setupLRsFromInput(lrs_filename);
        mgmol->setupConstraintsFromInput(constraints_filename);

        ct.checkNLrange();

        LocGridOrbitals::setDotProduct(ct.dot_product_type);

        Mesh* mymesh             = Mesh::instance();
        const pb::PEenv& myPEenv = mymesh->peenv();

        if (!ct.short_sighted)
        {
            MatricesBlacsContext::instance().setup(mmpi.commSpin(), ct.numst);

            dist_matrix::DistMatrix<DISTMATDTYPE>::setBlockSize(64);

            dist_matrix::DistMatrix<DISTMATDTYPE>::setDefaultBlacsContext(
                MatricesBlacsContext::instance().bcxt());

            ReplicatedWorkSpace<double>::instance().setup(ct.numst);

            dist_matrix::SparseDistMatrix<
                DISTMATDTYPE>::setNumTasksPerPartitioning(128);

            MGmol_MPI& mmpi = *(MGmol_MPI::instance());
            int npes        = mmpi.size();
            setSparseDistMatriConsolidationNumber(npes);
        }

        if (myPEenv.color() > 0)
        {
            cerr << "Code should be called with " << myPEenv.n_mpi_tasks()
                 << " MPI tasks only" << endl;
            ct.global_exit(2);
        }

        assert(myPEenv.color() == 0);

        if (myPEenv.color() == 0)
        {
            assert(ct.getMGlevels() >= -1);
            if (ct.getMGlevels() >= 0 && ct.getPrecondType() % 10 == 0)
            {
                const pb::Grid& mygrid = mymesh->grid();

                if ((mygrid.dim(0) % (1 << ct.getMGlevels())) != 0)
                {
                    cerr << "main: mygrid.dim(0)=" << mygrid.dim(0)
                         << " not evenly divisible by "
                         << "2^(ct.getMGlevels())=" << (1 << ct.getMGlevels())
                         << endl;
                    return -1;
                }
                if ((mygrid.dim(1) % (1 << ct.getMGlevels())) != 0)
                {
                    cerr << "main: mygrid.dim(1)=" << mygrid.dim(1)
                         << " not evenly divisible by "
                         << "2^(ct.getMGlevels())=" << (1 << ct.getMGlevels())
                         << endl;
                    return -1;
                }
                if ((mygrid.dim(2) % (1 << ct.getMGlevels())) != 0)
                {
                    cerr << "main: mygrid.dim(2)=" << mygrid.dim(2)
                         << " not evenly divisible by "
                         << "2^(ct.getMGlevels())=" << (1 << ct.getMGlevels())
                         << endl;
                    return -1;
                }
            }

            myPEenv.barrier(); // wait to see if everybody is OK before
                               // continuing...

            if (!tcheck)
            {
#ifdef DEBUG
                *MPIdata::sout << " Run begins on processor "
                               << myPEenv.mytask() << endl;
#endif

#ifdef USE_CNR
                /* use conditional numerical reproducibility for MKL */
                int my_cbwr_branch = mkl_cbwr_get_auto_branch();
                if (mkl_cbwr_set(my_cbwr_branch) != MKL_CBWR_SUCCESS)
                {
                    printf("Error in setting MKL_CBWR_BRANCH! Aborting…\n");
                    return (-1);
                }
#endif
                mgmol->run();
            }
            else
            {
                *MPIdata::sout << " Input parameters OK\n";
            }
        }
        delete mgmol;

        if (!ct.short_sighted)
        {
            // need to destroy any MPI based object befor calling MPI_Finalize
            MatricesBlacsContext::instance().clear();
        }
    } // close main scope

    // release memory for static arrays
    PackedCommunicationBuffer::deleteStorage();
    Mesh::deleteInstance();
    Control::deleteInstance();
    MGmol_MPI::deleteInstance();

#ifdef HAVE_MAGMA
    // Delete the data in the singleton before finalizing magma
    auto& magma_singleton = MagmaSingleton::get_magma_singleton();
    magma_singleton.free();

    magmalog = magma_finalize();

    if (magmalog == MAGMA_SUCCESS)
    {
        std::cout << "MAGMA Finalize: success" << std::endl;
    }
    else
    {
        if (magmalog == MAGMA_ERR_UNKNOWN)
            std::cout << "MAGMA Finalize: unknown error" << std::endl;
        if (magmalog == MAGMA_ERR_HOST_ALLOC)
            std::cout << "MAGMA FINALIZE: fails host alloc" << std::endl;
        return 1;
    }

#endif

    mpirc = MPI_Finalize();
    if (mpirc != MPI_SUCCESS)
    {
        cerr << "MPI Finalize failed!!!" << endl;
    }

    time_t tt;
    time(&tt);
    if (onpe0) cout << " Run ended at " << ctime(&tt) << endl;

    // MemTrack::TrackDumpBlocks();

    //    MemTrack::TrackListMemoryUsage();

    return 0;
}


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

#ifndef BOOST_MATH_RATIONAL_ADAPTER_HPP
#define BOOST_MATH_RATIONAL_ADAPTER_HPP

#include <iostream>
#include <iomanip>
#include <sstream>
#include <boost/cstdint.hpp>
#include <boost/functional/hash_fwd.hpp>
#include <boost/multiprecision/number.hpp>
#ifdef BOOST_MSVC
#  pragma warning(push)
#  pragma warning(disable:4512 4127)
#endif
#include <boost/rational.hpp>
#ifdef BOOST_MSVC
#  pragma warning(pop)
#endif

namespace boost{
namespace multiprecision{
namespace backends{

template <class IntBackend>
struct rational_adaptor
{
   typedef number<IntBackend>                integer_type;
   typedef boost::rational<integer_type>        rational_type;

   typedef typename IntBackend::signed_types    signed_types;
   typedef typename IntBackend::unsigned_types  unsigned_types;
   typedef typename IntBackend::float_types     float_types;

   rational_adaptor() BOOST_MP_NOEXCEPT_IF(noexcept(rational_type())) {}
   rational_adaptor(const rational_adaptor& o) BOOST_MP_NOEXCEPT_IF(noexcept(std::declval<rational_type&>() = std::declval<const rational_type&>()))
   {
      m_value = o.m_value;
   }
   rational_adaptor(const IntBackend& o) BOOST_MP_NOEXCEPT_IF(noexcept(rational_type(std::declval<const IntBackend&>()))) : m_value(o) {}

   template <class U>
   rational_adaptor(const U& u, typename enable_if_c<is_convertible<U, IntBackend>::value>::type* = 0)
      : m_value(static_cast<integer_type>(u)){}
   template <class U>
   explicit rational_adaptor(const U& u,
      typename enable_if_c<
         boost::multiprecision::detail::is_explicitly_convertible<U, IntBackend>::value && !is_convertible<U, IntBackend>::value
      >::type* = 0)
      : m_value(IntBackend(u)){}
   template <class U>
   typename enable_if_c<(boost::multiprecision::detail::is_explicitly_convertible<U, IntBackend>::value && !is_arithmetic<U>::value), rational_adaptor&>::type operator = (const U& u)
   {
      m_value = IntBackend(u);
   }

#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
   rational_adaptor(rational_adaptor&& o) BOOST_MP_NOEXCEPT_IF(noexcept(rational_type(std::declval<rational_type>()))) : m_value(static_cast<rational_type&&>(o.m_value)) {}
   rational_adaptor(IntBackend&& o) BOOST_MP_NOEXCEPT_IF(noexcept(rational_type(std::declval<IntBackend>()))) : m_value(static_cast<IntBackend&&>(o)) {}
   rational_adaptor& operator = (rational_adaptor&& o) BOOST_MP_NOEXCEPT_IF(noexcept(std::declval<rational_type&>() = std::declval<rational_type>()))
   {
      m_value = static_cast<rational_type&&>(o.m_value);
      return *this;
   }
#endif
   rational_adaptor& operator = (const rational_adaptor& o)
   {
      m_value = o.m_value;
      return *this;
   }
   rational_adaptor& operator = (const IntBackend& o)
   {
      m_value = o;
      return *this;
   }
   template <class Int>
   typename enable_if<is_integral<Int>, rational_adaptor&>::type operator = (Int i)
   {
      m_value = i;
      return *this;
   }
   template <class Float>
   typename enable_if<is_floating_point<Float>, rational_adaptor&>::type operator = (Float i)
   {
      int e;
      Float f = std::frexp(i, &e);
      f = std::ldexp(f, std::numeric_limits<Float>::digits);
      e -= std::numeric_limits<Float>::digits;
      integer_type num(f);
      integer_type denom(1u);
      if(e > 0)
      {
         num <<= e;
      }
      else if(e < 0)
      {
         denom <<= -e;
      }
      m_value.assign(num, denom);
      return *this;
   }
   rational_adaptor& operator = (const char* s)
   {
      std::string s1;
      multiprecision::number<IntBackend> v1, v2;
      char c;
      bool have_hex = false;
      const char* p = s; // saved for later

      while((0 != (c = *s)) && (c == 'x' || c == 'X' || c == '-' || c == '+' || (c >= '0' && c <= '9') || (have_hex && (c >= 'a' && c <= 'f')) || (have_hex && (c >= 'A' && c <= 'F'))))
      {
         if(c == 'x' || c == 'X')
            have_hex = true;
         s1.append(1, c);
         ++s;
      }
      v1.assign(s1);
      s1.erase();
      if(c == '/')
      {
         ++s;
         while((0 != (c = *s)) && (c == 'x' || c == 'X' || c == '-' || c == '+' || (c >= '0' && c <= '9') || (have_hex && (c >= 'a' && c <= 'f')) || (have_hex && (c >= 'A' && c <= 'F'))))
         {
            if(c == 'x' || c == 'X')
               have_hex = true;
            s1.append(1, c);
            ++s;
         }
         v2.assign(s1);
      }
      else
         v2 = 1;
      if(*s)
      {
         BOOST_THROW_EXCEPTION(std::runtime_error(std::string("Could not parse the string \"") + p + std::string("\" as a valid rational number.")));
      }
      data().assign(v1, v2);
      return *this;
   }
   void swap(rational_adaptor& o)
   {
      std::swap(m_value, o.m_value);
   }
   std::string str(std::streamsize digits, std::ios_base::fmtflags f)const
   {
      //
      // We format the string ourselves so we can match what GMP's mpq type does:
      //
      std::string result = data().numerator().str(digits, f);
      if(data().denominator() != 1)
      {
         result.append(1, '/');
         result.append(data().denominator().str(digits, f));
      }
      return result;
   }
   void negate()
   {
      m_value = -m_value;
   }
   int compare(const rational_adaptor& o)const
   {
      return m_value > o.m_value ? 1 : (m_value < o.m_value ? -1 : 0);
   }
   template <class Arithmatic>
   typename enable_if_c<is_arithmetic<Arithmatic>::value && !is_floating_point<Arithmatic>::value, int>::type compare(Arithmatic i)const
   {
      return m_value > i ? 1 : (m_value < i ? -1 : 0);
   }
   template <class Arithmatic>
   typename enable_if_c<is_floating_point<Arithmatic>::value, int>::type compare(Arithmatic i)const
   {
      rational_adaptor r;
      r = i;
      return this->compare(r);
   }
   rational_type& data() { return m_value; }
   const rational_type& data()const { return m_value; }

   template <class Archive>
   void serialize(Archive& ar, const mpl::true_&)
   {
      // Saving
      integer_type n(m_value.numerator()), d(m_value.denominator());
      ar & n;
      ar & d;
   }
   template <class Archive>
   void serialize(Archive& ar, const mpl::false_&)
   {
      // Loading
      integer_type n, d;
      ar & n;
      ar & d;
      m_value.assign(n, d);
   }
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/)
   {
      typedef typename Archive::is_saving tag;
      serialize(ar, tag());
   }
private:
   rational_type m_value;
};

template <class IntBackend>
inline void eval_add(rational_adaptor<IntBackend>& result, const rational_adaptor<IntBackend>& o)
{
   result.data() += o.data();
}
template <class IntBackend>
inline void eval_subtract(rational_adaptor<IntBackend>& result, const rational_adaptor<IntBackend>& o)
{
   result.data() -= o.data();
}
template <class IntBackend>
inline void eval_multiply(rational_adaptor<IntBackend>& result, const rational_adaptor<IntBackend>& o)
{
   result.data() *= o.data();
}
template <class IntBackend>
inline void eval_divide(rational_adaptor<IntBackend>& result, const rational_adaptor<IntBackend>& o)
{
   using default_ops::eval_is_zero;
   if(eval_is_zero(o))
   {
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero."));
   }
   result.data() /= o.data();
}

template <class R, class IntBackend>
inline typename enable_if_c<number_category<R>::value == number_kind_floating_point>::type eval_convert_to(R* result, const rational_adaptor<IntBackend>& backend)
{
   //
   // The generic conversion is as good as anything we can write here:
   //
   ::boost::multiprecision::detail::generic_convert_rational_to_float(*result, backend);
}

template <class R, class IntBackend>
inline typename enable_if_c<(number_category<R>::value != number_kind_integer) && (number_category<R>::value != number_kind_floating_point)>::type eval_convert_to(R* result, const rational_adaptor<IntBackend>& backend)
{
   typedef typename component_type<number<rational_adaptor<IntBackend> > >::type comp_t;
   comp_t num(backend.data().numerator());
   comp_t denom(backend.data().denominator());
   *result = num.template convert_to<R>();
   *result /= denom.template convert_to<R>();
}

template <class R, class IntBackend>
inline typename enable_if_c<number_category<R>::value == number_kind_integer>::type eval_convert_to(R* result, const rational_adaptor<IntBackend>& backend)
{
   typedef typename component_type<number<rational_adaptor<IntBackend> > >::type comp_t;
   comp_t t = backend.data().numerator();
   t /= backend.data().denominator();
   *result = t.template convert_to<R>();
}

template <class IntBackend>
inline bool eval_is_zero(const rational_adaptor<IntBackend>& val)
{
   return eval_is_zero(val.data().numerator().backend());
}
template <class IntBackend>
inline int eval_get_sign(const rational_adaptor<IntBackend>& val)
{
   return eval_get_sign(val.data().numerator().backend());
}

template<class IntBackend, class V>
inline void assign_components(rational_adaptor<IntBackend>& result, const V& v1, const V& v2)
{
   result.data().assign(v1, v2);
}

template <class IntBackend>
inline std::size_t hash_value(const rational_adaptor<IntBackend>& val)
{
   std::size_t result = hash_value(val.data().numerator());
   boost::hash_combine(result, val.data().denominator());
   return result;
}

} // namespace backends

template<class IntBackend>
struct expression_template_default<backends::rational_adaptor<IntBackend> > : public expression_template_default<IntBackend> {};

template<class IntBackend>
struct number_category<backends::rational_adaptor<IntBackend> > : public mpl::int_<number_kind_rational>{};

using boost::multiprecision::backends::rational_adaptor;

template <class T>
struct component_type<rational_adaptor<T> >
{
   typedef number<T> type;
};

template <class IntBackend, expression_template_option ET>
inline number<IntBackend, ET> numerator(const number<rational_adaptor<IntBackend>, ET>& val)
{
   return val.backend().data().numerator();
}
template <class IntBackend, expression_template_option ET>
inline number<IntBackend, ET> denominator(const number<rational_adaptor<IntBackend>, ET>& val)
{
   return val.backend().data().denominator();
}

#ifdef BOOST_NO_SFINAE_EXPR

namespace detail{

template<class U, class IntBackend>
struct is_explicitly_convertible<U, rational_adaptor<IntBackend> > : public is_explicitly_convertible<U, IntBackend> {};

}

#endif

}} // namespaces

namespace std{

template <class IntBackend, boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::rational_adaptor<IntBackend>, ExpressionTemplates> > : public std::numeric_limits<boost::multiprecision::number<IntBackend, ExpressionTemplates> >
{
   typedef std::numeric_limits<boost::multiprecision::number<IntBackend> > base_type;
   typedef boost::multiprecision::number<boost::multiprecision::rational_adaptor<IntBackend> > number_type;
public:
   BOOST_STATIC_CONSTEXPR bool is_integer = false;
   BOOST_STATIC_CONSTEXPR bool is_exact = true;
   BOOST_STATIC_CONSTEXPR number_type (min)() { return (base_type::min)(); }
   BOOST_STATIC_CONSTEXPR number_type (max)() { return (base_type::max)(); }
   BOOST_STATIC_CONSTEXPR number_type lowest() { return -(max)(); }
   BOOST_STATIC_CONSTEXPR number_type epsilon() { return base_type::epsilon(); }
   BOOST_STATIC_CONSTEXPR number_type round_error() { return epsilon() / 2; }
   BOOST_STATIC_CONSTEXPR number_type infinity() { return base_type::infinity(); }
   BOOST_STATIC_CONSTEXPR number_type quiet_NaN() { return base_type::quiet_NaN(); }
   BOOST_STATIC_CONSTEXPR number_type signaling_NaN() { return base_type::signaling_NaN(); }
   BOOST_STATIC_CONSTEXPR number_type denorm_min() { return base_type::denorm_min(); }
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION

template <class IntBackend, boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::rational_adaptor<IntBackend>, ExpressionTemplates> >::is_integer;
template <class IntBackend, boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::rational_adaptor<IntBackend>, ExpressionTemplates> >::is_exact;

#endif

}

#endif


/*
 * Copyright 2009-2020 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.
 *
 */

// Standard includes
#include <chrono>

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/constants.h>
#include <votca/tools/property.h>

// Local VOTCA includes
#include "votca/xtp/gnode.h"
#include "votca/xtp/qmstate.h"
#include "votca/xtp/topology.h"

// Local private VOTCA includes
#include "kmcmultiple.h"

namespace votca {
namespace xtp {
void KMCMultiple::ParseSpecificOptions(const tools::Property& options) {

  runtime_ = options.get(".runtime").as<double>();
  field_ = options.get(".field").as<Eigen::Vector3d>();
  double mtobohr = 1E9 * tools::conv::nm2bohr;
  field_ *=
      (tools::conv::ev2hrt / mtobohr);  // Converting from V/m to Hartree/bohr

  outputtime_ = options.get(".outputtime").as<double>();
  timefile_ = options.ifExistsReturnElseReturnDefault<std::string>(".timefile",
                                                                   timefile_);

  std::string carriertype = options.get(".carriertype").as<std::string>();
  carriertype_ = QMStateType(carriertype);
  if (!carriertype_.isKMCState()) {
    throw std::runtime_error("KMC cannot be run for state:" +
                             carriertype_.ToLongString());
  }

  log_.setReportLevel(Log::current_level);
  log_.setCommonPreface("\n ...");
}

void KMCMultiple::PrintDiffandMu(const Eigen::Matrix3d& avgdiffusiontensor,
                                 double simtime, unsigned long step) {
  double absolute_field = field_.norm();

  if (absolute_field == 0) {
    unsigned long diffusionsteps = step / diffusionresolution_;
    Eigen::Matrix3d result =
        avgdiffusiontensor /
        (double(diffusionsteps) * 2.0 * simtime * double(numberofcarriers_));
    XTP_LOG(Log::error, log_)
        << "\nStep: " << step
        << " Diffusion tensor averaged over all carriers (nm^2/s):\n"
        << result * tools::conv::bohr2nm * tools::conv::bohr2nm << std::flush;
  } else {
    double average_mobility = 0;
    double bohr2Hrts_to_nm2Vs =
        tools::conv::bohr2nm * tools::conv::bohr2nm / tools::conv::hrt2ev;
    XTP_LOG(Log::error, log_) << "\nMobilities (nm^2/Vs): " << std::flush;
    for (Index i = 0; i < numberofcarriers_; i++) {
      Eigen::Vector3d velocity = carriers_[i].get_dRtravelled() / simtime;
      double mobility =
          velocity.dot(field_) / (absolute_field * absolute_field);
      XTP_LOG(Log::error, log_)
          << std::scientific << "    carrier " << i + 1
          << ": mu=" << mobility * bohr2Hrts_to_nm2Vs << std::flush;
      average_mobility +=
          velocity.dot(field_) / (absolute_field * absolute_field);
    }
    average_mobility /= double(numberofcarriers_);
    XTP_LOG(Log::error, log_)
        << std::scientific
        << "  Overall average mobility in field direction <mu>="
        << average_mobility * bohr2Hrts_to_nm2Vs << " nm^2/Vs  " << std::flush;
  }
}

void KMCMultiple::WriteToTrajectory(std::fstream& traj,
                                    std::vector<Eigen::Vector3d>& startposition,
                                    double simtime, unsigned long step) const {
  traj << simtime << "\t";
  traj << step << "\t";
  for (Index i = 0; i < numberofcarriers_; i++) {
    Eigen::Vector3d pos = startposition[i] + carriers_[i].get_dRtravelled();
    traj << pos.x() * tools::conv::bohr2nm << "\t";
    traj << pos.y() * tools::conv::bohr2nm << "\t";
    traj << pos.z() * tools::conv::bohr2nm;
    if (i < numberofcarriers_ - 1) {
      traj << "\t";
    } else {
      traj << std::endl;
    }
  }
}

void KMCMultiple::WriteToEnergyFile(std::fstream& tfile, double simtime,
                                    unsigned long step) const {
  double absolute_field = field_.norm();
  double currentenergy = 0;
  double currentmobility = 0;
  Eigen::Vector3d dr_travelled_current = Eigen::Vector3d::Zero();
  double dr_travelled_field = 0.0;
  Eigen::Vector3d avgvelocity_current = Eigen::Vector3d::Zero();
  if (absolute_field != 0) {
    for (const auto& carrier : carriers_) {
      dr_travelled_current += carrier.get_dRtravelled();
      currentenergy += carrier.getCurrentEnergy();
    }
    dr_travelled_current /= double(numberofcarriers_);
    currentenergy /= double(numberofcarriers_);
    avgvelocity_current = dr_travelled_current / simtime;
    currentmobility =
        avgvelocity_current.dot(field_) / (absolute_field * absolute_field);
    dr_travelled_field = dr_travelled_current.dot(field_) / absolute_field;
  }
  double bohr2Hrts_to_nm2Vs =
      tools::conv::bohr2nm * tools::conv::bohr2nm / tools::conv::hrt2ev;
  tfile << simtime << "\t" << step << "\t"
        << currentenergy * tools::conv::hrt2ev << "\t"
        << currentmobility * bohr2Hrts_to_nm2Vs << "\t"
        << dr_travelled_field * tools::conv::bohr2nm << "\t"
        << dr_travelled_current.norm() * tools::conv::bohr2nm << "\t"
        << std::endl;
}

void KMCMultiple::PrintDiagDandMu(const Eigen::Matrix3d& avgdiffusiontensor,
                                  double simtime, unsigned long step) {
  unsigned long diffusionsteps = step / diffusionresolution_;
  Eigen::Matrix3d result =
      avgdiffusiontensor /
      (double(diffusionsteps) * 2.0 * simtime * double(numberofcarriers_));

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
  es.computeDirect(result);
  double bohr2_nm2 = tools::conv::bohr2nm * tools::conv::bohr2nm;
  XTP_LOG(Log::error, log_) << "\nEigenvalues:\n " << std::flush;
  for (Index i = 0; i < 3; i++) {
    XTP_LOG(Log::error, log_)
        << "Eigenvalue: " << es.eigenvalues()(i) * bohr2_nm2 << std::flush
        << "Eigenvector: " << es.eigenvectors().col(i).x() << "   "
        << es.eigenvectors().col(i).y() << "   " << es.eigenvectors().col(i).z()
        << "\n"
        << std::flush;
  }
  double bohr2Hrts_to_nm2Vs =
      tools::conv::bohr2nm * tools::conv::bohr2nm / tools::conv::hrt2ev;
  // calculate average mobility from the Einstein relation
  if (field_.norm() == 0) {
    XTP_LOG(Log::error, log_)
        << "The following value is calculated using the Einstein relation "
           "and assuming an isotropic medium"
        << std::flush;
    double avgD = 1. / 3. * es.eigenvalues().sum();
    double average_mobility = std::abs(avgD / temperature_);
    XTP_LOG(Log::error, log_)
        << std::scientific << "  Overall average mobility <mu>="
        << average_mobility * bohr2Hrts_to_nm2Vs << " nm^2/Vs " << std::flush;
  }
}

void KMCMultiple::PrintChargeVelocity(double simtime) {
  Eigen::Vector3d avg_dr_travelled = Eigen::Vector3d::Zero();
  for (Index i = 0; i < numberofcarriers_; i++) {
    XTP_LOG(Log::error, log_)
        << std::scientific << "    carrier " << i + 1 << ": "
        << carriers_[i].get_dRtravelled().transpose() / simtime *
               tools::conv::bohr2nm
        << std::flush;
    avg_dr_travelled += carriers_[i].get_dRtravelled();
  }
  avg_dr_travelled /= double(numberofcarriers_);

  Eigen::Vector3d avgvelocity = avg_dr_travelled / simtime;
  XTP_LOG(Log::error, log_)
      << std::scientific << "  Overall average velocity (nm/s): "
      << avgvelocity.transpose() * tools::conv::bohr2nm << std::flush;
}

void KMCMultiple::RunVSSM() {

  std::chrono::time_point<std::chrono::system_clock> realtime_start =
      std::chrono::system_clock::now();
  XTP_LOG(Log::error, log_)
      << "\nAlgorithm: VSSM for Multiple Charges" << std::flush;
  XTP_LOG(Log::error, log_)
      << "number of carriers: " << numberofcarriers_ << std::flush;
  XTP_LOG(Log::error, log_)
      << "number of nodes: " << nodes_.size() << std::flush;

  bool checkifoutput = (outputtime_ != 0);
  double nexttrajoutput = 0;
  unsigned long maxsteps = boost::numeric_cast<unsigned long>(runtime_);
  unsigned long outputstep = boost::numeric_cast<unsigned long>(outputtime_);
  bool stopontime = false;

  if (runtime_ > 100) {
    XTP_LOG(Log::error, log_)
        << "stop condition: " << maxsteps << " steps." << std::flush;

    if (checkifoutput) {
      XTP_LOG(Log::error, log_) << "output frequency: ";
      XTP_LOG(Log::error, log_)
          << "every " << outputstep << " steps." << std::flush;
    }
  } else {
    stopontime = true;
    XTP_LOG(Log::error, log_)
        << "stop condition: " << runtime_ << " seconds runtime." << std::flush;

    if (checkifoutput) {
      XTP_LOG(Log::error, log_) << "output frequency:\n "
                                   "every "
                                << outputtime_ << " seconds." << std::flush;
    }
  }
  XTP_LOG(Log::error, log_)
      << "(If you specify runtimes larger than 100 kmcmultiple assumes that "
         "you are specifying the number of steps for both runtime and "
         "outputtime.)"
      << std::flush;

  if (!stopontime && outputtime_ != 0 && floor(outputtime_) != outputtime_) {
    throw std::runtime_error(
        "ERROR in kmcmultiple: runtime was specified in steps (>100) and "
        "outputtime in seconds (not an integer). Please use the same units for "
        "both input parameters.");
  }

  if (numberofcarriers_ > Index(nodes_.size())) {
    throw std::runtime_error(
        "ERROR in kmcmultiple: specified number of carriers is greater than "
        "the "
        "number of nodes. This conflicts with single occupation.");
  }

  std::fstream traj;
  std::fstream tfile;

  if (checkifoutput) {

    XTP_LOG(Log::error, log_)
        << "Writing trajectory to " << trajectoryfile_ << "." << std::flush;
    traj.open(trajectoryfile_, std::fstream::out);

    traj << "time[s]\tsteps\t";
    for (Index i = 0; i < numberofcarriers_; i++) {
      traj << "carrier" << i + 1 << " x_\t";
      traj << "carrier" << i + 1 << " y_\t";
      traj << "carrier" << i + 1 << " z_";
      if (i < numberofcarriers_ - 1) {
        traj << "'\t";
      }
    }
    traj << std::endl;
    if (!timefile_.empty()) {
      XTP_LOG(Log::error, log_)
          << "Writing time dependence of energy and mobility to " << timefile_
          << "." << std::flush;
      tfile.open(timefile_, std::fstream::out);
      tfile << "time[s]\t "
               "steps\tenergy_per_carrier[eV]\tmobility[nm**2/"
               "Vs]\tdistance_fielddirection[nm]\tdistance_absolute[nm]"
            << std::endl;
    }
  }
  RandomlyCreateCharges();
  std::vector<Eigen::Vector3d> startposition(numberofcarriers_,
                                             Eigen::Vector3d::Zero());
  for (Index i = 0; i < numberofcarriers_; i++) {
    startposition[i] = carriers_[i].getCurrentPosition();
  }

  traj << 0 << "\t";
  traj << 0 << "\t";
  for (Index i = 0; i < numberofcarriers_; i++) {
    traj << startposition[i].x() * tools::conv::bohr2nm << "\t";
    traj << startposition[i].y() * tools::conv::bohr2nm << "\t";
    traj << startposition[i].z() * tools::conv::bohr2nm;
    if (i < numberofcarriers_ - 1) {
      traj << "\t";
    } else {
      traj << std::endl;
    }
  }

  std::vector<GNode*> forbiddennodes;
  std::vector<GNode*> forbiddendests;

  Eigen::Matrix3d avgdiffusiontensor = Eigen::Matrix3d::Zero();

  double simtime = 0.0;
  unsigned long step = 0;

  while (((stopontime && simtime < runtime_) ||
          (!stopontime && step < maxsteps))) {

    std::chrono::duration<double> elapsed_time =
        std::chrono::system_clock::now() - realtime_start;
    if (elapsed_time.count() > (maxrealtime_ * 60. * 60.)) {
      XTP_LOG(Log::error, log_)
          << "\nReal time limit of " << maxrealtime_ << " hours ("
          << Index(maxrealtime_ * 60 * 60 + 0.5)
          << " seconds) has been reached. Stopping here.\n"
          << std::flush;
      break;
    }

    double cumulated_rate = 0;
    for (const auto& carrier : carriers_) {
      cumulated_rate += carrier.getCurrentEscapeRate();
    }
    if (cumulated_rate <= 0) {  // this should not happen: no possible jumps
                                // defined for a node
      throw std::runtime_error(
          "ERROR in kmcmultiple: Incorrect rates. All "
          "the escape rates for the current setting are 0.");
    }

    double dt = Promotetime(cumulated_rate);

    simtime += dt;
    step++;

    for (auto& carrier : carriers_) {
      carrier.updateOccupationtime(dt);
    }

    ResetForbiddenlist(forbiddennodes);
    bool level1step = true;
    while (level1step) {

      // determine which electron will escape
      GNode* newnode = nullptr;
      Chargecarrier* affectedcarrier = ChooseAffectedCarrier(cumulated_rate);

      if (CheckForbidden(affectedcarrier->getCurrentNode(), forbiddennodes)) {
        continue;
      }
      ResetForbiddenlist(forbiddendests);
      while (true) {
        // LEVEL 2

        const GLink& event =
            ChooseHoppingDest(affectedcarrier->getCurrentNode());
        newnode = event.getDestination();

        if (newnode == nullptr) {
          AddtoForbiddenlist(affectedcarrier->getCurrentNode(), forbiddennodes);
          break;  // select new escape node (ends level 2 but without setting
                  // level1step to 1)
        }

        // check after the event if this was allowed
        if (CheckForbidden(*newnode, forbiddendests)) {
          continue;
        }

        // if the new segment is unoccupied: jump; if not: add to forbidden
        // list and choose new hopping destination
        if (newnode->isOccupied()) {
          if (CheckSurrounded(affectedcarrier->getCurrentNode(),
                              forbiddendests)) {
            AddtoForbiddenlist(affectedcarrier->getCurrentNode(),
                               forbiddennodes);
            break;  // select new escape node (ends level 2 but without
                    // setting level1step to 1)
          }
          AddtoForbiddenlist(*newnode, forbiddendests);
          continue;  // select new destination
        } else {
          affectedcarrier->jumpAccordingEvent(event);
          level1step = false;
          break;  // this ends LEVEL 2 , so that the time is updated and the
                  // next MC step started
        }
        // END LEVEL 2
      }
      // END LEVEL 1
    }

    if (step % diffusionresolution_ == 0) {
      for (const auto& carrier : carriers_) {
        avgdiffusiontensor += (carrier.get_dRtravelled()) *
                              (carrier.get_dRtravelled()).transpose();
      }
    }

    if (step != 0 && step % intermediateoutput_frequency_ == 0) {
      PrintDiffandMu(avgdiffusiontensor, simtime, step);
    }

    if (checkifoutput) {
      bool outputsteps = (!stopontime && step % outputstep == 0);
      bool outputtime = (stopontime && simtime > nexttrajoutput);
      if (outputsteps || outputtime) {
        // write to trajectory file
        nexttrajoutput = simtime + outputtime_;
        WriteToTrajectory(traj, startposition, simtime, step);
        if (!timefile_.empty()) {
          WriteToEnergyFile(tfile, simtime, step);
        }
      }
    }
  }  // KMC

  if (checkifoutput) {
    traj.close();
    if (!timefile_.empty()) {
      tfile.close();
    }
  }

  WriteOccupationtoFile(simtime, occfile_);

  XTP_LOG(Log::error, log_) << "\nfinished KMC simulation after " << step
                            << " steps.\n"
                               "simulated time "
                            << simtime << " seconds.\n"
                            << std::flush;

  PrintChargeVelocity(simtime);

  XTP_LOG(Log::error, log_) << "\nDistances travelled (nm): " << std::flush;
  for (Index i = 0; i < numberofcarriers_; i++) {
    XTP_LOG(Log::error, log_)
        << std::scientific << "    carrier " << i + 1 << ": "
        << carriers_[i].get_dRtravelled().transpose() * tools::conv::bohr2nm
        << std::flush;
  }

  PrintDiffandMu(avgdiffusiontensor, simtime, step);
  PrintDiagDandMu(avgdiffusiontensor, simtime, step);

  return;
}

bool KMCMultiple::Evaluate(Topology& top) {

  XTP_LOG(Log::error, log_) << "\n-----------------------------------"
                               "\n      KMC FOR MULTIPLE CHARGES"
                               "\n-----------------------------------\n"
                            << std::flush;

  XTP_LOG(Log::info, log_) << "\nInitialising random number generator"
                           << std::flush;
  RandomVariable_.init(seed_);

  LoadGraph(top);
  RunVSSM();
  std::cout << log_;
  return true;
}

}  // namespace xtp
}  // namespace votca