/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010 University of Bonn. All rights reserved.
 * Please see the LICENSE file or "Copyright notice" in builder.cpp for details.
 */

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

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

#include "Helpers/MemDebug.hpp"

#include "atom.hpp"
#include "atom_trajectoryparticle.hpp"
#include "config.hpp"
#include "element.hpp"
#include "Helpers/Info.hpp"
#include "Helpers/Log.hpp"
#include "parser.hpp"
#include "ThermoStatContainer.hpp"
#include "Helpers/Verbose.hpp"

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

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

/**
 *  returns the kinetic energy of this atom at a given time step
 */
double TrajectoryParticle::getKineticEnergy(unsigned int step) const{
  return getType()->getMass() * Trajectory.U.at(step).NormSquared();
}

Vector TrajectoryParticle::getMomentum(unsigned int step) const{
  return getType()->getMass()*Trajectory.U.at(step);
}

/** 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)));
};

/** 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]/getType()->getMass());     // 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]/getType()->getMass()); // 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;
};

std::ostream & TrajectoryParticle::operator << (std::ostream &ost) const
{
  ParticleInfo::operator<<(ost);
  ost << "," << getPosition();
  return ost;
}

std::ostream & operator << (std::ostream &ost, const TrajectoryParticle &a)
{
  a.ParticleInfo::operator<<(ost);
  ost << "," << a.getPosition();
  return ost;
}