// 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>.
//

#include <iostream>
#include <iomanip>
#include "linear_algebra.h"


void Vector::Display(std::ostream& out) const
{
//  out << std::setiosflags(std::ios::fixed);
//  out << std::setprecision(8);
  out << std::endl;
  for (int j=0;j<Size();++j)
    out << coord[j] << " ";
  out << std::endl;
}

void Matrix::Display(std::ostream& out) const
{
//  out << std::setiosflags(std::ios::fixed);
//  out << std::setprecision(8);
  for (int i=0;i<this->SizeL();++i)
  {
    out << std::endl;
    for (int j=0;j<this->SizeC();++j)
      out << (*this)(i,j) << " ";
  }
  out << std::endl;
}

#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
 a[k][l]=h+s*(g-h*tau);

int Square_Matrix::Eigen_Vectors(Matrix &M,Vector &d)
{
  int j,iq,ip,i;
  double tresh,theta,tau,t,sm,s,h,g,c;

  std::vector<double> b(Size());
  std::vector<double> z(Size());
  std::vector<std::vector<double> > a(Size());
  for (i=0;i<Size();i++)
  {
    a[i].resize(Size());
  }

  int nrot=0;

  for (ip=0;ip<Size();ip++)
  {
    for (iq=0;iq<Size();iq++)
    {
      mat[ip][iq]=0.0;
      a[ip][iq]=M(ip,iq);
    }
    mat[ip][ip]=1.0;
  }
  for (ip=0;ip<Size();ip++)
  {
    b[ip]=d[ip]=a[ip][ip];
    z[ip]=0.0;
  }
  nrot=0;
  for (i=0;i<100;i++)
  {
    sm=0.0;
    for (ip=0;ip<Size()-1;ip++)
    {
      for (iq=ip+1;iq<Size();iq++)
        sm += fabs(a[ip][iq]);
    }
    if (sm == 0.0)
    {
      break;
    }
    if (i < 3)
      tresh=0.2*sm/ (Size() *Size());
    else
      tresh=0.0;
    for (ip=0;ip<Size()-1;ip++)
    {
      for (iq=ip+1;iq<Size();iq++)
      {
        g=100.0*fabs(a[ip][iq]);
        if ((i > 3) && ((fabs(d[ip]) +g) == fabs(d[ip])) && ((fabs(d[iq]) +g) == fabs(d[iq])))
          a[ip][iq]=0.0;
        else
        {
          if (fabs(a[ip][iq]) > tresh)
          {
            h=d[iq]-d[ip];
            if ((fabs(h) +g) == fabs(h))
              t= (a[ip][iq]) /h;
            else
            {
              theta=0.5*h/ (a[ip][iq]);
              t=1.0/ (fabs(theta) +sqrt(1.0+theta*theta));
              if (theta < 0.0) t = -t;
            }
            c=1.0/sqrt(1+t*t);
            s=t*c;
            tau=s/ (1.0+c);
            h=t*a[ip][iq];
            z[ip] -= h;
            z[iq] += h;
            d[ip] -= h;
            d[iq] += h;
            a[ip][iq]=0.0;
            for (j=0;j<=ip-1;j++)
            {
              ROTATE(a,j,ip,j,iq)
            }
            for (j=ip+1;j<=iq-1;j++)
            {
              ROTATE(a,ip,j,j,iq)
            }
            for (j=iq+1;j<Size();j++)
            {
              ROTATE(a,ip,j,iq,j)
            }
            for (j=0;j<Size();j++)
            {
              ROTATE(mat,j,ip,j,iq)
            }
            ++nrot;
          }
        }
      }
    }
    for (ip=0;ip<Size();ip++)
    {
      b[ip] += z[ip];
      d[ip]=b[ip];
      z[ip]=0.0;
    }
  }
  if (i==100)
  {
    return 1; // there was a problem.
  }
  return 0;
}

#undef ROTATE


void Square_Matrix::Complete_Base(int num)
{
  int i,j,k;
  std::vector<Vector> Tab(Size());
  for (i=0;i<Size();i++)
    Tab[i].Resize(Size());

  double pscal,norm;
  for (i=0;i<num;i++)
  {
    for (j=0;j<Size();j++)
      Tab[i][j]=mat[i][j];
    Tab[i].Normalize();
  }

  for (i=num;i<Size();i++)
  {
    for (j=0;j<num;j++)
    {
      for (k=0;k<Size();k++)
        Tab[i][k]=0;
      Tab[i][(i+j) %Size()]=1.0;
      for (k=0;k<i;k++)
      {
        pscal=Tab[i]*Tab[k];
        Tab[i].Add(Tab[k],-pscal);
      }
      norm=Tab[i].Norm();
      if (norm>1e-4)
        break;

    }
    Tab[i].Normalize();
  }

  for (i=0;i<Size();i++)
    for (j=0;j<Size();j++)
      mat[i][j]=Tab[i][j];
}





