// nUtil - An utility Library for gnurbs
// Copyright (C) 2008-2026 Eric Bechet
//
// See the LICENSE file for contributions and license information.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//

//---------------------------------------------------------------------------
#ifndef LINEAR_ALGEBRA_H
#define LINEAR_ALGEBRA_H
//---------------------------------------------------------------------------

//#define DBUG


#include <iostream>
#include <cmath>
#include <vector>

// prototypes
class Vector;
class Matrix;
class Rect_Matrix;
class Square_Matrix;
class Symmetric_Matrix;
class Anti_Symmetric_Matrix;
class Metric_Tensor;
class Triangular_Matrix;

/// vector (geometrical)
class Vector
{

protected:
  std::vector<double> coord; //!< Coordinates of the vector.

public:

/// \brief Default constructor.
  Vector(int n=0) :coord(n)
  {
  }

/// \brief Takes two points v1 and v2 and constructs the vector linking v1 to v2.
/// \param[in] n space dimension.
/// \param[in] v1 first point.
/// \param[in] v2 second point.
  template<class V> Vector(int n,V &v1,V &v2) :coord(n)
  {
    Init(v1,v2);
  }

/// \brief Constructs a vector from an array.
/// \param[in] n length of the array.
/// \param[in] V array.
  Vector(int n,double *V) :coord(n)
  {
    Init(V);
  }

/// \brief Initializes the current vector so that it links two points v1 and v2.
/// \param[in] v1 first point.
/// \param[in] v2 second point.
  template <class V>  void Init(V &v1,V &v2)
  {
    for (int i=0;i<Size();++i) coord[i]=v2[i]-v1[i];
  }


/// Initializes the current vector from a point, array...
/// \param[in] v initialization data.
  template <class V> void Init(V &v)
  {
    for (int i=0;i<Size();++i) coord[i]=v[i];
  }

/// \brief get the size of the vector.
/// \return Size.
  int Size(void) const
  {
    return (int) coord.size();
  }

/// \brief Resize the vector.
/// \param[in] n new size.
  void Resize(int n)
  {
    coord.resize((unsigned) n);
  }

/// \brief Get the euclidean norm of the vector.
/// \return Norm.
  double Norm(void) const
  {
    double tmp;
    double sum=0;
    for (int i=0;i<Size();++i)
    {
      tmp=coord[i];
      sum+=tmp*tmp;
    }
    return (sqrt(sum));
  }

/// \brief Ensures that the norm is unity.
/// \return Old norm.
  double Normalize(void)
  {
    double N=Norm();
    if (N!=0.0)
      for (int i=0;i<Size();++i)
        coord[i]/=N;
    else
    {
      coord[0]=1.0;
      for (int i=1;i<Size();++i)
        coord[i]=0;
    }
    return N;
  }

/// \brief Cross product \f$ V1 \wedge V2 \f$ (in 3D only).
/// \param[in] V1 left vector.
/// \param[in] V2 right vector.
  void Cross(Vector &V1, Vector &V2)
  {
    if (Size() !=3) throw ;
    for (int i=0;i<3;++i)
      coord[i]=V1[(i+1) %3]*V2[(i+2) %3]-V1[(i+2) %3]*V2[(i+1) %3];
  }

/// \brief Dot product.
/// \param[in] V vector to compute the dot product with.
/// \return Dot product.
  double operator * (const Vector &V) const
  {
    double c=0;
    for (int i=0;i<Size();++i)
    {
      c+=coord[i]*V[i];
    }
    return (c);
  }

/// \brief Multiplication by a scalar factor. The vector is modified.
/// \param[in] c factor.
  void operator *= (double c)
  {
    for (int i=0;i<Size();++i)
    {
      coord[i]*=c;
    }
  }

/// \brief Gets ith component (mutable version).
/// \param[in] i wanted component number.
/// \return Component.
  double & operator[](int i)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// \brief Gets ith component (constant version).
/// \param[in] i wanted component number.
/// \return Component.
  const double & operator[](int i) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// \brief Gets ith component (mutable version).
/// \param[in] i wanted component number.
/// \return Component.
  double & operator()(int i)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// \brief Gets ith component (constant version).
/// \param[in] i wanted component number.
/// \return Component.
  const double & operator()(int i) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// \brief Adds a vector to the current vector.
/// \param[in] V1 vector to add.
  void Add(Vector &V1)
  {
    for (int i=0;i<Size();++i)
    {
      coord[i]+=V1[i];
    }
  }


/// Adds-multiply this vector V by a vector V1 and a factor: \f$ V = V + V1 \ factor \f$.
/// \param[in] V1 vector to add-multiply.
/// \param[in] factor multiplication factor.
  void Add(Vector &V1,double factor)
  {
    for (int i=0;i<Size();++i)
    {
      coord[i]+=factor*V1[i];
    }
  }

/// \brief Shows coordinates.
/// \param[in,out] out output stream.
  void Display(std::ostream& out=std::cout) const ;

};


