/*
 * molecule_dynamics.cpp
 *
 *  Created on: Oct 5, 2009
 *      Author: heber
 */

#include "atom.hpp"
#include "config.hpp"
#include "element.hpp"
#include "memoryallocator.hpp"
#include "molecule.hpp"
#include "parser.hpp"

/************************************* Functions for class molecule *********************************/

/** Penalizes long trajectories.
 * \param *Walker atom to check against others
 * \param *mol molecule with other atoms
 * \param &Params constraint potential parameters
 * \return penalty times each distance
 */
double SumDistanceOfTrajectories(atom *Walker, molecule *mol, struct EvaluatePotential &Params)
{
  gsl_matrix *A = gsl_matrix_alloc(NDIM,NDIM);
  gsl_vector *x = gsl_vector_alloc(NDIM);
  atom * Runner = mol->start;
  atom *Sprinter = NULL;
  Vector trajectory1, trajectory2, normal, TestVector;
  double Norm1, Norm2, tmp, result = 0.;

  while (Runner->next != mol->end) {
    Runner = Runner->next;
    if (Runner == Walker) // hence, we only go up to the Walker, not beyond (similar to i=0; i<j; i++)
      break;
    // determine normalized trajectories direction vector (n1, n2)
    Sprinter = Params.PermutationMap[Walker->nr];   // find first target point
    trajectory1.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));
    trajectory1.SubtractVector(&Walker->Trajectory.R.at(Params.startstep));
    trajectory1.Normalize();
    Norm1 = trajectory1.Norm();
    Sprinter = Params.PermutationMap[Runner->nr];   // find second target point
    trajectory2.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));
    trajectory2.SubtractVector(&Runner->Trajectory.R.at(Params.startstep));
    trajectory2.Normalize();
    Norm2 = trajectory1.Norm();
    // check whether either is zero()
    if ((Norm1 < MYEPSILON) && (Norm2 < MYEPSILON)) {
      tmp = Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.startstep));
    } else if (Norm1 < MYEPSILON) {
      Sprinter = Params.PermutationMap[Walker->nr];   // find first target point
      trajectory1.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));  // copy first offset
      trajectory1.SubtractVector(&Runner->Trajectory.R.at(Params.startstep));  // subtract second offset
      trajectory2.Scale( trajectory1.ScalarProduct(&trajectory2) ); // trajectory2 is scaled to unity, hence we don't need to divide by anything
      trajectory1.SubtractVector(&trajectory2);   // project the part in norm direction away
      tmp = trajectory1.Norm();  // remaining norm is distance
    } else if (Norm2 < MYEPSILON) {
      Sprinter = Params.PermutationMap[Runner->nr];   // find second target point
      trajectory2.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));  // copy second offset
      trajectory2.SubtractVector(&Walker->Trajectory.R.at(Params.startstep));  // subtract first offset
      trajectory1.Scale( trajectory2.ScalarProduct(&trajectory1) ); // trajectory1 is scaled to unity, hence we don't need to divide by anything
      trajectory2.SubtractVector(&trajectory1);   // project the part in norm direction away
      tmp = trajectory2.Norm();  // remaining norm is distance
    } else if ((fabs(trajectory1.ScalarProduct(&trajectory2)/Norm1/Norm2) - 1.) < MYEPSILON) { // check whether they're linear dependent
  //        *out << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear dependent: ";
  //        *out << trajectory1;
  //        *out << " and ";
  //        *out << trajectory2;
      tmp = Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.startstep));
  //        *out << " with distance " << tmp << "." << endl;
    } else { // determine distance by finding minimum distance
  //        *out << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear independent ";
  //        *out << endl;
  //        *out << "First Trajectory: ";
  //        *out << trajectory1 << endl;
  //        *out << "Second Trajectory: ";
  //        *out << trajectory2 << endl;
      // determine normal vector for both
      normal.MakeNormalVector(&trajectory1, &trajectory2);
      // print all vectors for debugging
  //        *out << "Normal vector in between: ";
  //        *out << normal << endl;
      // setup matrix
      for (int i=NDIM;i--;) {
        gsl_matrix_set(A, 0, i, trajectory1.x[i]);
        gsl_matrix_set(A, 1, i, trajectory2.x[i]);
        gsl_matrix_set(A, 2, i, normal.x[i]);
        gsl_vector_set(x,i, (Walker->Trajectory.R.at(Params.startstep).x[i] - Runner->Trajectory.R.at(Params.startstep).x[i]));
      }
      // solve the linear system by Householder transformations
      gsl_linalg_HH_svx(A, x);
      // distance from last component
      tmp = gsl_vector_get(x,2);
  //        *out << " with distance " << tmp << "." << endl;
      // test whether we really have the intersection (by checking on c_1 and c_2)
      TestVector.CopyVector(&Runner->Trajectory.R.at(Params.startstep));
      trajectory2.Scale(gsl_vector_get(x,1));
      TestVector.AddVector(&trajectory2);
      normal.Scale(gsl_vector_get(x,2));
      TestVector.AddVector(&normal);
      TestVector.SubtractVector(&Walker->Trajectory.R.at(Params.startstep));
      trajectory1.Scale(gsl_vector_get(x,0));
      TestVector.SubtractVector(&trajectory1);
      if (TestVector.Norm() < MYEPSILON) {
  //          *out << Verbose(2) << "Test: ok.\tDistance of " << tmp << " is correct." << endl;
      } else {
  //          *out << Verbose(2) << "Test: failed.\tIntersection is off by ";
  //          *out << TestVector;
  //          *out << "." << endl;
      }
    }
    // add up
    tmp *= Params.IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
    if (fabs(tmp) > MYEPSILON) {
      result += Params.PenaltyConstants[1] * 1./tmp;
      //*out << Verbose(4) << "Adding " << 1./tmp*constants[1] << "." << endl;
    }
  }
  return result;
};

