/*
 * 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_atominfo.cpp
 *
 *  Created on: Oct 19, 2009
 *      Author: heber
 */

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

#include "CodePatterns/MemDebug.hpp"

#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "config.hpp"
#include "element.hpp"
#include "parser.hpp"
#include "periodentafel.hpp"
#include "World.hpp"
#include "WorldTime.hpp"
#include "atom_atominfo.hpp"

#include <iomanip>

/** Constructor of class AtomInfo.
 */
AtomInfo::AtomInfo() :
  AtomicElement(NULL),
  FixedIon(false)
{
  AtomicPosition.reserve(1);
  AtomicPosition.push_back(zeroVec);
  AtomicVelocity.reserve(1);
  AtomicVelocity.push_back(zeroVec);
  AtomicForce.reserve(1);
  AtomicForce.push_back(zeroVec);
};

/** Copy constructor of class AtomInfo.
 */
AtomInfo::AtomInfo(const AtomInfo &_atom) :
    AtomicPosition(_atom.AtomicPosition),
    AtomicElement(_atom.AtomicElement),
    FixedIon(false)
{
  AtomicVelocity.reserve(1);
  AtomicVelocity.push_back(zeroVec);
  AtomicForce.reserve(1);
  AtomicForce.push_back(zeroVec);
};

AtomInfo::AtomInfo(const VectorInterface &_v) :
    AtomicElement(NULL),
    FixedIon(false)
{
  AtomicPosition[0] = _v.getPosition();
  AtomicVelocity.reserve(1);
  AtomicVelocity.push_back(zeroVec);
  AtomicForce.reserve(1);
  AtomicForce.push_back(zeroVec);
};

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

void AtomInfo::AppendTrajectoryStep()
{
  AtomicPosition.push_back(zeroVec);
  AtomicVelocity.push_back(zeroVec);
  AtomicForce.push_back(zeroVec);
  LOG(5,"AtomInfo::AppendTrajectoryStep() called, size is ("
      << AtomicPosition.size() << ","
      << AtomicVelocity.size() << ","
      << AtomicForce.size() << ")");
}

const element *AtomInfo::getType() const
{
  return AtomicElement;
}

