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

#include "Helpers/MemDebug.hpp"

#include "atom.hpp"
#include "bond.hpp"
#include "config.hpp"
#include "element.hpp"
#include "helpers.hpp"
#include "leastsquaremin.hpp"
#include "log.hpp"
#include "memoryallocator.hpp"
#include "molecule.hpp"
#include "World.hpp"
#include "Plane.hpp"
#include <boost/foreach.hpp>


/************************************* 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();
  double * const cell_size = World::getInstance().getDomain();
  double *M = ReturnFullMatrixforSymmetric(cell_size);
  double *Minv = InverseMatrix(M);

  // go through all atoms
  ActOnAllVectors( &Vector::SubtractVector, *Center);
  ActOnAllVectors( &Vector::SubtractVector, *CenterBox);
  ActOnAllVectors( &Vector::WrapPeriodically, (const double *)M, (const double *)Minv);

  delete[](M);
  delete[](Minv);
  delete(Center);
  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;
  double * const cell_size = World::getInstance().getDomain();
  double *M = ReturnFullMatrixforSymmetric(cell_size);
  double *Minv = InverseMatrix(M);

  // go through all atoms
  ActOnAllVectors( &Vector::WrapPeriodically, (const double *)M, (const double *)Minv);

  delete[](M);
  delete[](Minv);
  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)
{
  Vector *min = new Vector;

//  Log() << Verbose(3) << "Begin of CenterEdge." << endl;
  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)->x[i];
      min->at(i) = (*iter)->x[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)->x[i]) ? (*iter)->x[i] : max->at(i);
        min->at(i) = (min->at(i) > (*iter)->x[i]) ? (*iter)->x[i] : min->at(i);
      }
    }
//    Log() << Verbose(4) << "Maximum is ";
//    max->Output(out);
//    Log() << Verbose(0) << ", Minimum is ";
//    min->Output(out);
//    Log() << Verbose(0) << endl;
    min->Scale(-1.);
    (*max) += (*min);
    Translate(min);
    Center.Zero();
  }
  delete(min);
//  Log() << Verbose(3) << "End of CenterEdge." << endl;
};

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

  Center.Zero();

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

/** 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)->x;
    }
    a->Scale(1./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 double *cell_size = World::getInstance().getDomain();
  double *M = ReturnFullMatrixforSymmetric(cell_size);
  a->MatrixMultiplication(M);
  delete[](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()
{
  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)->type->mass;
      tmp = (*iter)->type->mass * (*iter)->x;
      (*a) += tmp;
    }
    a->Scale(1./Num); // divide through total mass (and sign for direction)
  }
//  Log() << Verbose(1) << "Resulting center of gravity: ";
//  a->Output(out);
//  Log() << Verbose(0) << endl;
  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()
{
  DeterminePeriodicCenter(Center);
};


/** 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)
{
  Center = *newcenter;
};


/** 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 (int j=0;j<MDSteps;j++)
      (*iter)->Trajectory.R.at(j).ScaleAll(*factor);
    (*iter)->x.ScaleAll(*factor);
  }
};

/** 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 (int j=0;j<MDSteps;j++)
      (*iter)->Trajectory.R.at(j) += (*trans);
    (*iter)->x += (*trans);
  }
};

/** Translate the molecule periodically in the box.
 * \param trans[] translation vector.
 * TODO treatment of trajetories missing
 */
void molecule::TranslatePeriodically(const Vector *trans)
{
  double * const cell_size = World::getInstance().getDomain();
  double *M = ReturnFullMatrixforSymmetric(cell_size);
  double *Minv = InverseMatrix(M);

  // go through all atoms
  ActOnAllVectors( &Vector::AddVector, *trans);
  ActOnAllVectors( &Vector::WrapPeriodically, (const double *)M, (const double *)Minv);

  delete[](M);
  delete[](Minv);
};


/** 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);
  BOOST_FOREACH( atom* iter, atoms ){
    (*iter->node) = p.mirrorVector(*iter->node);
  }
};

/** Determines center of molecule (yet not considering atom masses).
 * \param center reference to return vector
 */