/** Penalizes atoms heading to same target.
 * \param *Walker atom to check against others
 * \param *mol molecule with other atoms
 * \param &Params constrained potential parameters
 * \return \a penalty times the number of equal targets
 */
double PenalizeEqualTargets(atom *Walker, molecule *mol, struct EvaluatePotential &Params)
{
  double result = 0.;
  atom * Runner = mol->start;
  while (Runner->next != mol->end) {
    Runner = Runner->next;
    if ((Params.PermutationMap[Walker->nr] == Params.PermutationMap[Runner->nr]) && (Walker->nr < Runner->nr)) {
  //    atom *Sprinter = PermutationMap[Walker->nr];
  //        *out << *Walker << " and " << *Runner << " are heading to the same target at ";
  //        *out << Sprinter->Trajectory.R.at(endstep);
  //        *out << ", penalting." << endl;
      result += Params.PenaltyConstants[2];
      //*out << Verbose(4) << "Adding " << constants[2] << "." << endl;
    }
  }
  return result;
};

/** Evaluates the potential energy used for constrained molecular dynamics.
 * \f$V_i^{con} = c^{bond} \cdot | r_{P(i)} - R_i | + sum_{i \neq j} C^{min} \cdot \frac{1}{C_{ij}} + C^{inj} \Bigl (1 - \theta \bigl (\prod_{i \neq j} (P(i) - P(j)) \bigr ) \Bigr )\f$
 *     where the first term points to the target in minimum distance, the second is a penalty for trajectories lying too close to each other (\f$C_{ij}\f$ is minimum distance between
 *     trajectories i and j) and the third term is a penalty for two atoms trying to each the same target point.
 * Note that for the second term we have to solve the following linear system:
 * \f$-c_1 \cdot n_1 + c_2 \cdot n_2 + C \cdot n_3 = - p_2 + p_1\f$, where \f$c_1\f$, \f$c_2\f$ and \f$C\f$ are constants,
 * offset vector \f$p_1\f$ in direction \f$n_1\f$, offset vector \f$p_2\f$ in direction \f$n_2\f$,
 * \f$n_3\f$ is the normal vector to both directions. \f$C\f$ would be the minimum distance between the two lines.
 * \sa molecule::MinimiseConstrainedPotential(), molecule::VerletForceIntegration()
 * \param *out output stream for debugging
 * \param &Params constrained potential parameters
 * \return potential energy
 * \note This routine is scaling quadratically which is not optimal.
 * \todo There's a bit double counting going on for the first time, bu nothing to worry really about.
 */
