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


#include "defs.hpp"
#include "helpers.hpp"
#include "memoryallocator.hpp"
#include "leastsquaremin.hpp"
#include "vector.hpp"
#include "verbose.hpp"

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

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

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

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

/** 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 += (x[i]-y->x[i])*(x[i]-y->x[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
{
  double res = 0.;
  for (int i=NDIM;i--;)
    res += (x[i]-y->x[i])*(x[i]-y->x[i]);
  return (sqrt(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 Vector::PeriodicDistance(const Vector *y, const double *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.x[i] = (double)N[i];
        TranslationVector.MatrixMultiplication(matrix);
        // add onto the original vector to compare with
        Shiftedy.CopyVector(y);
        Shiftedy.AddVector(&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 Vector::PeriodicDistanceSquared(const Vector *y, const double *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.x[i] = (double)N[i];
        TranslationVector.MatrixMultiplication(matrix);
        // add onto the original vector to compare with
        Shiftedy.CopyVector(y);
        Shiftedy.AddVector(&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 Vector::KeepPeriodic(ofstream *out, double *matrix)
{
//  int N[NDIM];
//  bool flag = false;
  //vector Shifted, TranslationVector;
  Vector TestVector;
//  *out << Verbose(1) << "Begin of KeepPeriodic." << endl;
//  *out << Verbose(2) << "Vector is: ";
//  Output(out);
//  *out << endl;
  TestVector.CopyVector(this);
  TestVector.InverseMatrixMultiplication(matrix);
  for(int i=NDIM;i--;) { // correct periodically
    if (TestVector.x[i] < 0) {  // get every coefficient into the interval [0,1)
      TestVector.x[i] += ceil(TestVector.x[i]);
    } else {
      TestVector.x[i] -= floor(TestVector.x[i]);
    }
  }
  TestVector.MatrixMultiplication(matrix);
  CopyVector(&TestVector);
//  *out << Verbose(2) << "New corrected vector is: ";
//  Output(out);
//  *out << endl;
//  *out << 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 Vector::ScalarProduct(const Vector *y) const
{
  double res = 0.;
  for (int i=NDIM;i--;)
    res += x[i]*y->x[i];
  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;
  tmp.x[0] = x[1]* (y->x[2]) - x[2]* (y->x[1]);
  tmp.x[1] = x[2]* (y->x[0]) - x[0]* (y->x[2]);
  tmp.x[2] = x[0]* (y->x[1]) - x[1]* (y->x[0]);
  this->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 Vector::ProjectOntoPlane(const Vector *y)
{
  Vector tmp;
  tmp.CopyVector(y);
  tmp.Normalize();
  tmp.Scale(ScalarProduct(&tmp));
  this->SubtractVector(&tmp);
};

/** Calculates the intersection point between a line defined by \a *LineVector and \a *LineVector2 and a plane defined by \a *Normal and \a *PlaneOffset.
 * According to [Bronstein] the vectorial plane equation is:
 *   -# \f$\stackrel{r}{\rightarrow} \cdot \stackrel{N}{\rightarrow} + D = 0\f$,
 * where \f$\stackrel{r}{\rightarrow}\f$ is the vector to be testet, \f$\stackrel{N}{\rightarrow}\f$ is the plane's normal vector and
 * \f$D = - \stackrel{a}{\rightarrow} \stackrel{N}{\rightarrow}\f$, the offset with respect to origin, if \f$\stackrel{a}{\rightarrow}\f$,
 * is an offset vector onto the plane. The line is parametrized by \f$\stackrel{x}{\rightarrow} + k \stackrel{t}{\rightarrow}\f$, where
 * \f$\stackrel{x}{\rightarrow}\f$ is the offset and \f$\stackrel{t}{\rightarrow}\f$ the directional vector (NOTE: No need to normalize
 * the latter). Inserting the parametrized form into the plane equation and solving for \f$k\f$, which we insert then into the parametrization
 * of the line yields the intersection point on the plane.
 * \param *out output stream for debugging
 * \param *PlaneNormal Plane's normal vector
 * \param *PlaneOffset Plane's offset vector
 * \param *Origin first vector of line
 * \param *LineVector second vector of line
 * \return true -  \a this contains intersection point on return, false - line is parallel to plane
 */
