/*
 * ForceAnnealing.hpp
 *
 *  Created on: Aug 02, 2014
 *      Author: heber
 */

#ifndef FORCEANNEALING_HPP_
#define FORCEANNEALING_HPP_

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <algorithm>
#include <functional>
#include <iterator>

#include <boost/bind.hpp>

#include "Atom/atom.hpp"
#include "Atom/AtomSet.hpp"
#include "CodePatterns/Assert.hpp"
#include "CodePatterns/Info.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "Descriptors/AtomIdDescriptor.hpp"
#include "Dynamics/AtomicForceManipulator.hpp"
#include "Dynamics/BondVectors.hpp"
#include "Fragmentation/ForceMatrix.hpp"
#include "Graph/BoostGraphCreator.hpp"
#include "Graph/BoostGraphHelpers.hpp"
#include "Graph/BreadthFirstSearchGatherer.hpp"
#include "Helpers/helpers.hpp"
#include "Helpers/defs.hpp"
#include "LinearAlgebra/LinearSystemOfEquations.hpp"
#include "LinearAlgebra/MatrixContent.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "LinearAlgebra/VectorContent.hpp"
#include "Thermostats/ThermoStatContainer.hpp"
#include "Thermostats/Thermostat.hpp"
#include "World.hpp"

/** This class is the essential build block for performing structural optimization.
 *
 * Sadly, we have to use some static instances as so far values cannot be passed
 * between actions. Hence, we need to store the current step and the adaptive-
 * step width (we cannot perform a line search, as we have no control over the
 * calculation of the forces).
 *
 * However, we do use the bond graph, i.e. if a single atom needs to be shifted
 * to the left, then the whole molecule left of it is shifted, too. This is
 * controlled by the \a max_distance parameter.
 */
template <class T>
class ForceAnnealing : public AtomicForceManipulator<T>
{
public:
  /** Constructor of class ForceAnnealing.
   *
   * \note We use a fixed delta t of 1.
   *
   * \param _atoms set of atoms to integrate
   * \param _Deltat time step width in atomic units
   * \param _IsAngstroem whether length units are in angstroem or bohr radii
   * \param _maxSteps number of optimization steps to perform
   * \param _max_distance up to this bond order is bond graph taken into account.
   */
  ForceAnnealing(
      AtomSetMixin<T> &_atoms,
      const double _Deltat,
      bool _IsAngstroem,
      const size_t _maxSteps,
      const int _max_distance,
      const double _damping_factor) :
    AtomicForceManipulator<T>(_atoms, _Deltat, _IsAngstroem),
    maxSteps(_maxSteps),
    max_distance(_max_distance),
    damping_factor(_damping_factor)
  {}

  /** Destructor of class ForceAnnealing.
   *
   */
  ~ForceAnnealing()
  {}

  /** Performs Gradient optimization.
   *
   * We assume that forces have just been calculated.
   *
   *
   * \param _TimeStep time step to update (i.e. \f$ t + \Delta t \f$ in the sense of the velocity verlet)
   * \param offset offset in matrix file to the first force component
   * \todo This is not yet checked if it is correctly working with DoConstrainedMD set >0.
   */
  void operator()(
      const int _TimeStep,
      const size_t _offset,
      const bool _UseBondgraph)
  {
    const int CurrentTimeStep = _TimeStep-1;
    ASSERT( CurrentTimeStep >= 0,
        "ForceAnnealing::operator() - a new time step (upon which we work) must already have been copied.");

    // make sum of forces equal zero
    AtomicForceManipulator<T>::correctForceMatrixForFixedCenterOfMass(
        _offset,
        CurrentTimeStep);

    // are we in initial step? Then set static entities
    Vector maxComponents(zeroVec);
    if (currentStep == 0) {
      currentDeltat = AtomicForceManipulator<T>::Deltat;
      currentStep = 1;
      LOG(2, "DEBUG: Initial step, setting values, current step is #" << currentStep);

      // always use atomic annealing on first step
      maxComponents = anneal(_TimeStep);
    } else {
      ++currentStep;
      LOG(2, "DEBUG: current step is #" << currentStep);

      // bond graph annealing is always followed by a normal annealing
      if (_UseBondgraph)
        maxComponents = annealWithBondGraph_BarzilaiBorwein(_TimeStep);
      // cannot store RemnantGradient in Atom's Force as it ruins BB stepwidth calculation
      else
        maxComponents = anneal_BarzilaiBorwein(_TimeStep);
    }


    LOG(1, "STATUS: Largest remaining force components at step #"
        << currentStep << " are " << maxComponents);

    // are we in final step? Remember to reset static entities
    if (currentStep == maxSteps) {
      LOG(2, "DEBUG: Final step, resetting values");
      reset();
    }
  }

