Index: src/Dynamics/ForceAnnealing.hpp
===================================================================
--- src/Dynamics/ForceAnnealing.hpp	(revision 8f50b4adab0382b92f8e46cd26895315f53857e7)
+++ src/Dynamics/ForceAnnealing.hpp	(revision 59634f473698d6931b521d6809970437a66c95ba)
@@ -15,4 +15,5 @@
 
 #include <algorithm>
+#include <functional>
 #include <iterator>
 
@@ -27,4 +28,5 @@
 #include "Descriptors/AtomIdDescriptor.hpp"
 #include "Dynamics/AtomicForceManipulator.hpp"
+#include "Dynamics/BondVectors.hpp"
 #include "Fragmentation/ForceMatrix.hpp"
 #include "Graph/BoostGraphCreator.hpp"
@@ -100,5 +102,7 @@
   {
     // make sum of forces equal zero
-    AtomicForceManipulator<T>::correctForceMatrixForFixedCenterOfMass(_offset, _CurrentTimeStep);
+    AtomicForceManipulator<T>::correctForceMatrixForFixedCenterOfMass(
+        _offset,
+        _CurrentTimeStep-1>=0 ? _CurrentTimeStep - 1 : 0);
 
     // are we in initial step? Then set static entities
@@ -225,11 +229,4 @@
   {
     // get nodes on either side of selected bond via BFS discovery
-//    std::vector<atomId_t> atomids;
-//    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
-//        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
-//      atomids.push_back((*iter)->getId());
-//    }
-//    ASSERT( atomids.size() == AtomicForceManipulator<T>::atoms.size(),
-//        "ForceAnnealing() - could not gather all atomic ids?");
     BoostGraphCreator BGcreator;
     BGcreator.createFromRange(
@@ -247,5 +244,5 @@
     /// PositionUpdate. This is almost what we are going to do.
 
-    /// One more issue is: If we need to shorten bond, then we use the PositionUpdate
+    /// One issue is: If we need to shorten bond, then we use the PositionUpdate
     /// also on the the other bond partner already. This is because it is in the
     /// direction of the bond. Therefore, the update is actually performed twice on
@@ -257,4 +254,5 @@
     /// check whether each gradient points inwards out or outwards with respect
     /// to the bond and then shift accordingly.
+
     /// One more issue is that the projection onto the bond directions does not
     /// recover the gradient but may be larger as the bond directions are a
@@ -263,13 +261,42 @@
     /// overestimation and obtain a weighting for each bond.
 
-    // gather weights
+    // 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(),
+        CurrentTimeStep);
+    const BondVectors::container_t &sorted_bonds = bv.getSorted();
+
+    // knowing the number of bonds in total, we can setup the storage for the
+    // projected forces
+    enum whichatom_t {
+      leftside=0,
+      rightside=1,
+      MAX_sides
+    };
+    std::vector< // time step
+      std::vector< // which bond side
+        std::vector<double> > // over all bonds
+          > projected_forces(2); // one for leftatoms, one for rightatoms (and for both time steps)
+    for (size_t i=0;i<2;++i) {
+      projected_forces[i].resize(MAX_sides);
+      for (size_t j=0;j<MAX_sides;++j)
+        projected_forces[i][j].resize(sorted_bonds.size(), 0.);
+    }
+
+    // for each atom we need to gather weights and then project the gradient
     typedef std::deque<double> weights_t;
     typedef std::map<atomId_t, weights_t > weights_per_atom_t;
     std::vector<weights_per_atom_t> weights_per_atom(2);
     for (size_t timestep = 0; timestep <= 1; ++timestep) {
-      const size_t CurrentStep = CurrentTimeStep-2*timestep >= 0 ? CurrentTimeStep - 2*timestep : 0;
+      const size_t CurrentStep = CurrentTimeStep-timestep-1 >= 0 ? CurrentTimeStep-timestep-1 : 0;
       LOG(2, "DEBUG: CurrentTimeStep is " << CurrentTimeStep
           << ", timestep is " << timestep
           << ", and CurrentStep is " << CurrentStep);
+
+      // get all bond vectors for this time step (from the perspective of the
+      // bonds taken from the currentStep)
+      const BondVectors::mapped_t bondvectors = bv.getBondVectorsAtStep(CurrentStep);
 
       for(typename AtomSetMixin<T>::const_iterator iter = AtomicForceManipulator<T>::atoms.begin();
@@ -277,53 +304,70 @@
         const atom &walker = *(*iter);
         const Vector &walkerGradient = walker.getAtomicForceAtStep(CurrentStep);
-
+        LOG(3, "DEBUG: Gradient of atom #" << walker.getId() << ", namely "
+            << walker << " is " << walkerGradient << " with magnitude of "
+            << walkerGradient.Norm());
+
+        const BondList& ListOfBonds = walker.getListOfBonds();
         if (walkerGradient.Norm() > MYEPSILON) {
 
-          // gather BondVector and projected gradient over all bonds
-          const BondList& ListOfBonds = walker.getListOfBondsAtStep(CurrentStep);
-          std::vector<double> projected_forces;
+          // gather subset of BondVectors for the current atom
           std::vector<Vector> BondVectors;
-          projected_forces.reserve(ListOfBonds.size());
           for(BondList::const_iterator bonditer = ListOfBonds.begin();
               bonditer != ListOfBonds.end(); ++bonditer) {
             const bond::ptr &current_bond = *bonditer;
-            BondVectors.push_back(
-                current_bond->leftatom->getPositionAtStep(CurrentStep)
-                    - current_bond->rightatom->getPositionAtStep(CurrentStep));
-            Vector &BondVector = BondVectors.back();
-            BondVector.Normalize();
-            projected_forces.push_back( walkerGradient.ScalarProduct(BondVector) );
+            const BondVectors::mapped_t::const_iterator bviter =
+                bondvectors.find(current_bond);
+            ASSERT( bviter != bondvectors.end(),
+                "ForceAnnealing() - cannot find current_bond ?");
+            ASSERT( bviter != bondvectors.end(),
+                "ForceAnnealing - cannot find current bond "+toString(*current_bond)
+                +" in bonds.");
+            BondVectors.push_back(bviter->second);
           }
-
-          // go through all bonds and check what magnitude is represented by the others
-          // i.e. sum of scalar products against other bonds
+          LOG(4, "DEBUG: BondVectors for atom #" << walker.getId() << ": " << BondVectors);
+
+          // go through all its bonds and calculate what magnitude is represented
+          // by the others i.e. sum of scalar products against other bonds
           std::pair<weights_per_atom_t::iterator, bool> inserter =
-              weights_per_atom[timestep-1].insert(
+              weights_per_atom[timestep].insert(
                   std::make_pair(walker.getId(), weights_t()) );
           ASSERT( inserter.second,
               "ForceAnnealing::operator() - weight map for atom "+toString(walker)
-              +" and time step "+toString(timestep-1)+" already filled?");
+              +" and time step "+toString(timestep)+" already filled?");
           weights_t &weights = inserter.first->second;
           for (std::vector<Vector>::const_iterator iter = BondVectors.begin();
               iter != BondVectors.end(); ++iter) {
             std::vector<double> scps;
+            scps.reserve(BondVectors.size());
             std::transform(
                 BondVectors.begin(), BondVectors.end(),
                 std::back_inserter(scps),
-                boost::bind(&Vector::ScalarProduct, boost::cref(*iter), _1)
+                boost::bind(static_cast< double (*)(double) >(&fabs),
+                    boost::bind(&Vector::ScalarProduct, boost::cref(*iter), _1))
                 );
             const double scp_sum = std::accumulate(scps.begin(), scps.end(), 0.);
+            ASSERT( (scp_sum-1.) > -MYEPSILON,
+                "ForceAnnealing() - sum of weights must be equal or larger one but is "
+                +toString(scp_sum));
             weights.push_back( 1./scp_sum );
           }
+          LOG(4, "DEBUG: Weights for atom #" << walker.getId() << ": " << weights);
+
           // for testing we check whether all weighted scalar products now come out as 1.
 #ifndef NDEBUG
           for (std::vector<Vector>::const_iterator iter = BondVectors.begin();
               iter != BondVectors.end(); ++iter) {
-            double scp_sum = 0.;
-            weights_t::const_iterator weightiter = weights.begin();
-            for (std::vector<Vector>::const_iterator otheriter = BondVectors.begin();
-                otheriter != BondVectors.end(); ++otheriter, ++weightiter) {
-              scp_sum += (*weightiter)*(*iter).ScalarProduct(*otheriter);
-            }
+            std::vector<double> scps;
+            scps.reserve(BondVectors.size());
+            std::transform(
+                BondVectors.begin(), BondVectors.end(),
+                weights.begin(),
+                std::back_inserter(scps),
+                boost::bind(static_cast< double (*)(double) >(&fabs),
+                    boost::bind(std::multiplies<double>(),
+                        boost::bind(&Vector::ScalarProduct, boost::cref(*iter), _1),
+                        _2))
+                );
+            const double scp_sum = std::accumulate(scps.begin(), scps.end(), 0.);
             ASSERT( fabs(scp_sum - 1.) < MYEPSILON,
                 "ForceAnnealing::operator() - for BondVector "+toString(*iter)
@@ -331,7 +375,39 @@
           }
 #endif
+
+          // projected gradient over all bonds and place in one of projected_forces
+          // using the obtained weights
+          {
+            weights_t::const_iterator weightiter = weights.begin();
+            std::vector<Vector>::const_iterator vectoriter = BondVectors.begin();
+            Vector forcesum_check;
+            for(BondList::const_iterator bonditer = ListOfBonds.begin();
+                bonditer != ListOfBonds.end(); ++bonditer, ++weightiter, ++vectoriter) {
+              const bond::ptr &current_bond = *bonditer;
+              const Vector &BondVector = *vectoriter;
+
+              std::vector<double> &forcelist = (&walker == current_bond->leftatom) ?
+                  projected_forces[timestep][leftside] : projected_forces[timestep][rightside];
+              const size_t index = bv.getIndexForBond(current_bond);
+              ASSERT( index != (size_t)-1,
+                  "ForceAnnealing() - could not find bond "+toString(*current_bond)
+                  +" in bondvectors");
+              forcelist[index] = (*weightiter)*walkerGradient.ScalarProduct(BondVector);
+              LOG(4, "DEBUG: BondVector " << BondVector << " receives projected force of "
+                  << forcelist[index]);
+              forcesum_check += forcelist[index] * BondVector;
+            }
+            ASSERT( weightiter == weights.end(),
+                "ForceAnnealing - weightiter is not at end when it should be.");
+            ASSERT( vectoriter == BondVectors.end(),
+                "ForceAnnealing - vectoriter is not at end when it should be.");
+            LOG(3, "DEBUG: sum of projected forces is " << forcesum_check);
+          }
+
         } 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
         }
       }
@@ -340,74 +416,99 @@
     // step through each bond and shift the atoms
     std::map<atomId_t, Vector> GatheredUpdates; //!< gathers all updates which are applied at the end
-//    for (size_t i = 0;i<bondvector.size();++i) {
-//      const atom* bondatom[2] = {
-//           bondvector[i]->leftatom,
-//           bondvector[i]->rightatom};
-//      const double &bondforce = bondforces[i];
-//      const double &oldbondforce = oldbondforces[i];
-//      const double bondforcedifference = (bondforce - oldbondforce);
-//      Vector BondVector = (bondatom[0]->getPosition() - bondatom[1]->getPosition());
-//      BondVector.Normalize();
-//      double stepwidth = 0.;
-//      for (size_t n=0;n<2;++n) {
-//        const Vector oldPosition = bondatom[n]->getPositionAtStep(CurrentTimeStep-2 >= 0 ? CurrentTimeStep - 2 : 0);
-//        const Vector currentPosition = bondatom[n]->getPosition();
-//        stepwidth += fabs((currentPosition - oldPosition).ScalarProduct(BondVector))/bondforcedifference;
-//      }
-//      stepwidth = stepwidth/2;
-//      Vector PositionUpdate = stepwidth * BondVector;
-//      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.");
-//        PositionUpdate = currentDeltat * BondVector;
-//      }
-//      LOG(3, "DEBUG: Update would be " << PositionUpdate);
-//
-//      // remove the edge
-//#ifndef NDEBUG
-//      const bool status =
-//#endif
-//          BGcreator.removeEdge(bondatom[0]->getId(), bondatom[1]->getId());
-//      ASSERT( status, "ForceAnnealing() - edge to found bond is not present?");
-//
-//      // gather nodes for either atom
-//      BoostGraphHelpers::Nodeset_t bondside_set[2];
-//      BreadthFirstSearchGatherer::distance_map_t distance_map[2];
-//      for (size_t n=0;n<2;++n) {
-//        bondside_set[n] = NodeGatherer(bondatom[n]->getId(), max_distance);
-//        distance_map[n] = NodeGatherer.getDistances();
-//        std::sort(bondside_set[n].begin(), bondside_set[n].end());
-//      }
-//
-//      // re-add edge
-//      BGcreator.addEdge(bondatom[0]->getId(), bondatom[1]->getId());
-//
-//      // add PositionUpdate for all nodes in the bondside_set
-//      for (size_t n=0;n<2;++n) {
-//        for (BoostGraphHelpers::Nodeset_t::const_iterator setiter = bondside_set[n].begin();
-//            setiter != bondside_set[n].end(); ++setiter) {
-//          const BreadthFirstSearchGatherer::distance_map_t::const_iterator diter
-//            = distance_map[n].find(*setiter);
-//          ASSERT( diter != distance_map[n].end(),
-//              "ForceAnnealing() - could not find distance to an atom.");
-//          const double factor = pow(damping_factor, diter->second);
-//          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) );
-//          }
-//        }
-//        // invert for other atom
-//        PositionUpdate *= -1;
-//      }
-//    }
-//    delete[] bondforces;
-//    delete[] oldbondforces;
+
+    LOG(3, "DEBUG: current step is " << currentStep << ", given time step is " << CurrentTimeStep);
+    const BondVectors::mapped_t bondvectors = bv.getBondVectorsAtStep(CurrentTimeStep);
+
+    for (BondVectors::container_t::const_iterator bondsiter = sorted_bonds.begin();
+        bondsiter != sorted_bonds.end(); ++bondsiter) {
+      const bond::ptr &current_bond = *bondsiter;
+      const size_t index = bv.getIndexForBond(current_bond);
+      const atom* bondatom[MAX_sides] = {
+          current_bond->leftatom,
+          current_bond->rightatom
+      };
+
+      // remove the edge
+#ifndef NDEBUG
+      const bool status =
+#endif
+          BGcreator.removeEdge(bondatom[leftside]->getId(), bondatom[rightside]->getId());
+      ASSERT( status, "ForceAnnealing() - edge to found bond is not present?");
+
+      // gather nodes for either atom
+      BoostGraphHelpers::Nodeset_t bondside_set[MAX_sides];
+      BreadthFirstSearchGatherer::distance_map_t distance_map[MAX_sides];
+      for (size_t side=leftside;side<MAX_sides;++side) {
+        bondside_set[side] = NodeGatherer(bondatom[side]->getId(), max_distance);
+        distance_map[side] = NodeGatherer.getDistances();
+        std::sort(bondside_set[side].begin(), bondside_set[side].end());
+      }
+
+      // re-add edge
+      BGcreator.addEdge(bondatom[leftside]->getId(), bondatom[rightside]->getId());
+
+      // do for both leftatom and rightatom of bond
+      for (size_t side = leftside; side < MAX_sides; ++side) {
+        const double &bondforce = projected_forces[0][side][index];
+        const double &oldbondforce = projected_forces[1][side][index];
+        const double bondforcedifference = (bondforce - oldbondforce);
+        // if difference or bondforce itself is zero, do nothing
+        if ((fabs(bondforce) < MYEPSILON) || (fabs(bondforcedifference) < MYEPSILON))
+          continue;
+        const BondVectors::mapped_t::const_iterator bviter =
+            bondvectors.find(current_bond);
+        ASSERT( bviter != bondvectors.end(),
+            "ForceAnnealing() - cannot find current_bond ?");
+        const Vector &BondVector = bviter->second;
+
+        // calculate gradient and position differences for stepwidth
+        const Vector currentGradient = bondforce * BondVector;
+        LOG(4, "DEBUG: current projected gradient for "
+            << (side == leftside ? "left" : "right") << " side of bond is " << currentGradient);
+        const Vector &oldPosition = bondatom[side]->getPositionAtStep(CurrentTimeStep-2 >= 0 ? CurrentTimeStep - 2 : 0);
+        const Vector &currentPosition = bondatom[side]->getPositionAtStep(CurrentTimeStep-1>=0 ? CurrentTimeStep - 1 : 0);
+        const Vector PositionDifference = currentPosition - oldPosition;
+        LOG(4, "DEBUG: old position is " << oldPosition);
+        LOG(4, "DEBUG: current position is " << currentPosition);
+        LOG(4, "DEBUG: difference in position is " << PositionDifference);
+        LOG(4, "DEBUG: bondvector is " << BondVector);
+        const double projected_PositionDifference = PositionDifference.ScalarProduct(BondVector);
+        LOG(4, "DEBUG: difference in position projected onto bondvector is "
+            << projected_PositionDifference);
+        LOG(4, "DEBUG: abs. difference in forces is " << bondforcedifference);
+
+        // calculate step width
+        Vector PositionUpdate;
+        double stepwidth =
+            fabs(projected_PositionDifference)/bondforcedifference;
+        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.");
+          PositionUpdate = currentDeltat * BondVector;
+        }
+        LOG(3, "DEBUG: Update would be " << PositionUpdate);
+
+        // add PositionUpdate for all nodes in the bondside_set
+        for (BoostGraphHelpers::Nodeset_t::const_iterator setiter = bondside_set[side].begin();
+            setiter != bondside_set[side].end(); ++setiter) {
+          const BreadthFirstSearchGatherer::distance_map_t::const_iterator diter
+            = distance_map[side].find(*setiter);
+          ASSERT( diter != distance_map[side].end(),
+              "ForceAnnealing() - could not find distance to an atom.");
+          const double factor = pow(damping_factor, diter->second);
+          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) );
+          }
+        }
+      }
+    }
 
     for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
