Index: src/Dynamics/ForceAnnealing.hpp
===================================================================
--- src/Dynamics/ForceAnnealing.hpp	(revision e77580e4686fad9ef486fc0a8c08621388132a31)
+++ src/Dynamics/ForceAnnealing.hpp	(revision 355915e4ce604dedb0edaa8e40056fc7a41b812e)
@@ -13,4 +13,9 @@
 #include <config.h>
 #endif
+
+#include <algorithm>
+#include <iterator>
+
+#include <boost/bind.hpp>
 
 #include "Atom/atom.hpp"
@@ -249,28 +254,166 @@
     /// 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;
+    /// As a remedy we need to average the force on either end of the bond and
+    /// 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
+    /// generating system and not a basis (e.g. 3 bonds on a plane where 2 would
+    /// suffice to span the plane). To this end, we need to account for the
+    /// overestimation and obtain a weighting for each bond.
+
+    // gather weights
+    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;
+      LOG(2, "DEBUG: CurrentTimeStep is " << CurrentTimeStep
+          << ", timestep is " << timestep
+          << ", and CurrentStep is " << CurrentStep);
+
+      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(CurrentStep);
+
+        if (walkerGradient.Norm() > MYEPSILON) {
+
+          // gather BondVector and projected gradient over all bonds
+          const BondList& ListOfBonds = walker.getListOfBondsAtStep(CurrentStep);
+          std::vector<double> projected_forces;
+          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) );
+          }
+
+          // go through all bonds and check 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(
+                  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?");
+          weights_t &weights = inserter.first->second;
+          for (std::vector<Vector>::const_iterator iter = BondVectors.begin();
+              iter != BondVectors.end(); ++iter) {
+            std::vector<double> scps;
+            std::transform(
+                BondVectors.begin(), BondVectors.end(),
+                std::back_inserter(scps),
+                boost::bind(&Vector::ScalarProduct, boost::cref(*iter), _1)
+                );
+            const double scp_sum = std::accumulate(scps.begin(), scps.end(), 0.);
+            weights.push_back( 1./scp_sum );
+          }
+          // 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);
+            }
+            ASSERT( fabs(scp_sum - 1.) < MYEPSILON,
+                "ForceAnnealing::operator() - for BondVector "+toString(*iter)
+                +" we have weighted scalar product of "+toString(scp_sum)+" != 1.");
+          }
+#endif
+        } else {
+          LOG(2, "DEBUG: Gradient is " << walkerGradient << " less than "
+              << MYEPSILON << " for atom " << walker);
+        }
+      }
+    }
+
+    // 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;
+
     for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
         iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
-      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::ptr &current_bond = *bonditer;
-        allbonds.insert(current_bond);
-      }
-
+      atom &walker = *(*iter);
       // extract largest components for showing progress of annealing
-      const Vector currentGradient = (*iter)->getAtomicForce();
+      const Vector currentGradient = walker.getAtomicForce();
       for(size_t i=0;i<NDIM;++i)
         maxComponents[i] = std::max(maxComponents[i], fabs(currentGradient[i]));
@@ -278,130 +421,5 @@
       // 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;
-      }
+        walker.setAtomicForce(zeroVec);
     }
 