void molecule::DeterminePeriodicCenter(Vector &center)
{
  double * const cell_size = World::getInstance().getDomain();
  double *matrix = ReturnFullMatrixforSymmetric(cell_size);
  double *inversematrix = InverseMatrix(matrix);
  double tmp;
  bool flag;
  Vector Testvector, Translationvector;

  do {
    Center.Zero();
    flag = true;
    for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
#ifdef ADDHYDROGEN
      if ((*iter)->type->Z != 1) {
#endif
        Testvector = (*iter)->x;
        Testvector.MatrixMultiplication(inversematrix);
        Translationvector.Zero();
        for (BondList::const_iterator Runner = (*iter)->ListOfBonds.begin(); Runner != (*iter)->ListOfBonds.end(); (++Runner)) {
         if ((*iter)->nr < (*Runner)->GetOtherAtom((*iter))->nr) // otherwise we shift one to, the other fro and gain nothing
            for (int j=0;j<NDIM;j++) {
              tmp = (*iter)->x[j] - (*Runner)->GetOtherAtom(*iter)->x[j];
              if ((fabs(tmp)) > BondDistance) {
                flag = false;
                DoLog(0) && (Log() << Verbose(0) << "Hit: atom " << (*iter)->getName() << " in bond " << *(*Runner) << " has to be shifted due to " << tmp << "." << endl);
                if (tmp > 0)
                  Translationvector[j] -= 1.;
                else
                  Translationvector[j] += 1.;
              }
            }
        }
        Testvector += Translationvector;
        Testvector.MatrixMultiplication(matrix);
        Center += Testvector;
        Log() << Verbose(1) << "vector is: " << Testvector << endl;
#ifdef ADDHYDROGEN
        // now also change all hydrogens
        for (BondList::const_iterator Runner = (*iter)->ListOfBonds.begin(); Runner != (*iter)->ListOfBonds.end(); (++Runner)) {
          if ((*Runner)->GetOtherAtom((*iter))->type->Z == 1) {
            Testvector = (*Runner)->GetOtherAtom((*iter))->x;
            Testvector.MatrixMultiplication(inversematrix);
            Testvector += Translationvector;
            Testvector.MatrixMultiplication(matrix);
            Center += Testvector;
            Log() << Verbose(1) << "Hydrogen vector is: " << Testvector << endl;
          }
        }
      }
#endif
    }
  } while (!flag);
  delete[](matrix);
  delete[](inversematrix);

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

/** Transforms/Rotates the given molecule into its principal axis system.
 * \param *out output stream for debugging
 * \param DoRotate whether to rotate (true) or only to determine the PAS.
 * TODO treatment of trajetories missing
 */
void molecule::PrincipalAxisSystem(bool DoRotate)
{
  double InertiaTensor[NDIM*NDIM];
  Vector *CenterOfGravity = DetermineCenterOfGravity();

  CenterPeriodic();

  // reset inertia tensor
  for(int i=0;i<NDIM*NDIM;i++)
    InertiaTensor[i] = 0.;

  // sum up inertia tensor
  for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
    Vector x = (*iter)->x;
    //x.SubtractVector(CenterOfGravity);
    InertiaTensor[0] += (*iter)->type->mass*(x[1]*x[1] + x[2]*x[2]);
    InertiaTensor[1] += (*iter)->type->mass*(-x[0]*x[1]);
    InertiaTensor[2] += (*iter)->type->mass*(-x[0]*x[2]);
    InertiaTensor[3] += (*iter)->type->mass*(-x[1]*x[0]);
    InertiaTensor[4] += (*iter)->type->mass*(x[0]*x[0] + x[2]*x[2]);
    InertiaTensor[5] += (*iter)->type->mass*(-x[1]*x[2]);
    InertiaTensor[6] += (*iter)->type->mass*(-x[2]*x[0]);
    InertiaTensor[7] += (*iter)->type->mass*(-x[2]*x[1]);
    InertiaTensor[8] += (*iter)->type->mass*(x[0]*x[0] + x[1]*x[1]);
  }
  // print InertiaTensor for debugging
  DoLog(0) && (Log() << Verbose(0) << "The inertia tensor is:" << endl);
  for(int i=0;i<NDIM;i++) {
    for(int j=0;j<NDIM;j++)
      DoLog(0) && (Log() << Verbose(0) << InertiaTensor[i*NDIM+j] << " ");
    DoLog(0) && (Log() << Verbose(0) << endl);
  }
  DoLog(0) && (Log() << Verbose(0) << endl);

  // diagonalize to determine principal axis system
  gsl_eigen_symmv_workspace *T = gsl_eigen_symmv_alloc(NDIM);
  gsl_matrix_view m = gsl_matrix_view_array(InertiaTensor, NDIM, NDIM);
  gsl_vector *eval = gsl_vector_alloc(NDIM);
  gsl_matrix *evec = gsl_matrix_alloc(NDIM, NDIM);
  gsl_eigen_symmv(&m.matrix, eval, evec, T);
  gsl_eigen_symmv_free(T);
  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_DESC);

  for(int i=0;i<NDIM;i++) {
    DoLog(1) && (Log() << Verbose(1) << "eigenvalue = " << gsl_vector_get(eval, i));
    DoLog(0) && (Log() << Verbose(0) << ", eigenvector = (" << evec->data[i * evec->tda + 0] << "," << evec->data[i * evec->tda + 1] << "," << evec->data[i * evec->tda + 2] << ")" << endl);
  }

  // check whether we rotate or not
  if (DoRotate) {
    DoLog(1) && (Log() << Verbose(1) << "Transforming molecule into PAS ... ");
    // the eigenvectors specify the transformation matrix
    ActOnAllVectors( &Vector::MatrixMultiplication, (const double *) evec->data );
    DoLog(0) && (Log() << Verbose(0) << "done." << endl);

    // summing anew for debugging (resulting matrix has to be diagonal!)
    // reset inertia tensor
    for(int i=0;i<NDIM*NDIM;i++)
      InertiaTensor[i] = 0.;

    // sum up inertia tensor
    for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
      Vector x = (*iter)->x;
      InertiaTensor[0] += (*iter)->type->mass*(x[1]*x[1] + x[2]*x[2]);
      InertiaTensor[1] += (*iter)->type->mass*(-x[0]*x[1]);
      InertiaTensor[2] += (*iter)->type->mass*(-x[0]*x[2]);
      InertiaTensor[3] += (*iter)->type->mass*(-x[1]*x[0]);
      InertiaTensor[4] += (*iter)->type->mass*(x[0]*x[0] + x[2]*x[2]);
      InertiaTensor[5] += (*iter)->type->mass*(-x[1]*x[2]);
      InertiaTensor[6] += (*iter)->type->mass*(-x[2]*x[0]);
      InertiaTensor[7] += (*iter)->type->mass*(-x[2]*x[1]);
      InertiaTensor[8] += (*iter)->type->mass*(x[0]*x[0] + x[1]*x[1]);
    }
    // print InertiaTensor for debugging
    DoLog(0) && (Log() << Verbose(0) << "The inertia tensor is:" << endl);
    for(int i=0;i<NDIM;i++) {
      for(int j=0;j<NDIM;j++)
        DoLog(0) && (Log() << Verbose(0) << InertiaTensor[i*NDIM+j] << " ");
      DoLog(0) && (Log() << Verbose(0) << endl);
    }
    DoLog(0) && (Log() << Verbose(0) << endl);
  }

  // free everything
  delete(CenterOfGravity);
  gsl_vector_free(eval);
  gsl_matrix_free(evec);
};