void Cholesky_Matrix::CholeskyDec_intern(void)
{
  int k,i,p;
  for (k=0;k<Size();++k)
  {
    double sumsqakp=0.;
    for (p=0;p<k;++p)
    {
      double akp= (*this)(k,p);
      sumsqakp+=akp*akp;
    }
    double& akk= (*this)(k,k);
    if (akk<sumsqakp)
    {
      std::cerr << "Cholesky : (akk<sumsqakp) " << akk << " < " << sumsqakp << std::endl;
      throw "Cholesky : (akk<sumsqakp)";
    }
    akk=sqrt(akk-sumsqakp);
    //    Display();
    //    cout << "a" << k << k << " " <<  akk << " sumsqakp " << sumsqakp << endl;
    for (i=k+1;i<Size();++i)
    {
      double sumaipakp=0.0;
      for (p=0;p<k;++p)
      {
        sumaipakp+= (*this)(i,p) * (*this)(k,p);
      }
      double& aik= (*this)(i,k);
      aik= (aik-sumaipakp) /akk;
      //      Display();
      //      cout << "a" << i << k << " " <<  aik <<" sumaipakp " << sumaipakp << endl;
      //      cout << aik << " " ;
    }
    //    cout << endl;
  }
}


void LU_Matrix::LUDec_intern(void)
{

  int i,imax,j,k;
  double big,dum,sum,temp;
  std::vector<double> vv(Size());


  d=1.;

  for (i=0;i<Size();++i)
  {
    big=0.0;
    for (j=0;j<Size();++j)
      if ((temp=fabs(mat[i][j])) > big) big=temp;

    if (big == 0.0)
    {
      d=0.0;
      return;
    }

    vv[i]=1.0/big;
  }


  for (j=0;j<Size();++j)
  {
    for (i=0;i<j;++i)
    {
      sum=mat[i][j];
      for (k=0;k<i;++k) sum -= mat[i][k]*mat[k][j];
      mat[i][j]=sum;
    }

    big=0.0;
    for (i=j;i<Size();++i)
    {
      sum=mat[i][j];
      for (k=0;k<j;++k)
        sum -= mat[i][k]*mat[k][j];
      mat[i][j]=sum;
      if ((dum=vv[i]*fabs(sum)) >= big)
      {
        big=dum;
        imax=i;
      }
    }
    if (j != imax)
    {
      for (k=0;k<Size();++k)
      {
        dum=mat[imax][k];
        mat[imax][k]=mat[j][k];
        mat[j][k]=dum;
      }
      d = - (d);
      vv[imax]=vv[j];
    }
    tab[j]=imax;

    if (mat[j][j] == 0.0)
    {
      mat[j][j] = 1e-50;
    }

    if (j != Size()-1)
    {
      dum=1.0/ (mat[j][j]);
      for (i=j+1;i<Size();++i) mat[i][j] *= dum;
    }
  }
  for (i=0;i<Size();++i)
    d*=mat[i][i];
}

void LU_Matrix::Solve_Linear_System(Vector &rhsv,Vector &sol)
{
  int i,im1,ii=-1,ip,j;
  double sum;

  if (d==0.0) return;

  sol=rhsv;

  for (i=0;i<Size();++i)
  {
    ip=tab[i];
    sum=sol[ip];
    sol[ip]=sol[i];
    im1=i-1;
    if (ii>=0)
      for (j=ii;j<=im1;++j) sum -= mat[i][j]*sol[j];
    else if (sum!=0.0) ii=i;
    sol[i]=sum;
  }

  for (i=Size()-1;i>=0;i--)
  {
    sum=sol[i];
    for (j=i+1;j<Size();++j) sum -= mat[i][j]*sol[j];
    sol[i]=sum/mat[i][i];
  }
}




double det(double M[2][2])
{
  return M[0][0]*M[1][1]-M[1][0]*M[0][1];
}

double trace(double M[2][2])
{
  return M[0][0]+M[1][1];
}

double prodcc(double M1[2][2],double M2[2][2])
{
  double p=0.;
  for (int i=0;i<2;++i)
    for (int j=0;j<2;++j)
      p+=M1[i][j]*M2[i][j];
  return p;
}


void inverse(double M[2][2],double I[2][2])  // I : inverse de la matrice M
{
  double d = det(M);
  I[0][0] = M[1][1]/d;
  I[1][1] = M[0][0]/d;
  I[0][1] = -M[0][1]/d;
  I[1][0] = -M[1][0]/d;
}


void multi(double A[2][2],double B[2][2],double C[2][2]) // Effectue la multiplication de A et B et rentre le résultat dans la matrice C
{
  for (int i=0;i<2;i++)
  {
    for (int j=0;j<2;j++)
    {
      C[i][j] = 0;
      for (int k=0;k<2;k++)
      {
        C[i][j]+=A[i][k]*B[k][j];
      }
    }
  }
}

double multi(double V1[2],double M[2][2],double V2[2]) // Effectue la multiplication Ai Mij Cj 
{
  double res=0.;
  for (int i=0;i<2;i++)
  {
    for (int j=0;j<2;j++)
    {
      res+=V1[i]*M[i][j]*V2[j];
    }
  }
  return res;
}