// $Id: matrix.cc,v 1.2 1999-04-22 16:18:41 ansari Exp $

#include "machdefs.h"
#include <string.h>
#include <iostream.h>
#include <iomanip.h>
#include <values.h>
#include "peida.h"
#include "matrix.h"
#include "cvector.h"
#include "generalfit.h"

// Sous Linux, LN_MAXDOUBLE LN_MINDOUBLE ne semblent pas etre definies
//   Reza 11/02/99
#ifndef M_LN2
#define M_LN2 0.69314718055994530942
#endif
#ifndef LN_MINDOUBLE
#define LN_MINDOUBLE  (M_LN2 * (DMINEXP - 1))
#endif
#ifndef LN_MAXDOUBLE
#define LN_MAXDOUBLE  (M_LN2 * DMAXEXP)
#endif


//++
// Class	Matrix
// Lib	Outils++
// include	matrix.h
//
//	Classe générale de matrice, calculs matriciels et algèbre linéaire.
//--

//++
// Titre	Constructeurs
//--

//++
Matrix::Matrix()
//
//	Construit une matrice 1x1 (pour ppersist).
//--
: nr(1), nc(1), ndata(1), nalloc(1), data(new double[1])
{
  // if (r<=0 || c<=0) THROW(rangeCheckErr);
  memset(data, 0, nalloc*sizeof(double));
  END_CONSTRUCTOR
}

//++
Matrix::Matrix(int r, int c)
//
//	Construit une matrice de r lignes et c colonnes.
//--
: nr(r), nc(c), ndata(r*c), nalloc(r*c), data(new double[r*c])
{
  // if (r<=0 || c<=0) THROW(rangeCheckErr);
  memset(data, 0, nalloc*sizeof(double));
  END_CONSTRUCTOR
}

//++
Matrix::Matrix(int r, int c, double* values)
//
//	Construit une matrice de r lignes et c colonnes. On fournit
//	le tableau des valeurs : pas d'allocation.
//--
: nr(r), nc(c), ndata(r*c), nalloc(0), data(values)
{
  END_CONSTRUCTOR
}

//++
Matrix::Matrix(const Matrix& a)
//
//	Constructeur par copie.
//--
: nr(a.nr), nc(a.nc), ndata(a.nr*a.nc), nalloc(a.nr*a.nc),
  data(new double[a.nr*a.nc])
{
  memcpy(data, a.data, nalloc * sizeof(double));
  END_CONSTRUCTOR
}


Matrix::~Matrix()
{
  DBASSERT(ndata == nr*nc);
  if (nalloc) delete[] data;
}

//++
// Titre	Méthodes
//--


//++
void Matrix::Zero()
//
//	Remise à zero de tous les éléments
//--
{
  DBASSERT(ndata == nr*nc);
  for (int i=0; i<ndata; i++)
    data[i] = 0;
}

//++
void Matrix::Realloc(int r, int c, bool force)
//
//	Change la taille de la matrice. Réallocation physique seulement si
//	pas assez de place, ou forcée si force=true.
//--
{
  DBASSERT(ndata == nr*nc);
  if (!nalloc) THROW(allocationErr);
  int ncop = ndata;                      // ancien elements
  ndata = r*c;
  if (ndata < ncop) ncop = ndata;
  nr = r;
  nc = c;
  if (nalloc < ndata || force) {
    double* p = new double[ndata];
    memcpy(p,data,ncop * sizeof(double));
    delete[] data;
    data = p;
    nalloc = ndata;
  } else if (nalloc != ndata)                 // Sans doute facultatif, mais plus
    memset(data+ndata, 0, (nalloc-ndata)*sizeof(double));  //propre
}

//++
Matrix& Matrix::operator = (const Matrix& a)
//
//	Opérateur d'affectation.
//--
{
  if (this == &a) return *this;
  Realloc(a.nr, a.nc);
  memcpy(data, a.data, ndata * sizeof(double));
  return *this;
}