/** 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
  DoLog(0) && (Log() << Verbose(0) << "Begin of Aligning all atoms." << endl);
  alpha = atan(-n->at(0)/n->at(2));
  DoLog(1) && (Log() << Verbose(1) << "Z-X-angle: " << alpha << " ... ");
  for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
    tmp = (*iter)->x[0];
    (*iter)->x[0] =  cos(alpha) * tmp + sin(alpha) * (*iter)->x[2];
    (*iter)->x[2] = -sin(alpha) * tmp + cos(alpha) * (*iter)->x[2];
    for (int j=0;j<MDSteps;j++) {
      tmp = (*iter)->Trajectory.R.at(j)[0];
      (*iter)->Trajectory.R.at(j)[0] =  cos(alpha) * tmp + sin(alpha) * (*iter)->Trajectory.R.at(j)[2];
      (*iter)->Trajectory.R.at(j)[2] = -sin(alpha) * tmp + cos(alpha) * (*iter)->Trajectory.R.at(j)[2];
    }
  }
  // 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);
  DoLog(1) && (Log() << Verbose(1) << "alignment vector after first rotation: " << n << endl);

  // rotate on z-y plane
  alpha = atan(-n->at(1)/n->at(2));
  DoLog(1) && (Log() << Verbose(1) << "Z-Y-angle: " << alpha << " ... ");
  for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
    tmp = (*iter)->x[1];
    (*iter)->x[1] =  cos(alpha) * tmp + sin(alpha) * (*iter)->x[2];
    (*iter)->x[2] = -sin(alpha) * tmp + cos(alpha) * (*iter)->x[2];
    for (int j=0;j<MDSteps;j++) {
      tmp = (*iter)->Trajectory.R.at(j)[1];
      (*iter)->Trajectory.R.at(j)[1] =  cos(alpha) * tmp + sin(alpha) * (*iter)->Trajectory.R.at(j)[2];
      (*iter)->Trajectory.R.at(j)[2] = -sin(alpha) * tmp + cos(alpha) * (*iter)->Trajectory.R.at(j)[2];
    }
  }
  // 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);


  DoLog(1) && (Log() << Verbose(1) << "alignment vector after second rotation: " << n << endl);
  DoLog(0) && (Log() << Verbose(0) << "End of Aligning all atoms." << endl);
};


/** 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)->type == ((struct lsq_params *)params)->type) { // for specific type
      c = (*iter)->x - 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);
};