bool Vector::GetIntersectionWithPlane(ofstream *out, Vector *PlaneNormal, Vector *PlaneOffset, Vector *Origin, Vector *LineVector)
{
  double factor;
  Vector Direction, helper;

  // find intersection of a line defined by Offset and Direction with a  plane defined by triangle
  Direction.CopyVector(LineVector);
  Direction.SubtractVector(Origin);
  Direction.Normalize();
  //*out << Verbose(4) << "INFO: Direction is " << Direction << "." << endl;
  factor = Direction.ScalarProduct(PlaneNormal);
  if (factor < MYEPSILON) { // Uniqueness: line parallel to plane?
    *out << Verbose(2) << "WARNING: Line is parallel to plane, no intersection." << endl;
    return false;
  }
  helper.CopyVector(PlaneOffset);
  helper.SubtractVector(Origin);
  factor = helper.ScalarProduct(PlaneNormal)/factor;
  if (factor < MYEPSILON) { // Origin is in-plane
    //*out << Verbose(2) << "Origin of line is in-plane, simple." << endl;
    CopyVector(Origin);
    return true;
  }
  //factor = Origin->ScalarProduct(PlaneNormal)*(-PlaneOffset->ScalarProduct(PlaneNormal))/(Direction.ScalarProduct(PlaneNormal));
  Direction.Scale(factor);
  CopyVector(Origin);
  //*out << Verbose(4) << "INFO: Scaled direction is " << Direction << "." << endl;
  AddVector(&Direction);

  // test whether resulting vector really is on plane
  helper.CopyVector(this);
  helper.SubtractVector(PlaneOffset);
  if (helper.ScalarProduct(PlaneNormal) < MYEPSILON) {
    //*out << Verbose(2) << "INFO: Intersection at " << *this << " is good." << endl;
    return true;
  } else {
    *out << Verbose(2) << "WARNING: Intersection point " << *this << " is not on plane." << endl;
    return false;
  }
};

/** 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 Vector::DistanceToPlane(ofstream *out, Vector *PlaneNormal, Vector *PlaneOffset)
{
  Vector temp;

  // first create part that is orthonormal to PlaneNormal with withdraw
  temp.CopyVector(this);
  temp.SubtractVector(PlaneOffset);
  temp.MakeNormalVector(PlaneNormal);
  temp.Scale(-1.);
  // then add connecting vector from plane to point
  temp.AddVector(this);
  temp.SubtractVector(PlaneOffset);

  return temp.Norm();
};

/** Calculates the intersection of the two lines that are both on the same plane.
 * We construct auxiliary plane with its vector normal to one line direction and the PlaneNormal, then a vector
 * from the first line's offset onto the plane. Finally, scale by factor is 1/cos(angle(line1,line2..)) = 1/SP(...), and
 * project onto the first line's direction and add its offset.
 * \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
 * \param *PlaneNormal normal of plane, is supplemental/arbitrary
 * \return true - \a this will contain the intersection on return, false - lines are parallel
 */