const double& AtomInfo::operator[](size_t i) const
{
  ASSERT(AtomicPosition.size() > WorldTime::getTime(),
      "AtomInfo::operator[]() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[WorldTime::getTime()][i];
}

const double& AtomInfo::at(size_t i) const
{
  ASSERT(AtomicPosition.size() > WorldTime::getTime(),
      "AtomInfo::at() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[WorldTime::getTime()].at(i);
}

const double& AtomInfo::atStep(size_t i, unsigned int _step) const
{
  ASSERT(AtomicPosition.size() > _step,
      "AtomInfo::atStep() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[_step].at(i);
}

void AtomInfo::set(size_t i, const double value)
{
  ASSERT(AtomicPosition.size() > WorldTime::getTime(),
      "AtomInfo::set() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].at(i) = value;
}

const Vector& AtomInfo::getPosition() const
{
  ASSERT(AtomicPosition.size() > WorldTime::getTime(),
      "AtomInfo::getPosition() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[WorldTime::getTime()];
}

const Vector& AtomInfo::getPositionAtStep(const unsigned int _step) const
{
  ASSERT(_step < AtomicPosition.size(),
      "AtomInfo::getPositionAtStep() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[_step];
}

void AtomInfo::setType(const element* _type) {
  AtomicElement = _type;
}

void AtomInfo::setType(const int Z) {
  const element *elem = World::getInstance().getPeriode()->FindElement(Z);
  setType(elem);
}

//Vector& AtomInfo::getAtomicVelocity()
//{
//  return AtomicVelocity[0];
//}

//Vector& AtomInfo::getAtomicVelocity(const int _step)
//{
//  ASSERT(_step < AtomicVelocity.size(),
//      "AtomInfo::getAtomicVelocity() - Access out of range.");
//  return AtomicVelocity[_step];
//}

const Vector& AtomInfo::getAtomicVelocity() const
{
  ASSERT(AtomicVelocity.size() > 0,
      "AtomInfo::getAtomicVelocity() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicVelocity[WorldTime::getTime()];
}

const Vector& AtomInfo::getAtomicVelocityAtStep(const unsigned int _step) const
{
  ASSERT(_step < AtomicVelocity.size(),
      "AtomInfo::getAtomicVelocity() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicVelocity[_step];
}

void AtomInfo::setAtomicVelocity(const Vector &_newvelocity)
{
  ASSERT(WorldTime::getTime() < AtomicVelocity.size(),
      "AtomInfo::setAtomicVelocity() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicVelocity[WorldTime::getTime()] = _newvelocity;
}

void AtomInfo::setAtomicVelocityAtStep(const unsigned int _step, const Vector &_newvelocity)
{
  const unsigned int size = AtomicVelocity.size();
  ASSERT(_step <= size,
      "AtomInfo::setAtomicVelocityAtStep() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(size)+"].");
  if(_step < size) {
    AtomicVelocity[_step] = _newvelocity;
  } else if (_step == size) {
    UpdateSteps();
    AtomicVelocity[_step] = _newvelocity;
  }
}

const Vector& AtomInfo::getAtomicForce() const
{
  ASSERT(WorldTime::getTime() < AtomicForce.size(),
      "AtomInfo::getAtomicForce() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicForce[WorldTime::getTime()];
}

const Vector& AtomInfo::getAtomicForceAtStep(const unsigned int _step) const
{
  ASSERT(_step < AtomicForce.size(),
      "AtomInfo::getAtomicForce() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicForce[_step];
}

void AtomInfo::setAtomicForce(const Vector &_newforce)
{
  ASSERT(WorldTime::getTime() < AtomicForce.size(),
      "AtomInfo::setAtomicForce() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicForce[WorldTime::getTime()] = _newforce;
}

void AtomInfo::setAtomicForceAtStep(const unsigned int _step, const Vector &_newforce)
{
  const unsigned int size = AtomicForce.size();
  ASSERT(_step <= size,
      "AtomInfo::setAtomicForce() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(AtomicPosition.size())+"].");
  if(_step < size) {
    AtomicForce[_step] = _newforce;
  } else if (_step == size) {
    UpdateSteps();
    AtomicForce[_step] = _newforce;
  }
}

bool AtomInfo::getFixedIon() const
{
  return FixedIon;
}

void AtomInfo::setFixedIon(const bool _fixedion)
{
  FixedIon = _fixedion;
}

void AtomInfo::setPosition(const Vector& _vector)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::setPosition() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()] = _vector;
  //cout << "AtomInfo::setPosition: " << getType()->symbol << " at " << getPosition() << endl;
}

void AtomInfo::setPositionAtStep(unsigned int _step, const Vector& _vector)
{
  const unsigned int size = AtomicPosition.size();
  ASSERT(_step <= size,
      "AtomInfo::setPosition() - Access out of range: "
      +toString(_step)
      +" not in [0,"+toString(size)+"].");
  if(_step < size) {
    AtomicPosition[_step] = _vector;
  } else if (_step == size) {
    UpdateSteps();
    AtomicPosition[_step] = _vector;
  }
  //cout << "AtomInfo::setPosition: " << getType()->symbol << " at " << getPosition() << endl;
}

const VectorInterface& AtomInfo::operator+=(const Vector& b)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::operator+=() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()] += b;
  return *this;
}

const VectorInterface& AtomInfo::operator-=(const Vector& b)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::operator-=() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()] -= b;
  return *this;
}

Vector const AtomInfo::operator+(const Vector& b) const
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::operator+() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  Vector a(AtomicPosition[WorldTime::getTime()]);
  a += b;
  return a;
}

Vector const AtomInfo::operator-(const Vector& b) const
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::operator-() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  Vector a(AtomicPosition[WorldTime::getTime()]);
  a -= b;
  return a;
}

double AtomInfo::distance(const Vector &point) const
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::distance() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[WorldTime::getTime()].distance(point);
}

double AtomInfo::DistanceSquared(const Vector &y) const
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::DistanceSquared() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return AtomicPosition[WorldTime::getTime()].DistanceSquared(y);
}

double AtomInfo::distance(const VectorInterface &_atom) const
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::distance() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return _atom.distance(AtomicPosition[WorldTime::getTime()]);
}

double AtomInfo::DistanceSquared(const VectorInterface &_atom) const
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::DistanceSquared() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return _atom.DistanceSquared(AtomicPosition[WorldTime::getTime()]);
}

