/*
 * ForceAnnealing.hpp
 *
 *  Created on: Aug 02, 2014
 *      Author: heber
 */

#ifndef FORCEANNEALING_HPP_
#define FORCEANNEALING_HPP_

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

#include "Atom/atom.hpp"
#include "Atom/AtomSet.hpp"
#include "CodePatterns/Assert.hpp"
#include "CodePatterns/Info.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "Dynamics/AtomicForceManipulator.hpp"
#include "Fragmentation/ForceMatrix.hpp"
#include "Helpers/helpers.hpp"
#include "Helpers/defs.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "Thermostats/ThermoStatContainer.hpp"
#include "Thermostats/Thermostat.hpp"
#include "World.hpp"

/** This class is the essential build block for performing structual optimization.
 *
 * Sadly, we have to use some static instances as so far values cannot be passed
 * between actions. Hence, we need to store the current step and the adaptive
 * step width (we cannot perform a linesearch, as we have no control over the
 * calculation of the forces).
 */
template <class T>
class ForceAnnealing : public AtomicForceManipulator<T>
{
public:
  /** Constructor of class ForceAnnealing.
   *
   * \param _atoms set of atoms to integrate
   * \param _Deltat time step width in atomic units
   * \param _IsAngstroem whether length units are in angstroem or bohr radii
   * \param _maxSteps number of optimization steps to perform
   */
  ForceAnnealing(
      AtomSetMixin<T> &_atoms,
      double _Deltat,
      bool _IsAngstroem,
      const size_t _maxSteps) :
    AtomicForceManipulator<T>(_atoms, _Deltat, _IsAngstroem),
    maxSteps(_maxSteps)
  {}
  /** Destructor of class ForceAnnealing.
   *
   */
  ~ForceAnnealing()
  {}

  /** Performs Gradient optimization.
   *
   * We assume that forces have just been calculated.
   *
   *
   * \param NextStep current time step (i.e. \f$ t + \Delta t \f$ in the sense of the velocity verlet)
   * \param offset offset in matrix file to the first force component
   * \todo This is not yet checked if it is correctly working with DoConstrainedMD set >0.
   */
  void operator()(const int NextStep, const size_t offset)
  {
    // make sum of forces equal zero
    AtomicForceManipulator<T>::correctForceMatrixForFixedCenterOfMass(offset,NextStep);

    // are we in initial step? Then set static entities
    if (currentStep == 0) {
      currentDeltat = AtomicForceManipulator<T>::Deltat;
      currentStep = 1;
      LOG(2, "DEBUG: Initial step, setting values, current step is #" << currentStep);
    } else {
      ++currentStep;
      LOG(2, "DEBUG: current step is #" << currentStep);
    }

    // are we in initial step? Then don't check against velocity
    Vector maxComponents(zeroVec);
    for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
        iter != AtomicForceManipulator<T>::atoms.end(); ++iter) {
      // atom's force vector gives steepest descent direction
      const Vector currentPosition = (*iter)->getPosition();
      const Vector currentGradient = (*iter)->getAtomicForce();
      LOG(4, "DEBUG: Force for atom " << **iter << " is " << currentGradient);
      Vector PositionUpdate = currentDeltat * currentGradient;
      LOG(3, "DEBUG: Update would be " << PositionUpdate);

      // extract largest components for showing progress of annealing
      for(size_t i=0;i<NDIM;++i)
        if (currentGradient[i] > maxComponents[i])
          maxComponents[i] = currentGradient[i];

      // update with currentDelta tells us how the current gradient relates to
      // the last one: If it has become larger, reduce currentDelta
      if ((currentStep > 1) && (!(*iter)->getAtomicVelocity().IsZero()))
        if ((PositionUpdate.ScalarProduct((*iter)->getAtomicVelocity()) < 0)
            && (currentDeltat > MinimumDeltat)) {
          currentDeltat = .5*currentDeltat;
          LOG(2, "DEBUG: Upgrade in other direction: " << PositionUpdate.NormSquared()
              << " > " << (*iter)->getAtomicVelocity().NormSquared()
              << ", decreasing deltat: " << currentDeltat);
          PositionUpdate = currentDeltat * currentGradient;
        }

      // finally set new values
      (*iter)->setPosition(currentPosition + PositionUpdate);
      (*iter)->setAtomicVelocity(PositionUpdate);
      //std::cout << "Id of atom is " << (*iter)->getId() << std::endl;
//        (*iter)->VelocityVerletUpdateU((*iter)->getId(), NextStep-1, Deltat, IsAngstroem);

      // reset force vector for next step except on final one
      if (currentStep != maxSteps)
        (*iter)->setAtomicForce(zeroVec);
    }

    LOG(1, "STATUS: Largest remaining force components at step #"
        << currentStep << " are " << maxComponents);

    // are we in final step? Remember to reset static entities
    if (currentStep == maxSteps) {
      LOG(2, "DEBUG: Final step, resetting values");
      currentDeltat = 0.;
      currentStep = 0;

      // reset (artifical) velocities
      for(typename AtomSetMixin<T>::iterator iter = AtomicForceManipulator<T>::atoms.begin();
          iter != AtomicForceManipulator<T>::atoms.end(); ++iter)
        (*iter)->setAtomicVelocity(zeroVec);
    }
  }

private:
  //!> contains the current step in relation to maxsteps
  static size_t currentStep;
  //!> contains the maximum number of steps, determines initial and final step with currentStep
  size_t maxSteps;
  static double currentDeltat;
  //!> minimum deltat for internal while loop (adaptive step width)
  static double MinimumDeltat;
};

template <class T>
double ForceAnnealing<T>::currentDeltat = 0.;
template <class T>
size_t ForceAnnealing<T>::currentStep = 0;
template <class T>
double ForceAnnealing<T>::MinimumDeltat = 1e-8;

#endif /* FORCEANNEALING_HPP_ */