/// Generic matrix.
class Matrix
{
///
public :

/// \brief Gets the component (i,j).
/// \param[in] i line number.
/// \param[in] j column number.
/// \return Component (i, j).
  virtual const double operator()(int i,int j) const=0;

/// \brief Shows the matrix.
/// \param[in,out] out output stream.
  virtual void Display(std::ostream& out=std::cout) const;

/// \brief Gets the minimal size: min(nb lines, nb columns).
/// \return Minimal size.
  virtual int Size(void) const=0;    // # for square matrices, otherwise  = min (SizeL,SizeC)

/// \brief Gets the number of lines.
/// \return Number of lines. 
  virtual int SizeL(void) const
  {
    return Size();  // # lines
  }

/// \brief Gets the number of columns.
/// \return Number of columns.
  virtual int SizeC(void) const
  {
    return Size();  // # columns
  }

/// \brief Resize the matrix.
/// \param[in] N new size.
  virtual void Resize(int N) =0;

/// \brief Resize the number of lines.
/// \param[in] N new number of lines.
  virtual void ResizeL(int N)
  {
    Resize(N);
  }

/// \brief Resize the number of columns.
/// \param[in] N new number of columns.
  virtual void ResizeC(int N)
  {
    Resize(N);
  }
};

/// Rectangular matrix.
class Rect_Matrix : public Matrix
{
///
  std::vector<std::vector<double> > mat;

public:
/// \brief Default constructor.
  Rect_Matrix()
  {
  }

/// \brief Constructs a N1 x N2 matrix.
/// \param[in] N1 number of lines.
/// \param[in] N1 number of columns.
  Rect_Matrix(int N1,int N2)
  {
    mat.resize(N1);
    for (int i=0;i<N1;i++) mat[i].resize(N2);
  }

/// \brief Constructs a N1 x N2 matrix and initializes it from an array.
/// \param[in] N1 number of lines.
/// \param[in] N1 number of columns.
/// \param[in] matrix initialization array.
  Rect_Matrix(int N1,int N2,double **matrix)
  {
    mat.resize(N1);
    for (int i=0;i<N1;i++) mat[i].resize(N2);
    for (int i=0;i<N1;++i)
      for (int j=0;j<N2;++j)
        mat[i][j]=matrix[i][j];
  }

  int Size(void) const
  {
    int N1=mat.size();
    int N2=0;
    if (N1)
      N2=mat[0].size();
    if (N1<N2)
      return N1;
    else
      return N2;
  }

  int SizeL(void) const
  {
    return mat.size();  // # lines
  }
  int SizeC(void) const
  {
    int N1=mat.size();  // # columns
    int N2=0;
    if (N1) N2=mat[0].size();
    return N2;
  }

///
  void Resize(int N)
  {
    mat.resize(N);
    for (int i=0;i<N;i++) mat[i].resize(N);
  }

///
  double & operator()(int i,int j)
  {
#ifdef DBUG
    if ((i<0) || (i>=SizeL())) throw;
    if ((j<0) || (j>=SizeC())) throw;
#endif
    return mat[i][j];
  }

///
  const double operator()(int i,int j) const
  {
#ifdef DBUG
    if ((i<0) || (i>=SizeL())) throw;
    if ((j<0) || (j>=SizeC())) throw;
#endif
    return mat[i][j];
  }

/// \brief Multiplies two matrices T1 and T2 and stores the result in this matrix.
/// \param[in] T1 left matrix.
/// \param[in] T2 right matrix.
  void Mult(const Matrix &T1,const Matrix &T2)
  {
    for (int i=0;i<SizeL();++i)
      for (int j=0;j<SizeC();++j)
      {
        mat[i][j]=0.;
        for (int k=0;k<T1.SizeC();++k)
          mat[i][j]+=T1(i,k) *T2(k,j);
      }
  }

/// \brief Multiplies this matrix with a vector.
/// \param[in] V the vector the matrix is multiplied with.
/// \param[out] V1 the resulting vector.
  void Mult(Vector &V,Vector &V1) const
  {
    for (int i=0;i<SizeL();++i)
    {
      V1[i]=0.;
      for (int j=0;j<SizeC();++j)
        V1[i]+=V[j]*mat[i][j];
    }
  }

/// \brief Destructor.
  virtual ~Rect_Matrix()
  {
  }

};