  /** Helper function to calculate the Barzilai-Borwein stepwidth.
   *
   * \param _PositionDifference difference in position between current and last step
   * \param _GradientDifference difference in gradient between current and last step
   * \return step width according to Barzilai-Borwein
   */
  double getBarzilaiBorweinStepwidth(const Vector &_PositionDifference, const Vector &_GradientDifference)
  {
    double stepwidth = 0.;
    if (_GradientDifference.NormSquared() > MYEPSILON)
      stepwidth = fabs(_PositionDifference.ScalarProduct(_GradientDifference))/
          _GradientDifference.NormSquared();
    if (fabs(stepwidth) < 1e-10) {
      // dont' warn in first step, deltat usage normal
      if (currentStep != 1)
        ELOG(1, "INFO: Barzilai-Borwein stepwidth is zero, using deltat " << currentDeltat << " instead.");
      stepwidth = currentDeltat;
    }
    return stepwidth;
  }

  /** Performs Gradient optimization on the atoms.
   *
   * We assume that forces have just been calculated.
   *
   * \param _TimeStep time step to update (i.e. \f$ t + \Delta t \f$ in the sense of the velocity verlet)
   * \return to be filled with maximum force component over all atoms
   */
  Vector anneal(
      const int _TimeStep)
  {
    const int CurrentTimeStep = _TimeStep-1;
    ASSERT( CurrentTimeStep >= 0,
        "ForceAnnealing::anneal() - a new time step (upon which we work) must already have been copied.");

    LOG(1, "STATUS: performing simple anneal with default stepwidth " << currentDeltat << " at step #" << currentStep);

    Vector maxComponents;
    bool deltat_decreased = false;
    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
      // atom's force vector gives steepest descent direction
      const Vector currentPosition = (*iter)->getPositionAtStep(CurrentTimeStep);
      const Vector currentGradient = (*iter)->getAtomicForceAtStep(CurrentTimeStep);
      LOG(4, "DEBUG: currentPosition for atom #" << (*iter)->getId() << " is " << currentPosition);
      LOG(4, "DEBUG: currentGradient for atom #" << (*iter)->getId() << " is " << currentGradient);
//      LOG(4, "DEBUG: Force for atom " << **iter << " is " << currentGradient);

      // we use Barzilai-Borwein update with position reversed to get descent
      double stepwidth = currentDeltat;
      Vector PositionUpdate = stepwidth * currentGradient;
      LOG(3, "DEBUG: Update would be " << stepwidth << "*" << currentGradient << " = " << PositionUpdate);

      // extract largest components for showing progress of annealing
      for(size_t i=0;i<NDIM;++i)
        maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));

      // steps may go back and forth again (updates are of same magnitude but
      // have different sign: Check whether this is the case and one step with
      // deltat to interrupt this sequence
      if (currentStep > 1) {
        const int OldTimeStep = CurrentTimeStep-1;
        ASSERT( OldTimeStep >= 0,
            "ForceAnnealing::anneal() - if currentStep is "+toString(currentStep)
            +", then there should be at least three time steps.");
        const Vector &oldPosition = (*iter)->getPositionAtStep(OldTimeStep);
        const Vector PositionDifference = currentPosition - oldPosition;
        LOG(4, "DEBUG: oldPosition for atom #" << (*iter)->getId() << " is " << oldPosition);
        LOG(4, "DEBUG: PositionDifference for atom #" << (*iter)->getId() << " is " << PositionDifference);
        if ((PositionUpdate.ScalarProduct(PositionDifference) < 0)
            && (fabs(PositionUpdate.NormSquared()-PositionDifference.NormSquared()) < 1e-3)) {
          // for convergence we want a null sequence here, too
          if (!deltat_decreased) {
            deltat_decreased = true;
            currentDeltat = .5*currentDeltat;
          }
          LOG(2, "DEBUG: Upgrade in other direction: " << PositionUpdate
              << " > " << PositionDifference
              << ", using deltat: " << currentDeltat);
          PositionUpdate = currentDeltat * currentGradient;
        }
      }

      // finally set new values
      (*iter)->setPositionAtStep(_TimeStep, currentPosition + PositionUpdate);
    }

    return maxComponents;
  }

  /** Performs Gradient optimization on the atoms using BarzilaiBorwein step width.
   *
   * \note this can only be called when there are at least two optimization
   * time steps present, i.e. this must be preceeded by a simple anneal().
   *
   * We assume that forces have just been calculated.
   *
   * \param _TimeStep time step to update (i.e. \f$ t + \Delta t \f$ in the sense of the velocity verlet)
   * \return to be filled with maximum force component over all atoms
   */
  Vector anneal_BarzilaiBorwein(
      const int _TimeStep)
  {
    const int OldTimeStep = _TimeStep-2;
    const int CurrentTimeStep = _TimeStep-1;
    ASSERT( OldTimeStep >= 0,
        "ForceAnnealing::anneal_BarzilaiBorwein() - we need two present optimization steps to compute stepwidth.");
    ASSERT(currentStep > 1,
        "ForceAnnealing::anneal_BarzilaiBorwein() - we need two present optimization steps to compute stepwidth.");

    LOG(1, "STATUS: performing BarzilaiBorwein anneal at step #" << currentStep);

    Vector maxComponents;
    bool deltat_decreased = false;
    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
      // atom's force vector gives steepest descent direction
      const Vector &oldPosition = (*iter)->getPositionAtStep(OldTimeStep);
      const Vector &currentPosition = (*iter)->getPositionAtStep(CurrentTimeStep);
      const Vector &oldGradient = (*iter)->getAtomicForceAtStep(OldTimeStep);
      const Vector &currentGradient = (*iter)->getAtomicForceAtStep(CurrentTimeStep);
      LOG(4, "DEBUG: oldPosition for atom #" << (*iter)->getId() << " is " << oldPosition);
      LOG(4, "DEBUG: currentPosition for atom #" << (*iter)->getId() << " is " << currentPosition);
      LOG(4, "DEBUG: oldGradient for atom #" << (*iter)->getId() << " is " << oldGradient);
      LOG(4, "DEBUG: currentGradient for atom #" << (*iter)->getId() << " is " << currentGradient);
//      LOG(4, "DEBUG: Force for atom #" << (*iter)->getId() << " is " << currentGradient);

      // we use Barzilai-Borwein update with position reversed to get descent
      const Vector PositionDifference = currentPosition - oldPosition;
      const Vector GradientDifference = (currentGradient - oldGradient);
      double stepwidth = getBarzilaiBorweinStepwidth(PositionDifference, GradientDifference);
      Vector PositionUpdate = stepwidth * currentGradient;
      LOG(3, "DEBUG: Update would be " << stepwidth << "*" << currentGradient << " = " << PositionUpdate);

      // extract largest components for showing progress of annealing
      for(size_t i=0;i<NDIM;++i)
        maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));

