Index: src/Dynamics/ForceAnnealing.hpp
===================================================================
--- src/Dynamics/ForceAnnealing.hpp	(revision d3e1d5d4486b2e09af478c48ac829a25fb521200)
+++ src/Dynamics/ForceAnnealing.hpp	(revision e77580e4686fad9ef486fc0a8c08621388132a31)
@@ -28,5 +28,8 @@
 #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"
@@ -232,88 +235,175 @@
     BreadthFirstSearchGatherer NodeGatherer(BGcreator);
 
-    std::map<atomId_t, Vector> GatheredUpdates; //!< gathers all updates which are applied at the end
+    /// 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.
+
+    /// One more 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
+    /// each bond partner, i.e. the step size is twice as large as it should be.
+    /// This problem only occurs when bonds need to be shortened, not when they
+    /// need to be made longer (then the force vector is facing the other
+    /// direction than the bond vector).
+    /// As a remedy we need to know the forces "per bond" and not per atom.
+    /// In effect, the gradient is the error per atom. However, we need an
+    /// error per bond. To this end, we set up a matrix A that translate the
+    /// vector of bond energies into a vector of atomic force component and
+    /// then we simply solve the linear system (inverse problem) via an SVD
+    /// and use the bond gradients to get the PositionUpdate for both bond
+    /// partners at the same time when we go through all bonds.
+
+    // gather/enumerate all bonds
+    std::set<bond::ptr> allbonds;
+    std::vector<atomId_t> allatomids;
     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(CurrentTimeStep-1 >= 0 ? CurrentTimeStep - 1 : 0);
-      const Vector currentPosition = (*iter)->getPositionAtStep(CurrentTimeStep);
-      const Vector oldGradient = (*iter)->getAtomicForceAtStep(CurrentTimeStep-1 >= 0 ? CurrentTimeStep - 1 : 0);
-      const Vector currentGradient = (*iter)->getAtomicForceAtStep(CurrentTimeStep);
-      LOG(4, "DEBUG: Force for atom " << **iter << " is " << currentGradient);
-
-      // we use Barzilai-Borwein update with position reversed to get descent
-      const double stepwidth = getBarzilaiBorweinStepwidth(
-          currentPosition - oldPosition, currentGradient - oldGradient);
-      const Vector PositionUpdate = stepwidth * currentGradient;
-      LOG(3, "DEBUG: Update would be " << stepwidth << "*" << currentGradient << " = " << PositionUpdate);
-
-//      // add update to central atom
-//      const atomId_t atomid = (*iter)->getId();
-//      if (GatheredUpdates.count(atomid)) {
-//        GatheredUpdates[atomid] += PositionUpdate;
-//      } else
-//        GatheredUpdates.insert( std::make_pair(atomid, PositionUpdate) );
-
-      // 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 just what we are going to do.
-
-      /// get all nodes from bonds in the direction of the current force
-
-      // remove edges facing in the wrong direction
-      std::vector<bond::ptr> removed_bonds;
-      const BondList& ListOfBonds = (*iter)->getListOfBonds();
+      const atom &walker = *(*iter);
+      allatomids.push_back(walker.getId());
+      const BondList& ListOfBonds = walker.getListOfBonds();
       for(BondList::const_iterator bonditer = ListOfBonds.begin();
           bonditer != ListOfBonds.end(); ++bonditer) {
-        const bond &current_bond = *(*bonditer);
-        LOG(2, "DEBUG: Looking at bond " << current_bond);
-        Vector BondVector = (*iter)->getPositionAtStep(CurrentTimeStep);
-        BondVector -= ((*iter)->getId() == current_bond.rightatom->getId())
-            ? current_bond.rightatom->getPositionAtStep(CurrentTimeStep) : current_bond.leftatom->getPositionAtStep(CurrentTimeStep);
-        BondVector.Normalize();
-        if (BondVector.ScalarProduct(currentGradient) < 0) {
-          removed_bonds.push_back(*bonditer);
-#ifndef NDEBUG
-          const bool status =
-#endif
-              BGcreator.removeEdge(current_bond.leftatom->getId(), current_bond.rightatom->getId());
-          ASSERT( status, "ForceAnnealing() - edge to found bond is not present?");
-        }
-      }
-      BoostGraphHelpers::Nodeset_t bondside_set = NodeGatherer((*iter)->getId(), max_distance);
-      const BreadthFirstSearchGatherer::distance_map_t &distance_map = NodeGatherer.getDistances();
-      std::sort(bondside_set.begin(), bondside_set.end());
-      // re-add those edges
-      for (std::vector<bond::ptr>::const_iterator bonditer = removed_bonds.begin();
-          bonditer != removed_bonds.end(); ++bonditer)
-        BGcreator.addEdge((*bonditer)->leftatom->getId(), (*bonditer)->rightatom->getId());
-
-      // apply PositionUpdate to all nodes in the bondside_set
-      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);
-        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) );
-        }
+        const bond::ptr &current_bond = *bonditer;
+        allbonds.insert(current_bond);
       }
 
       // extract largest components for showing progress of annealing