bool Vector::GetIntersectionOfTwoLinesOnPlane(ofstream *out, Vector *Line1a, Vector *Line1b, Vector *Line2a, Vector *Line2b, const Vector *PlaneNormal)
{
  bool result = true;
  Vector Direction, OtherDirection;
  Vector AuxiliaryNormal;
  Vector Distance;
  const Vector *Normal = NULL;
  Vector *ConstructedNormal = NULL;
  bool FreeNormal = false;

  // construct both direction vectors
  Zero();
  Direction.CopyVector(Line1b);
  Direction.SubtractVector(Line1a);
  if (Direction.IsZero())
    return false;
  OtherDirection.CopyVector(Line2b);
  OtherDirection.SubtractVector(Line2a);
  if (OtherDirection.IsZero())
    return false;

  Direction.Normalize();
  OtherDirection.Normalize();

  //*out << Verbose(4) << "INFO: Normalized Direction " << Direction << " and OtherDirection " << OtherDirection << "." << endl;

  if (fabs(OtherDirection.ScalarProduct(&Direction) - 1.) < MYEPSILON) { // lines are parallel
    if ((Line1a == Line2a) || (Line1a == Line2b))
      CopyVector(Line1a);
    else if ((Line1b == Line2b) || (Line1b == Line2b))
        CopyVector(Line1b);
    else
      return false;
    *out << Verbose(4) << "INFO: Intersection is " << *this << "." << endl;
    return true;
  } else {
    // check whether we have a plane normal vector
    if (PlaneNormal == NULL) {
      ConstructedNormal = new Vector;
      ConstructedNormal->MakeNormalVector(&Direction, &OtherDirection);
      Normal = ConstructedNormal;
      FreeNormal = true;
    } else
      Normal = PlaneNormal;

    AuxiliaryNormal.MakeNormalVector(&OtherDirection, Normal);
    //*out << Verbose(4) << "INFO: PlaneNormal is " << *Normal << " and AuxiliaryNormal " << AuxiliaryNormal << "." << endl;

    Distance.CopyVector(Line2a);
    Distance.SubtractVector(Line1a);
    //*out << Verbose(4) << "INFO: Distance is " << Distance << "." << endl;
    if (Distance.IsZero()) {
      // offsets are equal, match found
      CopyVector(Line1a);
      result = true;
    } else {
      CopyVector(Distance.Projection(&AuxiliaryNormal));
      //*out << Verbose(4) << "INFO: Projected Distance is " << *this << "." << endl;
      double factor = Direction.ScalarProduct(&AuxiliaryNormal);
      //*out << Verbose(4) << "INFO: Scaling factor is " << factor << "." << endl;
      Scale(1./(factor*factor));
      //*out << Verbose(4) << "INFO: Scaled Distance is " << *this << "." << endl;
      CopyVector(Projection(&Direction));
      //*out << Verbose(4) << "INFO: Distance, projected into Direction, is " << *this << "." << endl;
      if (this->IsZero())
        result = false;
      else
        result = true;
      AddVector(Line1a);
    }

    if (FreeNormal)
      delete(ConstructedNormal);
  }
  if (result)
    *out << Verbose(4) << "INFO: Intersection is " << *this << "." << endl;

  return result;
};

/** Calculates the projection of a vector onto another \a *y.
 * \param *y array to second vector
 */
void Vector::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 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
{
  double res = 0.;
  for (int i=NDIM;i--;)
    res += this->x[i]*this->x[i];
  return (sqrt(res));
};

/** 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 res = 0.;
  for (int i=NDIM;i--;)
    res += this->x[i]*this->x[i];
  if (fabs(res) > MYEPSILON)
    res = 1./sqrt(res);
  Scale(&res);
};

/** Zeros all components of this vector.
 */
void Vector::Zero()
{
  for (int i=NDIM;i--;)
    this->x[i] = 0.;
};

/** Zeros all components of this vector.
 */
void Vector::One(double one)
{
  for (int i=NDIM;i--;)
    this->x[i] = one;
};

/** Initialises all components of this vector.
 */
void Vector::Init(double x1, double x2, double x3)
{
  x[0] = x1;
  x[1] = x2;
  x[2] = x3;
};

/** Checks whether vector has all components zero.
 * @return true - vector is zero, false - vector is not
 */
bool Vector::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 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;
};

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

/** 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
 */
void Vector::RotateVector(const Vector *axis, const double alpha)
{
  Vector a,y;
  // normalise this vector with respect to axis
  a.CopyVector(this);
  a.ProjectOntoPlane(axis);
  // construct normal vector
  bool rotatable = y.MakeNormalVector(axis,&a);
  // 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.
  if (!rotatable) {
    return;
  }
  y.Scale(Norm());
  // scale normal vector by sine and this vector by cosine
  y.Scale(sin(alpha));
  a.Scale(cos(alpha));
  CopyVector(Projection(axis));
  // add scaled normal vector onto this vector
  AddVector(&y);
  // add part in axis direction
  AddVector(&a);
};

/** Compares vector \a to vector \a b component-wise.
 * \param a base vector
 * \param b vector components to add
 * \return a == b
 */
bool operator==(const Vector& a, const Vector& b)
{
  bool status = true;
  for (int i=0;i<NDIM;i++)
    status = status && (fabs(a.x[i] - b.x[i]) < MYEPSILON);
  return status;
};

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

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

/** factor each component of \a a times a double \a m.
 * \param a base vector
 * \param m factor
 * \return lhs.x[i] * m
 */
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& operator+(const Vector& a, const Vector& b)
{
  Vector *x = new Vector;
  x->CopyVector(&a);
  x->AddVector(&b);
  return *x;
};

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

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

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