/// Matrix that will be decomposed in L*U.
class LU_Matrix : public Matrix
{
///
  std::vector<std::vector<double> > mat;
///
  std::vector<int> tab;
///
  double d;

///
  void LUDec_intern(void);
///
  bool Pivot(int jcol);

///
public :
/// \brief Construct the LU matrix from a generic matrix.
/// \param[in] M generic matrix.
  LU_Matrix(Matrix &M)
  {
    Resize(M.Size());
    for (int i=0;i<Size();++i)
    {
      for (int j=0;j<Size();++j)
        mat[i][j]=M(i,j);
      tab[i]=0;
    }
    d=0.0;
    LUDec_intern();
  }

  int Size(void) const
  {
    return (int) tab.size();
  }

  void Resize(int N)
  {
    tab.resize((unsigned) N);
    mat.resize((unsigned) N);
    for (int i=0;i<N;i++) mat[i].resize(N);
  }

// gets the matrix's component (i,j) (non mutable)
  const double operator()(int i,int j) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    return mat[i][j];
  }

// gets the matrix's component (i,j) (mutable)
  double & operator()(int i,int j)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    return mat[i][j];
  }

/// \brief Gets the determinant.
/// \return Determinant.
  double Determinant(void)
  {
    return d;
  }

/// \brief Solves a linear system (using LU decomposition).
/// \param[in] rhsv right-hand size vector (non-homogeneous terms).
/// \param[out] sol solution of the linear system.
  void Solve_Linear_System(Vector &rhsv,Vector &sol);

/// \brief Destructor.
  virtual ~LU_Matrix()
  {
  }
};



/// A square matrix.
class Square_Matrix : public Matrix
{
///
  std::vector<std::vector<double> > mat;

public:
/// \brief Default constructor.
  Square_Matrix()
  {
  }

/// \brief Constructs a N x N square matrix.
/// \param[in] N order of the matrix.
  Square_Matrix(int N)
  {
    Resize(N);
  }

/// \brief Constructs a N x N square matrix and initializes it with an array.
/// \param[in] N order of the matrix.
/// \param[in] matrix initialization array.
  Square_Matrix(int N,double **matrix)
  {
    Resize(N);
    for (int i=0;i<N;++i)
      for (int j=0;j<N;++j)
        mat[i][j]=matrix[i][j];
  }

  int Size(void) const
  {
    return mat.size();
  }

/// \brief Constructs a square matrix with its diagonal filled by a constant.
/// \param[in] N order of the matrix.
/// \param[in] x filling constant.
  Square_Matrix(int N,double x)
  {
    Resize(N);
    Set(x);
  }

/// \brief Constructs a square matrix with its diagonal filled by a constant.
/// \param[in] N order of the matrix.
/// \param[in] x filling constant.
  Square_Matrix(int N,double *x)
  {
    Resize(N);
    Set(x);
  }

/// \brief Constructs a diagonal square matrix. The diagonal is filled by x.
/// \param[in] N order of the matrix.
/// \param[in] x vector filling the diagonal.
  Square_Matrix(int N,Vector &x)
  {
    Resize(N);
    Set(x);
  }

/// \brief Sets the matrix diagonal to a constant and other entries to zero.
/// \param[in] x filling constant.
  void Set(double x)
  {
    for (int i=0;i<Size();++i)
    {
      for (int j=0;j<Size();++j)
        mat[i][j]=0.;
      mat[i][i]=x;
    }
  }

/// \brief Sets the matrix diagonal to a constant and other entries to zero.
/// \param[in] x filling constant.
  void Set(double *x)
  {
    for (int i=0;i<Size();++i)
    {
      for (int j=0;j<Size();++j)
        mat[i][j]=0.;
      mat[i][i]=x[i];
    }
  }

/// \brief Sets the matrix diagonal to a vector and other entries to zero.
/// \param[in] x filling vector.
  void Set(Vector &x)
  {
    for (int i=0;i<Size();++i)
    {
      for (int j=0;j<Size();++j)
        mat[i][j]=0.;
      mat[i][i]=x[i];
    }
  }

///
  void Resize(int N)
  {
    mat.resize(N);
    for (int i=0;i<N;i++) mat[i].resize(N);
  }


/// \brief Computes the trace of the matrix.
/// \return Trace.
  double Trace(void)
  {
    double t=0.;
    for (int i=0;i<Size();++i) t+=mat[i][i];
    return (t);
  }

///
  double & operator()(int i,int j)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    return mat[i][j];
  }

