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

/** Calculates the intersection of the two lines that are both on the same plane.
 * This is taken from Weisstein, Eric W. "Line-Line Intersection." From MathWorld--A Wolfram Web Resource. http://mathworld.wolfram.com/Line-LineIntersection.html
 * \param *out output stream for debugging
 * \param *Line1a first vector of first line
 * \param *Line1b second vector of first line
 * \param *Line2a first vector of second line
 * \param *Line2b second vector of second line
 * \return true - \a this will contain the intersection on return, false - lines are parallel
 */
Vector GetIntersectionOfTwoLinesOnPlane(const Vector &Line1a, const Vector &Line1b, const Vector &Line2a, const Vector &Line2b)
{
  Info FunctionInfo(__func__);

  Vector res;

  auto_ptr<GSLMatrix> M = auto_ptr<GSLMatrix>(new GSLMatrix(4,4));

  M->SetAll(1.);
  for (int i=0;i<3;i++) {
    M->Set(0, i, Line1a[i]);
    M->Set(1, i, Line1b[i]);
    M->Set(2, i, Line2a[i]);
    M->Set(3, i, Line2b[i]);
  }

  //Log() << Verbose(1) << "Coefficent matrix is:" << endl;
  //for (int i=0;i<4;i++) {
  //  for (int j=0;j<4;j++)
  //    cout << "\t" << M->Get(i,j);
  //  cout << endl;
  //}
  if (fabs(M->Determinant()) > MYEPSILON) {
    Log() << Verbose(1) << "Determinant of coefficient matrix is NOT zero." << endl;
    throw LinearDependenceException(__FILE__,__LINE__);
  }

  Log() << Verbose(1) << "INFO: Line1a = " << Line1a << ", Line1b = " << Line1b << ", Line2a = " << Line2a << ", Line2b = " << Line2b << "." << endl;


  // constuct a,b,c
  Vector a = Line1b - Line1a;
  Vector b = Line2b - Line2a;
  Vector c = Line2a - Line1a;
  Vector d = Line2b - Line1b;
  Log() << Verbose(1) << "INFO: a = " << a << ", b = " << b << ", c = " << c << "." << endl;
  if ((a.NormSquared() < MYEPSILON) || (b.NormSquared() < MYEPSILON)) {
   res.Zero();
   Log() << Verbose(1) << "At least one of the lines is ill-defined, i.e. offset equals second vector." << endl;
   throw LinearDependenceException(__FILE__,__LINE__);
  }

  // check for parallelity
  Vector parallel;
  double factor = 0.;
  if (fabs(a.ScalarProduct(b)*a.ScalarProduct(b)/a.NormSquared()/b.NormSquared() - 1.) < MYEPSILON) {
    parallel = Line1a - Line2a;
    factor = parallel.ScalarProduct(a)/a.Norm();
    if ((factor >= -MYEPSILON) && (factor - 1. < MYEPSILON)) {
      res = Line2a;
      Log() << Verbose(1) << "Lines conincide." << endl;
      return res;
    } else {
      parallel = Line1a - Line2b;
      factor = parallel.ScalarProduct(a)/a.Norm();
      if ((factor >= -MYEPSILON) && (factor - 1. < MYEPSILON)) {
        res = Line2b;
        Log() << Verbose(1) << "Lines conincide." << endl;
        return res;
      }
    }
    Log() << Verbose(1) << "Lines are parallel." << endl;
    res.Zero();
    throw LinearDependenceException(__FILE__,__LINE__);
  }

  // obtain s
  double s;
  Vector temp1, temp2;
  temp1 = c;
  temp1.VectorProduct(b);
  temp2 = a;
  temp2.VectorProduct(b);
  Log() << Verbose(1) << "INFO: temp1 = " << temp1 << ", temp2 = " << temp2 << "." << endl;
  if (fabs(temp2.NormSquared()) > MYEPSILON)
    s = temp1.ScalarProduct(temp2)/temp2.NormSquared();
  else
    s = 0.;
  Log() << Verbose(1) << "Factor s is " << temp1.ScalarProduct(temp2) << "/" << temp2.NormSquared() << " = " << s << "." << endl;

  // construct intersection
  res = a;
  res.Scale(s);
  res += Line1a;
  Log() << Verbose(1) << "Intersection is at " << res << "." << endl;

  return res;
};