//++
Matrix& Matrix::operator = (double x)
//
//	Opérateur d'affectation depuis scalaire : identité * scalaire.
//--
{
  if (nr != nc) THROW(sizeMismatchErr);
  for (int r=0; r<nr; r++)
    for (int c=0; c<nc; c++)
      (*this)(r,c) = (r==c) ? x : 0.0;
  return *this;
}

//++
// r_8& [const]         operator()(int r, int c) [const]
//	Accès aux éléments
//--

//++
ostream& operator << (ostream& s, const Matrix& a)
//
//	Impression
//--
{
  for (int r=0; r<a.nr; r++) {
    s << "| ";
    for (int c=0; c<a.nc; c++)
      s << setw(6) << a(r,c) << " ";
    s << "|\n";
    }
  return s;
}

// ****************** MATRIX / SCALAR ******************************


//++
Matrix& Matrix::operator *= (double b)
//
//--
{
  double* p    = data;
  double* pEnd = data + ndata;

  while (p < pEnd)
    *p++ *= b;

  return *this;
}

//++
Matrix& Matrix::operator += (double b)
//
//--
{
  double* p    = data;
  double* pEnd = data + ndata;

  while (p < pEnd)
    *p++ += b;

  return *this;
}

//++
Matrix& Matrix::operator -= (double b)
//
//--
{
  double* p    = data;
  double* pEnd = data + ndata;

  while (p < pEnd)
    *p++ -= b;

  return *this;
}

//++
Matrix& Matrix::operator /= (double b)
//
//--
{
  DBASSERT( b != 0. );
  double* p    = data;
  double* pEnd = data + ndata;

  while (p < pEnd)
    *p++ /= b;

  return *this;
}

//++
Matrix operator * (const Matrix& a, double b)
//
//--
{
  Matrix result(a);
  return (result *= b);
}

//++
Matrix operator * (double b, const Matrix& a)
//
//--
{
  Matrix result(a);
  return (result *= b);
}

//++
Matrix operator + (const Matrix& a, double b)
//
//--
{
  Matrix result(a);
  return (result += b);
}

//++
Matrix operator + (double b, const Matrix& a)
//
//--
{
  Matrix result(a);
  return (result += b);
}

//++
Matrix operator - (const Matrix& a, double b)
//
//--
{
  Matrix result(a);
  return (result -= b);
}

//++
Matrix operator - (double b, const Matrix& a)
//
//--
{
  Matrix result(a);
  result *= -1;
  return (result += b);
}

//++
Matrix operator / (const Matrix& a, double b)
//
//--
{
  Matrix result(a);
  return (result /= b);
}

//++
Matrix operator * (const Matrix& a, int b)
//
//--
{
  Matrix result(a);
  return (result *= b);
}

//++
Matrix operator * (int b, const Matrix& a)
//
//--
{
  Matrix result(a);
  return (result *= b);
}

//++
Matrix operator + (const Matrix& a, int b)
//
//--
{
  Matrix result(a);
  return (result += b);
}

//++
Matrix operator + (int b, const Matrix& a)
//
//--
{
  Matrix result(a);
  return (result += b);
}

//++
Matrix operator - (const Matrix& a, int b)
//
//--
{
  Matrix result(a);
  return (result -= b);
}

//++
Matrix operator - (int b, const Matrix& a)
//
//--
{
  Matrix result(a);
  result *= -1;
  return (result += b);
}

//++
Matrix operator / (const Matrix& a, int b)
//
//--
{
  Matrix result(a);
  return (result /= b);
}


// **************** MATRIX / MATRIX ***********************88

//++
Matrix& Matrix::operator += (const Matrix& a)
//
//--
{
  if (nc!=a.nc || nr!=a.nr) THROW(sizeMismatchErr);

  double* p    = data;
  double* pEnd = data + ndata;
  double* q    = a.data;

  while (p < pEnd)
    *p++ += *q++;

  return *this;
}

//++
Matrix& Matrix::operator -= (const Matrix& a)
//
//--
{
  if (nc!=a.nc || nr!=a.nr) THROW(sizeMismatchErr);

  double* p    = data;
  double* pEnd = data + ndata;
  double* q    = a.data;

  while (p < pEnd)
    *p++ -= *q++;

  return *this;
}