VectorInterface &AtomInfo::operator=(const Vector& _vector)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::operator=() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()] = _vector;
  return *this;
}

void AtomInfo::ScaleAll(const double *factor)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::ScaleAll() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].ScaleAll(factor);
}

void AtomInfo::ScaleAll(const Vector &factor)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::ScaleAll() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].ScaleAll(factor);
}

void AtomInfo::Scale(const double factor)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::Scale() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].Scale(factor);
}

void AtomInfo::Zero()
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::Zero() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].Zero();
}

void AtomInfo::One(const double one)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::One() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].One(one);
}

void AtomInfo::LinearCombinationOfVectors(const Vector &x1, const Vector &x2, const Vector &x3, const double * const factors)
{
  ASSERT(WorldTime::getTime() < AtomicPosition.size(),
      "AtomInfo::LinearCombinationOfVectors() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  AtomicPosition[WorldTime::getTime()].LinearCombinationOfVectors(x1,x2,x3,factors);
}

/**
 *  returns the kinetic energy of this atom at a given time step
 */
double AtomInfo::getKineticEnergy(const unsigned int _step) const{
  ASSERT(_step < AtomicPosition.size(),
      "AtomInfo::getKineticEnergy() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return getMass() * AtomicVelocity[_step].NormSquared();
}

Vector AtomInfo::getMomentum(const unsigned int _step) const{
  ASSERT(_step < AtomicPosition.size(),
      "AtomInfo::getMomentum() - Access out of range: "
      +toString(WorldTime::getTime())
      +" not in [0,"+toString(AtomicPosition.size())+").");
  return getMass()*AtomicVelocity[_step];
}

/** Extends the trajectory STL vector to the new size.
 * Does nothing if \a MaxSteps is smaller than current size.
 * \param MaxSteps
 */
void AtomInfo::ResizeTrajectory(size_t MaxSteps)
{
  for (;AtomicPosition.size() <= (unsigned int)(MaxSteps);)
    UpdateSteps();
}

size_t AtomInfo::getTrajectorySize() const
{
  return AtomicPosition.size();
}

double AtomInfo::getMass() const{
  return AtomicElement->getMass();
}

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

  ASSERT(dest < AtomicPosition.size(),
      "AtomInfo::CopyStepOnStep() - destination outside of current trajectory array: "
      +toString(dest)
      +" not in [0,"+toString(AtomicPosition.size())+").");
  ASSERT(src < AtomicPosition.size(),
      "AtomInfo::CopyStepOnStep() - source outside of current trajectory array: "
      +toString(src)
      +" not in [0,"+toString(AtomicPosition.size())+").");
  for (int n=NDIM;n--;) {
    AtomicPosition.at(dest)[n] = AtomicPosition.at(src)[n];
    AtomicVelocity.at(dest)[n] = AtomicVelocity.at(src)[n];
    AtomicForce.at(dest)[n] = AtomicForce.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 Deltat time step width
 * \param IsAngstroem whether the force's underlying unit of length is angstroem or bohr radii
 * \param *Force matrix with forces
 */
void AtomInfo::VelocityVerletUpdate(int nr, const unsigned int NextStep, double Deltat, bool IsAngstroem, ForceMatrix *Force, const size_t offset)
{
  LOG(2, "INFO: Particle that currently " << *this);
  LOG(2, "INFO: Integrating with mass=" << getMass() << " and Deltat="
      << Deltat << " at NextStep=" << NextStep);
  // update force
  // (F+F_old)/2m = a and thus: v = (F+F_old)/2m * t = (F + F_old) * a
  Vector tempVector;
  for (int d=0; d<NDIM; d++) {
    ASSERT(Force->RowCounter[0] > nr,
        "AtomInfo::VelocityVerletUpdate() - "+toString(nr)
        +" out of bounds [0,"+toString(Force->RowCounter[0])
        +"] access on force matrix.");
    ASSERT(Force->ColumnCounter[0] > d+offset,
        "AtomInfo::VelocityVerletUpdate() - "+toString(d+offset)
        +" out of bounds [0,"+toString(Force->ColumnCounter[0])
        +"] access on force matrix.");
    tempVector[d] = -Force->Matrix[0][nr][d+offset]*(IsAngstroem ? AtomicLengthToAngstroem : 1.);
  }
  LOG(3, "INFO: Force at step " << NextStep << " set to " << tempVector);
  setAtomicForceAtStep(NextStep, tempVector);

  // update position
  tempVector = getPositionAtStep(NextStep-1);
  LOG(4, "INFO: initial position from last step " << setprecision(4) << tempVector);
  tempVector += Deltat*(getAtomicVelocityAtStep(NextStep-1));     // s(t) = s(0) + v * deltat + 1/2 a * deltat^2
  LOG(4, "INFO: position with velocity " << getAtomicVelocityAtStep(NextStep-1) << " from last step " << tempVector);
  tempVector += 0.5*Deltat*Deltat*(getAtomicForceAtStep(NextStep))*(1./getMass());     // F = m * a and s =
  LOG(4, "INFO: position with force " << getAtomicForceAtStep(NextStep) << " from last step " << tempVector);
  setPositionAtStep(NextStep, tempVector);
  LOG(3, "INFO: Position at step " << NextStep << " set to " << tempVector);

  // Update U
  tempVector = getAtomicVelocityAtStep(NextStep-1);
  LOG(4, "INFO: initial velocity from last step " << tempVector);
  tempVector += Deltat * (getAtomicForceAtStep(NextStep)+getAtomicForceAtStep(NextStep-1))*(1./getMass()); // v = F/m * t
  LOG(4, "INFO: Velocity with force from last " << getAtomicForceAtStep(NextStep-1)
      << " and present " << getAtomicForceAtStep(NextStep) << " step " << tempVector);
  setAtomicVelocityAtStep(NextStep, tempVector);
  LOG(3, "INFO: Velocity at step " << NextStep << " set to " << tempVector);
};

//const AtomInfo& operator*=(AtomInfo& a, const double m)
//{
//  a.Scale(m);
//  return a;
//}
//
//AtomInfo const operator*(const AtomInfo& a, const double m)
//{
//  AtomInfo copy(a);
//  copy *= m;
//  return copy;
//}
//
//AtomInfo const operator*(const double m, const AtomInfo& a)
//{
//  AtomInfo copy(a);
//  copy *= m;
//  return copy;
//}

std::ostream & AtomInfo::operator << (std::ostream &ost) const
{
  return (ost << getPosition());
}

std::ostream & operator << (std::ostream &ost, const AtomInfo &a)
{
  const size_t terminalstep = a.getTrajectorySize()-1;
  if (terminalstep) {
    ost << "starts at "
        << a.getPositionAtStep(0) << " and ends at "
        << a.getPositionAtStep(terminalstep)
        << " at time step " << terminalstep;
  } else {
    ost << "is at "
        << a.getPositionAtStep(0) << " with a single time step only";
  }
  return ost;
}