/** Prints a 3dim vector.
 * prints no end of line.
 * \param *out output stream
 */
bool Vector::Output(ofstream *out) const
{
  if (out != NULL) {
    *out << "(";
    for (int i=0;i<NDIM;i++) {
      *out << x[i];
      if (i != 2)
        *out << ",";
    }
    *out << ")";
    return true;
  } else
    return false;
};

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

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

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

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

/** Translate atom by given vector.
 * \param trans[] translation vector.
 */
void Vector::Translate(const Vector *trans)
{
  for (int i=NDIM;i--;)
    x[i] += trans->x[i];
};

/** 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 Vector::WrapPeriodically(const double *M, const double *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 Vector::MatrixMultiplication(const double *M)
{
  Vector C;
  // do the matrix multiplication
  C.x[0] = M[0]*x[0]+M[3]*x[1]+M[6]*x[2];
  C.x[1] = M[1]*x[0]+M[4]*x[1]+M[7]*x[2];
  C.x[2] = M[2]*x[0]+M[5]*x[1]+M[8]*x[2];
  // transfer the result into this
  for (int i=NDIM;i--;)
    x[i] = C.x[i];
};

/** Calculate the inverse of a 3x3 matrix.
 * \param *matrix NDIM_NDIM array
 */
double * Vector::InverseMatrix(double *A)
{
  double *B = Malloc<double>(NDIM * NDIM, "Vector::InverseMatrix: *B");
  double detA = RDET3(A);
  double detAReci;

  for (int i=0;i<NDIM*NDIM;++i)
    B[i] = 0.;
  // 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
  }
  return B;
};

/** Do a matrix multiplication with the \a *A' inverse.
 * \param *matrix NDIM_NDIM array
 */
void Vector::InverseMatrixMultiplication(const double *A)
{
  Vector 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.x[0] = B[0]*x[0]+B[3]*x[1]+B[6]*x[2];
    C.x[1] = B[1]*x[0]+B[4]*x[1]+B[7]*x[2];
    C.x[2] = B[2]*x[0]+B[5]*x[1]+B[8]*x[2];
    // transfer the result into this
    for (int i=NDIM;i--;)
      x[i] = C.x[i];
  } else {
    cerr << "ERROR: inverse of matrix does not exists: det A = " << detA << "." << endl;
  }
};


/** 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, double *factors)
{
  for(int i=NDIM;i--;)
    x[i] = factors[0]*x1->x[i] + factors[1]*x2->x[i] + factors[2]*x3->x[i];
};

/** Mirrors atom against a given plane.
 * \param n[] normal vector of mirror plane.
 */
void Vector::Mirror(const Vector *n)
{
  double projection;
  projection = ScalarProduct(n)/n->ScalarProduct(n);    // remove constancy from n (keep as logical one)
  // withdraw projected vector twice from original one
  cout << Verbose(1) << "Vector: ";
  Output((ofstream *)&cout);
  cout << "\t";
  for (int i=NDIM;i--;)
    x[i] -= 2.*projection*n->x[i];
  cout << "Projected vector: ";
  Output((ofstream *)&cout);
  cout << endl;
};

/** Calculates normal vector for three given vectors (being three points in space).
 * Makes this vector orthonormal to the three given points, making up a place in 3d space.
 * \param *y1 first vector
 * \param *y2 second vector
 * \param *y3 third vector
 * \return true - success, vectors are linear independent, false - failure due to linear dependency
 */
bool Vector::MakeNormalVector(const Vector *y1, const Vector *y2, const Vector *y3)
{
  Vector x1, x2;

  x1.CopyVector(y1);
  x1.SubtractVector(y2);
  x2.CopyVector(y3);
  x2.SubtractVector(y2);
  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
    cout << Verbose(4) << "WARNING: Given vectors are linear dependent." << endl;
    return false;
  }
//  cout << Verbose(4) << "relative, first plane coordinates:";
//  x1.Output((ofstream *)&cout);
//  cout << endl;
//  cout << Verbose(4) << "second plane coordinates:";
//  x2.Output((ofstream *)&cout);
//  cout << endl;

  this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
  this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
  this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
  Normalize();

  return true;
};