//      // steps may go back and forth again (updates are of same magnitude but
//      // have different sign: Check whether this is the case and one step with
//      // deltat to interrupt this sequence
//      if (!PositionDifference.IsZero())
//        if ((PositionUpdate.ScalarProduct(PositionDifference) < 0)
//            && (fabs(PositionUpdate.NormSquared()-PositionDifference.NormSquared()) < 1e-3)) {
//          // for convergence we want a null sequence here, too
//          if (!deltat_decreased) {
//            deltat_decreased = true;
//            currentDeltat = .5*currentDeltat;
//          }
//          LOG(2, "DEBUG: Upgrade in other direction: " << PositionUpdate
//              << " > " << PositionDifference
//              << ", using deltat: " << currentDeltat);
//          PositionUpdate = currentDeltat * currentGradient;
//      }

      // finally set new values
      (*iter)->setPositionAtStep(_TimeStep, currentPosition + PositionUpdate);
    }

    return maxComponents;
  }

  /** Performs Gradient optimization on the bonds with BarzilaiBorwein stepwdith.
   *
   * \note this can only be called when there are at least two optimization
   * time steps present, i.e. this must be preceeded by a simple anneal().
   *
   * We assume that forces have just been calculated. These forces are projected
   * onto the bonds and these are annealed subsequently by moving atoms in the
   * bond neighborhood on either side conjunctively.
   *
   *
   * \param _TimeStep time step to update (i.e. \f$ t + \Delta t \f$ in the sense of the velocity verlet)
   * \param maxComponents to be filled with maximum force component over all atoms
   */
  Vector annealWithBondGraph_BarzilaiBorwein(
      const int _TimeStep)
  {
    const int OldTimeStep = _TimeStep-2;
    const int CurrentTimeStep = _TimeStep-1;
    ASSERT(OldTimeStep >= 0,
        "annealWithBondGraph_BarzilaiBorwein() - we need two present optimization steps to compute stepwidth, and the new one to update on already present.");
    ASSERT(currentStep > 1,
        "annealWithBondGraph_BarzilaiBorwein() - we need two present optimization steps to compute stepwidth.");

    LOG(1, "STATUS: performing BarzilaiBorwein anneal on bonds at step #" << currentStep);

    Vector maxComponents;

    // get nodes on either side of selected bond via BFS discovery
    BoostGraphCreator BGcreator;
    BGcreator.createFromRange(
        AtomicForceManipulator<T>::atoms.begin(),
        AtomicForceManipulator<T>::atoms.end(),
        AtomicForceManipulator<T>::atoms.size(),
        BreadthFirstSearchGatherer::AlwaysTruePredicate);
    BreadthFirstSearchGatherer NodeGatherer(BGcreator);

    /** We assume that a force is local, i.e. a bond is too short yet and hence
     * the atom needs to be moved. However, all the adjacent (bound) atoms might
     * already be at the perfect distance. If we just move the atom alone, we ruin
     * all the other bonds. Hence, it would be sensible to move every atom found
     * through the bond graph in the direction of the force as well by the same
     * PositionUpdate. This is almost what we are going to do, see below.
     *
     * This is to make the force a little more global in the sense of a multigrid
     * solver that uses various coarser grids to transport errors more effectively
     * over finely resolved grids.
     *
     */

    /** The idea is that we project the gradients onto the bond vectors and determine
     * from the sum of projected gradients from either side whether the bond is
     * to contract or to expand. As the gradient acting as the normal vector of
     * a plane supported at the position of the atom separates all bonds into two
     * sets, we check whether all on one side are contracting and all on the other
     * side are expanding. In this case we may move not only the atom itself but
     * may propagate its update along a limited-horizon BFS to neighboring atoms.
     *
     */

    // initialize helper class for bond vectors using bonds from range of atoms
    BondVectors bv;
    bv.setFromAtomRange< T >(
        AtomicForceManipulator<T>::atoms.begin(),
        AtomicForceManipulator<T>::atoms.end(),
        _TimeStep); // use time step to update here as this is the current set of bonds

    std::vector< // which bond side
      std::vector<double> > // over all bonds
        projected_forces; // one for leftatoms, one for rightatoms
    projected_forces.resize(BondVectors::MAX_sides);
    for (size_t j=0;j<BondVectors::MAX_sides;++j)
      projected_forces[j].resize(bv.size(), 0.);

    // for each atom we need to project the gradient
    for(typename AtomSetMixin<T>::const_iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
      const atom &walker = *(*iter);
      const Vector &walkerGradient = walker.getAtomicForceAtStep(CurrentTimeStep);
      const double GradientNorm = walkerGradient.Norm();
      LOG(3, "DEBUG: Gradient of atom #" << walker.getId() << ", namely "
          << walker << " is " << walkerGradient << " with magnitude of "
          << GradientNorm);

      if (GradientNorm > MYEPSILON) {
        bv.getProjectedGradientsForAtomAtStep(
            walker, walkerGradient, CurrentTimeStep, projected_forces
        );
      } else {
        LOG(2, "DEBUG: Gradient is " << walkerGradient << " less than "
            << MYEPSILON << " for atom " << walker);
        // note that projected_forces is initialized to full length and filled
        // with zeros. Hence, nothing to do here
      }
    }

    std::map<atomId_t, Vector> GatheredUpdates; //!< gathers all updates which are applied at the end
    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
      atom &walker = *(*iter);

      /// calculate step width
      const Vector &oldPosition = (*iter)->getPositionAtStep(OldTimeStep);
      const Vector &currentPosition = (*iter)->getPositionAtStep(CurrentTimeStep);
      const Vector &oldGradient = (*iter)->getAtomicForceAtStep(OldTimeStep);
      const Vector &currentGradient = (*iter)->getAtomicForceAtStep(CurrentTimeStep);
      LOG(4, "DEBUG: oldPosition for atom #" << (*iter)->getId() << " is " << oldPosition);
      LOG(4, "DEBUG: currentPosition for atom #" << (*iter)->getId() << " is " << currentPosition);
      LOG(4, "DEBUG: oldGradient for atom #" << (*iter)->getId() << " is " << oldGradient);
      LOG(4, "DEBUG: currentGradient for atom #" << (*iter)->getId() << " is " << currentGradient);
//      LOG(4, "DEBUG: Force for atom #" << (*iter)->getId() << " is " << currentGradient);

      // we use Barzilai-Borwein update with position reversed to get descent
      const Vector PositionDifference = currentPosition - oldPosition;
      const Vector GradientDifference = (currentGradient - oldGradient);
      double stepwidth = 0.;
      if (GradientDifference.Norm() > MYEPSILON)
        stepwidth = fabs(PositionDifference.ScalarProduct(GradientDifference))/
            GradientDifference.NormSquared();
      if (fabs(stepwidth) < 1e-10) {
        // dont' warn in first step, deltat usage normal
        if (currentStep != 1)
          ELOG(1, "INFO: Barzilai-Borwein stepwidth is zero, using deltat " << currentDeltat << " instead.");
        stepwidth = currentDeltat;
      }
      Vector PositionUpdate = stepwidth * currentGradient;
      LOG(3, "DEBUG: Update would be " << stepwidth << "*" << currentGradient << " = " << PositionUpdate);

      /** go through all bonds and check projected_forces and side of plane
       * the idea is that if all bonds on one side are contracting ones or expanding,
       * respectively, then we may shift not only the atom with respect to its
       * gradient but also its neighbors (towards contraction or towards
       * expansion depending on direction of gradient).
       * if they are mixed on both sides of the plane, then we simply shift
       * only the atom itself.
       * if they are not mixed on either side, then we also only shift the
       * atom, namely away from expanding and towards contracting bonds.
       */

      // sign encodes side of plane and also encodes contracting(-) or expanding(+)
      typedef std::vector<int> sides_t;
      typedef std::vector<int> types_t;
      sides_t sides;
      types_t types;
      const BondList& ListOfBonds = walker.getListOfBonds();
      for(BondList::const_iterator bonditer = ListOfBonds.begin();
          bonditer != ListOfBonds.end(); ++bonditer) {
        const bond::ptr &current_bond = *bonditer;

        // BondVector goes from bond::rightatom to bond::leftatom
        const size_t index = bv.getIndexForBond(current_bond);
        std::vector<double> &forcelist = (&walker == current_bond->leftatom) ?
            projected_forces[BondVectors::leftside] : projected_forces[BondVectors::rightside];
        const double &temp = forcelist[index];
        sides.push_back( -1.*temp/fabs(temp) ); // BondVectors has exactly opposite sign for sides decision
        const double sum =
            projected_forces[BondVectors::leftside][index]+projected_forces[BondVectors::rightside][index];
        types.push_back( sum/fabs(sum) );
        LOG(4, "DEBUG: Bond " << *current_bond << " is on side " << sides.back()
            << " and has type " << types.back());
      }
//      /// check whether both conditions are compatible:
//      // i.e. either we have ++/-- for all entries in sides and types
//      // or we have +-/-+ for all entries
//      // hence, multiplying and taking the sum and its absolute value
//      // should be equal to the maximum number of entries
//      sides_t results;
//      std::transform(
//          sides.begin(), sides.end(),
//          types.begin(),
//          std::back_inserter(results),
//          std::multiplies<int>);
//      int result = abs(std::accumulate(results.begin(), results.end(), 0, std::plus<int>));

      std::vector<size_t> first_per_side(2, (size_t)-1); //!< mark down one representative from either side
      std::vector< std::vector<int> > types_per_side(2); //!< gather all types on each side
      types_t::const_iterator typesiter = types.begin();
      for (sides_t::const_iterator sidesiter = sides.begin();
          sidesiter != sides.end(); ++sidesiter, ++typesiter) {
        const size_t index = (*sidesiter+1)/2;
        types_per_side[index].push_back(*typesiter);
        if (first_per_side[index] == (size_t)-1)
          first_per_side[index] = std::distance(const_cast<const sides_t &>(sides).begin(), sidesiter);
      }
      LOG(4, "DEBUG: First on side minus is " << first_per_side[0] << ", and first on side plus is "
          << first_per_side[1]);
      //!> enumerate types per side with a little witching with the numbers to allow easy setting from types
      enum whichtypes_t {
        contracting=0,
        unset=1,
        expanding=2,
        mixed
      };
      std::vector<int> typeside(2, unset);
      for(size_t i=0;i<2;++i) {
        for (std::vector<int>::const_iterator tpsiter = types_per_side[i].begin();
            tpsiter != types_per_side[i].end(); ++tpsiter) {
          if (typeside[i] == unset) {
            typeside[i] = *tpsiter+1; //contracting(0) or expanding(2)
          } else {
            if (typeside[i] != (*tpsiter+1)) // no longer he same type
              typeside[i] = mixed;
          }
        }
      }
      LOG(4, "DEBUG: Minus side is " << typeside[0] << " and plus side is " << typeside[1]);

      typedef std::vector< std::pair<atomId_t, atomId_t> > RemovedEdges_t;
      if ((typeside[0] != mixed) || (typeside[1] != mixed)) {
        const size_t sideno = ((typeside[0] != mixed) && (typeside[0] != unset)) ? 0 : 1;
        LOG(4, "DEBUG: Chosen side is " << sideno << " with type " << typeside[sideno]);
        ASSERT( (typeside[sideno] == contracting) || (typeside[sideno] == expanding),
            "annealWithBondGraph_BB() - chosen side is either expanding nor contracting.");
        // one side is not mixed, all bonds on one side are of same type
        // hence, find out which bonds to exclude
        const BondList& ListOfBonds = walker.getListOfBonds();

        // away from gradient (minus) and contracting
        // or towards gradient (plus) and expanding
        // gather all on same side and remove
        const double sign =
            (sides[first_per_side[sideno]] == types[first_per_side[sideno]])
            ? sides[first_per_side[sideno]] : -1.*sides[first_per_side[sideno]];
        LOG(4, "DEBUG: Removing edges from side with sign " << sign);
        BondList::const_iterator bonditer = ListOfBonds.begin();
        RemovedEdges_t RemovedEdges;
        for (sides_t::const_iterator sidesiter = sides.begin();
            sidesiter != sides.end(); ++sidesiter, ++bonditer) {
          if (*sidesiter == sign) {
            // remove the edge
            const bond::ptr &current_bond = *bonditer;
            LOG(5, "DEBUG: Removing edge " << *current_bond);
            RemovedEdges.push_back( std::make_pair(
                current_bond->leftatom->getId(),
                current_bond->rightatom->getId())
            );
#ifndef NDEBUG
            const bool status =
#endif
                BGcreator.removeEdge(RemovedEdges.back());
            ASSERT( status, "ForceAnnealing() - edge to found bond is not present?");
          }
        }
        // perform limited-horizon BFS
        BoostGraphHelpers::Nodeset_t bondside_set;
        BreadthFirstSearchGatherer::distance_map_t distance_map;
        bondside_set = NodeGatherer(walker.getId(), max_distance);
        distance_map = NodeGatherer.getDistances();
        std::sort(bondside_set.begin(), bondside_set.end());

        // re-add edge
        for (RemovedEdges_t::const_iterator edgeiter = RemovedEdges.begin();
            edgeiter != RemovedEdges.end(); ++edgeiter)
          BGcreator.addEdge(edgeiter->first, edgeiter->second);

        // update position with dampening factor on the discovered bonds
        for (BoostGraphHelpers::Nodeset_t::const_iterator setiter = bondside_set.begin();
            setiter != bondside_set.end(); ++setiter) {
          const BreadthFirstSearchGatherer::distance_map_t::const_iterator diter
            = distance_map.find(*setiter);
          ASSERT( diter != distance_map.end(),
              "ForceAnnealing() - could not find distance to an atom.");
          const double factor = pow(damping_factor, diter->second+1);
          LOG(3, "DEBUG: Update for atom #" << *setiter << " will be "
              << factor << "*" << PositionUpdate);
          if (GatheredUpdates.count((*setiter))) {
            GatheredUpdates[(*setiter)] += factor*PositionUpdate;
          } else {
            GatheredUpdates.insert(
                std::make_pair(
                    (*setiter),
                    factor*PositionUpdate) );
          }
        }
      } else {
        // simple atomic annealing, i.e. damping factor of 1
        LOG(3, "DEBUG: Update for atom #" << walker.getId() << " will be " << PositionUpdate);
        GatheredUpdates.insert(
            std::make_pair(
                walker.getId(),
                PositionUpdate) );
      }
    }

    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
      atom &walker = *(*iter);
      // extract largest components for showing progress of annealing
      const Vector currentGradient = walker.getAtomicForceAtStep(CurrentTimeStep);
      for(size_t i=0;i<NDIM;++i)
        maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));
    }

