/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010-2012 University of Bonn. All rights reserved.
 * 
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    MoleCuilder is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * molecule_geometry.cpp
 *
 *  Created on: Oct 5, 2009
 *      Author: heber
 */

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

//#include "CodePatterns/MemDebug.hpp"

#include "Atom/atom.hpp"
#include "Bond/bond.hpp"
#include "Box.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/Verbose.hpp"
#include "config.hpp"
#include "Element/element.hpp"
#include "Graph/BondGraph.hpp"
#include "LinearAlgebra/leastsquaremin.hpp"
#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/RealSpaceMatrix.hpp"
#include "LinearAlgebra/Plane.hpp"
#include "molecule.hpp"
#include "World.hpp"

#include <boost/foreach.hpp>

#include <gsl/gsl_eigen.h>
#include <gsl/gsl_multimin.h>


/************************************* Functions for class molecule *********************************/

/** Returns vector pointing to center of the domain.
 * \return pointer to center of the domain
 */
#ifdef HAVE_INLINE
inline
#else
static
#endif
const Vector DetermineCenterOfBox()
{
  Vector a(0.5,0.5,0.5);
  const RealSpaceMatrix &M = World::getInstance().getDomain().getM();
  a *= M;
  return a;
}

/** Centers the molecule in the box whose lengths are defined by vector \a *BoxLengths.
 * \param *out output stream for debugging
 */
bool molecule::CenterInBox()
{
  bool status = true;
  const Vector Center = DetermineCenterOfAll();
  const Vector CenterBox = DetermineCenterOfBox();
  Box &domain = World::getInstance().getDomain();

  // go through all atoms
  Translate(CenterBox - Center);
  getAtomSet().transformNodes(boost::bind(&Box::enforceBoundaryConditions,domain,_1));

  return status;
}


/** Bounds the molecule in the box whose lengths are defined by vector \a *BoxLengths.
 * \param *out output stream for debugging
 */
bool molecule::BoundInBox()
{
  bool status = true;
  Box &domain = World::getInstance().getDomain();

  // go through all atoms
  getAtomSet().transformNodes(boost::bind(&Box::enforceBoundaryConditions,domain,_1));

  return status;
}

/** Centers the edge of the atoms at (0,0,0).
 */
void molecule::CenterEdge()
{
  const_iterator iter = const_cast<const molecule &>(*this).begin();
  if (iter != const_cast<const molecule &>(*this).end()) {   //list not empty?
    Vector min = (*begin())->getPosition();
    for (;iter != const_cast<const molecule &>(*this).end(); ++iter) { // continue with second if present
      const Vector &currentPos = (*iter)->getPosition();
      for (size_t i=0;i<NDIM;++i)
        if (min[i] > currentPos[i])
          min[i] = currentPos[i];
    }
    Translate(-1.*min);
  }
}

/** Centers the center of the atoms at (0,0,0).
 * \param *out output stream for debugging
 * \param *center return vector for translation vector
 */
void molecule::CenterOrigin()
{
  int Num = 0;
  const_iterator iter = const_cast<const molecule &>(*this).begin();  // start at first in list
  Vector Center;

  Center.Zero();
  if (iter != const_cast<const molecule &>(*this).end()) {   //list not empty?
    for (; iter != const_cast<const molecule &>(*this).end(); ++iter) {  // continue with second if present
      Num++;
      Center += (*iter)->getPosition();
    }
    Center.Scale(-1./(double)Num); // divide through total number (and sign for direction)
    Translate(Center);
  }
}

/** Returns vector pointing to center of all atoms.
 * \return pointer to center of all vector
 */
const Vector molecule::DetermineCenterOfAll() const
{
  const_iterator iter = begin();  // start at first in list
  Vector a;
  double Num = 0;

  a.Zero();

  if (iter != end()) {   //list not empty?
    for (; iter != end(); ++iter) {  // continue with second if present
      Num++;
      a += (*iter)->getPosition();
    }
    a.Scale(1./(double)Num); // divide through total mass (and sign for direction)
  }
  return a;
}


