/*
 * LinearInterpolationBetweenSteps.hpp
 *
 *  Created on: Feb 23, 2011
 *      Author: heber
 */

#ifndef LINEARINTERPOLATIONBETWEENSTEPS_HPP_
#define LINEARINTERPOLATIONBETWEENSTEPS_HPP_

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <vector>

#include "Atom/atom.hpp"
#include "Atom/AtomSet.hpp"
#include "CodePatterns/Assert.hpp"
#include "CodePatterns/Info.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/toString.hpp"
#include "CodePatterns/Verbose.hpp"
#include "Dynamics/MinimiseConstrainedPotential.hpp"
#include "molecule.hpp"
#include "World.hpp"

template <class Set>
class LinearInterpolationBetweenSteps
{
public:
  LinearInterpolationBetweenSteps(AtomSetMixin<Set> &_atoms, unsigned int _MaxOuterStep) :
    MaxOuterStep(_MaxOuterStep),
    IsAngstroem(true),
    atoms(_atoms)
  {}
  ~LinearInterpolationBetweenSteps()
  {}

  /** 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 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()
   */
  void operator()(int startstep, int endstep, bool MapByIdentity)
  {
    // TODO: rewrite permutationMaps using enumeration objects
    int MaxSteps = MaxOuterStep;

    // Get the Permutation Map by MinimiseConstrainedPotential
    std::map<atom*, atom *> PermutationMap;
    if (!MapByIdentity) {
      LOG(1, "STATUS: Constructing atom mapping from start to end position.");
      MinimiseConstrainedPotential Minimiser(atoms, PermutationMap);
      Minimiser(startstep, endstep, IsAngstroem);
    } else {
      LOG(1, "STATUS: Using identity mapping.");
      // TODO: construction of enumeration goes here
      for(typename AtomSetMixin<Set>::const_iterator iter = atoms.begin(); iter != atoms.end();++iter){
        PermutationMap[(*iter)] = (*iter);
      }
    }

    // check whether we have sufficient space in Trajectories for each atom
    LOG(1, "STATUS: Extending trajectories.");
    for(typename AtomSetMixin<Set>::iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
      (*iter)->ResizeTrajectory(MaxSteps);
    }
    // push endstep to last one
    for(typename AtomSetMixin<Set>::iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
      (*iter)->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
    LOG(1, "STATUS: Filling intermediate " << MaxSteps << " steps." << endl);
    for (int step = 0; step <= MaxSteps; step++) {
      LOG(2, "INFO: Current step is " << step << "." << endl);
      for (typename AtomSetMixin<Set>::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
        // add to Trajectories
        ASSERT(PermutationMap.count((*iter)),
            "LinearInterpolationBetweenSteps::operator() - atom "+toString(**iter)+" not found in PermutationMap.");
        Vector temp = (*iter)->getPositionAtStep(startstep) + (PermutationMap[(*iter)]->getPositionAtStep(endstep) - (*iter)->getPositionAtStep(startstep))*((double)step/(double)MaxSteps);
        (*iter)->setPositionAtStep(step,temp);
        (*iter)->setAtomicVelocityAtStep(step, zeroVec);
        (*iter)->setAtomicForceAtStep(step, zeroVec);
        LOG(2, "INFO: Adding trajectory point for atom " << **iter << " at " << temp << ".");
      }
    }
  };

private:
  unsigned int MaxOuterStep;
  bool IsAngstroem;
  AtomSetMixin<Set> atoms;

};

#endif /* LINEARINTERPOLATIONBETWEENSTEPS_HPP_ */