double molecule::ConstrainedPotential(ofstream *out, struct EvaluatePotential &Params)
{
  double tmp, result;

  // go through every atom
  atom *Runner = NULL;
  atom *Walker = start;
  while (Walker->next != end) {
    Walker = Walker->next;
    // first term: distance to target
    Runner = Params.PermutationMap[Walker->nr];   // find target point
    tmp = (Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.endstep)));
    tmp *= Params.IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
    result += Params.PenaltyConstants[0] * tmp;
    //*out << Verbose(4) << "Adding " << tmp*constants[0] << "." << endl;

    // second term: sum of distances to other trajectories
    result += SumDistanceOfTrajectories(Walker, this, Params);

    // third term: penalty for equal targets
    result += PenalizeEqualTargets(Walker, this, Params);
  }

  return result;
};

/** print the current permutation map.
 * \param *out output stream for debugging
 * \param &Params constrained potential parameters
 * \param AtomCount number of atoms
 */
void PrintPermutationMap(ofstream *out, int AtomCount, struct EvaluatePotential &Params)
{
  stringstream zeile1, zeile2;
  int *DoubleList = Malloc<int>(AtomCount, "PrintPermutationMap: *DoubleList");
  int doubles = 0;
  for (int i=0;i<AtomCount;i++)
    DoubleList[i] = 0;
  zeile1 << "PermutationMap: ";
  zeile2 << "                ";
  for (int i=0;i<AtomCount;i++) {
    Params.DoubleList[Params.PermutationMap[i]->nr]++;
    zeile1 << i << " ";
    zeile2 << Params.PermutationMap[i]->nr << " ";
  }
  for (int i=0;i<AtomCount;i++)
    if (Params.DoubleList[i] > 1)
    doubles++;
  if (doubles >0)
    *out << "Found " << doubles << " Doubles." << endl;
  Free(&DoubleList);
//  *out << zeile1.str() << endl << zeile2.str() << endl;
};

/** \f$O(N^2)\f$ operation of calculation distance between each atom pair and putting into DistanceList.
 * \param *mol molecule to scan distances in
 * \param &Params constrained potential parameters
 */
void FillDistanceList(molecule *mol, struct EvaluatePotential &Params)
{
  for (int i=mol->AtomCount; i--;) {
    Params.DistanceList[i] = new DistanceMap;    // is the distance sorted target list per atom
    Params.DistanceList[i]->clear();
  }

  atom *Runner = NULL;
  atom *Walker = mol->start;
  while (Walker->next != mol->end) {
    Walker = Walker->next;
    Runner = mol->start;
    while(Runner->next != mol->end) {
      Runner = Runner->next;
      Params.DistanceList[Walker->nr]->insert( DistancePair(Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.endstep)), Runner) );
    }
  }
};

/** initialize lists.
 * \param *out output stream for debugging
 * \param *mol molecule to scan distances in
 * \param &Params constrained potential parameters
 */
void CreateInitialLists(ofstream *out, molecule *mol, struct EvaluatePotential &Params)
{
  for (int i=mol->AtomCount; i--;)
    Params.DoubleList[i] = 0;  // stores for how many atoms in startstep this atom is a target in endstep

  atom *Walker = mol->start;
  while (Walker->next != mol->end) {
    Walker = Walker->next;
    Params.StepList[Walker->nr] = Params.DistanceList[Walker->nr]->begin();    // stores the step to the next iterator that could be a possible next target
    Params.PermutationMap[Walker->nr] = Params.DistanceList[Walker->nr]->begin()->second;   // always pick target with the smallest distance
    Params.DoubleList[Params.DistanceList[Walker->nr]->begin()->second->nr]++;            // increase this target's source count (>1? not injective)
    Params.DistanceIterators[Walker->nr] = Params.DistanceList[Walker->nr]->begin();    // and remember which one we picked
    *out << *Walker << " starts with distance " << Params.DistanceList[Walker->nr]->begin()->first << "." << endl;
  }
};

/** Try the next nearest neighbour in order to make the permutation map injective.
 * \param *out output stream for debugging
 * \param *mol molecule
 * \param *Walker atom to change its target
 * \param &OldPotential old value of constraint potential to see if we do better with new target
 * \param &Params constrained potential parameters
 */