@@ -415,5 +516,5 @@
       atom &walker = *(*iter);
       // extract largest components for showing progress of annealing
-      const Vector currentGradient = walker.getAtomicForce();
+      const Vector currentGradient = walker.getAtomicForceAtStep(CurrentTimeStep-1>=0 ? CurrentTimeStep-1 : 0);
       for(size_t i=0;i<NDIM;++i)
         maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));
@@ -434,5 +535,8 @@
       LOG(3, "DEBUG: Applying update " << update << " to atom #" << atomid
           << ", namely " << *walker);
-      walker->setPosition( walker->getPosition() + update );
+      walker->setPosition(
+          walker->getPositionAtStep(CurrentTimeStep-1>=0 ? CurrentTimeStep - 1 : 0)
+          + update);
+//      walker->setAtomicForce( RemnantGradient_per_atom[walker->getId()] );
     }
   }
Index: tests/Python/ForceAnnealing/post/five_carbon_test_no-bondgraph.data
===================================================================
--- tests/Python/ForceAnnealing/post/five_carbon_test_no-bondgraph.data	(revision 8f50b4adab0382b92f8e46cd26895315f53857e7)
+++ tests/Python/ForceAnnealing/post/five_carbon_test_no-bondgraph.data	(revision 59634f473698d6931b521d6809970437a66c95ba)
@@ -79,5 +79,5 @@
 C	5	12.39	10	10	-2.11671e-05	0	0	-5.29177e-05	0	0	4	0	0	0	
 # ATOMDATA	type	Id	x=3	u=3	F=3	neighbors=4
