Index: src/builder.cpp
===================================================================
--- src/builder.cpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
+++ src/builder.cpp	(revision a1fe77b80222d4b06bce07cbd04191ac9a489a93)
@@ -995,6 +995,10 @@
               if (!mol->VerletForceIntegration((ofstream *)&cout, argv[argptr], configuration.Deltat, configuration.GetIsAngstroem(), configuration.DoConstrainedMD))
                 cout << Verbose(2) << "File not found." << endl;
-              else
+              else {
                 cout << Verbose(2) << "File found and parsed." << endl;
+                if (configuration.DoConstrainedMD)
+                  // increase source step in expectancy of a new step in the config
+                  configuration.DoConstrainedMD++;
+              }
               argptr+=1;
               break;
Index: src/config.cpp
===================================================================
--- src/config.cpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
+++ src/config.cpp	(revision a1fe77b80222d4b06bce07cbd04191ac9a489a93)
@@ -1000,4 +1000,5 @@
     *output << "DoPerturbation\t" << config::DoPerturbation << "\t# Do perturbation calculate and determine susceptibility and shielding" << endl;
     *output << "DoFullCurrent\t" << config::DoFullCurrent << "\t# Do full perturbation" << endl;
+    *output << "DoConstrainedMD\t" << config::DoConstrainedMD << "\t# Do perform a constrained (>0, relating to current MD step) instead of unconstrained (0) MD" << endl;
     *output << "CommonWannier\t" << config::CommonWannier << "\t# Put virtual centers at indivual orbits, all common, merged by variance, to grid point, to cell center" << endl;
     *output << "SawtoothStart\t" << config::SawtoothStart << "\t# Absolute value for smooth transition at cell border " << endl;
Index: src/molecules.cpp
===================================================================
--- src/molecules.cpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
+++ src/molecules.cpp	(revision a1fe77b80222d4b06bce07cbd04191ac9a489a93)
@@ -1017,5 +1017,5 @@
  * \sa molecule::MinimiseConstrainedPotential(), molecule::VerletForceIntegration()
  * \param *out output stream for debugging
- * \param *permutation gives target index for each atom, array of size molecule::AtomCount (this is "x" in \f$V^{con}(x)\f$)
+ * \param *PermutationMap gives target ptr for each atom, array of size molecule::AtomCount (this is "x" in \f$V^{con}(x)\f$)
  * \param startstep start configuration (MDStep in molecule::trajectories)
  * \param endstep end configuration (MDStep in molecule::trajectories)
@@ -1025,5 +1025,5 @@
  * \note This routine is scaling quadratically which is not optimal.
  */
-double molecule::ConstrainedPotential(ofstream *out, int *permutation, int startstep, int endstep, double *constants, bool IsAngstroem)
+double molecule::ConstrainedPotential(ofstream *out, atom **PermutationMap, int startstep, int endstep, double *constants, bool IsAngstroem)
 {
   double result = 0., tmp;
@@ -1038,6 +1038,8 @@
     Walker = Walker->next;
     // first term: distance to target
-    Runner = FindAtom(permutation[Walker->nr]);   // find target point
-    result += constants[0] * (Trajectories[Walker].R.at(startstep).Distance(&Trajectories[Runner].R.at(endstep)));
+    Runner = PermutationMap[Walker->nr];   // find target point
+    tmp = (Trajectories[Walker].R.at(startstep).Distance(&Trajectories[Runner].R.at(endstep)));
+    tmp *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
+    result += constants[0] * tmp;
     
     // second term: sum of distances to other trajectories
@@ -1046,9 +1048,9 @@
       Runner = Runner->next;
       // determine normalized trajectories direction vector (n1, n2)
-      Sprinter = FindAtom(permutation[Walker->nr]);   // find target point
+      Sprinter = PermutationMap[Walker->nr];   // find target point
       trajectory1.CopyVector(&Trajectories[Sprinter].R.at(endstep));
       trajectory1.SubtractVector(&Trajectories[Walker].R.at(startstep));
       trajectory1.Normalize();
-      Sprinter = FindAtom(permutation[Runner->nr]);   // find target point
+      Sprinter = PermutationMap[Runner->nr];   // find target point
       trajectory2.CopyVector(&Trajectories[Sprinter].R.at(endstep));
       trajectory2.SubtractVector(&Trajectories[Runner].R.at(startstep));
@@ -1088,5 +1090,5 @@
           *out << "Test: ok.\tDistance of " << tmp << " is correct." << endl;  
         } else {
-          *out << "Test: failed.\tDifference in intersection is ";
+          *out << "Test: failed.\tIntersection is off by ";
           *out << TestVector;
           *out << "." << endl;  
@@ -1094,4 +1096,5 @@
       }
       // add up
+      tmp *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
       result += constants[1] * 1./tmp;
     }