double TryNextNearestNeighbourForInjectivePermutation(ofstream *out, molecule *mol, atom *Walker, double &OldPotential, struct EvaluatePotential &Params)
{
  double Potential = 0;
  DistanceMap::iterator NewBase = Params.DistanceIterators[Walker->nr];  // store old base
  do {
    NewBase++;  // take next further distance in distance to targets list that's a target of no one
  } while ((Params.DoubleList[NewBase->second->nr] != 0) && (NewBase != Params.DistanceList[Walker->nr]->end()));
  if (NewBase != Params.DistanceList[Walker->nr]->end()) {
    Params.PermutationMap[Walker->nr] = NewBase->second;
    Potential = fabs(mol->ConstrainedPotential(out, Params));
    if (Potential > OldPotential) { // undo
      Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second;
    } else {  // do
      Params.DoubleList[Params.DistanceIterators[Walker->nr]->second->nr]--;  // decrease the old entry in the doubles list
      Params.DoubleList[NewBase->second->nr]++;    // increase the old entry in the doubles list
      Params.DistanceIterators[Walker->nr] = NewBase;
      OldPotential = Potential;
      *out << Verbose(3) << "Found a new permutation, new potential is " << OldPotential << "." << endl;
    }
  }
  return Potential;
};

/** Permutes \a **&PermutationMap until the penalty is below constants[2].
 * \param *out output stream for debugging
 * \param *mol molecule to scan distances in
 * \param &Params constrained potential parameters
 */
void MakeInjectivePermutation(ofstream *out, molecule *mol, struct EvaluatePotential &Params)
{
  atom *Walker = mol->start;
  DistanceMap::iterator NewBase;
  double Potential = fabs(mol->ConstrainedPotential(out, Params));

  while ((Potential) > Params.PenaltyConstants[2]) {
    PrintPermutationMap(out, mol->AtomCount, Params);
    Walker = Walker->next;
    if (Walker == mol->end) // round-robin at the end
      Walker = mol->start->next;
    if (Params.DoubleList[Params.DistanceIterators[Walker->nr]->second->nr] <= 1)  // no need to make those injective that aren't
      continue;
    // now, try finding a new one
    Potential = TryNextNearestNeighbourForInjectivePermutation(out, mol, Walker, Potential, Params);
  }
  for (int i=mol->AtomCount; i--;) // now each single entry in the DoubleList should be <=1
    if (Params.DoubleList[i] > 1) {
      cerr << "Failed to create an injective PermutationMap!" << endl;
      exit(1);
    }
  *out << Verbose(1) << "done." << endl;
};

/** Minimises the extra potential for constrained molecular dynamics and gives forces and the constrained potential energy.
 * We do the following:
 *  -# 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, 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
 *     right direction).
 *  -# Finally, we calculate the potential energy and return.
 * \param *out output stream for debugging
 * \param **PermutationMap on return: mapping between the atom label of the initial and the final configuration
 * \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 (and allocated **PermutationMap (array of molecule::AtomCount ^2)
 * \todo The constrained potential's constants are set to fixed values right now, but they should scale based on checks of the system in order
 *       to ensure they're properties (e.g. constants[2] always greater than the energy of the system).
 * \bug this all is not O(N log N) but O(N^2)
 */
