/*
 * vector_ops.cpp
 *
 *  Created on: Apr 1, 2010
 *      Author: crueger
 */

#include "vector.hpp"
#include "Plane.hpp"
#include "log.hpp"
#include "verbose.hpp"
#include "gslmatrix.hpp"
#include "leastsquaremin.hpp"
#include "info.hpp"
#include "Helpers/fast_functions.hpp"
#include "Exceptions/LinearDependenceException.hpp"
#include "Exceptions/SkewException.hpp"

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>

/**
 * !@file
 * These files defines several common operation on vectors that should not
 * become part of the main vector class, because they are either to complex
 * or need methods from other subsystems that should not be moved to
 * the LinAlg-Subsystem
 */

/** Creates a new vector as the one with least square distance to a given set of \a vectors.
 * \param *vectors set of vectors
 * \param num number of vectors
 * \return true if success, false if failed due to linear dependency
 */
bool LSQdistance(Vector &res,const Vector **vectors, int num)
{
  int j;

  for (j=0;j<num;j++) {
    Log() << Verbose(1) << j << "th atom's vector: " << vectors[j] << endl;
  }

  int np = 3;
  struct LSQ_params par;

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

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

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

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

   /* Starting point */
   par.vectors = vectors;
   par.num = num;

   for (i=NDIM;i--;)
    gsl_vector_set(y, i, (vectors[0]->at(i) - vectors[1]->at(i))/2.);

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

   s = gsl_multimin_fminimizer_alloc (T, np);
   gsl_multimin_fminimizer_set (s, &minex_func, y, 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=(size_t)np;i--;)
    res[i] = gsl_vector_get(s->x, i);
   gsl_vector_free(y);
   gsl_vector_free(ss);
   gsl_multimin_fminimizer_free (s);

  return true;
};

/** Rotates the vector relative to the origin around the axis given by \a *axis by an angle of \a alpha.
 * \param *axis rotation axis
 * \param alpha rotation angle in radian
 */
Vector RotateVector(const Vector &vec,const Vector &axis, const double alpha)
{
  Vector a,y;
  Vector res;
  // normalise this vector with respect to axis
  a = vec;
  a.ProjectOntoPlane(axis);
  // construct normal vector
  try {
    y = Plane(axis,a,0).getNormal();
  }
  catch (MathException &excp) {
    // The normal vector cannot be created if there is linar dependency.
    // Then the vector to rotate is on the axis and any rotation leads to the vector itself.
    return vec;
  }
  y.Scale(vec.Norm());
  // scale normal vector by sine and this vector by cosine
  y.Scale(sin(alpha));
  a.Scale(cos(alpha));
  res = vec.Projection(axis);
  // add scaled normal vector onto this vector
  res += y;
  // add part in axis direction
  res += a;
  return res;
};