source: src/Dynamics/ForceAnnealing.hpp@ 0682c2

/*
 * 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 <math.h>

#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),
    FORCE_THRESHOLD(1e-8)
  {}

  /** 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
   * \return false - need to continue annealing, true - may stop because forces very small
   * \todo This is not yet checked if it is correctly working with DoConstrainedMD set >0.
   */
  bool 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);

    // check whether are smaller than threshold
    bool AnnealingFinished = false;
    double maxcomp = 0.;
    for (size_t i=0;i<NDIM;++i)
      maxcomp = std::max(maxcomp, fabs(maxComponents[i]));
    if (maxcomp < FORCE_THRESHOLD) {
      LOG(1, "STATUS: Force components are all less than " << FORCE_THRESHOLD
          << ", stopping.");
      currentStep = maxSteps;
      AnnealingFinished = true;
    }

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

    return AnnealingFinished;
  }

  /** 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.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;
    }
    return std::min(1., 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 a single atom using BarzilaiBorwein step width.
   *
   * \param _atom atom to anneal
   * \param OldTimeStep old time step
   * \param CurrentTimeStep current time step whose gradient we've just calculated
   * \param TimeStepToSet time step to update (i.e. \f$ t + \Delta t \f$ in the sense of the velocity verlet)
   */
  void annealAtom_BarzilaiBorwein(
      atom * const _atom,
      const int &OldTimeStep,
      const int &CurrentTimeStep,
      const int &TimeStepToSet
      )
  {
    // atom's force vector gives steepest descent direction
    const Vector &oldPosition = _atom->getPositionAtStep(OldTimeStep);
    const Vector &currentPosition = _atom->getPositionAtStep(CurrentTimeStep);
    const Vector &oldGradient = _atom->getAtomicForceAtStep(OldTimeStep);
    const Vector &currentGradient = _atom->getAtomicForceAtStep(CurrentTimeStep);
    LOG(4, "DEBUG: oldPosition for atom #" << _atom->getId() << " is " << oldPosition);
    LOG(4, "DEBUG: currentPosition for atom #" << _atom->getId() << " is " << currentPosition);
    LOG(4, "DEBUG: oldGradient for atom #" << _atom->getId() << " is " << oldGradient);
    LOG(4, "DEBUG: currentGradient for atom #" << _atom->getId() << " is " << currentGradient);
//      LOG(4, "DEBUG: Force for atom #" << _atom->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);

    // finally set new values
    _atom->setPositionAtStep(TimeStepToSet, currentPosition + PositionUpdate);
  }

  /** 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 preceded 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;
    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {

      annealAtom_BarzilaiBorwein(*iter, OldTimeStep, CurrentTimeStep, _TimeStep);

      // extract largest components for showing progress of annealing
      const Vector &currentGradient = (*iter)->getAtomicForceAtStep(CurrentTimeStep);
      for(size_t i=0;i<NDIM;++i)
        maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));
    }

    return maxComponents;
  }

        /** Helper function to insert \a PositionUpdate into a map for every atom.
         *
         * \param _GatheredUpdates map of updates per atom
         * \param _LargestUpdate_per_Atom map with the largest update per atom for checking
         * \param _atomno key for map
         * \param _PositionUpdate update to add
         * \param _factor optional dampening factor
         */
        void updateInserter(
            std::map<atomId_t, Vector> &_GatheredUpdates,
            std::map<atomId_t, double> &_LargestUpdate_per_Atom,
            const atomId_t _atomno,
            const Vector &_PositionUpdate,
            const double _factor = 1.
            )
        {
          if (_GatheredUpdates.count(_atomno)) {
            _GatheredUpdates[_atomno] += _factor*_PositionUpdate;
            _LargestUpdate_per_Atom[_atomno] =
                std::max(_LargestUpdate_per_Atom[_atomno], _factor*_PositionUpdate.Norm());
          } else {
            _GatheredUpdates.insert(
                std::make_pair(
                    _atomno,
                    _factor*_PositionUpdate) );
            _LargestUpdate_per_Atom.insert(
                std::make_pair(
                    _atomno,
                    _PositionUpdate.Norm()) );
          }
        }

  /** 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
    std::map<atomId_t, double> LargestUpdate_per_Atom; //!< check whether updates cancelled each other
    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 = getBarzilaiBorweinStepwidth(PositionDifference, GradientDifference);
      Vector PositionUpdate = stepwidth * currentGradient;
      // cap updates (if non-zero) at 0.2 angstroem. BB tends to overshoot.
      for (size_t i=0;i<NDIM;++i)
        if (fabs(PositionUpdate[i]) > MYEPSILON)
          PositionUpdate[i] = std::min(0.2, fabs(PositionUpdate[i]))*PositionUpdate[i]/fabs(PositionUpdate[i]);
      LOG(3, "DEBUG: Update would be " << stepwidth << "*" << currentGradient << " = " << PositionUpdate);

      if (walker.getElementNo() != 1) {
        /** for each atom, we imagine a plane at the position of the atom with
         * its atomic gradient as the normal vector. We go through all its bonds
         * and check on which side of the plane the bond is. This defines whether
         * the bond is contracting (+) or expanding (-) with respect to this atom.
         *
         * A bond has two atoms, however. Hence, we do this for either atom and
         * look at the combination: Is it in sum contracting or expanding given
         * both projected_forces?
         */

        /** 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.
         *
         * We may get this information right away by looking at the projected_forces.
         * They give the atomic gradient of either atom projected onto the BondVector
         * with an additional weight in [0,1].
         */

        // 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];
          // note that projected_forces has sign such as to indicate whether
          // atomic gradient wants bond to contract (-) or expand (+).
          // This goes into sides: Minus side points away from gradient, plus side point
          // towards gradient.
          //
          // the sum of both bond sides goes into types, depending on which is
          // stronger if either wants a different thing
          const double &temp = forcelist[index];
          if (fabs(temp) < MYEPSILON)
            sides.push_back(1);
          else
            sides.push_back( -1.*temp/fabs(temp) ); // BondVectors has exactly opposite sign for sides decision
          ASSERT( (sides.back() == 1) || (sides.back() == -1),
              "ForceAnnealing() - sides is not in {-1,1}.");
          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 neither 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();

          // sideno is away (0) or in direction (1) of gradient
          // tpyes[first_per_side[sideno]] is either contracting (-1) or expanding (+1)
          // : side (i), where (i) means which bonds we keep for the BFS, bonds
          // on side (-i) are removed
          // If all bonds on side away (0) want expansion (+1), move towards side with atom: side 1
          // if all bonds side towards (1) want contraction (-1), move away side with atom : side -1

          // unsure whether this or do nothing in the remaining cases:
          // If all bonds on side toward (1) want expansion (+1), move away side with atom : side -1
          //    (the reasoning is that the bond's other atom must have a stronger
          //     gradient in the same direction and they push along atoms in
          //     gradient direction: we don't want to interface with those.
          //     Hence, move atoms along on away side
          // if all bonds side away (0) want contraction (-1), move towards side with atom: side 1
          //    (the reasoning is the same, don't interfere with update from
          //     stronger gradient)
          // hence, the decision is only based on sides once we have picked a side
          // depending on all bonds associated with have same good type.

          // 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);
            updateInserter(GatheredUpdates, LargestUpdate_per_Atom, *setiter, PositionUpdate, factor);
          }
        } else {
          // simple atomic annealing, i.e. damping factor of 1
          updateInserter(GatheredUpdates, LargestUpdate_per_Atom, walker.getId(), PositionUpdate);
        }
      } else {
        // hydrogens (are light-weighted and therefore) are always updated normally
        LOG(3, "DEBUG: Update for hydrogen #" << walker.getId() << " will be " << PositionUpdate);
        updateInserter(GatheredUpdates, LargestUpdate_per_Atom, 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]));

      std::map<atomId_t, double>::const_iterator largestiter = LargestUpdate_per_Atom.find(atomid);
      ASSERT( largestiter != LargestUpdate_per_Atom.end(),
          "ForceAnnealing() - walker with id "+toString(atomid)+" not in LargestUpdates.");
      // if we had large updates but their sum is very small
      if (update.Norm()/largestiter->second > MYEPSILON) {
        walker->setPositionAtStep(_TimeStep,
            walker->getPositionAtStep(CurrentTimeStep) + update); // - CommonTranslation);
      } else {
        // then recalc update with simple anneal
        LOG(2, "WARNING: Updates on atom " << *iter << " cancel themselves, performing simple anneal step.");
        annealAtom_BarzilaiBorwein(walker, OldTimeStep, CurrentTimeStep, _TimeStep);
      }
    }
    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;
  //!> threshold for force components to stop annealing
  const double FORCE_THRESHOLD;
};

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_ */