/** \file vector.cpp
 *
 * Function implementations for the class vector.
 *
 */


#include "defs.hpp"
#include "gslmatrix.hpp"
#include "leastsquaremin.hpp"
#include "memoryallocator.hpp"
#include "SingleVector.hpp"
#include "Helpers/fast_functions.hpp"
#include "Helpers/Assert.hpp"
#include "Plane.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>

#include <iostream>

using namespace std;

/************************************ Functions for class vector ************************************/

/** Constructor of class vector.
 */
SingleVector::SingleVector() :
  Vector(Baseconstructor())
{
  x[0] = x[1] = x[2] = 0.;
};

/** Constructor of class vector.
 */
SingleVector::SingleVector(const double x1, const double x2, const double x3) :
  Vector(Baseconstructor())
{
  x[0] = x1;
  x[1] = x2;
  x[2] = x3;
};

/**
 * Copy constructor
 */
SingleVector::SingleVector(const Vector& src) :
  Vector(Baseconstructor())
{
  x[0] = src[0];
  x[1] = src[1];
  x[2] = src[2];
}

SingleVector* SingleVector::clone() const{
  return new SingleVector(x[0],x[1],x[2]);
}

/** Desctructor of class vector.
 */
SingleVector::~SingleVector() {};

/** Calculates square of distance between this and another vector.
 * \param *y array to second vector
 * \return \f$| x - y |^2\f$
 */
double SingleVector::DistanceSquared(const Vector &y) const
{
  double res = 0.;
  for (int i=NDIM;i--;)
    res += (x[i]-y[i])*(x[i]-y[i]);
  return (res);
};

/** Calculates distance between this and another vector in a periodic cell.
 * \param *y array to second vector
 * \param *cell_size 6-dimensional array with (xx, xy, yy, xz, yz, zz) entries specifying the periodic cell
 * \return \f$| x - y |\f$
 */
double SingleVector::PeriodicDistance(const Vector &y, const double * const cell_size) const
{
  double res = Distance(y), tmp, matrix[NDIM*NDIM];
  Vector Shiftedy, TranslationVector;
  int N[NDIM];
  matrix[0] = cell_size[0];
  matrix[1] = cell_size[1];
  matrix[2] = cell_size[3];
  matrix[3] = cell_size[1];
  matrix[4] = cell_size[2];
  matrix[5] = cell_size[4];
  matrix[6] = cell_size[3];
  matrix[7] = cell_size[4];
  matrix[8] = cell_size[5];
  // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells
  for (N[0]=-1;N[0]<=1;N[0]++)
    for (N[1]=-1;N[1]<=1;N[1]++)
      for (N[2]=-1;N[2]<=1;N[2]++) {
        // create the translation vector
        TranslationVector.Zero();
        for (int i=NDIM;i--;)
          TranslationVector[i] = (double)N[i];
        TranslationVector.MatrixMultiplication(matrix);
        // add onto the original vector to compare with
        Shiftedy = y + TranslationVector;
        // get distance and compare with minimum so far
        tmp = Distance(Shiftedy);
        if (tmp < res) res = tmp;
      }
  return (res);
};

/** Calculates distance between this and another vector in a periodic cell.
 * \param *y array to second vector
 * \param *cell_size 6-dimensional array with (xx, xy, yy, xz, yz, zz) entries specifying the periodic cell
 * \return \f$| x - y |^2\f$
 */
double SingleVector::PeriodicDistanceSquared(const Vector &y, const double * const cell_size) const
{
  double res = DistanceSquared(y), tmp, matrix[NDIM*NDIM];
  Vector Shiftedy, TranslationVector;
  int N[NDIM];
  matrix[0] = cell_size[0];
  matrix[1] = cell_size[1];
  matrix[2] = cell_size[3];
  matrix[3] = cell_size[1];
  matrix[4] = cell_size[2];
  matrix[5] = cell_size[4];
  matrix[6] = cell_size[3];
  matrix[7] = cell_size[4];
  matrix[8] = cell_size[5];
  // in order to check the periodic distance, translate one of the vectors into each of the 27 neighbouring cells
  for (N[0]=-1;N[0]<=1;N[0]++)
    for (N[1]=-1;N[1]<=1;N[1]++)
      for (N[2]=-1;N[2]<=1;N[2]++) {
        // create the translation vector
        TranslationVector.Zero();
        for (int i=NDIM;i--;)
          TranslationVector[i] = (double)N[i];
        TranslationVector.MatrixMultiplication(matrix);
        // add onto the original vector to compare with
        Shiftedy = y + TranslationVector;
        // get distance and compare with minimum so far
        tmp = DistanceSquared(Shiftedy);
        if (tmp < res) res = tmp;
      }
  return (res);
};