///
  const double operator()(int i,int j) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    return mat[i][j];
  }

/// \brief Multiplies two matrices T1 and T2 and stores the result in this matrix.
/// \param[in] T1 left matrix.
/// \param[in] T2 right matrix.
  void Mult(const Matrix &T1,const Matrix &T2)
  {
    for (int i=0;i<SizeL();++i)
      for (int j=0;j<SizeC();++j)
      {
        mat[i][j]=0.;
        for (int k=0;k<T1.SizeC();++k)
          mat[i][j]+=T1(i,k) *T2(k,j);
      }
  }

/// \brief Stores T1*transpose(T1) in this matrix.
/// \param[in] T1 input matrix.
  void Square(const Matrix &T1)
  {
    for (int i=0;i<SizeL();++i)
      for (int j=0;j<SizeC();++j)
      {
        mat[i][j]=0.;
        for (int k=0;k<T1.SizeC();++k)
          mat[i][j]+=T1(i,k) *T1(j,k);
      }
  }

/// \brief Multiplies this matrix with a vector.
/// \param[in] V the vector the matrix is multiplied with.
/// \param[out] V1 the resulting vector.
  void Mult(Vector &V,Vector &V1) const
  {
    for (int i=0;i<Size();++i)
    {
      V1[i]=0.;
      for (int j=0;j<Size();++j)
        V1[i]+=V[j]*mat[i][j];
    }
  }

/// \brief Transpose this matrix.
  void Transpose(void)
  {
    double tmp;
    for (int j=1;j<Size();++j)
      for (int i=0;i<j;++i)
      {
        tmp=mat[i][j];
        mat[i][j]=mat[j][i];
        mat[j][i]=tmp;
      }
  }

/// \brief Computes the inverse of this matrix.
/// \param[out] Minv matrix inverse.
  void Invert(Square_Matrix& Minv)
  {
    LU_Matrix *LU;
    Vector *VEC,*SOL;
    int i,j;
    LU=new LU_Matrix(*this);
    VEC=new Vector(Size());
    SOL=new Vector(Size());

    for (i=0;i<Size();++i)
      (*VEC)[i]=0.0;
    for (j=0;j<Size();++j)
    {
      (*VEC)[j]=1.0;
      LU->Solve_Linear_System(*VEC,*SOL);
      for (i=0;i<Size();++i)
        Minv(i,j) = (*SOL)[i];
      (*VEC)[j]=0.0;
    }
    delete VEC;
    delete SOL;
    delete LU;
  }


/// \brief Computes the determinant.
/// \return Determinant.
  double Determinant(void)
  {
    double det;
    switch (Size())
    {
      case 1:
        det=mat[0][0];
        break;
      case 2:
        det= (mat[0][0]*mat[1][1]-mat[1][0]*mat[0][1]);
        break;
      case 3:
        det= ((mat[0][0]*mat[1][1]*mat[2][2]+
               mat[1][0]*mat[2][1]*mat[0][2]+
               mat[2][0]*mat[0][1]*mat[1][2])-
              (mat[0][2]*mat[1][1]*mat[2][0]+
               mat[1][2]*mat[2][1]*mat[0][0]+
               mat[2][2]*mat[0][1]*mat[1][0]));
        break;
      default:
      {
        LU_Matrix *LU;
        LU=new LU_Matrix(*this);
        det=LU->Determinant();
        delete LU;
      }
    }
    return det;
  }