//++
Matrix& Matrix::operator *= (const Matrix& a)
//
//--
{
  if (nc != a.nc || nr != a.nr || nc != nr) THROW(sizeMismatchErr);

  double* oldRow = new double[nr];
  double* orEnd = oldRow + nc;
  double* orp;
  double* trp;

  for (double* tr = data; tr < data+ndata; tr += nc) {
    for (orp = oldRow, trp = tr;
         orp < orEnd;)
      *orp++ = *trp++;

    double* ac;
    double* acp;

    for (trp = tr, ac = a.data; ac< a.data+nr; ac++, trp++) {
      double sum = 0;
      for (orp = oldRow, acp = ac; acp < ac+ndata; acp += nr, orp++)
        sum += *orp * *acp;
      *trp = sum;
    }
  }
  delete[] oldRow;
  return *this;
}

//++
Matrix operator + (const Matrix& a, const Matrix& b)
//
//--
{
  if (b.nc != a.nc || b.nr != a.nr) THROW(sizeMismatchErr);

  Matrix c(a);
  return (c += b);
}

//++
Matrix operator - (const Matrix& a, const Matrix& b)
//
//--
{
  if (b.nc != a.nc || b.nr != a.nr) THROW(sizeMismatchErr);

  Matrix c(a);
  return (c -= b);
}

#if 0

Matrix operator * (const Matrix& a, const Matrix& b)
{
  if (a.nc != b.nr) THROW(sizeMismatchErr);

  Matrix c(a.nr, b.nc);

  double* pc = c.data;

  double* pa = a.data;

  double* pb = b.data;

  for (int rc = 0; rc < c.nr; rc++) {
    for (int cc = 0; cc < c.nc; cc++) {      // boucle sur c
      double sum = 0;
      for (int ca = 0; ca < a.nc; ca++) {     // boucle sur a & b
        sum += *pa * *pb;
        pa++;
        pb += b.nc;
      }
      *pc++ = sum;
      pa -= a.nc;                           // retour meme ligne
      pb -= b.ndata - 1;                    // haut de la colonne suiv
    }
                                            // ligne suivante c
    pb = b.data;                            // en revient en b(1,1)
    pa += a.nc;                             // debut ligne suivante
  }

  return c;
}
#endif

//++
Matrix operator * (const Matrix& a, const Matrix& b)
//
//--
{
  if (a.nc != b.nr) THROW(sizeMismatchErr);

  Matrix c(a.nr, b.nc);

  for (int rc = 0; rc < c.nr; rc++)
    for (int cc = 0; cc < c.nc; cc++) {      // boucle sur c
      double sum = 0;
      for (int ca = 0; ca < a.nc; ca++)     // boucle sur a & b
        sum += a(rc,ca) * b(ca,cc);
      c(rc,cc) = sum;
    }

  return c;

}


//++
Matrix Matrix::Transpose() const
//
//	Retourne la transposée.
//--
#if HAS_NAMED_RETURN
return a(nc,nr)
#endif
{
#if !HAS_NAMED_RETURN
Matrix a(nc,nr);
#endif
  for (int i=0; i<nr; i++)
    for (int j=0; j<nc; j++)
      a(j,i) = (*this)(i,j);
#if !HAS_NAMED_RETURN
return a;
#endif
}


//++
Matrix Matrix::Inverse() const
//
//	Retourne la matrice inverse.
//
//	*Exception* : singMatxErr si la matrice est singulière.
//--
#if HAS_NAMED_RETURN
return b(nr,nc)
#endif
{
#if !HAS_NAMED_RETURN
Matrix b(nc,nr);
#endif
  Matrix a(*this);
  b = 1.0;
  if (fabs(Matrix::GausPiv(a,b)) < 1.e-50) THROW(singMatxErr);
#if !HAS_NAMED_RETURN
return b;
#endif
}


void Matrix::WriteSelf(POutPersist& s) const
{
  DBASSERT(ndata == nr*nc);
  s << nr << nc;
  s.PutR8s(data, ndata);
}