-C	1	5.98964	10	10	1.06994e-21	0	0	5.29177e-06	0	0	2	0	0	0	
+C	1	5.98964	10	10	0	0	0	5.29177e-06	0	0	2	0	0	0	
 C	2	7.58965	10	10	1.03588e-05	0	0	5.82095e-05	0	0	1	3	0	0	
 C	3	9.18977	10	10	6.06392e-06	0	0	5.29177e-06	0	0	2	4	0	0	
@@ -98,5 +98,5 @@
 # ATOMDATA	type	Id	x=3	u=3	F=3	neighbors=4
 C	1	5.98964	10	10	6.68821e-07	0	0	3.70424e-05	0	0	2	0	0	0	
-C	2	7.58971	10	10	1.21594e-21	0	0	5.29177e-06	0	0	1	3	0	0	
+C	2	7.58971	10	10	0	0	0	5.29177e-06	0	0	1	3	0	0	
 C	3	9.18979	10	10	4.10578e-06	0	0	1.58753e-05	0	0	2	4	0	0	
 C	4	10.7899	10	10	-3.17506e-06	0	0	-5.29177e-06	0	0	3	5	0	0	
@@ -115,5 +115,5 @@
 C	5	12.39	10	10	-5.29177e-06	0	0	-5.29177e-05	0	0	4	0	0	0	
 # ATOMDATA	type	Id	x=3	u=3	F=3	neighbors=4
-C	1	5.98971	10	10	4.44509e-05	0	0	1.35525e-21	0	0	2	0	0	0	
+C	1	5.98971	10	10	4.44509e-05	0	0	0	0	0	2	0	0	0	
 C	2	7.58971	10	10	2.11671e-06	0	0	4.23342e-05	0	0	1	3	0	0	
 C	3	9.18979	10	10	1.58753e-06	0	0	1.58753e-05	0	0	2	4	0	0	
@@ -121,5 +121,5 @@
 C	5	12.39	10	10	-5.29177e-06	0	0	-5.29177e-05	0	0	4	0	0	0	
 # ATOMDATA	type	Id	x=3	u=3	F=3	neighbors=4
-C	1	5.98971	10	10	2.84603e-21	0	0	0	0	0	2	0	0	0	
+C	1	5.98971	10	10	0	0	0	0	0	0	2	0	0	0	
 C	2	7.58971	10	10	4.23342e-06	0	0	0	0	0	1	3	0	0	
 C	3	9.1898	10	10	1.58753e-06	0	0	0	0	0	2	4	0	0	