/** Returns vector pointing to center of gravity.
 * \param *out output stream for debugging
 * \return pointer to center of gravity vector
 */
const Vector molecule::DetermineCenterOfGravity() const
{
  const_iterator iter = begin();  // start at first in list
  Vector a;
  Vector tmp;
  double Num = 0;

  a.Zero();

  if (iter != end()) {   //list not empty?
    for (; iter != end(); ++iter) {  // continue with second if present
      Num += (*iter)->getType()->getMass();
      tmp = (*iter)->getType()->getMass() * (*iter)->getPosition();
      a += tmp;
    }
    a.Scale(1./Num); // divide through total mass
  }
  LOG(1, "INFO: Resulting center of gravity: " << a << ".");
  return a;
}

/** Centers the center of gravity of the atoms at (0,0,0).
 * \param *out output stream for debugging
 * \param *center return vector for translation vector
 */
void molecule::CenterPeriodic()
{
  Vector NewCenter;
  DeterminePeriodicCenter(NewCenter);
  Translate(-1.*NewCenter);
}


/** Centers the center of gravity of the atoms at (0,0,0).
 * \param *out output stream for debugging
 * \param *center return vector for translation vector
 */
void molecule::CenterAtVector(const Vector &newcenter)
{
  Translate(-1.*newcenter);
}

/** Calculate the inertia tensor of a the molecule.
 *
 * @return inertia tensor
 */
RealSpaceMatrix molecule::getInertiaTensor() const
{
  RealSpaceMatrix InertiaTensor;
  const Vector CenterOfGravity = DetermineCenterOfGravity();

  // reset inertia tensor
  InertiaTensor.setZero();

  // sum up inertia tensor
  for (const_iterator iter = begin(); iter != end(); ++iter) {
    Vector x = (*iter)->getPosition();
    x -= CenterOfGravity;
    const double mass = (*iter)->getType()->getMass();
    InertiaTensor.at(0,0) += mass*(x[1]*x[1] + x[2]*x[2]);
    InertiaTensor.at(0,1) += mass*(-x[0]*x[1]);
    InertiaTensor.at(0,2) += mass*(-x[0]*x[2]);
    InertiaTensor.at(1,0) += mass*(-x[1]*x[0]);
    InertiaTensor.at(1,1) += mass*(x[0]*x[0] + x[2]*x[2]);
    InertiaTensor.at(1,2) += mass*(-x[1]*x[2]);
    InertiaTensor.at(2,0) += mass*(-x[2]*x[0]);
    InertiaTensor.at(2,1) += mass*(-x[2]*x[1]);
    InertiaTensor.at(2,2) += mass*(x[0]*x[0] + x[1]*x[1]);
  }
  // print InertiaTensor
  LOG(1, "INFO: The inertia tensor of molecule " << getName() <<  " is:" << InertiaTensor);

  return InertiaTensor;
}

/** Rotates the molecule in such a way that biggest principal axis corresponds
 * to given \a Axis.
 *
 * @param Axis Axis to align with biggest principal axis
 */
void molecule::RotateToPrincipalAxisSystem(const Vector &Axis)
{
  const Vector CenterOfGravity = DetermineCenterOfGravity();
  RealSpaceMatrix InertiaTensor = getInertiaTensor();

  // diagonalize to determine principal axis system
  Vector Eigenvalues = InertiaTensor.transformToEigenbasis();

  for(int i=0;i<NDIM;i++)
    LOG(0, "eigenvalue = " << Eigenvalues[i] << ", eigenvector = " << InertiaTensor.column(i));

  LOG(0, "STATUS: Transforming to PAS ... ");

  // obtain first column, eigenvector to biggest eigenvalue
  const Vector BiggestEigenvector(InertiaTensor.column(Eigenvalues.SmallestComponent()));
  Vector DesiredAxis(Axis.getNormalized());

  // Creation Line that is the rotation axis
  DesiredAxis.VectorProduct(BiggestEigenvector);
  Line RotationAxis(Vector(0.,0.,0.), DesiredAxis);

  // determine angle
  const double alpha = BiggestEigenvector.Angle(Axis);

  LOG(1, "INFO: Rotation angle is " << alpha);

  // and rotate
  for (iterator iter = begin(); iter != end(); ++iter) {
    *(*iter) -= CenterOfGravity;
    (*iter)->setPosition(RotationAxis.rotateVector((*iter)->getPosition(), alpha));
    *(*iter) += CenterOfGravity;
  }
  LOG(0, "STATUS: done.");
}