//    // remove center of weight translation from gathered updates
//    Vector CommonTranslation;
//    for (std::map<atomId_t, Vector>::const_iterator iter = GatheredUpdates.begin();
//        iter != GatheredUpdates.end(); ++iter) {
//      const Vector &update = iter->second;
//      CommonTranslation += update;
//    }
//    CommonTranslation *= 1./(double)GatheredUpdates.size();
//    LOG(3, "DEBUG: Subtracting common translation " << CommonTranslation
//        << " from all updates.");

    // apply the gathered updates and set remnant gradients for atomic annealing
    Vector LargestUpdate;
    for (std::map<atomId_t, Vector>::const_iterator iter = GatheredUpdates.begin();
        iter != GatheredUpdates.end(); ++iter) {
      const atomId_t &atomid = iter->first;
      const Vector &update = iter->second;
      atom* const walker = World::getInstance().getAtom(AtomById(atomid));
      ASSERT( walker != NULL,
          "ForceAnnealing() - walker with id "+toString(atomid)+" has suddenly disappeared.");
      LOG(3, "DEBUG: Applying update " << update << " to atom #" << atomid
          << ", namely " << *walker);
      for (size_t i=0;i<NDIM;++i)
        LargestUpdate[i] = std::max(LargestUpdate[i], fabs(update[i]));
      walker->setPositionAtStep(_TimeStep,
          walker->getPositionAtStep(CurrentTimeStep) + update); // - CommonTranslation);
    }
    LOG(1, "STATUS: Largest absolute update components are " << LargestUpdate);

    return maxComponents;
  }

  /** Reset function to unset static entities and artificial velocities.
   *
   */
  void reset()
  {
    currentDeltat = 0.;
    currentStep = 0;
  }

private:
  //!> contains the current step in relation to maxsteps
  static size_t currentStep;
  //!> contains the maximum number of steps, determines initial and final step with currentStep
  size_t maxSteps;
  static double currentDeltat;
  //!> minimum deltat for internal while loop (adaptive step width)
  static double MinimumDeltat;
  //!> contains the maximum bond graph distance up to which shifts of a single atom are spread
  const int max_distance;
  //!> the shifted is dampened by this factor with the power of the bond graph distance to the shift causing atom
  const double damping_factor;
};

template <class T>
double ForceAnnealing<T>::currentDeltat = 0.;
template <class T>
size_t ForceAnnealing<T>::currentStep = 0;
template <class T>
double ForceAnnealing<T>::MinimumDeltat = 1e-8;

#endif /* FORCEANNEALING_HPP_ */