/** Keeps the vector in a periodic cell, defined by the symmetric \a *matrix.
 * \param *out ofstream for debugging messages
 * Tries to translate a vector into each adjacent neighbouring cell.
 */
void SingleVector::KeepPeriodic(const double * const matrix)
{
//  int N[NDIM];
//  bool flag = false;
  //vector Shifted, TranslationVector;
//  Log() << Verbose(1) << "Begin of KeepPeriodic." << endl;
//  Log() << Verbose(2) << "Vector is: ";
//  Output(out);
//  Log() << Verbose(0) << endl;
  SingleVector TestVector(*this);
  TestVector.InverseMatrixMultiplication(matrix);
  for(int i=NDIM;i--;) { // correct periodically
    if (TestVector[i] < 0) {  // get every coefficient into the interval [0,1)
      TestVector[i] += ceil(TestVector[i]);
    } else {
      TestVector[i] -= floor(TestVector[i]);
    }
  }
  TestVector.MatrixMultiplication(matrix);
  CopyVector(TestVector);
//  Log() << Verbose(2) << "New corrected vector is: ";
//  Output(out);
//  Log() << Verbose(0) << endl;
//  Log() << Verbose(1) << "End of KeepPeriodic." << endl;
};

/** Calculates scalar product between this and another vector.
 * \param *y array to second vector
 * \return \f$\langle x, y \rangle\f$
 */
double SingleVector::ScalarProduct(const Vector &y) const
{
  double res = 0.;
  for (int i=NDIM;i--;)
    res += x[i]*y[i];
  return (res);
};


void SingleVector::AddVector(const Vector &y) {
  for(int i=NDIM;i--;)
    x[i] += y[i];
}

void SingleVector::SubtractVector(const Vector &y){
  for(int i=NDIM;i--;)
    x[i] -= y[i];
}

/** Calculates VectorProduct between this and another vector.
 *  -# returns the Product in place of vector from which it was initiated
 *  -# ATTENTION: Only three dim.
 *  \param *y array to vector with which to calculate crossproduct
 *  \return \f$ x \times y \f&
 */
void SingleVector::VectorProduct(const Vector &y)
{
  SingleVector tmp;
  tmp[0] = x[1]* (y[2]) - x[2]* (y[1]);
  tmp[1] = x[2]* (y[0]) - x[0]* (y[2]);
  tmp[2] = x[0]* (y[1]) - x[1]* (y[0]);
  CopyVector(tmp);
};


/** projects this vector onto plane defined by \a *y.
 * \param *y normal vector of plane
 * \return \f$\langle x, y \rangle\f$
 */
void SingleVector::ProjectOntoPlane(const Vector &y)
{
  Vector tmp;
  tmp = y;
  tmp.Normalize();
  tmp.Scale(ScalarProduct(tmp));
  *this -= tmp;
};

/** Calculates the minimum distance of this vector to the plane.
 * \param *out output stream for debugging
 * \param *PlaneNormal normal of plane
 * \param *PlaneOffset offset of plane
 * \return distance to plane
 */
double SingleVector::DistanceToPlane(const Vector &PlaneNormal, const Vector &PlaneOffset) const
{
  // first create part that is orthonormal to PlaneNormal with withdraw
  Vector temp = VecFromRep(this)- PlaneOffset;
  temp.MakeNormalTo(PlaneNormal);
  temp.Scale(-1.);
  // then add connecting vector from plane to point
  temp += VecFromRep(this)-PlaneOffset;
  double sign = temp.ScalarProduct(PlaneNormal);
  if (fabs(sign) > MYEPSILON)
    sign /= fabs(sign);
  else
    sign = 0.;

  return (temp.Norm()*sign);
};

/** Calculates the projection of a vector onto another \a *y.
 * \param *y array to second vector
 */
void SingleVector::ProjectIt(const Vector &y)
{
  Vector helper = y;
  helper.Scale(-(ScalarProduct(y)));
  AddVector(helper);
};

/** Calculates the projection of a vector onto another \a *y.
 * \param *y array to second vector
 * \return Vector
 */
Vector SingleVector::Projection(const Vector &y) const
{
  Vector helper = y;
  helper.Scale((ScalarProduct(y)/y.NormSquared()));

  return helper;
};