/** Scales all atoms by \a *factor.
 * \param *factor pointer to scaling factor
 *
 * TODO: Is this realy what is meant, i.e.
 * x=(x[0]*factor[0],x[1]*factor[1],x[2]*factor[2]) (current impl)
 * or rather
 * x=(**factor) * x (as suggested by comment)
 */
void molecule::Scale(const double *factor)
{
  for (iterator iter = begin(); iter != end(); ++iter)
    for (size_t j=0;j<(*iter)->getTrajectorySize();j++)
      if ((*iter)->isStepPresent(j)) {
        Vector temp = (*iter)->getPositionAtStep(j);
        temp.ScaleAll(factor);
        (*iter)->setPositionAtStep(j,temp);
      }
};

/** Translate all atoms by given vector.
 * \param trans[] translation vector.
 */
void molecule::Translate(const Vector &trans)
{
  for (iterator iter = begin(); iter != end(); ++iter)
    for (size_t j=0;j<(*iter)->getTrajectorySize();j++)
      if ((*iter)->isStepPresent(j))
        (*iter)->setPositionAtStep(j, (*iter)->getPositionAtStep(j) + (trans));
};

/** Translate the molecule periodically in the box.
 * \param trans[] translation vector.
 * TODO treatment of trajectories missing
 */
void molecule::TranslatePeriodically(const Vector &trans)
{
  Translate(trans);
  Box &domain = World::getInstance().getDomain();
  getAtomSet().transformNodes(boost::bind(&Box::enforceBoundaryConditions,domain,_1));
};


/** Mirrors all atoms against a given plane.
 * \param n[] normal vector of mirror plane.
 */
void molecule::Mirror(const Vector &n)
{
  Plane p(n,0);
  getAtomSet().transformNodes(boost::bind(&Plane::mirrorVector,p,_1));
};

/** Determines center of molecule (yet not considering atom masses).
 * \param center reference to return vector
 * \param treatment whether to treat hydrogen special or not
 */
void molecule::DeterminePeriodicCenter(Vector &center, const enum HydrogenTreatment treatment)
{
  const RealSpaceMatrix &matrix = World::getInstance().getDomain().getM();
  const RealSpaceMatrix &inversematrix = World::getInstance().getDomain().getM();
  double tmp;
  bool flag;
  Vector Testvector, Translationvector;
  Vector Center;
  const BondGraph * const BG = World::getInstance().getBondGraph();

  do {
    Center.Zero();
    flag = true;
    for (const_iterator iter = const_cast<const molecule &>(*this).begin();
        iter != const_cast<const molecule &>(*this).end();
        ++iter) {
      if ((treatment == IncludeHydrogen) || ((*iter)->getType()->getAtomicNumber() != 1)) {
        Testvector = inversematrix * (*iter)->getPosition();
        Translationvector.Zero();
        const BondList& ListOfBonds = (*iter)->getListOfBonds();
        for (BondList::const_iterator Runner = ListOfBonds.begin();
            Runner != ListOfBonds.end();
            ++Runner) {
         if ((*iter)->getNr() < (*Runner)->GetOtherAtom((*iter))->getNr()) // otherwise we shift one to, the other fro and gain nothing
            for (int j=0;j<NDIM;j++) {
              tmp = (*iter)->at(j) - (*Runner)->GetOtherAtom(*iter)->at(j);
              const range<double> MinMaxBondDistance(
                  BG->getMinMaxDistance((*iter), (*Runner)->GetOtherAtom(*iter)));
              if (fabs(tmp) > MinMaxBondDistance.last) {  // check against Min is not useful for components
                flag = false;
                LOG(0, "Hit: atom " << (*iter)->getName() << " in bond " << *(*Runner) << " has to be shifted due to " << tmp << ".");
                if (tmp > 0)
                  Translationvector[j] -= 1.;
                else
                  Translationvector[j] += 1.;
              }
            }
        }
        Testvector += Translationvector;
        Testvector *= matrix;
        Center += Testvector;
        LOG(1, "vector is: " << Testvector);
        if (treatment == ExcludeHydrogen) {
          // now also change all hydrogens
          for (BondList::const_iterator Runner = ListOfBonds.begin();
              Runner != ListOfBonds.end();
              ++Runner) {
            if ((*Runner)->GetOtherAtom((*iter))->getType()->getAtomicNumber() == 1) {
              Testvector = inversematrix * (*Runner)->GetOtherAtom((*iter))->getPosition();
              Testvector += Translationvector;
              Testvector *= matrix;
              Center += Testvector;
              LOG(1, "Hydrogen vector is: " << Testvector);
            }
          }
        }
      }
    }
  } while (!flag);

  Center.Scale(1./static_cast<double>(getAtomCount()));
  CenterAtVector(Center);
};

