// 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:
/// coordinates of the vector
  std::vector<double> coord;

public:


/// default constructor
  Vector(int n=0) :coord(n)
  {
  }

///
  template<class V> Vector(int n,V &v1,V &v2) :coord(n)
  {
    Init(v1,v2);
  }

///
  Vector(int n,double *V) :coord(n)
  {
    Init(V);
  }

///
  template <class V>  void Init(V &v1,V &v2)
  {
    for (int i=0;i<Size();++i) coord[i]=v2[i]-v1[i];

  }


///
  template <class V> void Init(V &v)
  {
    for (int i=0;i<Size();++i) coord[i]=v[i];
  }


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

  void Resize(int n)
  {
    coord.resize((unsigned) n);
  }


/// get a norm of the vector
  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));
  }

/// ensures that the norm is unity
  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;
  }

/// cross product (in 3D only)
  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];
  }

/// scalar product
  double operator * (const Vector &V) const
  {
    double c=0;
    for (int i=0;i<Size();++i)
    {
      c+=coord[i]*V[i];
    }
    return (c);
  }

  void operator *= (double c)
  {
    for (int i=0;i<Size();++i)
    {
      coord[i]*=c;
    }
  }


/// gets ith component (mutable)
  double & operator[](int i)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// gets ith component (non mutable)
  const double & operator[](int i) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// gets ith component (mutable)
  double & operator()(int i)
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

/// gets ith component (non mutable)
  const double & operator()(int i) const
  {
#ifdef DBUG
    if ((i<0) || (i>=Size())) throw;
#endif
    return coord[i];
  }

///
  void Add(Vector &V1)
  {
    for (int i=0;i<Size();++i)
    {
      coord[i]+=V1[i];
    }
  }


///
  void Add(Vector &V1,double factor)
  {
    for (int i=0;i<Size();++i)
    {
      coord[i]+=factor*V1[i];
    }
  }

/// shows coordinates
  void Display(std::ostream& out=std::cout) const ;

};


/// generic matrix
class Matrix
{
///
public :

/// get the component (i,j)
  virtual const double operator()(int i,int j) const=0;

/// shows the matrix
  virtual void Display(std::ostream& out=std::cout) const;

  virtual int Size(void) const=0;    // # for square matrices, otherwise  = min (SizeL,SizeC)
  virtual int SizeL(void) const
  {
    return Size();  // # lines
  }
  virtual int SizeC(void) const
  {
    return Size();  // # columns
  }
  virtual void Resize(int N) =0;
  virtual void ResizeL(int N)
  {
    Resize(N);
  }
  virtual void ResizeC(int N)
  {
    Resize(N);
  }
};

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

public:
///
  Rect_Matrix()
  {
  }

  Rect_Matrix(int N1,int N2)
  {
    mat.resize(N1);
    for (int i=0;i<N1;i++) mat[i].resize(N2);
  }

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

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

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

  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 :
/// init
  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];
  }

/// gets the determinant
  double Determinant(void)
  {
    return d;
  }

/// solve a linear system (using LU decomposition)
  void Solve_Linear_System(Vector &rhsv,Vector &sol);

  virtual ~LU_Matrix()
  {
  }
};



/// a generic matrix
class Square_Matrix : public Matrix
{
///
  std::vector<std::vector<double> > mat;

public:
///
  Square_Matrix()
  {
  }

  Square_Matrix(int N)
  {
    Resize(N);
  }

///
  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();
  }

///
  Square_Matrix(int N,double x)
  {
    Resize(N);
    Set(x);
  }

///
  Square_Matrix(int N,double *x)
  {
    Resize(N);
    Set(x);
  }

///
  Square_Matrix(int N,Vector &x)
  {
    Resize(N);
    Set(x);
  }

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

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

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



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

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

  /// T1*transpose(T1)
  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);
      }
  }

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

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

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


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

///
  void Solve_Linear_System(Vector &rhsv,Vector &sol)
  {
    LU_Matrix *LU;
    LU=new LU_Matrix(*this);
    LU->Solve_Linear_System(rhsv,sol);
    delete LU;
  }

///
  int Eigen_Vectors(Matrix &M,Vector &d);

///
  void Complete_Base(int n);

  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 :
///
  Symmetric_Matrix(int N=0)
  {
    Resize(N);
  }

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

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

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

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

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


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

///
  void tTST(Symmetric_Matrix &S,Symmetric_Matrix &T)    // typiquement : transforamtion 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);
      }
    }
  }

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

///
  void Transpose(void)
  {
  }

  virtual ~Symmetric_Matrix()
  {
  }
};



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

public:
  Cholesky_Matrix(Symmetric_Matrix &M) :Symmetric_Matrix(M)
  {
    CholeskyDec_intern();
  }

  virtual ~Cholesky_Matrix()
  {
  }

private:

  void CholeskyDec_intern(void);

};




class Triangular_Matrix : public Symmetric_Matrix
{
  bool upper;
public :
  Triangular_Matrix(bool up=false) :Symmetric_Matrix(),upper(up)
  {
  }

  Triangular_Matrix(const Symmetric_Matrix &M, bool up=false) :Symmetric_Matrix(M),upper(up)
  {
  }


  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);
      }
    }
  }


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


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

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

  virtual ~Triangular_Matrix()
  {
  }
};


/// anti-symmetrical matrix
class Anti_Symmetric_Matrix : public Symmetric_Matrix
{

public:


  Anti_Symmetric_Matrix(void) :Symmetric_Matrix()
  {
  }

///
  Anti_Symmetric_Matrix(int N,double x) :Symmetric_Matrix(N,x)
  {
  }


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

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

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

  virtual ~Anti_Symmetric_Matrix()
  {
  }
};


/// metric tensor
class Metric_Tensor : public Symmetric_Matrix
{

public :
///
  Metric_Tensor(int N=0) :Symmetric_Matrix(N)
  {
  }

///
  Metric_Tensor(int N,double target_size)
      : Symmetric_Matrix(N,1./ (target_size*target_size))
  {
  }

/// intersection de 2 metriques ...renvoie un majorant...pas top !!!!
  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;
  }

///
  double Calculate_Length(Vector &V)
  {
    Vector V1(Size());
    Mult(V,V1);
    return (sqrt(V1*V));
  }

  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