/// \brief Solves a linear system (using LU decomposition).
/// \param[in] rhsv right-hand size vector (non-homogeneous terms).
/// \param[out] sol solution of the linear system.
  void Solve_Linear_System(Vector &rhsv,Vector &sol)
  {
    LU_Matrix *LU;
    LU=new LU_Matrix(*this);
    LU->Solve_Linear_System(rhsv,sol);
    delete LU;
  }

/// \brief Computes the eigen vectors and eigen values.
/// \param[out] Matrix matrix of eigen vectors (in column).
/// \param[out] d corresponding vector of eigen values.
  int Eigen_Vectors(Matrix &M,Vector &d);

//
  void Complete_Base(int n);

/// \brief Destructor.
  virtual ~Square_Matrix()
  {
  }

};



/// Symmetrical matrix.
class Symmetric_Matrix : public Matrix
{

protected :
///
  std::vector<double> vec;
  int sz;
  // 0 . .
  // 1 2 . en 3x3 par exemple.
  // 3 4 5
public :
/// \brief default constructor.
  Symmetric_Matrix(int N=0)
  {
    Resize(N);
  }

/// \brief Construct a N x N matrix with its diagonal filled by a constant.
/// \param[in] N size.
/// \param[in] x filling constant.
  Symmetric_Matrix(int N,double x)
  {
    Resize(N);
    for (unsigned i=0;i<vec.size();++i) vec[i]=0;
    for (int j=0;j<sz;++j)(*this)(j,j) =x;
  }

  int Size(void) const
  {
    return sz;
  }

  void Resize(int N)
  {
    sz=N;
    vec.resize((sz+sz*sz) /2);
  }

/// \brief Computes the trace.
/// \return Trace.
  double Trace(void)
  {
    double t=0.;
    for (int i=0;i<sz;++i) t+= (*this)(i,i);
    return (t);
  }

///
  virtual double & operator()(int i,int j)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif

    if (j<i)
      return vec[j+ (i*(i+1)) /2];
    else
      return vec[i+ (j*(j+1)) /2];
  }

///
  virtual const double operator()(int i,int j) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    if (j<i)
      return vec[j+ (i*(i+1)) /2];
    else
      return vec[i+ (j*(j+1)) /2];
  }

/// \brief Sets the diagonal to a constant.
/// \param[in] x filling constant.
  void Set(double x)
  {
    for (unsigned i=0;i<vec.size();++i) vec[i]=0;
    for (int j=0;j<sz;++j)
      (*this)(j,j) =x;
  }

/// \brief Sets the diagonal to a constant.
/// \param[in] x filling constant.
  void Set(double *x)
  {
    for (unsigned i=0;i<vec.size();++i) vec[i]=0;
    for (int j=0;j<sz;++j)
      (*this)(j,j) =x[j];
  }

/// \brief Sets the diagonal to a vector.
/// \param[in] x filling vector.
  void Set(Vector &x)
  {
    for (unsigned i=0;i<vec.size();++i) vec[i]=0;
    for (int j=0;j<sz;++j)
      (*this)(j,j) =x[j];
  }


/// \brief Computes \f$ R^t S R \f$ and stores the result.
/// \param[in] S input matrix.
/// \param[in] R typically, rotation matrix.
  void tRSR(Matrix &S,Matrix &R)    // typiquement : rotation
  {
    Vector X(sz);
    int i,j,k;

    for (k=0;k<sz;++k)
    {
      for (j=0;j<sz;++j)
      {
        X[j]=0;
        for (i=0;i<sz;++i)
          X[j]+=S(i,j) *R(i,k);
      }
      for (j=k;j<sz;++j)
      {
        (*this)(k,j) =0;
        for (i=0;i<sz;++i)
          (*this)(k,j) +=X[i]*R(i,j);
      }
    }
  }

