/*
 * atom_trajectoryparticle.cpp
 *
 *  Created on: Oct 19, 2009
 *      Author: heber
 */

#include "Helpers/MemDebug.hpp"

#include "atom.hpp"
#include "atom_trajectoryparticle.hpp"
#include "config.hpp"
#include "element.hpp"
#include "info.hpp"
#include "log.hpp"
#include "parser.hpp"
#include "ThermoStatContainer.hpp"
#include "verbose.hpp"

/** Constructor of class TrajectoryParticle.
 */
TrajectoryParticle::TrajectoryParticle()
{
};

/** Destructor of class TrajectoryParticle.
 */
TrajectoryParticle::~TrajectoryParticle()
{
};


/** Adds kinetic energy of this atom to given temperature value.
 * \param *temperature add on this value
 * \param step given step of trajectory to add
 */
void TrajectoryParticle::AddKineticToTemperature(double *temperature, int step) const
{
  for (int i=NDIM;i--;)
    *temperature += type->mass * Trajectory.U.at(step)[i]* Trajectory.U.at(step)[i];
};

/** Evaluates some constraint potential if atom moves from \a startstep at once to \endstep in trajectory.
 * \param startstep trajectory begins at
 * \param endstep trajectory ends at
 * \param **PermutationMap if atom switches places with some other atom, there is no translation but a permutaton noted here (not in the trajectories of ea
 * \param *Force Force matrix to store result in
 */
void TrajectoryParticle::EvaluateConstrainedForce(int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force) const
{
  double constant = 10.;
  TrajectoryParticle *Sprinter = PermutationMap[nr];
  // set forces
  for (int i=NDIM;i++;)
    Force->Matrix[0][nr][5+i] += 2.*constant*sqrt(Trajectory.R.at(startstep).distance(Sprinter->Trajectory.R.at(endstep)));
};

/** Correct velocity against the summed \a CoGVelocity for \a step.
 * \param *ActualTemp sum up actual temperature meanwhile
 * \param Step MD step in atom::Tracjetory
 * \param *CoGVelocity remnant velocity (i.e. vector sum of all atom velocities)
 */
void TrajectoryParticle::CorrectVelocity(double *ActualTemp, int Step, Vector *CoGVelocity)
{
  for(int d=0;d<NDIM;d++) {
    Trajectory.U.at(Step)[d] -= CoGVelocity->at(d);
    *ActualTemp += 0.5 * type->mass * Trajectory.U.at(Step)[d] * Trajectory.U.at(Step)[d];
  }
};

/** Extends the trajectory STL vector to the new size.
 * Does nothing if \a MaxSteps is smaller than current size.
 * \param MaxSteps
 */
void TrajectoryParticle::ResizeTrajectory(int MaxSteps)
{
  Info FunctionInfo(__func__);
  if (Trajectory.R.size() <= (unsigned int)(MaxSteps)) {
    DoLog(0) && (Log() << Verbose(0) << "Increasing size for trajectory array of " << nr << " from " << Trajectory.R.size() << " to " << (MaxSteps+1) << "." << endl);
    Trajectory.R.resize(MaxSteps+1);
    Trajectory.U.resize(MaxSteps+1);
    Trajectory.F.resize(MaxSteps+1);
  }
};

/** Copies a given trajectory step \a src onto another \a dest
 * \param dest index of destination step
 * \param src index of source step
 */
void TrajectoryParticle::CopyStepOnStep(int dest, int src)
{
  if (dest == src)  // self assignment check
    return;

  for (int n=NDIM;n--;) {
    Trajectory.R.at(dest)[n] = Trajectory.R.at(src)[n];
    Trajectory.U.at(dest)[n] = Trajectory.U.at(src)[n];
    Trajectory.F.at(dest)[n] = Trajectory.F.at(src)[n];
  }
};

/** Performs a velocity verlet update of the trajectory.
 * Parameters are according to those in configuration class.
 * \param NextStep index of sequential step to set
 * \param *configuration pointer to configuration with parameters
 * \param *Force matrix with forces
 */
void TrajectoryParticle::VelocityVerletUpdate(int NextStep, config *configuration, ForceMatrix *Force, const size_t offset)
{
  //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
  for (int d=0; d<NDIM; d++) {
    Trajectory.F.at(NextStep)[d] = -Force->Matrix[0][nr][d+offset]*(configuration->GetIsAngstroem() ? AtomicLengthToAngstroem : 1.);
    Trajectory.R.at(NextStep)[d] = Trajectory.R.at(NextStep-1)[d];
    Trajectory.R.at(NextStep)[d] += configuration->Deltat*(Trajectory.U.at(NextStep-1)[d]);     // s(t) = s(0) + v * deltat + 1/2 a * deltat^2
    Trajectory.R.at(NextStep)[d] += 0.5*configuration->Deltat*configuration->Deltat*(Trajectory.F.at(NextStep)[d]/type->mass);     // F = m * a and s =
  }
  // Update U
  for (int d=0; d<NDIM; d++) {
    Trajectory.U.at(NextStep)[d] = Trajectory.U.at(NextStep-1)[d];
    Trajectory.U.at(NextStep)[d] += configuration->Deltat * (Trajectory.F.at(NextStep)[d]+Trajectory.F.at(NextStep-1)[d]/type->mass); // v = F/m * t
  }
  // Update R (and F)
//      out << "Integrated position&velocity of step " << (NextStep) << ": (";
//      for (int d=0;d<NDIM;d++)
//        out << Trajectory.R.at(NextStep).x[d] << " ";          // next step
//      out << ")\t(";
//      for (int d=0;d<NDIM;d++)
//        Log() << Verbose(0) << Trajectory.U.at(NextStep).x[d] << " ";          // next step
//      out << ")" << endl;
};