/** Checks whether vector has all components zero.
 * @return true - vector is zero, false - vector is not
 */
bool SingleVector::IsZero() const
{
  return (fabs(x[0])+fabs(x[1])+fabs(x[2]) < MYEPSILON);
};

/** Checks whether vector has length of 1.
 * @return true - vector is normalized, false - vector is not
 */
bool SingleVector::IsOne() const
{
  return (fabs(Norm() - 1.) < MYEPSILON);
};

/** Checks whether vector is normal to \a *normal.
 * @return true - vector is normalized, false - vector is not
 */
bool SingleVector::IsNormalTo(const Vector &normal) const
{
  if (ScalarProduct(normal) < MYEPSILON)
    return true;
  else
    return false;
};

/** Checks whether vector is normal to \a *normal.
 * @return true - vector is normalized, false - vector is not
 */
bool SingleVector::IsEqualTo(const Vector &a) const
{
  bool status = true;
  for (int i=0;i<NDIM;i++) {
    if (fabs(x[i] - a[i]) > MYEPSILON)
      status = false;
  }
  return status;
};

/** Calculates the angle between this and another vector.
 * \param *y array to second vector
 * \return \f$\acos\bigl(frac{\langle x, y \rangle}{|x||y|}\bigr)\f$
 */
double SingleVector::Angle(const Vector &y) const
{
  double norm1 = Norm(), norm2 = y.Norm();
  double angle = -1;
  if ((fabs(norm1) > MYEPSILON) && (fabs(norm2) > MYEPSILON))
    angle = this->ScalarProduct(y)/norm1/norm2;
  // -1-MYEPSILON occured due to numerical imprecision, catch ...
  //Log() << Verbose(2) << "INFO: acos(-1) = " << acos(-1) << ", acos(-1+MYEPSILON) = " << acos(-1+MYEPSILON) << ", acos(-1-MYEPSILON) = " << acos(-1-MYEPSILON) << "." << endl;
  if (angle < -1)
    angle = -1;
  if (angle > 1)
    angle = 1;
  return acos(angle);
};


double& SingleVector::operator[](size_t i){
  ASSERT(i<=NDIM && i>=0,"Vector Index out of Range");
  return x[i];
}

const double& SingleVector::operator[](size_t i) const{
  ASSERT(i<=NDIM && i>=0,"Vector Index out of Range");
  return x[i];
}

double* SingleVector::get(){
  return x;
}
/** Scales each atom coordinate by an individual \a factor.
 * \param *factor pointer to scaling factor
 */
void SingleVector::ScaleAll(const double *factor)
{
  for (int i=NDIM;i--;)
    x[i] *= factor[i];
};

void SingleVector::Scale(const double factor)
{
  for (int i=NDIM;i--;)
    x[i] *= factor;
};

/** Given a box by its matrix \a *M and its inverse *Minv the vector is made to point within that box.
 * \param *M matrix of box
 * \param *Minv inverse matrix
 */
void SingleVector::WrapPeriodically(const double * const M, const double * const Minv)
{
  MatrixMultiplication(Minv);
  // truncate to [0,1] for each axis
  for (int i=0;i<NDIM;i++) {
    x[i] += 0.5;  // set to center of box
    while (x[i] >= 1.)
      x[i] -= 1.;
    while (x[i] < 0.)
      x[i] += 1.;
  }
  MatrixMultiplication(M);
};

/** Do a matrix multiplication.
 * \param *matrix NDIM_NDIM array
 */
void SingleVector::MatrixMultiplication(const double * const M)
{
  SingleVector C;
  // do the matrix multiplication
  C[0] = M[0]*x[0]+M[3]*x[1]+M[6]*x[2];
  C[1] = M[1]*x[0]+M[4]*x[1]+M[7]*x[2];
  C[2] = M[2]*x[0]+M[5]*x[1]+M[8]*x[2];

  CopyVector(C);
};

/** Do a matrix multiplication with the \a *A' inverse.
 * \param *matrix NDIM_NDIM array
 */