/** Calculates orthonormal vector to two given vectors.
 * Makes this vector orthonormal to two given vectors. This is very similar to the other
 * vector::MakeNormalVector(), only there three points whereas here two difference
 * vectors are given.
 * \param *x1 first vector
 * \param *x2 second vector
 * \return true - success, vectors are linear independent, false - failure due to linear dependency
 */
bool Vector::MakeNormalVector(const Vector *y1, const Vector *y2)
{
  Vector x1,x2;
  x1.CopyVector(y1);
  x2.CopyVector(y2);
  Zero();
  if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(&x2)) < MYEPSILON)) {
    cout << Verbose(4) << "WARNING: Given vectors are linear dependent." << endl;
    return false;
  }
//  cout << Verbose(4) << "relative, first plane coordinates:";
//  x1.Output((ofstream *)&cout);
//  cout << endl;
//  cout << Verbose(4) << "second plane coordinates:";
//  x2.Output((ofstream *)&cout);
//  cout << endl;

  this->x[0] = (x1.x[1]*x2.x[2] - x1.x[2]*x2.x[1]);
  this->x[1] = (x1.x[2]*x2.x[0] - x1.x[0]*x2.x[2]);
  this->x[2] = (x1.x[0]*x2.x[1] - x1.x[1]*x2.x[0]);
  Normalize();

  return true;
};

/** 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::MakeNormalVector(const Vector *y1)
{
  bool result = false;
  double factor = y1->ScalarProduct(this)/y1->NormSquared();
  Vector x1;
  x1.CopyVector(y1);
  x1.Scale(factor);
  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 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;

  cout << Verbose(4);
  GivenVector->Output((ofstream *)&cout);
  cout << endl;
  for (j=NDIM;j--;)
    Components[j] = -1;
  // find two components != 0
  for (j=0;j<NDIM;j++)
    if (fabs(GivenVector->x[j]) > MYEPSILON)
      Components[Last++] = j;
  cout << Verbose(4) << Last << " Components != 0: (" << Components[0] << "," << Components[1] << "," << Components[2] << ")" << endl;

  switch(Last) {
    case 3:  // threecomponent system
    case 2:  // two component system
      norm = sqrt(1./(GivenVector->x[Components[1]]*GivenVector->x[Components[1]]) + 1./(GivenVector->x[Components[0]]*GivenVector->x[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->x[Components[1]] / norm;
      x[Components[0]] = 1./GivenVector->x[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;
  }
};

/** Determines parameter needed to multiply this vector to obtain intersection point with plane defined by \a *A, \a *B and \a *C.
 * \param *A first plane vector
 * \param *B second plane vector
 * \param *C third plane vector
 * \return scaling parameter for this vector
 */
double Vector::CutsPlaneAt(Vector *A, Vector *B, Vector *C)
{
//  cout << Verbose(3) << "For comparison: ";
//  cout << "A " << A->Projection(this) << "\t";
//  cout << "B " << B->Projection(this) << "\t";
//  cout << "C " << C->Projection(this) << "\t";
//  cout << endl;
  return A->ScalarProduct(this);
};

/** 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 Vector::LSQdistance(Vector **vectors, int num)
{
  int j;

  for (j=0;j<num;j++) {
    cout << Verbose(1) << j << "th atom's vector: ";
    (vectors[j])->Output((ofstream *)&cout);
    cout << 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]->x[i] - vectors[1]->x[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--;)
    this->x[i] = gsl_vector_get(s->x, i);
   gsl_vector_free(y);
   gsl_vector_free(ss);
   gsl_multimin_fminimizer_free (s);

  return true;
};

/** Adds vector \a *y componentwise.
 * \param *y vector
 */
void Vector::AddVector(const Vector *y)
{
  for (int i=NDIM;i--;)
    this->x[i] += y->x[i];
}

/** Adds vector \a *y componentwise.
 * \param *y vector
 */
void Vector::SubtractVector(const Vector *y)
{
  for (int i=NDIM;i--;)
    this->x[i] -= y->x[i];
}

/** Copy vector \a *y componentwise.
 * \param *y vector
 */
void Vector::CopyVector(const Vector *y)
{
  for (int i=NDIM;i--;)
    this->x[i] = y->x[i];
}

/** Copy vector \a y componentwise.
 * \param y vector
 */
void Vector::CopyVector(const Vector y)
{
  for (int i=NDIM;i--;)
    this->x[i] = y.x[i];
}