void Matrix::ReadSelf(PInPersist& s)
{
  int_4 r,c;
  s >> r >> c;
  Realloc(r,c);
  DBASSERT(ndata == nr*nc);
  s.GetR8s(data, ndata);
}


MatrixRC::MatrixRC()
: matrix(0), data(0), index(0), step(0)
{}

MatrixRC::MatrixRC(Matrix& m, RCKind rckind, int ind)
: matrix(&m), data(Org(m,rckind,ind)),
  index(ind), step(Step(m,rckind)), kind(rckind)
{
  if (kind == matrixDiag & m.nc != m.nr) THROW(sizeMismatchErr);
}


int MatrixRC::Next()
{
  if (!matrix) return -1;             // Failure ?
  if (kind == matrixDiag) return -1;  // Failure ?
  index++;
  if (kind == matrixRow) {
    if (index > matrix->nr) {
      index = matrix->nr;
      return -1;
    }
    data += matrix->nc;
  } else {
    if (index > matrix->nc) {
      index = matrix->nc;
      return -1;
    }
    data++;
  }
  return index;
}


int MatrixRC::Prev()
{
  if (!matrix) return -1;             // Failure ?
  if (kind == matrixDiag) return -1;  // Failure ?
  index--;
  if (index < 0) {
    index = 0;
    return -1;
  }
  if (kind == matrixRow)
    data -= matrix->nc;
  else
    data--;
  return index;
}


int MatrixRC::SetCol(int c)
{
  if (!matrix) return -1;             // Failure ?
  if (c<0 || c>matrix->nc) return -1;
  kind = matrixCol;
  index = c;
  step = Step(*matrix, matrixCol);
  data = Org(*matrix, matrixCol, c);
  return c;
}


int MatrixRC::SetRow(int r)
{
  if (!matrix) return -1;             // Failure ?
  if (r<0 || r>matrix->nr) return -1;
  kind = matrixRow;
  index = r;
  step = Step(*matrix, matrixRow);
  data = Org(*matrix, matrixRow, r);
  return r;
}


int MatrixRC::SetDiag()
{
  if (!matrix) return -1;             // Failure ?
  if (matrix->nc != matrix->nr) THROW(sizeMismatchErr);
  kind = matrixDiag;
  index = 0;
  step = Step(*matrix, matrixDiag);
  data = Org(*matrix, matrixDiag);
  return 0;
}


MatrixRC& MatrixRC::operator = (const MatrixRC& rc)
{
  matrix = rc.matrix;
  data   = rc.data;
  index  = rc.index;
  step   = rc.step;
  kind   = rc.kind;
  return *this;
}

//MatrixRC::operator Vector() const
Vector MatrixRC::GetVect() const
#if HAS_NAMED_RETURN
return v(NElts())
#endif
{
#if !HAS_NAMED_RETURN
  Vector v(NElts());
#endif
  int n = NElts();
  for (int i=0; i<n; i++)
    v(i) = (*this)(i);
#if !HAS_NAMED_RETURN
  return v;
#endif
}


MatrixRC& MatrixRC::operator += (const MatrixRC& rc)
{
  int n = NElts();
  if ( n != rc.NElts() ) THROW(sizeMismatchErr);
  if ( kind != rc.kind ) THROW(sizeMismatchErr);

  for (int i=0; i<n; i++)
    (*this)(i) += rc(i);

  return *this;
}



MatrixRC& MatrixRC::operator -= (const MatrixRC& rc)
{
  int n = NElts();
  if ( n != rc.NElts() ) THROW(sizeMismatchErr);
  if ( kind != rc.kind ) THROW(sizeMismatchErr);

  for (int i=0; i<n; i++)
    (*this)(i) -= rc(i);

  return *this;
}


MatrixRC& MatrixRC::operator *= (double x)
{
  int n = NElts();

  for (int i=0; i<n; i++)
    (*this)(i) *= x;

  return *this;
}


MatrixRC& MatrixRC::operator /= (double x)
{
  int n = NElts();

  for (int i=0; i<n; i++)
    (*this)(i) /= x;

  return *this;
}


