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

#include "Helpers/MemDebug.hpp"

#include "vector.hpp"
#include "VectorContent.hpp"
#include "verbose.hpp"
#include "World.hpp"
#include "Helpers/Assert.hpp"
#include "Helpers/fast_functions.hpp"
#include "Exceptions/MathException.hpp"

#include <iostream>
#include <gsl/gsl_blas.h>


using namespace std;


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

/** Constructor of class vector.
 */
Vector::Vector()
{
  content = new VectorContent();
};

/**
 * Copy constructor
 */

Vector::Vector(const Vector& src)
{
  content = new VectorContent();
  gsl_vector_memcpy(content->content, src.content->content);
}

/** Constructor of class vector.
 */
Vector::Vector(const double x1, const double x2, const double x3)
{
  content = new VectorContent();
  gsl_vector_set(content->content,0,x1);
  gsl_vector_set(content->content,1,x2);
  gsl_vector_set(content->content,2,x3);
};

Vector::Vector(VectorContent *_content) :
  content(_content)
{}

/**
 * Assignment operator
 */
Vector& Vector::operator=(const Vector& src){
  // check for self assignment
  if(&src!=this){
    gsl_vector_memcpy(content->content, src.content->content);
  }
  return *this;
}

/** Desctructor of class vector.
 */
Vector::~Vector() {
  delete content;
};

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

/** Calculates distance between this and another vector.
 * \param *y array to second vector
 * \return \f$| x - y |\f$
 */
double Vector::distance(const Vector &y) const
{
  return (sqrt(DistanceSquared(y)));
};

Vector Vector::getClosestPoint(const Vector &point) const{
  // the closest point to a single point space is always the single point itself
  return *this;
}

/** Calculates scalar product between this and another vector.
 * \param *y array to second vector
 * \return \f$\langle x, y \rangle\f$
 */
double Vector::ScalarProduct(const Vector &y) const
{
  double res = 0.;
  gsl_blas_ddot(content->content, y.content->content, &res);
  return (res);
};


/** 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 Vector::VectorProduct(const Vector &y)
{
  Vector tmp;
  for(int i=NDIM;i--;)
    tmp[i] = at((i+1)%NDIM)*y[(i+2)%NDIM] - at((i+2)%NDIM)*y[(i+1)%NDIM];
  (*this) = tmp;
};


/** projects this vector onto plane defined by \a *y.
 * \param *y normal vector of plane
 * \return \f$\langle x, y \rangle\f$
 */
void Vector::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.
 * \sa Vector::GetDistanceVectorToPlane()
 * \param *out output stream for debugging
 * \param *PlaneNormal normal of plane
 * \param *PlaneOffset offset of plane
 * \return distance to plane
 */
double Vector::DistanceToSpace(const Space &space) const
{
  return space.distance(*this);
};

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

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

  return helper;
};

/** Calculates norm of this vector.
 * \return \f$|x|\f$
 */
double Vector::Norm() const
{
  return (sqrt(NormSquared()));
};

/** Calculates squared norm of this vector.
 * \return \f$|x|^2\f$
 */
double Vector::NormSquared() const
{
  return (ScalarProduct(*this));
};

/** Normalizes this vector.
 */
void Vector::Normalize()
{
  double factor = Norm();
  (*this) *= 1/factor;
};

/** Zeros all components of this vector.
 */
void Vector::Zero()
{
  at(0)=at(1)=at(2)=0;
};

/** Zeros all components of this vector.
 */
void Vector::One(const double one)
{
  at(0)=at(1)=at(2)=one;
};

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

/** Checks whether vector has length of 1.
 * @return true - vector is normalized, false - vector is not
 */