/** Sums up mass and kinetics.
 * \param Step step to sum for
 * \param *TotalMass pointer to total mass sum
 * \param *TotalVelocity pointer to tota velocity sum
 */
void TrajectoryParticle::SumUpKineticEnergy( int Step, double *TotalMass, Vector *TotalVelocity ) const
{
  *TotalMass += type->mass;  // sum up total mass
  for(int d=0;d<NDIM;d++) {
    TotalVelocity->at(d) += Trajectory.U.at(Step)[d]*type->mass;
  }
};

/** Scales velocity of atom according to Woodcock thermostat.
 * \param ScaleTempFactor factor to scale the velocities with (i.e. sqrt of energy scale factor)
 * \param Step MD step to scale
 * \param *ekin sum of kinetic energy
 */
void TrajectoryParticle::Thermostat_Woodcock(double ScaleTempFactor, int Step, double *ekin)
{
  Vector &U = Trajectory.U.at(Step);
  if (FixedIon == 0) // even FixedIon moves, only not by other's forces
    for (int d=0; d<NDIM; d++) {
      U[d] *= ScaleTempFactor;
      *ekin += 0.5*type->mass * U[d]*U[d];
    }
};

/** Scales velocity of atom according to Gaussian thermostat.
 * \param Step MD step to scale
 * \param *G
 * \param *E
 */
void TrajectoryParticle::Thermostat_Gaussian_init(int Step, double *G, double *E)
{
  Vector &U = Trajectory.U.at(Step);
  Vector &F = Trajectory.F.at(Step);
  if (FixedIon == 0) // even FixedIon moves, only not by other's forces
    for (int d=0; d<NDIM; d++) {
      *G += U[d] * F[d];
      *E += U[d]*U[d]*type->mass;
    }
};

/** Determines scale factors according to Gaussian thermostat.
 * \param Step MD step to scale
 * \param GE G over E ratio
 * \param *ekin sum of kinetic energy
 * \param *configuration configuration class with TempFrequency and TargetTemp
 */
void TrajectoryParticle::Thermostat_Gaussian_least_constraint(int Step, double G_over_E, double *ekin, config *configuration)
{
  Vector &U = Trajectory.U.at(Step);
  if (FixedIon == 0) // even FixedIon moves, only not by other's forces
    for (int d=0; d<NDIM; d++) {
      U[d] += configuration->Deltat/type->mass * ( (G_over_E) * (U[d]*type->mass) );
      *ekin += type->mass * U[d]*U[d];
    }
};

/** Scales velocity of atom according to Langevin thermostat.
 * \param Step MD step to scale
 * \param *r random number generator
 * \param *ekin sum of kinetic energy
 * \param *configuration configuration class with TempFrequency and TargetTemp
 */
void TrajectoryParticle::Thermostat_Langevin(int Step, gsl_rng * r, double *ekin, config *configuration)
{
  double sigma  = sqrt(configuration->Thermostats->TargetTemp/type->mass); // sigma = (k_b T)/m (Hartree/atomicmass = atomiclength/atomictime)
  Vector &U = Trajectory.U.at(Step);
  if (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->Thermostats->TempFrequency) < 1.)) {
      DoLog(3) && (Log() << Verbose(3) << "Particle " << *this << " 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 (int d=0; d<NDIM; d++) {
        U[d] = gsl_ran_gaussian (r, sigma);
      }
      DoLog(2) && (Log() << Verbose(2) << sqrt(U[0]*U[0]+U[1]*U[1]+U[2]*U[2]) << endl);
    }
    for (int d=0; d<NDIM; d++)
      *ekin += 0.5*type->mass * U[d]*U[d];
  }
};

/** Scales velocity of atom according to Berendsen thermostat.
 * \param Step MD step to scale
 * \param ScaleTempFactor factor to scale energy (not velocity!) with
 * \param *ekin sum of kinetic energy
 * \param *configuration configuration class with TempFrequency and Deltat
 */
void TrajectoryParticle::Thermostat_Berendsen(int Step, double ScaleTempFactor, double *ekin, config *configuration)
{
  Vector &U = Trajectory.U.at(Step);
  if (FixedIon == 0) { // even FixedIon moves, only not by other's forces
    for (int d=0; d<NDIM; d++) {
      U[d] *= sqrt(1+(configuration->Deltat/configuration->Thermostats->TempFrequency)*(ScaleTempFactor-1));
      *ekin += 0.5*type->mass * U[d]*U[d];
    }
  }
};

/** Initializes current run of NoseHoover thermostat.
 * \param Step MD step to scale
 * \param *delta_alpha additional sum of kinetic energy on return
 */
void TrajectoryParticle::Thermostat_NoseHoover_init(int Step, double *delta_alpha)
{
  Vector &U = Trajectory.U.at(Step);
  if (FixedIon == 0) { // even FixedIon moves, only not by other's forces
    for (int d=0; d<NDIM; d++) {
      *delta_alpha += U[d]*U[d]*type->mass;
    }
  }
};

/** Initializes current run of NoseHoover thermostat.
 * \param Step MD step to scale
 * \param *ekin sum of kinetic energy
 * \param *configuration configuration class with TempFrequency and Deltat
 */
void TrajectoryParticle::Thermostat_NoseHoover_scale(int Step, double *ekin, config *configuration)
{
  Vector &U = Trajectory.U.at(Step);
  if (FixedIon == 0) { // even FixedIon moves, only not by other's forces
    for (int d=0; d<NDIM; d++) {
        U[d] += configuration->Deltat/type->mass * (configuration->Thermostats->alpha * (U[d] * type->mass));
        *ekin += (0.5*type->mass) * U[d]*U[d];
      }
  }
};