MatrixRC& MatrixRC::operator -= (double x)
{
  int n = NElts();

  for (int i=0; i<n; i++)
    (*this)(i) -= x;

  return *this;
}


MatrixRC& MatrixRC::operator += (double x)
{
  int n = NElts();

  for (int i=0; i<n; i++)
    (*this)(i) += x;

  return *this;

}


double operator * (const MatrixRC& a, const MatrixRC& b)
{
  int n = a.NElts();
  if ( n != b.NElts() ) THROW(sizeMismatchErr);
  if ( a.kind != b.kind ) THROW(sizeMismatchErr);
  double sum = 0;
  for (int i=0; i<n; i++)
    sum += a(i)*b(i);
  return sum;
}


MatrixRC& MatrixRC::LinComb(double a, double b, const MatrixRC& rc, int first)
{
  int n = NElts();
  if ( n != rc.NElts() ) THROW(sizeMismatchErr);
  if ( kind != rc.kind ) THROW(sizeMismatchErr);

  for (int i=first; i<n; i++)
    (*this)(i) = (*this)(i)*a + rc(i)*b;

  return *this;
}


MatrixRC& MatrixRC::LinComb(double b, const MatrixRC& rc, int first)
{
  int n = NElts();
  if ( n != rc.NElts() ) THROW(sizeMismatchErr);
  if ( kind != rc.kind ) THROW(sizeMismatchErr);

  for (int i=first; i<n; i++)
    (*this)(i) += rc(i)*b;

  return *this;
}



MatrixRC Matrix::Row(int r) const
#if HAS_NAMED_RETURN
return rc((Matrix&)*this, matrixRow, r)
#endif
{
#if !HAS_NAMED_RETURN
        MatrixRC rc((Matrix&)*this, matrixRow, r);
        return rc;
#endif
}


MatrixRC Matrix::Col(int c) const
#if HAS_NAMED_RETURN
return rc((Matrix&)*this, matrixCol, c)
#endif
{
#if !HAS_NAMED_RETURN
        MatrixRC rc((Matrix&)*this, matrixCol, c);
        return rc;
#endif
}


MatrixRC Matrix::Diag() const
#if HAS_NAMED_RETURN
return rc((Matrix&)*this, matrixDiag)
#endif
{
#if !HAS_NAMED_RETURN
        MatrixRC rc((Matrix&)*this, matrixDiag);
        return rc;
#endif
}


int MatrixRC::IMaxAbs(int first)
{
  int n=NElts();
  if (first>n) THROW(rangeCheckErr);
  int imax=first;
  double vmax = fabs((*this)(first));
  double v;
  for (int i=first+1; i<n; i++)
    if ((v=fabs((*this)(i))) > vmax) {
      vmax = v;
      imax = i;
    }
  return imax;
}


void MatrixRC::Swap(MatrixRC& rc1, MatrixRC& rc2)
{
  int n=rc1.NElts();
  if (n != rc2.NElts()) THROW(sizeMismatchErr);
  if (rc1.kind != rc2.kind) THROW(sizeMismatchErr);
  if (rc1.data == rc2.data) return; // C'est le meme
  for (int i=0; i<n; i++)
    {double tmp = rc1(i); rc1(i) = rc2(i); rc2(i) = tmp;}
}