bool SingleVector::InverseMatrixMultiplication(const double * const A)
{
  SingleVector C;
  double B[NDIM*NDIM];
  double detA = RDET3(A);
  double detAReci;

  // calculate the inverse B
  if (fabs(detA) > MYEPSILON) {;  // RDET3(A) yields precisely zero if A irregular
    detAReci = 1./detA;
    B[0] =  detAReci*RDET2(A[4],A[5],A[7],A[8]);    // A_11
    B[1] = -detAReci*RDET2(A[1],A[2],A[7],A[8]);    // A_12
    B[2] =  detAReci*RDET2(A[1],A[2],A[4],A[5]);    // A_13
    B[3] = -detAReci*RDET2(A[3],A[5],A[6],A[8]);    // A_21
    B[4] =  detAReci*RDET2(A[0],A[2],A[6],A[8]);    // A_22
    B[5] = -detAReci*RDET2(A[0],A[2],A[3],A[5]);    // A_23
    B[6] =  detAReci*RDET2(A[3],A[4],A[6],A[7]);    // A_31
    B[7] = -detAReci*RDET2(A[0],A[1],A[6],A[7]);    // A_32
    B[8] =  detAReci*RDET2(A[0],A[1],A[3],A[4]);    // A_33

    // do the matrix multiplication
    C[0] = B[0]*x[0]+B[3]*x[1]+B[6]*x[2];
    C[1] = B[1]*x[0]+B[4]*x[1]+B[7]*x[2];
    C[2] = B[2]*x[0]+B[5]*x[1]+B[8]*x[2];

    CopyVector(C);
    return true;
  } else {
    return false;
  }
};


/** Mirrors atom against a given plane.
 * \param n[] normal vector of mirror plane.
 */
void SingleVector::Mirror(const Vector &n)
{
  double projection;
  projection = ScalarProduct(n)/n.NormSquared();    // remove constancy from n (keep as logical one)
  // withdraw projected vector twice from original one
  for (int i=NDIM;i--;)
    x[i] -= 2.*projection*n[i];
};


/** Calculates orthonormal vector to one given vector.
 * Just subtracts the projection onto the given vector from this vector.
 * The removed part of the vector is Vector::Projection()
 * \param *x1 vector
 * \return true - success, false - vector is zero
 */
bool SingleVector::MakeNormalTo(const Vector &y1)
{
  bool result = false;
  double factor = y1.ScalarProduct(*this)/y1.NormSquared();
  Vector x1;
  x1 = factor * y1;
  SubtractVector(x1);
  for (int i=NDIM;i--;)
    result = result || (fabs(x[i]) > MYEPSILON);

  return result;
};

/** Creates this vector as one of the possible orthonormal ones to the given one.
 * Just scan how many components of given *vector are unequal to zero and
 * try to get the skp of both to be zero accordingly.
 * \param *vector given vector
 * \return true - success, false - failure (null vector given)
 */
bool SingleVector::GetOneNormalVector(const Vector &GivenVector)
{
  int Components[NDIM]; // contains indices of non-zero components
  int Last = 0;   // count the number of non-zero entries in vector
  int j;  // loop variables
  double norm;

  for (j=NDIM;j--;)
    Components[j] = -1;
  // find two components != 0
  for (j=0;j<NDIM;j++)
    if (fabs(GivenVector[j]) > MYEPSILON)
      Components[Last++] = j;

  switch(Last) {
    case 3:  // threecomponent system
    case 2:  // two component system
      norm = sqrt(1./(GivenVector[Components[1]]*GivenVector[Components[1]]) + 1./(GivenVector[Components[0]]*GivenVector[Components[0]]));
      x[Components[2]] = 0.;
      // in skp both remaining parts shall become zero but with opposite sign and third is zero
      x[Components[1]] = -1./GivenVector[Components[1]] / norm;
      x[Components[0]] = 1./GivenVector[Components[0]] / norm;
      return true;
      break;
    case 1: // one component system
      // set sole non-zero component to 0, and one of the other zero component pendants to 1
      x[(Components[0]+2)%NDIM] = 0.;
      x[(Components[0]+1)%NDIM] = 1.;
      x[Components[0]] = 0.;
      return true;
      break;
    default:
      return false;
  }
};

/**
 * Checks whether this vector is within the parallelepiped defined by the given three vectors and
 * their offset.
 *
 * @param offest for the origin of the parallelepiped
 * @param three vectors forming the matrix that defines the shape of the parallelpiped
 */
bool SingleVector::IsInParallelepiped(const Vector &offset, const double * const parallelepiped) const
{
  Vector a = VecFromRep(this);
  a -= offset;
  a.InverseMatrixMultiplication(parallelepiped);
  bool isInside = true;

  for (int i=NDIM;i--;)
    isInside = isInside && ((a[i] <= 1) && (a[i] >= 0));

  return isInside;
}


void SingleVector::CopyVector(SingleVector& vec) {
  for(int i =NDIM; i--;)
    x[i]=vec.x[i];
}

bool SingleVector::isBaseClass() const{
  return false;
}