/** Align all atoms in such a manner that given vector \a *n is along z axis.
 * \param n[] alignment vector.
 */
void molecule::Align(const Vector &n)
{
  double alpha, tmp;
  Vector z_axis;
  Vector alignment(n);
  z_axis[0] = 0.;
  z_axis[1] = 0.;
  z_axis[2] = 1.;

  // rotate on z-x plane
  LOG(0, "Begin of Aligning all atoms.");
  alpha = atan(-alignment.at(0)/alignment.at(2));
  LOG(1, "INFO: Z-X-angle: " << alpha << " ... ");
  for (iterator iter = begin(); iter != end(); ++iter) {
    tmp = (*iter)->at(0);
    (*iter)->set(0,  cos(alpha) * tmp + sin(alpha) * (*iter)->at(2));
    (*iter)->set(2, -sin(alpha) * tmp + cos(alpha) * (*iter)->at(2));
    for (int j=0;j<MDSteps;j++) {
      Vector temp;
      temp[0] =  cos(alpha) * (*iter)->getPositionAtStep(j)[0] + sin(alpha) * (*iter)->getPositionAtStep(j)[2];
      temp[2] = -sin(alpha) * (*iter)->getPositionAtStep(j)[0] + cos(alpha) * (*iter)->getPositionAtStep(j)[2];
      (*iter)->setPositionAtStep(j,temp);
    }
  }
  // rotate n vector
  tmp = alignment.at(0);
  alignment.at(0) =  cos(alpha) * tmp +  sin(alpha) * alignment.at(2);
  alignment.at(2) = -sin(alpha) * tmp +  cos(alpha) * alignment.at(2);
  LOG(1, "alignment vector after first rotation: " << alignment);

  // rotate on z-y plane
  alpha = atan(-alignment.at(1)/alignment.at(2));
  LOG(1, "INFO: Z-Y-angle: " << alpha << " ... ");
  for (iterator iter = begin(); iter != end(); ++iter) {
    tmp = (*iter)->at(1);
    (*iter)->set(1,  cos(alpha) * tmp + sin(alpha) * (*iter)->at(2));
    (*iter)->set(2, -sin(alpha) * tmp + cos(alpha) * (*iter)->at(2));
    for (int j=0;j<MDSteps;j++) {
      Vector temp;
      temp[1] =  cos(alpha) * (*iter)->getPositionAtStep(j)[1] + sin(alpha) * (*iter)->getPositionAtStep(j)[2];
      temp[2] = -sin(alpha) * (*iter)->getPositionAtStep(j)[1] + cos(alpha) * (*iter)->getPositionAtStep(j)[2];
      (*iter)->setPositionAtStep(j,temp);
    }
  }
  // rotate n vector (for consistency check)
  tmp = alignment.at(1);
  alignment.at(1) =  cos(alpha) * tmp +  sin(alpha) * alignment.at(2);
  alignment.at(2) = -sin(alpha) * tmp +  cos(alpha) * alignment.at(2);

  LOG(1, "alignment vector after second rotation: " << alignment);
  LOG(0, "End of Aligning all atoms.");
};