/** Asks for position, checks for boundary.
 * \param cell_size unitary size of cubic cell, coordinates must be within 0...cell_size
 * \param check whether bounds shall be checked (true) or not (false)
 */
void Vector::AskPosition(double *cell_size, bool check)
{
  char coords[3] = {'x','y','z'};
  int j = -1;
  for (int i=0;i<3;i++) {
    j += i+1;
    do {
      cout << Verbose(0) << coords[i] << "[0.." << cell_size[j] << "]: ";
      cin >> x[i];
    } while (((x[i] < 0) || (x[i] >= cell_size[j])) && (check));
  }
};

/** Solves a vectorial system consisting of two orthogonal statements and a norm statement.
 * This is linear system of equations to be solved, however of the three given (skp of this vector\
 * with either of the three hast to be zero) only two are linear independent. The third equation
 * is that the vector should be of magnitude 1 (orthonormal). This all leads to a case-based solution
 * where very often it has to be checked whether a certain value is zero or not and thus forked into
 * another case.
 * \param *x1 first vector
 * \param *x2 second vector
 * \param *y third vector
 * \param alpha first angle
 * \param beta second angle
 * \param c norm of final vector
 * \return a vector with \f$\langle x1,x2 \rangle=A\f$, \f$\langle x1,y \rangle = B\f$ and with norm \a c.
 * \bug this is not yet working properly
 */