double molecule::MinimiseConstrainedPotential(ofstream *out, atom **&PermutationMap, int startstep, int endstep, bool IsAngstroem)
{
  double Potential, OldPotential, OlderPotential;
  struct EvaluatePotential Params;
  Params.PermutationMap = Malloc<atom*>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.**PermutationMap");
  Params.DistanceList = Malloc<DistanceMap*>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.**DistanceList");
  Params.DistanceIterators = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.*DistanceIterators");
  Params.DoubleList = Malloc<int>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.*DoubleList");
  Params.StepList = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.*StepList");
  int round;
  atom *Walker = NULL, *Runner = NULL, *Sprinter = NULL;
  DistanceMap::iterator Rider, Strider;

  /// Minimise the potential
  // set Lagrange multiplier constants
  Params.PenaltyConstants[0] = 10.;
  Params.PenaltyConstants[1] = 1.;
  Params.PenaltyConstants[2] = 1e+7;    // just a huge penalty
  // generate the distance list
  *out << Verbose(1) << "Allocating, initializting and filling the distance list ... " << endl;
  FillDistanceList(this, Params);

  // create the initial PermutationMap (source -> target)
  CreateInitialLists(out, this, Params);

  // make the PermutationMap injective by checking whether we have a non-zero constants[2] term in it
  *out << Verbose(1) << "Making the PermutationMap injective ... " << endl;
  MakeInjectivePermutation(out, this, Params);
  Free(&Params.DoubleList);

  // argument minimise the constrained potential in this injective PermutationMap
  *out << Verbose(1) << "Argument minimising the PermutationMap, at current potential " << OldPotential << " ... " << endl;
  OldPotential = 1e+10;
  round = 0;
  do {
    *out << "Starting round " << ++round << " ... " << endl;
    OlderPotential = OldPotential;
    do {
      Walker = start;
      while (Walker->next != end) { // pick one
        Walker = Walker->next;
        PrintPermutationMap(out, AtomCount, Params);
        Sprinter = Params.DistanceIterators[Walker->nr]->second;   // store initial partner
        Strider = Params.DistanceIterators[Walker->nr];  //remember old iterator
        Params.DistanceIterators[Walker->nr] = Params.StepList[Walker->nr];
        if (Params.DistanceIterators[Walker->nr] == Params.DistanceList[Walker->nr]->end()) {// stop, before we run through the list and still on
          Params.DistanceIterators[Walker->nr] == Params.DistanceList[Walker->nr]->begin();
          break;
        }
        //*out << Verbose(2) << "Current Walker: " << *Walker << " with old/next candidate " << *Sprinter << "/" << *DistanceIterators[Walker->nr]->second << "." << endl;
        // find source of the new target
        Runner = start->next;
        while(Runner != end) { // find the source whose toes we might be stepping on (Walker's new target should be in use by another already)
          if (Params.PermutationMap[Runner->nr] == Params.DistanceIterators[Walker->nr]->second) {
            //*out << Verbose(2) << "Found the corresponding owner " << *Runner << " to " << *PermutationMap[Runner->nr] << "." << endl;
            break;
          }
          Runner = Runner->next;
        }
        if (Runner != end) { // we found the other source
          // then look in its distance list for Sprinter
          Rider = Params.DistanceList[Runner->nr]->begin();
          for (; Rider != Params.DistanceList[Runner->nr]->end(); Rider++)
            if (Rider->second == Sprinter)
              break;
          if (Rider != Params.DistanceList[Runner->nr]->end()) { // if we have found one
            //*out << Verbose(2) << "Current Other: " << *Runner << " with old/next candidate " << *PermutationMap[Runner->nr] << "/" << *Rider->second << "." << endl;
            // exchange both
            Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second; // put next farther distance into PermutationMap
            Params.PermutationMap[Runner->nr] = Sprinter;  // and hand the old target to its respective owner
            PrintPermutationMap(out, AtomCount, Params);
            // calculate the new potential
            //*out << Verbose(2) << "Checking new potential ..." << endl;
            Potential = ConstrainedPotential(out, Params);
            if (Potential > OldPotential) { // we made everything worse! Undo ...
              //*out << Verbose(3) << "Nay, made the potential worse: " << Potential << " vs. " << OldPotential << "!" << endl;
              //*out << Verbose(3) << "Setting " << *Runner << "'s source to " << *Params.DistanceIterators[Runner->nr]->second << "." << endl;
              // Undo for Runner (note, we haven't moved the iteration yet, we may use this)
              Params.PermutationMap[Runner->nr] = Params.DistanceIterators[Runner->nr]->second;
              // Undo for Walker
              Params.DistanceIterators[Walker->nr] = Strider;  // take next farther distance target
              //*out << Verbose(3) << "Setting " << *Walker << "'s source to " << *Params.DistanceIterators[Walker->nr]->second << "." << endl;
              Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second;
            } else {
              Params.DistanceIterators[Runner->nr] = Rider;  // if successful also move the pointer in the iterator list
              *out << Verbose(3) << "Found a better permutation, new potential is " << Potential << " vs." << OldPotential << "." << endl;
              OldPotential = Potential;
            }
            if (Potential > Params.PenaltyConstants[2]) {
              cerr << "ERROR: The two-step permutation procedure did not maintain injectivity!" << endl;
              exit(255);
            }
            //*out << endl;
          } else {
            cerr << "ERROR: " << *Runner << " was not the owner of " << *Sprinter << "!" << endl;
            exit(255);
          }
        } else {
          Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second; // new target has no source!
        }
        Params.StepList[Walker->nr]++; // take next farther distance target
      }
    } while (Walker->next != end);
  } while ((OlderPotential - OldPotential) > 1e-3);
  *out << Verbose(1) << "done." << endl;


  /// free memory and return with evaluated potential
  for (int i=AtomCount; i--;)
    Params.DistanceList[i]->clear();
  Free(&Params.DistanceList);
  Free(&Params.DistanceIterators);
  return ConstrainedPotential(out, Params);
};