/// \brief Computes \f$ T^t S T \f$ and stores the result.
/// \param[in] S input symmetric matrix.
/// \param[in] T typically, affine transformation matrix.
  void tTST(Symmetric_Matrix &S,Symmetric_Matrix &T)    // typiquement : transformation affine
  {
    Vector X(sz);
    int i,j,k;

    for (k=0;k<sz;++k)
    {
      for (j=0;j<sz;++j)
      {
        X[j]=0;
        for (i=0;i<sz;++i)
          X[j]+=S(i,j) *T(i,k);
      }
      for (j=k;j<sz;++j)
      {
        (*this)(k,j) =0;
        for (i=0;i<sz;++i)
          (*this)(k,j) +=X[i]*T(i,j);
      }
    }
  }

/// \brief Multiplies this matrix with a vector.
/// \param[in] V the vector the matrix is multiplied with.
/// \param[out] V1 the resulting vector.
  void Mult(Vector &V,Vector &V1) const
  {
    for (int i=0;i<sz;++i)
    {
      V1[i]=0.;
      for (int j=0;j<sz;++j)
        V1[i]+=V[j]* (*this)(i,j);
    }
  }

/// \brief Transpose this matrix.
  void Transpose(void)
  {
  }

/// \brief Destructor.
  virtual ~Symmetric_Matrix()
  {
  }
};



/// Matrix that will be decomposed in G*tG (G=cholesky lower triangle).
class Cholesky_Matrix : public Symmetric_Matrix
{

public:
/// \brief Construct a Cholesky matrix from a symmetric matrix.
/// \param[in] M input symmetric matrix.
  Cholesky_Matrix(Symmetric_Matrix &M) :Symmetric_Matrix(M)
  {
    CholeskyDec_intern();
  }

/// \brief Destructor.
  virtual ~Cholesky_Matrix()
  {
  }

private:

  void CholeskyDec_intern(void);

};



/// Triangular matrix.
class Triangular_Matrix : public Symmetric_Matrix
{
  bool upper;
public :
/// \brief Constructs an empty triangular matrix.
/// \param[in] up if true: upper triangular matrix. Lower triangular matrix elsewhere.
  Triangular_Matrix(bool up=false) :Symmetric_Matrix(),upper(up)
  {
  }

/// \brief Constructs a triangular matrix from a symmetric matrix.
/// \param[in] M input symmetric matrix.
/// \param[in] up if true: upper triangular matrix. Lower triangular matrix elsewhere.
  Triangular_Matrix(const Symmetric_Matrix &M, bool up=false) :Symmetric_Matrix(M),upper(up)
  {
  }


/// \brief Solves a linear system.
/// \param[in] rhsv right-hand size vector (non-homogeneous terms).
/// \param[out] sol solution of the linear system.
  void Solve_Linear_System(Vector &rhsv,Vector &sol)
  {
    int i,j;
    if (upper)
    {
      for (i=Size()-1;i>=0;i--)
      {
        double sum=0.0;
        for (j=Size()-1;j>i;j--)
          sum+=sol[j]* (*this)(i,j);
        sol[i]= (rhsv[i]-sum) / (*this)(i,i);
      }
    }
    else
    {
      for (i=0;i<Size();i++)
      {
        double sum=0.0;
        for (j=0;j<i;j++)
          sum+=sol[j]* (*this)(i,j);
        sol[i]= (rhsv[i]-sum) / (*this)(i,i);
      }
    }
  }

/// \brief Inverse this matrix.
/// \param[in] Minv inversed matrix.
  void Invert(Triangular_Matrix& Minv)
  {
    Vector *VEC,*SOL;
    int i,j;
    VEC=new Vector(Size());
    SOL=new Vector(Size());
    for (i=0;i<Size();++i)
      (*VEC)[i]=0.0;
    for (j=0;j<Size();++j)
    {
      (*VEC)[j]=1.0;
      Solve_Linear_System(*VEC,*SOL);
      if (upper)
        for (i=0;i<=j;++i)
          Minv(i,j, (*SOL)[i]);
      else
        for (i=j;i<Size();++i)
          Minv(i,j, (*SOL)[i]);
      (*VEC)[j]=0.0;
    }
    delete VEC;
    delete SOL;
  }

/// \brief Sets entry (i, j).
/// \param[in] i line number.
/// \param[in] j column number.
/// \param[in] k value to set the entry (i, j) to.
  virtual void operator()(int i,int j,double k)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    if (upper)
    {
      if (j<i)
        throw;
      else
        vec[i+ (j*(j+1)) /2]=k;
    }
    else
    {
      if (j<=i)
        vec[j+ (i* (i+1)) /2]=k;
      else
        throw;
    }
  }

