/*
 * 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 *********************************/


/** 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 *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)
 * \param *constants constant in front of each term
 * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
 * \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, atom **PermutationMap, int startstep, int endstep, double *constants, bool IsAngstroem)
{
  double result = 0., tmp, Norm1, Norm2;
  atom *Walker = NULL, *Runner = NULL, *Sprinter = NULL;
  Vector trajectory1, trajectory2, normal, TestVector;
  gsl_matrix *A = gsl_matrix_alloc(NDIM,NDIM);
  gsl_vector *x = gsl_vector_alloc(NDIM);

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

    // second term: sum of distances to other trajectories
    Runner = start;
    while (Runner->next != 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 = PermutationMap[Walker->nr];   // find first target point
      trajectory1.CopyVector(&Sprinter->Trajectory.R.at(endstep));
      trajectory1.SubtractVector(&Walker->Trajectory.R.at(startstep));
      trajectory1.Normalize();
      Norm1 = trajectory1.Norm();
      Sprinter = PermutationMap[Runner->nr];   // find second target point
      trajectory2.CopyVector(&Sprinter->Trajectory.R.at(endstep));
      trajectory2.SubtractVector(&Runner->Trajectory.R.at(startstep));
      trajectory2.Normalize();
      Norm2 = trajectory1.Norm();
      // check whether either is zero()
      if ((Norm1 < MYEPSILON) && (Norm2 < MYEPSILON)) {
        tmp = Walker->Trajectory.R.at(startstep).Distance(&Runner->Trajectory.R.at(startstep));
      } else if (Norm1 < MYEPSILON) {
        Sprinter = PermutationMap[Walker->nr];   // find first target point
        trajectory1.CopyVector(&Sprinter->Trajectory.R.at(endstep));  // copy first offset
        trajectory1.SubtractVector(&Runner->Trajectory.R.at(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 = PermutationMap[Runner->nr];   // find second target point
        trajectory2.CopyVector(&Sprinter->Trajectory.R.at(endstep));  // copy second offset
        trajectory2.SubtractVector(&Walker->Trajectory.R.at(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(startstep).Distance(&Runner->Trajectory.R.at(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(startstep).x[i] - Runner->Trajectory.R.at(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(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(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 *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
      if (fabs(tmp) > MYEPSILON) {
        result += constants[1] * 1./tmp;
        //*out << Verbose(4) << "Adding " << 1./tmp*constants[1] << "." << endl;
      }
    }

    // third term: penalty for equal targets
    Runner = start;
    while (Runner->next != end) {
      Runner = Runner->next;
      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 << Sprinter->Trajectory.R.at(endstep);
//        *out << ", penalting." << endl;
        result += constants[2];
        //*out << Verbose(4) << "Adding " << constants[2] << "." << endl;
      }
    }
  }

  return result;
};

void PrintPermutationMap(ofstream *out, atom **PermutationMap, int Nr)
{
  stringstream zeile1, zeile2;
  int *DoubleList = Malloc<int>(Nr, "PrintPermutationMap: *DoubleList");
  int doubles = 0;
  for (int i=0;i<Nr;i++)
    DoubleList[i] = 0;
  zeile1 << "PermutationMap: ";
  zeile2 << "                ";
  for (int i=0;i<Nr;i++) {
    DoubleList[PermutationMap[i]->nr]++;
    zeile1 << i << " ";
    zeile2 << PermutationMap[i]->nr << " ";
  }
  for (int i=0;i<Nr;i++)
    if (DoubleList[i] > 1)
    doubles++;
 // *out << "Found " << doubles << " Doubles." << endl;
  Free(&DoubleList);
//  *out << zeile1.str() << endl << zeile2.str() << 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;
  PermutationMap = Malloc<atom*>(AtomCount, "molecule::MinimiseConstrainedPotential: **PermutationMap");
  DistanceMap **DistanceList = Malloc<DistanceMap*>(AtomCount, "molecule::MinimiseConstrainedPotential: **DistanceList");
  DistanceMap::iterator *DistanceIterators = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: *DistanceIterators");
  int *DoubleList = Malloc<int>(AtomCount, "molecule::MinimiseConstrainedPotential: *DoubleList");
  DistanceMap::iterator *StepList = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: *StepList");
  double constants[3];
  int round;
  atom *Walker = NULL, *Runner = NULL, *Sprinter = NULL;
  DistanceMap::iterator Rider, Strider;

  /// Minimise the potential
  // set Lagrange multiplier constants
  constants[0] = 10.;
  constants[1] = 1.;
  constants[2] = 1e+7;    // just a huge penalty
  // generate the distance list
  *out << Verbose(1) << "Creating the distance list ... " << endl;
  for (int i=AtomCount; i--;) {
    DoubleList[i] = 0;  // stores for how many atoms in startstep this atom is a target in endstep
    DistanceList[i] = new DistanceMap;    // is the distance sorted target list per atom
    DistanceList[i]->clear();
  }
  *out << Verbose(1) << "Filling the distance list ... " << endl;
  Walker = start;
  while (Walker->next != end) {
    Walker = Walker->next;
    Runner = start;
    while(Runner->next != end) {
      Runner = Runner->next;
      DistanceList[Walker->nr]->insert( DistancePair(Walker->Trajectory.R.at(startstep).Distance(&Runner->Trajectory.R.at(endstep)), Runner) );
    }
  }
  // create the initial PermutationMap (source -> target)
  Walker = start;
  while (Walker->next != end) {
    Walker = Walker->next;
    StepList[Walker->nr] = DistanceList[Walker->nr]->begin();    // stores the step to the next iterator that could be a possible next target
    PermutationMap[Walker->nr] = DistanceList[Walker->nr]->begin()->second;   // always pick target with the smallest distance
    DoubleList[DistanceList[Walker->nr]->begin()->second->nr]++;            // increase this target's source count (>1? not injective)
    DistanceIterators[Walker->nr] = DistanceList[Walker->nr]->begin();    // and remember which one we picked
    *out << *Walker << " starts with distance " << DistanceList[Walker->nr]->begin()->first << "." << endl;
  }
  *out << Verbose(1) << "done." << endl;
  // 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;
  Walker = start;
  DistanceMap::iterator NewBase;
  OldPotential = fabs(ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem));
  while ((OldPotential) > constants[2]) {
    PrintPermutationMap(out, PermutationMap, AtomCount);
    Walker = Walker->next;
    if (Walker == end) // round-robin at the end
      Walker = start->next;
    if (DoubleList[DistanceIterators[Walker->nr]->second->nr] <= 1)  // no need to make those injective that aren't
      continue;
    // now, try finding a new one
    NewBase = 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 ((DoubleList[NewBase->second->nr] != 0) && (NewBase != DistanceList[Walker->nr]->end()));
    if (NewBase != DistanceList[Walker->nr]->end()) {
      PermutationMap[Walker->nr] = NewBase->second;
      Potential = fabs(ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem));
      if (Potential > OldPotential) { // undo
        PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
      } else {  // do
        DoubleList[DistanceIterators[Walker->nr]->second->nr]--;  // decrease the old entry in the doubles list
        DoubleList[NewBase->second->nr]++;    // increase the old entry in the doubles list
        DistanceIterators[Walker->nr] = NewBase;
        OldPotential = Potential;
        *out << Verbose(3) << "Found a new permutation, new potential is " << OldPotential << "." << endl;
      }
    }
  }
  for (int i=AtomCount; i--;) // now each single entry in the DoubleList should be <=1
    if (DoubleList[i] > 1) {
      cerr << "Failed to create an injective PermutationMap!" << endl;
      exit(1);
    }
  *out << Verbose(1) << "done." << endl;
  Free(&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, PermutationMap, AtomCount);
        Sprinter = DistanceIterators[Walker->nr]->second;   // store initial partner
        Strider = DistanceIterators[Walker->nr];  //remember old iterator
        DistanceIterators[Walker->nr] = StepList[Walker->nr];
        if (DistanceIterators[Walker->nr] == DistanceList[Walker->nr]->end()) {// stop, before we run through the list and still on
          DistanceIterators[Walker->nr] == 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 (PermutationMap[Runner->nr] == 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 = DistanceList[Runner->nr]->begin();
          for (; Rider != DistanceList[Runner->nr]->end(); Rider++)
            if (Rider->second == Sprinter)
              break;
          if (Rider != 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
            PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second; // put next farther distance into PermutationMap
            PermutationMap[Runner->nr] = Sprinter;  // and hand the old target to its respective owner
            PrintPermutationMap(out, PermutationMap, AtomCount);
            // calculate the new potential
            //*out << Verbose(2) << "Checking new potential ..." << endl;
            Potential = ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem);
            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 " << *DistanceIterators[Runner->nr]->second << "." << endl;
              // Undo for Runner (note, we haven't moved the iteration yet, we may use this)
              PermutationMap[Runner->nr] = DistanceIterators[Runner->nr]->second;
              // Undo for Walker
              DistanceIterators[Walker->nr] = Strider;  // take next farther distance target
              //*out << Verbose(3) << "Setting " << *Walker << "'s source to " << *DistanceIterators[Walker->nr]->second << "." << endl;
              PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
            } else {
              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 > constants[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 {
          PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second; // new target has no source!
        }
        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--;)
    DistanceList[i]->clear();
  Free(&DistanceList);
  Free(&DistanceIterators);
  return ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem);
};

/** 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)
{
  double constant = 10.;
  atom *Walker = NULL, *Sprinter = NULL;

  /// 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.*constant*sqrt(Walker->Trajectory.R.at(startstep).Distance(&Sprinter->Trajectory.R.at(endstep)));
  }
  *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");
    Walker = start;
    while (Walker->next != end) {
      Walker = Walker->next;
      PermutationMap[Walker->nr] = Walker;   // always pick target with the smallest distance
    }
  }

  // check whether we have sufficient space in Trajectories for each atom
  Walker = start;
  while (Walker->next != end) {
    Walker = Walker->next;
    if (Walker->Trajectory.R.size() <= (unsigned int)(MaxSteps)) {
      //cout << "Increasing size for trajectory array of " << keyword << " to " << (MaxSteps+1) << "." << endl;
      Walker->Trajectory.R.resize(MaxSteps+1);
      Walker->Trajectory.U.resize(MaxSteps+1);
      Walker->Trajectory.F.resize(MaxSteps+1);
    }
  }
  // push endstep to last one
  Walker = start;
  while (Walker->next != end) { // remove the endstep (is now the very last one)
    Walker = Walker->next;
    for (int n=NDIM;n--;) {
      Walker->Trajectory.R.at(MaxSteps).x[n] = Walker->Trajectory.R.at(endstep).x[n];
      Walker->Trajectory.U.at(MaxSteps).x[n] = Walker->Trajectory.U.at(endstep).x[n];
      Walker->Trajectory.F.at(MaxSteps).x[n] = Walker->Trajectory.F.at(endstep).x[n];
    }
  }
  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)
{
  atom *walker = NULL;
  ifstream input(file);
  string token;
  stringstream item;
  double IonMass, Vector[NDIM], ConstrainedPotentialEnergy, ActualTemp;
  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
    for(int d=0;d<NDIM;d++)
      Vector[d] = 0.;
    for(int i=0;i<AtomCount;i++)
      for(int d=0;d<NDIM;d++) {
        Vector[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] -= Vector[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
    walker = start;
    while (walker->next != end) { // go through every atom of this element
      walker = walker->next;
      //a = configuration.Deltat*0.5/walker->type->mass;        // (F+F_old)/2m = a and thus: v = (F+F_old)/2m * t = (F + F_old) * a
      // check size of vectors
      if (walker->Trajectory.R.size() <= (unsigned int)(MDSteps)) {
        //out << "Increasing size for trajectory array of " << *walker << " to " << (size+10) << "." << endl;
        walker->Trajectory.R.resize(MDSteps+10);
        walker->Trajectory.U.resize(MDSteps+10);
        walker->Trajectory.F.resize(MDSteps+10);
      }

      // Update R (and F)
      for (int d=0; d<NDIM; d++) {
        walker->Trajectory.F.at(MDSteps).x[d] = -Force.Matrix[0][walker->nr][d+5]*(configuration.GetIsAngstroem() ? AtomicLengthToAngstroem : 1.);
        walker->Trajectory.R.at(MDSteps).x[d] = walker->Trajectory.R.at(MDSteps-1).x[d];
        walker->Trajectory.R.at(MDSteps).x[d] += configuration.Deltat*(walker->Trajectory.U.at(MDSteps-1).x[d]);     // s(t) = s(0) + v * deltat + 1/2 a * deltat^2
        walker->Trajectory.R.at(MDSteps).x[d] += 0.5*configuration.Deltat*configuration.Deltat*(walker->Trajectory.F.at(MDSteps).x[d]/walker->type->mass);     // F = m * a and s = 0.5 * F/m * t^2 = F * a * t
      }
      // Update U
      for (int d=0; d<NDIM; d++) {
        walker->Trajectory.U.at(MDSteps).x[d] = walker->Trajectory.U.at(MDSteps-1).x[d];
        walker->Trajectory.U.at(MDSteps).x[d] += configuration.Deltat * (walker->Trajectory.F.at(MDSteps).x[d]+walker->Trajectory.F.at(MDSteps-1).x[d]/walker->type->mass); // v = F/m * t
      }
//      out << "Integrated position&velocity of step " << (MDSteps) << ": (";
//      for (int d=0;d<NDIM;d++)
//        out << walker->Trajectory.R.at(MDSteps).x[d] << " ";          // next step
//      out << ")\t(";
//      for (int d=0;d<NDIM;d++)
//        cout << walker->Trajectory.U.at(MDSteps).x[d] << " ";          // next step
//      out << ")" << endl;
            // next atom
    }
  }
  // correct velocities (rather momenta) so that center of mass remains motionless
  for(int d=0;d<NDIM;d++)
    Vector[d] = 0.;
  IonMass = 0.;
  walker = start;
  while (walker->next != end) { // go through every atom
    walker = walker->next;
    IonMass += walker->type->mass;  // sum up total mass
    for(int d=0;d<NDIM;d++) {
      Vector[d] += walker->Trajectory.U.at(MDSteps).x[d]*walker->type->mass;
    }
  }
  // correct velocities (rather momenta) so that center of mass remains motionless
  for(int d=0;d<NDIM;d++)
    Vector[d] /= IonMass;
  ActualTemp = 0.;
  walker = start;
  while (walker->next != end) { // go through every atom of this element
    walker = walker->next;
    for(int d=0;d<NDIM;d++) {
      walker->Trajectory.U.at(MDSteps).x[d] -= Vector[d];
      ActualTemp += 0.5 * walker->type->mass * walker->Trajectory.U.at(MDSteps).x[d] * walker->Trajectory.U.at(MDSteps).x[d];
    }
  }
  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;
  double sigma;
  double IonMass;
  int d;
  gsl_rng * r;
  const gsl_rng_type * T;
  double *U = NULL, *F = NULL, FConstraint[NDIM];
  atom *walker = NULL;

  // 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;
        walker = start;
        while (walker->next != end) { // go through every atom of this element
          walker = walker->next;
          IonMass = walker->type->mass;
          U = walker->Trajectory.U.at(MDSteps).x;
          if (walker->FixedIon == 0) // even FixedIon moves, only not by other's forces
            for (d=0; d<NDIM; d++) {
              U[d] *= sqrt(ScaleTempFactor);
              ekin += 0.5*IonMass * U[d]*U[d];
            }
        }
      }
      break;
     case Gaussian:
      cout << Verbose(2) <<  "Applying Gaussian thermostat..." << endl;
      walker = start;
      while (walker->next != end) { // go through every atom of this element
        walker = walker->next;
        IonMass = walker->type->mass;
        U = walker->Trajectory.U.at(MDSteps).x;
        F = walker->Trajectory.F.at(MDSteps).x;
        if (walker->FixedIon == 0) // even FixedIon moves, only not by other's forces
          for (d=0; d<NDIM; d++) {
            G += U[d] * F[d];
            E += U[d]*U[d]*IonMass;
          }
      }
      cout << Verbose(1) << "Gaussian Least Constraint constant is " << G/E << "." << endl;
      walker = start;
      while (walker->next != end) { // go through every atom of this element
        walker = walker->next;
        IonMass = walker->type->mass;
        U = walker->Trajectory.U.at(MDSteps).x;
        F = walker->Trajectory.F.at(MDSteps).x;
        if (walker->FixedIon == 0) // even FixedIon moves, only not by other's forces
          for (d=0; d<NDIM; d++) {
            FConstraint[d] = (G/E) * (U[d]*IonMass);
            U[d] += configuration.Deltat/IonMass * (FConstraint[d]);
            ekin += IonMass * U[d]*U[d];
          }
      }
      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
      walker = start;
      while (walker->next != end) { // go through every atom of this element
        walker = walker->next;
        IonMass = walker->type->mass;
        sigma  = sqrt(configuration.TargetTemp/IonMass); // sigma = (k_b T)/m (Hartree/atomicmass = atomiclength/atomictime)
        U = walker->Trajectory.U.at(MDSteps).x;
        F = walker->Trajectory.F.at(MDSteps).x;
        if (walker->FixedIon == 0) { // even FixedIon moves, only not by other's forces
          // throw a dice to determine whether it gets hit by a heat bath particle
          if (((((rand()/(double)RAND_MAX))*configuration.TempFrequency) < 1.)) {
            cout << Verbose(3) << "Particle " << *walker << " was hit (sigma " << sigma << "): " << sqrt(U[0]*U[0]+U[1]*U[1]+U[2]*U[2]) << " -> ";
            // pick three random numbers from a Boltzmann distribution around the desired temperature T for each momenta axis
            for (d=0; d<NDIM; d++) {
              U[d] = gsl_ran_gaussian (r, sigma);
            }
            cout << sqrt(U[0]*U[0]+U[1]*U[1]+U[2]*U[2]) << endl;
          }
          for (d=0; d<NDIM; d++)
            ekin += 0.5*IonMass * U[d]*U[d];
        }
      }
      break;
     case Berendsen:
      cout << Verbose(2) <<  "Applying Berendsen-VanGunsteren thermostat..." << endl;
      walker = start;
      while (walker->next != end) { // go through every atom of this element
        walker = walker->next;
        IonMass = walker->type->mass;
        U = walker->Trajectory.U.at(MDSteps).x;
        F = walker->Trajectory.F.at(MDSteps).x;
        if (walker->FixedIon == 0) { // even FixedIon moves, only not by other's forces
          for (d=0; d<NDIM; d++) {
            U[d] *= sqrt(1+(configuration.Deltat/configuration.TempFrequency)*(ScaleTempFactor-1));
            ekin += 0.5*IonMass * U[d]*U[d];
          }
        }
      }
      break;
     case NoseHoover:
      cout << Verbose(2) <<  "Applying Nose-Hoover thermostat..." << endl;
      // dynamically evolve alpha (the additional degree of freedom)
      delta_alpha = 0.;
      walker = start;
      while (walker->next != end) { // go through every atom of this element
        walker = walker->next;
        IonMass = walker->type->mass;
        U = walker->Trajectory.U.at(MDSteps).x;
        if (walker->FixedIon == 0) { // even FixedIon moves, only not by other's forces
          for (d=0; d<NDIM; d++) {
            delta_alpha += U[d]*U[d]*IonMass;
          }
        }
      }
      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
      walker = start;
      while (walker->next != end) { // go through every atom of this element
        walker = walker->next;
        IonMass = walker->type->mass;
        U = walker->Trajectory.U.at(MDSteps).x;
        if (walker->FixedIon == 0) { // even FixedIon moves, only not by other's forces
          for (d=0; d<NDIM; d++) {
              FConstraint[d] = - configuration.alpha * (U[d] * IonMass);
              U[d] += configuration.Deltat/IonMass * (FConstraint[d]);
              ekin += (0.5*IonMass) * U[d]*U[d];
            }
        }
      }
      break;
  }
  cout << Verbose(1) << "Kinetic energy is " << ekin << "." << endl;
};