/** Evaluates the (distance-related part) of the constrained potential for the constrained forces.
 * \param *out output stream for debugging
 * \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 **PermutationMap mapping between the atom label of the initial and the final configuration
 * \param *Force ForceMatrix containing force vectors from the external energy functional minimisation.
 * \todo the constant for the constrained potential distance part is hard-coded independently of the hard-coded value in MinimiseConstrainedPotential()
 */
void molecule::EvaluateConstrainedForces(ofstream *out, int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force)
{
  /// evaluate forces (only the distance to target dependent part) with the final PermutationMap
  *out << Verbose(1) << "Calculating forces and adding onto ForceMatrix ... " << endl;
  ActOnAllAtoms( &atom::EvaluateConstrainedForce, startstep, endstep, PermutationMap, Force );
  *out << Verbose(1) << "done." << endl;
};

/** Performs a linear interpolation between two desired atomic configurations with a given number of steps.
 * Note, step number is config::MaxOuterStep
 * \param *out output stream for debugging
 * \param startstep stating initial configuration in molecule::Trajectories
 * \param endstep stating final configuration in molecule::Trajectories
 * \param &config configuration structure
 * \param MapByIdentity if true we just use the identity to map atoms in start config to end config, if not we find mapping by \sa MinimiseConstrainedPotential()
 * \return true - success in writing step files, false - error writing files or only one step in molecule::Trajectories
 */
bool molecule::LinearInterpolationBetweenConfiguration(ofstream *out, int startstep, int endstep, const char *prefix, config &configuration, bool MapByIdentity)
{
  molecule *mol = NULL;
  bool status = true;
  int MaxSteps = configuration.MaxOuterStep;
  MoleculeListClass *MoleculePerStep = new MoleculeListClass();
  // Get the Permutation Map by MinimiseConstrainedPotential
  atom **PermutationMap = NULL;
  atom *Walker = NULL, *Sprinter = NULL;
  if (!MapByIdentity)
    MinimiseConstrainedPotential(out, PermutationMap, startstep, endstep, configuration.GetIsAngstroem());
  else {
    PermutationMap = Malloc<atom *>(AtomCount, "molecule::LinearInterpolationBetweenConfiguration: **PermutationMap");
    SetIndexedArrayForEachAtomTo( PermutationMap, &atom::nr );
  }

  // check whether we have sufficient space in Trajectories for each atom
  ActOnAllAtoms( &atom::ResizeTrajectory, MaxSteps );
  // push endstep to last one
  ActOnAllAtoms( &atom::CopyStepOnStep, MaxSteps, endstep );
  endstep = MaxSteps;

  // go through all steps and add the molecular configuration to the list and to the Trajectories of \a this molecule
  *out << Verbose(1) << "Filling intermediate " << MaxSteps << " steps with MDSteps of " << MDSteps << "." << endl;
  for (int step = 0; step <= MaxSteps; step++) {
    mol = new molecule(elemente);
    MoleculePerStep->insert(mol);
    Walker = start;
    while (Walker->next != end) {
      Walker = Walker->next;
      // add to molecule list
      Sprinter = mol->AddCopyAtom(Walker);
      for (int n=NDIM;n--;) {
        Sprinter->x.x[n] = Walker->Trajectory.R.at(startstep).x[n] + (PermutationMap[Walker->nr]->Trajectory.R.at(endstep).x[n] - Walker->Trajectory.R.at(startstep).x[n])*((double)step/(double)MaxSteps);
        // add to Trajectories
        //*out << Verbose(3) << step << ">=" << MDSteps-1 << endl;
        if (step < MaxSteps) {
          Walker->Trajectory.R.at(step).x[n] = Walker->Trajectory.R.at(startstep).x[n] + (PermutationMap[Walker->nr]->Trajectory.R.at(endstep).x[n] - Walker->Trajectory.R.at(startstep).x[n])*((double)step/(double)MaxSteps);
          Walker->Trajectory.U.at(step).x[n] = 0.;
          Walker->Trajectory.F.at(step).x[n] = 0.;
        }
      }
    }
  }
  MDSteps = MaxSteps+1;   // otherwise new Trajectories' points aren't stored on save&exit

  // store the list to single step files
  int *SortIndex = Malloc<int>(AtomCount, "molecule::LinearInterpolationBetweenConfiguration: *SortIndex");
  for (int i=AtomCount; i--; )
    SortIndex[i] = i;
  status = MoleculePerStep->OutputConfigForListOfFragments(out, &configuration, SortIndex);

  // free and return
  Free(&PermutationMap);
  delete(MoleculePerStep);
  return status;
};