@@ -1101,6 +1104,6 @@
     while (Runner->next != end) {
       Runner = Runner->next;
-      if ((permutation[Walker->nr] == permutation[Runner->nr]) && (Walker->nr < Runner->nr)) {
-        Sprinter = FindAtom(permutation[Walker->nr]);
+      if ((PermutationMap[Walker->nr] == PermutationMap[Runner->nr]) && (Walker->nr < Runner->nr)) {
+        Sprinter = PermutationMap[Walker->nr];
         *out << *Walker << " and " << *Runner << " are heading to the same target at ";
         *out << Trajectories[Sprinter].R.at(endstep);
@@ -1114,7 +1117,28 @@
 };
 
+void PrintPermutationMap(ofstream *out, atom **PermutationMap, int Nr) 
+{
+  stringstream zeile1, zeile2;
+  zeile1 << "PermutationMap: ";
+  zeile2 << "                ";
+  for (int i=0;i<Nr;i++) {
+    zeile1 << i << "\t";
+    zeile2 << PermutationMap[i]->nr << "\t";
+  }
+  *out << zeile1 << endl << zeile2 << endl;
+};
+
 /** Minimises the extra potential for constrained molecular dynamics and gives forces and the constrained potential energy.
  * We do the following:
- *  -# First, we minimise the potential, see molecule::ConstrainedPotential()
+ *  -# Generate a distance list from all source to all target points
+ *  -# Sort this per source point
+ *  -# Take for each source point the target point with minimum distance, use this as initial permutation
+ *  -# check whether molecule::ConstrainedPotential() is greater than injective penalty
+ *     -# If so, we go through each source point, stepping down in the sorted target point distance list and re-checking potential.
+ *  -# Next, we only apply transformations that keep the injectivity of the permutations list.
+ *  -# Hence, we for one source point we step down the ladder and seek the corresponding owner of this new target 
+ *     point and try to change it for one with lesser distance, or for the next one with greater distance, but only
+ *     if this decreases the conditional potential.
+ *  -# finished. 
  *  -# Then, we calculate the forces by taking the spatial derivative, where we scale the potential to such a degree,
  *     that the total force is always pointing in direction of the constraint force (ensuring that we move in the
@@ -1123,17 +1147,125 @@
  * \param *out output stream for debugging
  * \param *Force ForceMatrix containing force vectors from the external energy functional minimisation.
- * \param targetstep target step between which and the predecessor we perform the constrained MD
+ * \param startstep current MD step giving initial position between which and \a endstep we perform the constrained MD (as further steps are always concatenated)
+ * \param endstep step giving final position in constrained MD 
+ * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
  * \sa molecule::VerletForceIntegration()
  * \return potential energy
  */
-double molecule::MinimiseConstrainedPotential(ofstream *out, ForceMatrix *Force, int targetstep)
-{
-  double potential = 0.;
-  // Minimise the potential
-  
-  // evaluate forces
-  
-  // evaluate potential and return
-  return potential;
+double molecule::MinimiseConstrainedPotential(ofstream *out, ForceMatrix *Force, int startstep, int endstep, bool IsAngstroem)
+{
+  double Potential, OldPotential;
+  atom **PermutationMap = (atom **) Malloc(AtomCount*sizeof(atom *), "molecule::MinimiseConstrainedPotential: *PermutationMap");
+  DistanceMap **DistanceList = (DistanceMap **) Malloc(AtomCount*sizeof(DistanceMap *), "molecule::MinimiseConstrainedPotential: **DistanceList");
+  DistanceMap::iterator *DistanceIterators = (DistanceMap::iterator *) Malloc(AtomCount*sizeof(DistanceMap::iterator), "molecule::MinimiseConstrainedPotential: *DistanceIterators"); 
+  double constants[3];
+  atom *Walker = NULL, *Runner = NULL, *Sprinter = NULL;
+
+  /// Minimise the potential
+  // set constants (TODO)
+  constants[0] = 10.;
+  constants[1] = 1.;
+  constants[2] = 1e+7;    // just a huge penalty
+  // generate the distance list
+  *out << Verbose(1) << "Generating the distance list ... " << endl;
+  for (int i=AtomCount; i--;) {
+    DistanceList[i] = new DistanceMap;
+    DistanceList[i]->clear();
+  }
+  Walker = start;
+  while (Walker->next != NULL) {
+    Walker = Walker->next;
+    Runner = start;
+    while(Runner->next != NULL) {
+      Runner = Runner->next;
+      DistanceList[Walker->nr]->insert( DistancePair(Trajectories[Walker].R.at(startstep).Distance(&Trajectories[Runner].R.at(endstep)), Runner) );
+    }
+  }
+  // create the initial PermutationMap
+  Walker = start;
+  while (Walker->next != NULL) {
+    Walker = Walker->next;
+    PermutationMap[Walker->nr] = DistanceList[Walker->nr]->begin()->second;
+    DistanceIterators[Walker->nr] = DistanceList[Walker->nr]->begin();
+    *out << *Walker << " starts with distance " << DistanceList[Walker->nr]->begin()->first << "." << endl;
+  }
+  // make the PermutationMap injective 
+  *out << Verbose(1) << "Making the PermutationMap injective ... " << endl;
+  Walker = start;
+  while ((OldPotential = fabs(ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem)) > constants[2])) {
+    PrintPermutationMap(out, PermutationMap, AtomCount);
+    Walker = Walker->next;
+    if (Walker == end) // round-robin at the end
+      Walker = start->next;
+    DistanceIterators[Walker->nr]++;  // take next further distance in distance to targets list
+    PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
+  }
+  // argument minimise the constrained potential in this injective PermutationMap
+  *out << Verbose(1) << "Argument minimising the PermutationMap ... " << endl;
+  do {
+    PrintPermutationMap(out, PermutationMap, AtomCount);
+    Walker = start;
+    while (Walker->next != end) { // pick one
+      Walker = Walker->next;
+      Sprinter = DistanceIterators[Walker->nr]->second;   // store initial partner
+      DistanceIterators[Walker->nr]++;  // take next farther distance target
+      PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
+      Runner = start;
+      while(Runner->next != end) { // find the other whose toes were stepping on (this target should be used by another already
+        Runner = Runner->next;
+        if (PermutationMap[Runner->nr] == DistanceIterators[Walker->nr]->second)
+          break;
+      }
+      if (Runner->next != end) { // we found one a tripel
+        // then look in distance list for Sprinter
+        DistanceMap::iterator Rider = DistanceList[Walker->nr]->begin();
+        for (; Rider != DistanceList[Walker->nr]->end(); Rider++)
+          if (Rider->second == Sprinter)
+            break;
+        if (Rider != DistanceList[Walker->nr]->end()) { // if we have found one
+          // exchange the second also 
+          PermutationMap[Runner->nr] = Rider->second;
+          // calculate the new potential
+          Potential = ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem);
+          if (Potential > OldPotential) { // we made everything worse! Undo ...
+            // Undo for Walker
+            DistanceIterators[Walker->nr]--;  // take next farther distance target
+            PermutationMap[Walker->nr] = Sprinter;
+            // Undo for Runner (note, we haven't moved the iteration yet, we may use this)
+            PermutationMap[Runner->nr] = DistanceIterators[Runner->nr]->second;
+          } else {
+            DistanceIterators[Runner->nr] = Rider;  // if successful also move the pointer in the iterator list
+            OldPotential = Potential;
+            *out << "Found a better permutation, new potential is " << OldPotential << "." << endl;
+            break;
+          }
+          if (Potential > constants[2])
+            cerr << "ERROR: The two-step permutation procedure did not maintain injectivity!" << endl;
+        }
+      } else {
+        DistanceIterators[Walker->nr]--;  // take next farther distance target
+        PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
+      }
+    }
+  } while (Walker->next != end);
+
+  /// evaluate forces (only the distance to target dependent part) with the final PermutationMap
+  *out << Verbose(1) << "Calculating forces and adding onto ForceMatrix ... " << endl;
+  Walker = start;
+  while (Walker->next != NULL) {
+    Walker = Walker->next;
+    Sprinter = PermutationMap[Walker->nr];
+    // set forces
+    for (int i=NDIM;i++;)
+      Force->Matrix[0][Walker->nr][5+i] += 2.*constants[0]*sqrt(Trajectories[Walker].R.at(startstep).Distance(&Trajectories[Sprinter].R.at(endstep)));
+  }  
+  
+  /// evaluate potential, free memory and return
+  for (int i=AtomCount; i--;)
+    DistanceList[i]->clear();
+  Free((void **)&DistanceList, "molecule::MinimiseConstrainedPotential: **DistanceList");
+  Free((void **)&DistanceIterators, "molecule::MinimiseConstrainedPotential: *DistanceIterators");
+  Free((void **)&PermutationMap, "molecule::MinimiseConstrainedPotential: *PermutationMap");
+  return ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem);
 };
 