/// \brief Gets the entry (i, j).
/// \return Entry (i, j).
  virtual const double operator()(int i,int j) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    if (upper)
    {
      if (j<i)
        return 0.0;
      else
        return vec[i+ (j*(j+1)) /2];
    }
    else
    {
      if (j<=i)
        return vec[j+ (i* (i+1)) /2];
      else
        return 0.0;
    }
  }

/// \brief Destructor.
  virtual ~Triangular_Matrix()
  {
  }
};


/// Anti-symmetrical matrix.
class Anti_Symmetric_Matrix : public Symmetric_Matrix
{

public:

/// \brief Default constructor.
  Anti_Symmetric_Matrix(void) :Symmetric_Matrix()
  {
  }

/// \brief Construct a N x N matrix with its diagonal filled by a constant.
/// \param[in] N size.
/// \param[in] x filling constant.
  Anti_Symmetric_Matrix(int N,double x) :Symmetric_Matrix(N,x)
  {
  }

/// \brief Gets the entry (i, j).
/// \return Entry (i, j).
  virtual const double operator()(int i,int j) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    if (j<i)
      return -vec[j+ (i*(i+1)) /2];
    else
      return  vec[i+ (j*(j+1)) /2];
  }

/// \brief Sets entry (i, j).
/// \param[in] i line number.
/// \param[in] j column number.
/// \param[in] k value to set the entry (i, j) to.
  virtual void operator()(int i,int j,double k)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
    if ((j<0) || (j>=Size())) throw;
#endif
    if (j<i)
      vec[j+ (i*(i+1)) /2]=-k;
    else
      vec[i+ (j*(j+1)) /2]=k;
  }

/// \brief Transpose matrix.
  void Transpose(void)
  {
    for (int j=1;j<Size();++j)
      for (int i=0;i<j;++i)
      {
        (*this)(i,j, (*this)(i,j) *-1);
      }
  }

/// \brief Destructor.
  virtual ~Anti_Symmetric_Matrix()
  {
  }
};


/// Metric tensor.
class Metric_Tensor : public Symmetric_Matrix
{

public :
/// \brief Default constructor.
  Metric_Tensor(int N=0) :Symmetric_Matrix(N)
  {
  }

/// \brief Constructs N x N diagonal metric with a given target size.
/// \param[in] target_size target size.
  Metric_Tensor(int N,double target_size)
      : Symmetric_Matrix(N,1./ (target_size*target_size))
  {
  }

/// \brief Constructs the intersection of 2 metrics 
/// ...returns a not quite good upper bound !!!!
/// \param[in] M1 first metric.
/// \param[in] M2 second metric.
  Metric_Tensor(Metric_Tensor& M1,Metric_Tensor& M2) :Symmetric_Matrix(M1.Size())
  {
    for (unsigned i=0;i<vec.size();++i)
      if (M1.vec[i]>M2.vec[i]) vec[i]=M1.vec[i];
      else vec[i]=M2.vec[i];
  }

//
  void Set_Isotropic_Size(double target_size)
  {
    double t2;
    t2=1./ (target_size*target_size);
    for (unsigned i=0;i<vec.size();++i) vec[i]=0;
    for (int j=0;j<Size();++j)
      (*this)(j,j) =t2;
  }

/// \brief Computes the length ov vector V within this metric.
/// \return Length.
  double Calculate_Length(Vector &V)
  {
    Vector V1(Size());
    Mult(V,V1);
    return (sqrt(V1*V));
  }

/// \brief Destructor.
  virtual ~Metric_Tensor()
  {
  }
};




// some useful functions on 2x2 matrices
double det(double M[2][2]);
double trace(double M[2][2]);
void inverse(double M[2][2],double I[2][2]);  // I : inverse of matrix M
double prodcc(double M1[2][2],double M2[2][2]); // doubly contracted product
void multi(double A[2][2],double B[2][2],double C[2][2]); // Cij = Aik Bkj
double multi(double V1[2],double M[2][2],double V2[2]); // ret = Ai Mij Cj 




#endif // LINEAR_ALGEBRA_H