/** Parses nuclear forces from file and performs Verlet integration.
 * Note that we assume the parsed forces to be in atomic units (hence, if coordinates are in angstroem, we
 * have to transform them).
 * This adds a new MD step to the config file.
 * \param *out output stream for debugging
 * \param *file filename
 * \param config structure with config::Deltat, config::IsAngstroem, config::DoConstrained
 * \param delta_t time step width in atomic units
 * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
 * \param DoConstrained whether we perform a constrained (>0, target step in molecule::trajectories) or unconstrained (0) molecular dynamics, \sa molecule::MinimiseConstrainedPotential()
 * \return true - file found and parsed, false - file not found or imparsable
 * \todo This is not yet checked if it is correctly working with DoConstrained set to true.
 */
bool molecule::VerletForceIntegration(ofstream *out, char *file, config &configuration)
{
  ifstream input(file);
  string token;
  stringstream item;
  double IonMass, ConstrainedPotentialEnergy, ActualTemp;
  Vector Velocity;
  ForceMatrix Force;

  CountElements();  // make sure ElementsInMolecule is up to date

  // check file
  if (input == NULL) {
    return false;
  } else {
    // parse file into ForceMatrix
    if (!Force.ParseMatrix(file, 0,0,0)) {
      cerr << "Could not parse Force Matrix file " << file << "." << endl;
      return false;
    }
    if (Force.RowCounter[0] != AtomCount) {
      cerr << "Mismatch between number of atoms in file " << Force.RowCounter[0] << " and in molecule " << AtomCount << "." << endl;
      return false;
    }
    // correct Forces
    Velocity.Zero();
    for(int i=0;i<AtomCount;i++)
      for(int d=0;d<NDIM;d++) {
        Velocity.x[d] += Force.Matrix[0][i][d+5];
      }
    for(int i=0;i<AtomCount;i++)
      for(int d=0;d<NDIM;d++) {
        Force.Matrix[0][i][d+5] -= Velocity.x[d]/(double)AtomCount;
      }
    // solve a constrained potential if we are meant to
    if (configuration.DoConstrainedMD) {
      // calculate forces and potential
      atom **PermutationMap = NULL;
      ConstrainedPotentialEnergy = MinimiseConstrainedPotential(out, PermutationMap,configuration.DoConstrainedMD, 0, configuration.GetIsAngstroem());
      EvaluateConstrainedForces(out, configuration.DoConstrainedMD, 0, PermutationMap, &Force);
      Free(&PermutationMap);
    }

    // and perform Verlet integration for each atom with position, velocity and force vector
    // check size of vectors
    ActOnAllAtoms( &atom::ResizeTrajectory, MDSteps+10 );

    ActOnAllAtoms( &atom::VelocityVerletUpdate, MDSteps, &configuration, &Force);
  }
  // correct velocities (rather momenta) so that center of mass remains motionless
  Velocity.Zero();
  IonMass = 0.;
  ActOnAllAtoms ( &atom::SumUpKineticEnergy, MDSteps, &IonMass, &Velocity );

  // correct velocities (rather momenta) so that center of mass remains motionless
  Velocity.Scale(1./IonMass);
  ActualTemp = 0.;
  ActOnAllAtoms ( &atom::CorrectVelocity, &ActualTemp, MDSteps, &Velocity );
  Thermostats(configuration, ActualTemp, Berendsen);
  MDSteps++;

  // exit
  return true;
};