@@ -1188,5 +1320,6 @@
     // solve a constrained potential if we are meant to
     if (DoConstrained) {
-      ConstrainedPotentialEnergy = MinimiseConstrainedPotential(out, &Force, DoConstrained);
+      // calculate forces and potential
+      ConstrainedPotentialEnergy = MinimiseConstrainedPotential(out, &Force, DoConstrained, 0, IsAngstroem);
     }
     
Index: src/molecules.hpp
===================================================================
--- src/molecules.hpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
+++ src/molecules.hpp	(revision a1fe77b80222d4b06bce07cbd04191ac9a489a93)
@@ -261,6 +261,6 @@
 	double VolumeOfConvexEnvelope(ofstream *out, bool IsAngstroem);
 	bool VerletForceIntegration(ofstream *out, char *file, double delta_t, bool IsAngstroem, int DoConstrained);
-	double ConstrainedPotential(ofstream *out, int *permutation, int start, int end, double *constants, bool IsAngstroem);
-	double MinimiseConstrainedPotential(ofstream *out, ForceMatrix *Force, int targetstep);
+	double ConstrainedPotential(ofstream *out, atom **permutation, int start, int end, double *constants, bool IsAngstroem);
+	double MinimiseConstrainedPotential(ofstream *out, ForceMatrix *Force, int startstep, int endstep, bool IsAngstroem);
 	
   bool CheckBounds(const Vector *x) const;