bool Vector::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 Vector::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 Vector::IsEqualTo(const Vector &a) const
{
  bool status = true;
  for (int i=0;i<NDIM;i++) {
    if (fabs(at(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 Vector::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& Vector::operator[](size_t i){
  ASSERT(i<=NDIM && i>=0,"Vector Index out of Range");
  return *gsl_vector_ptr (content->content, i);
}

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

double& Vector::at(size_t i){
  return (*this)[i];
}

const double& Vector::at(size_t i) const{
  return (*this)[i];
}

VectorContent* Vector::get(){
  return content;
}

/** Compares vector \a to vector \a b component-wise.
 * \param a base vector
 * \param b vector components to add
 * \return a == b
 */
bool Vector::operator==(const Vector& b) const
{
  return IsEqualTo(b);
};

bool Vector::operator!=(const Vector& b) const
{
  return !IsEqualTo(b);
}

/** Sums vector \a to this lhs component-wise.
 * \param a base vector
 * \param b vector components to add
 * \return lhs + a
 */
const Vector& Vector::operator+=(const Vector& b)
{
  this->AddVector(b);
  return *this;
};

/** Subtracts vector \a from this lhs component-wise.
 * \param a base vector
 * \param b vector components to add
 * \return lhs - a
 */
const Vector& Vector::operator-=(const Vector& b)
{
  this->SubtractVector(b);
  return *this;
};

/** factor each component of \a a times a double \a m.
 * \param a base vector
 * \param m factor
 * \return lhs.x[i] * m
 */
const Vector& operator*=(Vector& a, const double m)
{
  a.Scale(m);
  return a;
};

/** Sums two vectors \a  and \b component-wise.
 * \param a first vector
 * \param b second vector
 * \return a + b
 */
Vector const Vector::operator+(const Vector& b) const
{
  Vector x = *this;
  x.AddVector(b);
  return x;
};

/** Subtracts vector \a from \b component-wise.
 * \param a first vector
 * \param b second vector
 * \return a - b
 */
Vector const Vector::operator-(const Vector& b) const
{
  Vector x = *this;
  x.SubtractVector(b);
  return x;
};

/** Factors given vector \a a times \a m.
 * \param a vector
 * \param m factor
 * \return m * a
 */
Vector const operator*(const Vector& a, const double m)
{
  Vector x(a);
  x.Scale(m);
  return x;
};

/** Factors given vector \a a times \a m.
 * \param m factor
 * \param a vector
 * \return m * a
 */
Vector const operator*(const double m, const Vector& a )
{
  Vector x(a);
  x.Scale(m);
  return x;
};

ostream& operator<<(ostream& ost, const Vector& m)
{
  ost << "(";
  for (int i=0;i<NDIM;i++) {
    ost << m[i];
    if (i != 2)
      ost << ",";
  }
  ost << ")";
  return ost;
};


void Vector::ScaleAll(const double *factor)
{
  for (int i=NDIM;i--;)
    at(i) *= factor[i];
};

void Vector::ScaleAll(const Vector &factor){
  gsl_vector_mul(content->content, factor.content->content);
}


void Vector::Scale(const double factor)
{
  gsl_vector_scale(content->content,factor);
};

std::pair<Vector,Vector> Vector::partition(const Vector &rhs) const{
  double factor = ScalarProduct(rhs)/rhs.NormSquared();
  Vector res= factor * rhs;
  return make_pair(res,(*this)-res);
}

std::pair<pointset,Vector> Vector::partition(const pointset &points) const{
  Vector helper = *this;
  pointset res;
  for(pointset::const_iterator iter=points.begin();iter!=points.end();++iter){
    pair<Vector,Vector> currPart = helper.partition(*iter);
    res.push_back(currPart.first);
    helper = currPart.second;
  }
  return make_pair(res,helper);
}

/** Creates this vector as the b y *factors' components scaled linear combination of the given three.
 * this vector = x1*factors[0] + x2* factors[1] + x3*factors[2]
 * \param *x1 first vector
 * \param *x2 second vector
 * \param *x3 third vector
 * \param *factors three-component vector with the factor for each given vector
 */
void Vector::LinearCombinationOfVectors(const Vector &x1, const Vector &x2, const Vector &x3, const double * const factors)
{
  (*this) = (factors[0]*x1) +
            (factors[1]*x2) +
            (factors[2]*x3);
};

/** Calculates orthonormal vector to one given vectors.
 * 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 Vector::MakeNormalTo(const Vector &y1)
{
  bool result = false;
  double factor = y1.ScalarProduct(*this)/y1.NormSquared();
  Vector x1 = factor * y1;
  SubtractVector(x1);
  for (int i=NDIM;i--;)
    result = result || (fabs(at(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 Vector::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;

  // in two component-systems we need to find the one position that is zero
  int zeroPos = -1;
  // find two components != 0
  for (j=0;j<NDIM;j++){
    if (fabs(GivenVector[j]) > MYEPSILON)
      Components[Last++] = j;
    else
      // this our zero Position
      zeroPos = j;
  }

  switch(Last) {
    case 3:  // threecomponent system
      // the position of the zero is arbitrary in three component systems
      zeroPos = Components[2];
    case 2:  // two component system
      norm = sqrt(1./(GivenVector[Components[1]]*GivenVector[Components[1]]) + 1./(GivenVector[Components[0]]*GivenVector[Components[0]]));
      at(zeroPos) = 0.;
      // in skp both remaining parts shall become zero but with opposite sign and third is zero
      at(Components[1]) = -1./GivenVector[Components[1]] / norm;
      at(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
      at((Components[0]+2)%NDIM) = 0.;
      at((Components[0]+1)%NDIM) = 1.;
      at(Components[0]) = 0.;
      return true;
      break;
    default:
      return false;
  }
};

/** Adds vector \a *y componentwise.
 * \param *y vector
 */
void Vector::AddVector(const Vector &y)
{
  gsl_vector_add(content->content, y.content->content);
}

/** Adds vector \a *y componentwise.
 * \param *y vector
 */
void Vector::SubtractVector(const Vector &y)
{
  gsl_vector_sub(content->content, y.content->content);
}


// some comonly used vectors
const Vector zeroVec(0,0,0);
const Vector e1(1,0,0);
const Vector e2(0,1,0);
const Vector e3(0,0,1);