/** Calculates sum over least square distance to line hidden in \a *x.
 * \param *x offset and direction vector
 * \param *params pointer to lsq_params structure
 * \return \f$ sum_i^N | y_i - (a + t_i b)|^2\f$
 */
double LeastSquareDistance (const gsl_vector * x, void * params)
{
  double res = 0, t;
  Vector a,b,c,d;
  struct lsq_params *par = (struct lsq_params *)params;

  // initialize vectors
  a[0] = gsl_vector_get(x,0);
  a[1] = gsl_vector_get(x,1);
  a[2] = gsl_vector_get(x,2);
  b[0] = gsl_vector_get(x,3);
  b[1] = gsl_vector_get(x,4);
  b[2] = gsl_vector_get(x,5);
  // go through all atoms
  for (molecule::const_iterator iter = par->mol->begin(); iter != par->mol->end(); ++iter) {
    if ((*iter)->getType() == ((struct lsq_params *)params)->type) { // for specific type
      c = (*iter)->getPosition() - a;
      t = c.ScalarProduct(b);           // get direction parameter
      d = t*b;       // and create vector
      c -= d;   // ... yielding distance vector
      res += d.ScalarProduct(d);        // add squared distance
    }
  }
  return res;
};

/** By minimizing the least square distance gains alignment vector.
 * \bug this is not yet working properly it seems
 */
void molecule::GetAlignvector(struct lsq_params * par) const
{
    int np = 6;

   const gsl_multimin_fminimizer_type *T =
     gsl_multimin_fminimizer_nmsimplex;
   gsl_multimin_fminimizer *s = NULL;
   gsl_vector *ss;
   gsl_multimin_function minex_func;

   size_t iter = 0, i;
   int status;
   double size;

   /* Initial vertex size vector */
   ss = gsl_vector_alloc (np);

   /* Set all step sizes to 1 */
   gsl_vector_set_all (ss, 1.0);

   /* Starting point */
   par->x = gsl_vector_alloc (np);
   par->mol = this;

   gsl_vector_set (par->x, 0, 0.0);  // offset
   gsl_vector_set (par->x, 1, 0.0);
   gsl_vector_set (par->x, 2, 0.0);
   gsl_vector_set (par->x, 3, 0.0);  // direction
   gsl_vector_set (par->x, 4, 0.0);
   gsl_vector_set (par->x, 5, 1.0);

   /* Initialize method and iterate */
   minex_func.f = &LeastSquareDistance;
   minex_func.n = np;
   minex_func.params = (void *)par;

   s = gsl_multimin_fminimizer_alloc (T, np);
   gsl_multimin_fminimizer_set (s, &minex_func, par->x, ss);

   do
     {
       iter++;
       status = gsl_multimin_fminimizer_iterate(s);

       if (status)
         break;

       size = gsl_multimin_fminimizer_size (s);
       status = gsl_multimin_test_size (size, 1e-2);

       if (status == GSL_SUCCESS)
         {
           printf ("converged to minimum at\n");
         }

       printf ("%5d ", (int)iter);
       for (i = 0; i < (size_t)np; i++)
         {
           printf ("%10.3e ", gsl_vector_get (s->x, i));
         }
       printf ("f() = %7.3f size = %.3f\n", s->fval, size);
     }
   while (status == GSL_CONTINUE && iter < 100);

  for (i=0;i<(size_t)np;i++)
    gsl_vector_set(par->x, i, gsl_vector_get(s->x, i));
   //gsl_vector_free(par->x);
   gsl_vector_free(ss);
   gsl_multimin_fminimizer_free (s);
};