/*
 * 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.
 */

/*
 * 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.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 *********************************/


/** 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
  BOOST_FOREACH(atom* iter, atoms){
    std::cout << "atom before is at " << *iter << std::endl;
    *iter -= *Center;
    *iter += *CenterBox;
    std::cout << "atom after is at " << *iter << std::endl;
  }
  atoms.transformNodes(boost::bind(&Box::WrapPeriodically,domain,_1));

  delete(Center);
  delete(CenterBox);
  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
  atoms.transformNodes(boost::bind(&Box::WrapPeriodically,domain,_1));

  return status;
};

/** Centers the edge of the atoms at (0,0,0).
 * \param *out output stream for debugging
 * \param *max coordinates of other edge, specifying box dimensions.
 */
void molecule::CenterEdge(Vector *max)
{
//  Info info(__func__);
  Vector *min = new Vector;

  molecule::const_iterator iter = begin();  // start at first in list
  if (iter != end()) { //list not empty?
    for (int i=NDIM;i--;) {
      max->at(i) = (*iter)->at(i);
      min->at(i) = (*iter)->at(i);
    }
    for (; iter != end(); ++iter) {// continue with second if present
      //(*iter)->Output(1,1,out);
      for (int i=NDIM;i--;) {
        max->at(i) = (max->at(i) < (*iter)->at(i)) ? (*iter)->at(i) : max->at(i);
        min->at(i) = (min->at(i) > (*iter)->at(i)) ? (*iter)->at(i) : min->at(i);
      }
    }
    LOG(1, "INFO: Maximum is " << *max << ", Minimum is " << *min << ".");
    min->Scale(-1.);
    (*max) += (*min);
    Translate(min);
  }
  delete(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;
  molecule::const_iterator iter = begin();  // start at first in list
  Vector Center;

  Center.Zero();
  if (iter != end()) {   //list not empty?
    for (; iter != 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
 */
Vector * molecule::DetermineCenterOfAll() const
{
  molecule::const_iterator iter = begin();  // start at first in list
  Vector *a = new Vector();
  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 the domain.
 * \return pointer to center of the domain
 */
Vector * molecule::DetermineCenterOfBox() const
{
  Vector *a = new Vector(0.5,0.5,0.5);
  const RealSpaceMatrix &M = World::getInstance().getDomain().getM();
  (*a) *= M;
  return a;
};

/** Returns vector pointing to center of gravity.
 * \param *out output stream for debugging
 * \return pointer to center of gravity vector
 */
Vector * molecule::DetermineCenterOfGravity() const
{
  molecule::const_iterator iter = begin();  // start at first in list
  Vector *a = new Vector();
  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);
  // go through all atoms
  BOOST_FOREACH(atom* iter, atoms){
    *iter -= 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(Vector *newcenter)
{
  // go through all atoms
  BOOST_FOREACH(atom* iter, atoms){
    *iter -= *newcenter;
  }
};

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

  // reset inertia tensor
  InertiaTensor.setZero();

  // sum up inertia tensor
  for (molecule::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);

  delete CenterOfGravity;
  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(Vector &Axis)
{
  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
  Vector BiggestEigenvector(InertiaTensor.column(Eigenvalues.SmallestComponent()));
  Vector DesiredAxis(Axis);

  // 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 (molecule::iterator iter = begin(); iter != end(); ++iter) {
    *(*iter) -= *CenterOfGravity;
    (*iter)->setPosition(RotationAxis.rotateVector((*iter)->getPosition(), alpha));
    *(*iter) += *CenterOfGravity;
  }
  LOG(0, "STATUS: done.");

  delete CenterOfGravity;
}

/** 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 ** const factor)
{
  for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
    for (size_t j=0;j<(*iter)->getTrajectorySize();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 (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
    for (size_t j=0;j<(*iter)->getTrajectorySize();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)
{
  Box &domain = World::getInstance().getDomain();

  // go through all atoms
  BOOST_FOREACH(atom* iter, atoms){
    *iter += *trans;
  }
  atoms.transformNodes(boost::bind(&Box::WrapPeriodically,domain,_1));

};


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

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

  do {
    Center.Zero();
    flag = true;
    for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
      if ((saturation == DontSaturate) || ((*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 (saturation == DoSaturate) {
          // 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(Vector *n)
{
  double alpha, tmp;
  Vector z_axis;
  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(-n->at(0)/n->at(2));
  LOG(1, "INFO: Z-X-angle: " << alpha << " ... ");
  for (molecule::const_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 = n->at(0);
  n->at(0) =  cos(alpha) * tmp +  sin(alpha) * n->at(2);
  n->at(2) = -sin(alpha) * tmp +  cos(alpha) * n->at(2);
  LOG(1, "alignment vector after first rotation: " << n);

  // rotate on z-y plane
  alpha = atan(-n->at(1)/n->at(2));
  LOG(1, "INFO: Z-Y-angle: " << alpha << " ... ");
  for (molecule::const_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 = n->at(1);
  n->at(1) =  cos(alpha) * tmp +  sin(alpha) * n->at(2);
  n->at(2) = -sin(alpha) * tmp +  cos(alpha) * n->at(2);


  LOG(1, "alignment vector after second rotation: " << n);
  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);
};