/** Implementation of various thermostats.
 * All these thermostats apply an additional force which has the following forms:
 * -# Woodcock
 *  \f$p_i \rightarrow \sqrt{\frac{T_0}{T}} \cdot p_i\f$
 * -# Gaussian
 *  \f$ \frac{ \sum_i \frac{p_i}{m_i} \frac{\partial V}{\partial q_i}} {\sum_i \frac{p^2_i}{m_i}} \cdot p_i\f$
 * -# Langevin
 *  \f$p_{i,n} \rightarrow \sqrt{1-\alpha^2} p_{i,0} + \alpha p_r\f$
 * -# Berendsen
 *  \f$p_i \rightarrow \left [ 1+ \frac{\delta t}{\tau_T} \left ( \frac{T_0}{T} \right ) \right ]^{\frac{1}{2}} \cdot p_i\f$
 * -# Nose-Hoover
 *  \f$\zeta p_i \f$ with \f$\frac{\partial \zeta}{\partial t} = \frac{1}{M_s} \left ( \sum^N_{i=1} \frac{p_i^2}{m_i} - g k_B T \right )\f$
 * These Thermostats either simply rescale the velocities, thus this function should be called after ion velocities have been updated, and/or
 * have a constraint force acting additionally on the ions. In the latter case, the ion speeds have to be modified
 * belatedly and the constraint force set.
 * \param *P Problem at hand
 * \param i which of the thermostats to take: 0 - none, 1 - Woodcock, 2 - Gaussian, 3 - Langevin, 4 - Berendsen, 5 - Nose-Hoover
 * \sa InitThermostat()
 */
void molecule::Thermostats(config &configuration, double ActualTemp, int Thermostat)
{
  double ekin = 0.;
  double E = 0., G = 0.;
  double delta_alpha = 0.;
  double ScaleTempFactor;
  gsl_rng * r;
  const gsl_rng_type * T;

  // calculate scale configuration
  ScaleTempFactor = configuration.TargetTemp/ActualTemp;

  // differentating between the various thermostats
  switch(Thermostat) {
     case None:
      cout << Verbose(2) <<  "Applying no thermostat..." << endl;
      break;
     case Woodcock:
      if ((configuration.ScaleTempStep > 0) && ((MDSteps-1) % configuration.ScaleTempStep == 0)) {
        cout << Verbose(2) <<  "Applying Woodcock thermostat..." << endl;
        ActOnAllAtoms( &atom::Thermostat_Woodcock, sqrt(ScaleTempFactor), MDSteps, &ekin );
      }
      break;
     case Gaussian:
      cout << Verbose(2) <<  "Applying Gaussian thermostat..." << endl;
      ActOnAllAtoms( &atom::Thermostat_Gaussian_init, MDSteps, &G, &E );

      cout << Verbose(1) << "Gaussian Least Constraint constant is " << G/E << "." << endl;
      ActOnAllAtoms( &atom::Thermostat_Gaussian_least_constraint, MDSteps, G/E, &ekin, &configuration);

      break;
     case Langevin:
      cout << Verbose(2) <<  "Applying Langevin thermostat..." << endl;
      // init random number generator
      gsl_rng_env_setup();
      T = gsl_rng_default;
      r = gsl_rng_alloc (T);
      // Go through each ion
      ActOnAllAtoms( &atom::Thermostat_Langevin, MDSteps, r, &ekin, &configuration );
      break;

     case Berendsen:
      cout << Verbose(2) <<  "Applying Berendsen-VanGunsteren thermostat..." << endl;
      ActOnAllAtoms( &atom::Thermostat_Berendsen, MDSteps, ScaleTempFactor, &ekin, &configuration );
      break;

     case NoseHoover:
      cout << Verbose(2) <<  "Applying Nose-Hoover thermostat..." << endl;
      // dynamically evolve alpha (the additional degree of freedom)
      delta_alpha = 0.;
      ActOnAllAtoms( &atom::Thermostat_NoseHoover_init, MDSteps, &delta_alpha );
      delta_alpha = (delta_alpha - (3.*AtomCount+1.) * configuration.TargetTemp)/(configuration.HooverMass*Units2Electronmass);
      configuration.alpha += delta_alpha*configuration.Deltat;
      cout << Verbose(3) << "alpha = " << delta_alpha << " * " << configuration.Deltat << " = " << configuration.alpha << "." << endl;
      // apply updated alpha as additional force
      ActOnAllAtoms( &atom::Thermostat_NoseHoover_scale, MDSteps, &ekin, &configuration );
      break;
  }
  cout << Verbose(1) << "Kinetic energy is " << ekin << "." << endl;
};