double Matrix::GausPiv(Matrix& a, Matrix& b)
{
  int n = a.NRows();
  if ( n != b.NRows()) THROW(sizeMismatchErr);

  // On fait une normalisation un peu brutale...
  double vmin=MAXDOUBLE;
  double vmax=0;
  for (int iii=0; iii<a.NRows(); iii++)
    for (int jjj=0; jjj<a.NCol(); jjj++) {
      if (fabs(a(iii,jjj)) > vmax) vmax = fabs(a(iii,jjj));
      if (fabs(a(iii,jjj)) < vmin && fabs(a(iii,jjj))>0) vmin = fabs(a(iii,jjj));
    }
  double nrm = sqrt(vmin*vmax);
  if (nrm > 1.e5 || nrm < 1.e-5) {
    a /= nrm;
    b /= nrm;
    //cout << "normalisation matrice " << nrm << endl;
  } else 
    nrm=1;

  double det = 1.0;
  if (nrm != 1) {
    double ld = a.NRows() * log(nrm);
    if (ld <= LN_MINDOUBLE || ld >= LN_MAXDOUBLE) {
     // cerr << "Matrix warning, overflow for det" << endl;
    } else {
      det = exp(ld);
    }
  }

  MatrixRC pivRowa(a,matrixRow);
  MatrixRC pivRowb(b,matrixRow);

  for (int k=0; k<n-1; k++) {
    int iPiv = a.Col(k).IMaxAbs(k);
    if (iPiv != k) {
      MatrixRC aIPiv(a.Row(iPiv));
      MatrixRC aK(a.Row(k));
      MatrixRC::Swap(aIPiv,aK);
      MatrixRC bIPiv(b.Row(iPiv));
      MatrixRC bK(b.Row(k));
      MatrixRC::Swap(bIPiv,bK);
    }
    double pivot = a(k,k);
    if (fabs(pivot) < 1.e-50) return 0.0;
    //det *= pivot;
    pivRowa.SetRow(k); // to avoid constructors
    pivRowb.SetRow(k);
    for (int i=k+1; i<n; i++) {
      double r = -a(i,k)/pivot;
      a.Row(i).LinComb(r, pivRowa); // + rapide que -= r * pivRowa
      b.Row(i).LinComb(r, pivRowb);
    }
  }
  det *= a(n-1, n-1);

  // on remonte

  for (int kk=n-1; kk>0; kk--) {
    double pivot = a(kk,kk);
    if (fabs(pivot) <= 1.e-50) return 0.0;
    pivRowa.SetRow(kk); // to avoid constructors
    pivRowb.SetRow(kk);
    for (int jj=0; jj<kk; jj++) {
      double r = -a(jj,kk)/pivot;
      a.Row(jj).LinComb(r, pivRowa);
      b.Row(jj).LinComb(r, pivRowb);
    }
  }

 
  for (int l=0; l<n; l++) {
    if (fabs(a(l,l)) <= 1.e-50) return 0.0;
    b.Row(l) /= a(l,l);
  }

  return det;
}

//++
double Matrix::Norm1()
//
//	Norme 1 : somme des valeurs absolues.
//--
{
  double s = 0;
  for (int k = 0; k < nr*nc; k++)
    s += fabs(data[k]);
  return s;
}

//++
double Matrix::Norm2()
//
//	Norme 2, euclidienne.
//--
{
  double s = 0;
  for (int k = 0; k < nr*nc; k++)
    s += data[k] * data[k];
  return sqrt(s);
}

//////////////////////////////////////////////////////////
//++
Matrix* Matrix::FitResidus(GeneralFit& gfit)
//
//	Retourne une classe contenant les residus du fit ``gfit''.
//	On suppose que x=j (colonnes) et y=i (lignes) pour m(i,j).
//--
{
if(NCol()<=0||NRows()<=0) return NULL;
GeneralFunction* f = gfit.GetFunction();
if(f==NULL) return NULL;
Vector par = gfit.GetParm();
Matrix* m = new Matrix(*this);
for(int i=0;i<NRows();i++) for(int j=0;j<NCol();j++) {
  double x[2] = {(double)j,(double)i};
  (*m)(i,j) -= f->Value(x,par.Data());
}
return m;
}

//++
Matrix* Matrix::FitFunction(GeneralFit& gfit)
//
//	Retourne une classe contenant la fonction du fit ``gfit''.
//	On suppose que x=j (colonnes) et y=i (lignes) pour m(i,j).
//--
{
if(NCol()<=0||NRows()<=0) return NULL;
GeneralFunction* f = gfit.GetFunction();
if(f==NULL) return NULL;
Vector par = gfit.GetParm();
Matrix* m = new Matrix(*this);
for(int i=0;i<NRows();i++) for(int j=0;j<NCol();j++) {
  double x[2] = {(double)j,(double)i};
  (*m)(i,j) = f->Value(x,par.Data());
}
return m;
}