+      const Vector currentGradient = (*iter)->getAtomicForce();
       for(size_t i=0;i<NDIM;++i)
         maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));
-    }
+
+      // reset force vector for next step except on final one
+      if (currentStep != maxSteps)
+        (*iter)->setAtomicForce(zeroVec);
+    }
+    std::sort(allatomids.begin(), allatomids.end());
+    // converting set back to vector is fastest when requiring sorted and unique,
+    // see https://stackoverflow.com/questions/1041620/whats-the-most-efficient-way-to-erase-duplicates-and-sort-a-vector
+    typedef std::vector<bond::ptr> bondvector_t;
+    bondvector_t bondvector( allbonds.begin(), allbonds.end() );
+
+    // setup matrix A given the enumerated bonds
+    LinearSystemOfEquations lseq(AtomicForceManipulator<T>::atoms.size(), bondvector.size());
+    for (size_t i = 0;i<bondvector.size();++i) {
+      const atom* bondatom[2] = {
+           bondvector[i]->leftatom,
+           bondvector[i]->rightatom
+      };
+      size_t index[2];
+      for (size_t n=0;n<2;++n) {
+        const std::pair<std::vector<atomId_t>::iterator, std::vector<atomId_t>::iterator> atomiditer =
+            std::equal_range(allatomids.begin(), allatomids.end(), bondatom[n]->getId());
+        index[n] = std::distance(allatomids.begin(), atomiditer.first);
+        (*lseq.A).at(index[0],index[1]) = 1.;
+        (*lseq.A).at(index[1],index[0]) = 1.;
+      }
+    }
+    lseq.SetSymmetric(true);
+
+    // for each component and for current and last time step
+    // solve Ax=y with given A and y being the vectorized atomic force
+    double *tmpforces = new double[bondvector.size()];
+    double *bondforces = new double[bondvector.size()];
+    double *oldbondforces = new double[bondvector.size()];
+    double *bondforceref[2] = {
+        bondforces,
+        oldbondforces
+    };
+    for (size_t n=0;n<bondvector.size();++n) {
+      bondforces[n] = 0.;
+      oldbondforces[n] = 0.;
+    }
+    for (size_t timestep = 0; timestep <= 1; ++timestep) {
+      for (size_t component = 0; component < NDIM; ++component) {
+        for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
+            iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
+          const atom &walker = *(*iter);
+          const std::pair<std::vector<atomId_t>::iterator, std::vector<atomId_t>::iterator> atomiditer =
+              std::equal_range(allatomids.begin(), allatomids.end(), walker.getId());
+          const size_t i = std::distance(allatomids.begin(), atomiditer.first);
+        (*lseq.b).at(i) = timestep == 0 ?
+            walker.getAtomicForce()[component] :
+            walker.getAtomicForceAtStep(CurrentTimeStep-2 >= 0 ? CurrentTimeStep - 2 : 0)[component];
+      }
+      lseq.GetSolutionAsArray(tmpforces);
+      for (size_t i = 0;i<bondvector.size();++i)
+        bondforceref[timestep][i] += tmpforces[i];
+      }
+    }
+
+    // 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;
+      }
+    }
+
     // apply the gathered updates
     for (std::map<atomId_t, Vector>::const_iterator iter = GatheredUpdates.begin();