bool Vector::SolveSystem(Vector *x1, Vector *x2, Vector *y, double alpha, double beta, double c)
{
  double D1,D2,D3,E1,E2,F1,F2,F3,p,q=0., A, B1, B2, C;
  double ang; // angle on testing
  double sign[3];
  int i,j,k;
  A = cos(alpha) * x1->Norm() * c;
  B1 = cos(beta + M_PI/2.) * y->Norm() * c;
  B2 = cos(beta) * x2->Norm() * c;
  C = c * c;
  cout << Verbose(2) << "A " << A << "\tB " << B1 << "\tC " << C << endl;
  int flag = 0;
  if (fabs(x1->x[0]) < MYEPSILON) { // check for zero components for the later flipping and back-flipping
    if (fabs(x1->x[1]) > MYEPSILON) {
      flag = 1;
    } else if (fabs(x1->x[2]) > MYEPSILON) {
       flag = 2;
    } else {
      return false;
    }
  }
  switch (flag) {
    default:
    case 0:
      break;
    case 2:
      flip(x1->x[0],x1->x[1]);
      flip(x2->x[0],x2->x[1]);
      flip(y->x[0],y->x[1]);
      //flip(x[0],x[1]);
      flip(x1->x[1],x1->x[2]);
      flip(x2->x[1],x2->x[2]);
      flip(y->x[1],y->x[2]);
      //flip(x[1],x[2]);
    case 1:
      flip(x1->x[0],x1->x[1]);
      flip(x2->x[0],x2->x[1]);
      flip(y->x[0],y->x[1]);
      //flip(x[0],x[1]);
      flip(x1->x[1],x1->x[2]);
      flip(x2->x[1],x2->x[2]);
      flip(y->x[1],y->x[2]);
      //flip(x[1],x[2]);
      break;
  }
  // now comes the case system
  D1 = -y->x[0]/x1->x[0]*x1->x[1]+y->x[1];
  D2 = -y->x[0]/x1->x[0]*x1->x[2]+y->x[2];
  D3 = y->x[0]/x1->x[0]*A-B1;
  cout << Verbose(2) << "D1 " << D1 << "\tD2 " << D2 << "\tD3 " << D3 << "\n";
  if (fabs(D1) < MYEPSILON) {
    cout << Verbose(2) << "D1 == 0!\n";
    if (fabs(D2) > MYEPSILON) {
      cout << Verbose(3) << "D2 != 0!\n";
      x[2] = -D3/D2;
      E1 = A/x1->x[0] + x1->x[2]/x1->x[0]*D3/D2;
      E2 = -x1->x[1]/x1->x[0];
      cout << Verbose(3) << "E1 " << E1 << "\tE2 " << E2 << "\n";
      F1 = E1*E1 + 1.;
      F2 = -E1*E2;
      F3 = E1*E1 + D3*D3/(D2*D2) - C;
      cout << Verbose(3) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
      if (fabs(F1) < MYEPSILON) {
        cout << Verbose(4) << "F1 == 0!\n";
        cout << Verbose(4) << "Gleichungssystem linear\n";
        x[1] = F3/(2.*F2);
      } else {
        p = F2/F1;
        q = p*p - F3/F1;
        cout << Verbose(4) << "p " << p << "\tq " << q << endl;
        if (q < 0) {
          cout << Verbose(4) << "q < 0" << endl;
          return false;
        }
        x[1] = p + sqrt(q);
      }
      x[0] =  A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
    } else {
      cout << Verbose(2) << "Gleichungssystem unterbestimmt\n";
      return false;
    }
  } else {
    E1 = A/x1->x[0]+x1->x[1]/x1->x[0]*D3/D1;
    E2 = x1->x[1]/x1->x[0]*D2/D1 - x1->x[2];
    cout << Verbose(2) << "E1 " << E1 << "\tE2 " << E2 << "\n";
    F1 = E2*E2 + D2*D2/(D1*D1) + 1.;
    F2 = -(E1*E2 + D2*D3/(D1*D1));
    F3 = E1*E1 + D3*D3/(D1*D1) - C;
    cout << Verbose(2) << "F1 " << F1 << "\tF2 " << F2 << "\tF3 " << F3 << "\n";
    if (fabs(F1) < MYEPSILON) {
      cout << Verbose(3) << "F1 == 0!\n";
      cout << Verbose(3) << "Gleichungssystem linear\n";
      x[2] = F3/(2.*F2);
    } else {
      p = F2/F1;
      q = p*p - F3/F1;
      cout << Verbose(3) << "p " << p << "\tq " << q << endl;
      if (q < 0) {
        cout << Verbose(3) << "q < 0" << endl;
        return false;
      }
      x[2] = p + sqrt(q);
    }
    x[1] = (-D2 * x[2] - D3)/D1;
    x[0] = A/x1->x[0] - x1->x[1]/x1->x[0]*x[1] + x1->x[2]/x1->x[0]*x[2];
  }
  switch (flag) { // back-flipping
    default:
    case 0:
      break;
    case 2:
      flip(x1->x[0],x1->x[1]);
      flip(x2->x[0],x2->x[1]);
      flip(y->x[0],y->x[1]);
      flip(x[0],x[1]);
      flip(x1->x[1],x1->x[2]);
      flip(x2->x[1],x2->x[2]);
      flip(y->x[1],y->x[2]);
      flip(x[1],x[2]);
    case 1:
      flip(x1->x[0],x1->x[1]);
      flip(x2->x[0],x2->x[1]);
      flip(y->x[0],y->x[1]);
      //flip(x[0],x[1]);
      flip(x1->x[1],x1->x[2]);
      flip(x2->x[1],x2->x[2]);
      flip(y->x[1],y->x[2]);
      flip(x[1],x[2]);
      break;
  }
  // one z component is only determined by its radius (without sign)
  // thus check eight possible sign flips and determine by checking angle with second vector
  for (i=0;i<8;i++) {
    // set sign vector accordingly
    for (j=2;j>=0;j--) {
      k = (i & pot(2,j)) << j;
      cout << Verbose(2) << "k " << k << "\tpot(2,j) " << pot(2,j) << endl;
      sign[j] = (k == 0) ? 1. : -1.;
    }
    cout << Verbose(2) << i << ": sign matrix is " << sign[0] << "\t" << sign[1] << "\t" << sign[2] << "\n";
    // apply sign matrix
    for (j=NDIM;j--;)
      x[j] *= sign[j];
    // calculate angle and check
    ang = x2->Angle (this);
    cout << Verbose(1) << i << "th angle " << ang << "\tbeta " << cos(beta) << " :\t";
    if (fabs(ang - cos(beta)) < MYEPSILON) {
      break;
    }
    // unapply sign matrix (is its own inverse)
    for (j=NDIM;j--;)
      x[j] *= sign[j];
  }
  return true;
};

/**
 * 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 Vector::IsInParallelepiped(const Vector offset, const double *parallelepiped) const
{
  Vector a;
  a.CopyVector(this);
  a.SubtractVector(&offset);
  a.InverseMatrixMultiplication(parallelepiped);
  bool isInside = true;

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

  return isInside;
}