// $Id: fmatrix.h,v 1.1.1.1 2000/11/02 08:47:16 fritzi Exp $

// This file is part of the DEAL Library
// DEAL is Copyright(1995) by
// Roland Becker, Guido Kanschat, Franz-Theo Suttmeier

#ifndef __fmatrix_h
#define __fmatrix_h

#ifndef __stdio_h
#include <stdio.h>
#endif
#ifndef __dvector_h
#include <deal/dvector.h>
#endif
#ifndef __vector_h
#include <deal/vector.h>
#endif
#ifndef __ivector_h
#include <deal/ivector.h>
#endif
#ifndef __errors_h
#include <deal/errors.h>
#endif

/*
CLASS
   FMatrix

   Full matrix of T
   */
template<class T,int M, int N>
class FMatrix
{
  T val[M][N];
  void init()
  {
    for (int i=0;i<M;i++)
	for(int j=0;j<N;j++)
	    val[i][j] = 0.;
  }
  FMatrix(const FMatrix&);

  T& el(int i, int j)  { return val[i][j];}
  T el(int i, int j) const { return val[i][j];}
 public:
  //////////
  //
  // Number of rows
  //
  int m() const { return M; }
  //////////
  //
  // Number of columns
  //
  int n() const { return N; }

  //////////
  //
  // Constructor for rectangular matrices with m rows and n columns.
  //
  FMatrix(int m=M,int n=N) { init(); }
  //////////
  //
  // Destructor.
  //
  ~FMatrix(){}

  //////////
  //
  // Reinitialization of rectangular matrix.
  //
  void reinit() { init(); }
  //////////
  //
  // Reinitialization of rectangular matrix.
  //
  void reinit(int m, int n) { init(); }
  //////////
  //
  // Reinitialization of quadratic matrix
  //
  void reinit(int n) { init(); }
  //////////
  //
  // Reinitialization to the same dimensions of another matrix.
  //
  void reinit(const FMatrix& A) { reinit(A.m(), A.n()); }

  //////////
  //
  // Read access to matrix elements.
  //
  T operator() (int i, int j) const
  {
    THROW2((i<0) || (i>=M), IntError(IntError::Range,i));
    THROW2((j<0) || (j>=N), IntError(IntError::Range,i));
    return el(i,j);
  }

  //////////
  //
  // Read-Write access to matrix elements
  //
  T& operator() (int i, int j)
  {
    THROW2((i<0) || (i>=M), IntError(IntError::Range,i));
    THROW2((j<0) || (j>=N), IntError(IntError::Range,i));
    return el(i,j);
  }

  // GROUP: Copying from other matrices

  //////////
  //
  // Assignment operator
  //
  FMatrix& operator = (const FMatrix&);
  //////////
  void add(double s, const FMatrix& src);
  //////////
  void add_diag(double s, const FMatrix& src);
  //////////
  void Tadd(double s, const FMatrix& src);

  // GROUP: Manipulation of rows and columns

  //////////
  //
  // Add different rows of a matrix
  //
  // a(i,.) += s * a(j,.)
  //
  void add_row(int i, double s, int j);
  //////////
  //
  // Add different rows of a matrix
  //
  // a(i,.) += s * a(j,.) + t * a(k,.)
  //
  void add_row(int i, double s, int j, double t, int k);
  //////////
  //
  // Add different columns of a matrix
  //
  // a(.,i) += s * a(.,j)
  //
  void add_col(int i, double s, int j);
  //////////
  //
  // Add different columns of a matrix
  //
  // a(.,i) += s * a(.,j) + t * a(.,k)
  //
  void add_col(int i, double s, int j, double t, int k);
  //////////
  //
  // Exchange contents of rows i and j
  //
  void swap_row(int i, int j);
  //////////
  //
  // Exchange contents of columns i and j
  //
  void swap_col(int i, int j);

  // GROUP: Multiplication Routines

  //////////
  //
  // Matrix-matrix-multiplication
  //
  // dst = this * src
  //
  void mmult(FMatrix& dst, const FMatrix& src) const;
  //////////
  //
  // Application of a matrix to a vector.
  //
  // [flag] adding determines if the result is copied to dst or added.
  //
  // dst (+)= this * src
  //
  void vmult(dVector& dst, const dVector& src,const int adding = 0) const;
  void vmult(Vector<T,N>& dst, const Vector<T,N>& src,const int adding = 0) const;
  //////////
  void gsmult(dVector& dst, const dVector& src,const iVector& gl) const;
  //////////
  //
  // Application of the transpose matrix to a vector.
  //
  // [flag] adding determines if the result is copied to dst or added.
  //
  // dst (+)= src * this
  //
  void Tvmult(dVector& dst,const dVector& src,const int adding=0) const;
  void Tvmult(Vector<T,N>& dst,const Vector<T,N>& src,const int adding=0) const;
  //////////
  //
  // Calculation of the residual<br>
  // dst = right - this * src<br>
  // and return of its norm.
  //
  double residual(dVector& dst, const dVector& src, const dVector& right) const;
  double residual(Vector<T,N>& dst, const Vector<T,N>& src, const Vector<T,N>& right) const;
  //////////
  //
  // Inversion of lower triangle
  //
  void forward(dVector& dst, const dVector& src) const;
  void forward(Vector<T,N>& dst, const Vector<T,N>& src) const;

  // GROUP: Solvers

  //////////
  //
  // Inversion of upper triangle
  //
  void backward(dVector& dst, const dVector& src) const;
  void backward(Vector<T,N>& dst, const Vector<T,N>& src) const;

  // GROUP: Inversion Techniques

  //////////
  //
  // Replace this by its inverse matrix calculated with Gau&szlig;-Jordan algorithm.
  //
  void gauss_jordan();

  //////////
  //
  // QR - factorization of a matrix.
  // The orthogonal transformation Q is applied to the vector y and the
  // matrix. After execution of householder, the upper triangle contains
  // the resulting matrix R, the lower the incomplete factorization matrices.
  //
  void householder(dVector& y);
  void householder(Vector<T,N>& y);

  //////////
  //
  // Least - Squares - Approximation by QR-factorization.
  // 
  //
  double least_squares(dVector& dst, dVector& src);
  double least_squares(Vector<T,N>& dst, Vector<T,N>& src);

  // GROUP: I/O

  //////////
  //
  // Output of the matrix in user-defined format.
  //
  void print(FILE* fp, const char* format = 0) const;
};

template<class T,int M, int N>
void FMatrix<T,M,N>::vmult(dVector& dst, const dVector& src,const int adding) const
{
  THROW2 (dst.n() != M, Error(Error::Dimension,"FMatrix<T,M,N>::vmult"));
  if(!adding) dst.reinit(M);
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  dst(i) += val[i][j]*src(j);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::vmult(Vector<T,N>& dst, const Vector<T,N>& src,const int adding) const
{
  THROW2 (dst.n() != M, Error(Error::Dimension,"FMatrix<T,M,N>::vmult"));
  if(!adding) dst.reinit(M);
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  dst(i) += val[i][j]*src(j);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::gsmult(dVector& dst, const dVector& src,const iVector& gl) const
{
  THROW2 (gl.n() != dst.n(), Error(Error::Dimension,"FMatrix<T,M,N>::gsmult"));
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  if(j<i)
	      dst(i) += val[i][j]*src(j);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::Tvmult(dVector& dst, const dVector& src, const int adding) const
{
  THROW2 (dst.n() != M, Error(Error::Dimension,"FMatrix<T,M,N>::vmult"));
  if(!adding) dst.reinit(M);
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  dst(i) += val[j][i]*src(j);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::Tvmult(Vector<T,N>& dst, const Vector<T,N>& src, const int adding) const
{
  THROW2 (dst.n() != M, Error(Error::Dimension,"FMatrix<T,M,N>::vmult"));
  if(!adding) dst.reinit(M);
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  dst(i) += val[j][i]*src(j);
}

template<class T,int M, int N>
double FMatrix<T,M,N>::residual(dVector& dst, const dVector& src,
			  const dVector& right) const
{
  THROW2 (dst.n() != M, Error(Error::Dimension,"FMatrix<T,M,N>::vmult"));
  double s,res = 0.;
  for (int i=0;i<M;i++)
  {
    s = right(i);
    for(int j=0;j<N;j++)
    {
      s -= val[i][j]*src(j);
    }
    res += s*s;
    dst(i) = s;
  }
}

template<class T,int M, int N>
double FMatrix<T,M,N>::residual(Vector<T,N>& dst, const Vector<T,N>& src,
			  const Vector<T,N>& right) const
{
  THROW2 (dst.n() != M, Error(Error::Dimension,"FMatrix<T,M,N>::vmult"));
  double s,res = 0.;
  for (int i=0;i<M;i++)
  {
    s = right(i);
    for(int j=0;j<N;j++)
    {
      s -= val[i][j]*src(j);
    }
    res += s*s;
    dst(i) = s;
  }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::forward(dVector& dst, const dVector& src) const
{
  int i,j;
  int nu = MIN(m(),n());
  double s;
  for (i=0;i<nu;i++)
    {
      s = src(i);
      for (j=0;j<i;j++) s -= dst(j) * el(i,j);
      dst(i) = s/el(i,i);
    }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::backward(dVector& dst, const dVector& src) const
{
  int i,j;
  int nu = MIN(m(),n());
  double s;
  for (i=nu-1;i>=0;i--)
    {
      s = src(i);
      for (j=i+1;j<nu;j++) s -= dst(j) * el(i,j);
      dst(i) = s/el(i,i);
    }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::forward(Vector<T,N>& dst, const Vector<T,N>& src) const
{
  int i,j;
  int nu = MIN(m(),n());
  double s;
  for (i=0;i<nu;i++)
    {
      s = src(i);
      for (j=0;j<i;j++) s -= dst(j) * el(i,j);
      dst(i) = s/el(i,i);
    }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::backward(Vector<T,N>& dst, const Vector<T,N>& src) const
{
  int i,j;
  int nu = MIN(m(),n());
  double s;
  for (i=nu-1;i>=0;i--)
    {
      s = src(i);
      for (j=i+1;j<nu;j++) s -= dst(j) * el(i,j);
      dst(i) = s/el(i,i);
    }
}

template<class T,int M, int N>
FMatrix<T,M,N>& FMatrix<T,M,N>::operator = (const FMatrix<T,M,N>& B)
{
  reinit(B);
  int nn = n()*m();
  for (int i=0;i<nn;i++) val[i] = B.val[i];
  return *this;
}

template<class T,int M, int N>
void FMatrix<T,M,N>::add_row(int i, double s, int j)
{
  int k;
  for (k=0;k<m();k++) el(i,k) += s*el(j,k);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::add_row(int i, double s, int j, double t, int k)
{
  int l;
  for (l=0;l<m();l++) el(i,l) += s*el(j,l) + t*el(k,l);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::add_col(int i, double s, int j)
{
  int k;
  for (k=0;k<n();k++) el(k,i) += s*el(k,j);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::add_col(int i, double s, int j, double t, int k)
{
  int l;
  for (l=0;l<n();l++) el(l,i) += s*el(l,j) + t*el(l,k);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::swap_row(int i, int j)
{
  int k;
  double s;
  for (k=0;k<m();k++)
  {
    s = el(i,k); el(i,k) = el(j,k); el(j,k) = s;
  }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::swap_col(int i, int j)
{
  int k;
  double s;
  for (k=0;k<n();k++)
  {
    s = el(k,i); el(k,i) = el(k,j); el(k,j) = s;
  }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::mmult(FMatrix<T,M,N>& dst, const FMatrix<T,M,N>& src) const
{
  int i,j,k;
  double s = 1.;
  THROW1 (m() != src.n(), Error(Error::Dimension,"mmult"));
  dst.reinit(n(), src.m());

  for (i=0;i<n();i++)
    for (j=0;j<src.m();j++)
      {
	s = 0.;
	for (k=0;k<m();k++) s+= el(i,k) * src.el(k,j);
	dst.el(i,j) = s;
      }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::print(FILE* f, const char* format) const
{
  if (!format) format = " %5.2f";
  int i,j;
  for (i=0;i<m();i++)
    {
      for (j=0;j<n();j++) fprintf(f, format, el(i,j));
      fprintf(f, "\n");
    }
}

template<class T,int M, int N>
void FMatrix<T,M,N>::add(double s,const FMatrix<T,M,N>& src)
{
  THROW1 (M != src.m(), Error(Error::Dimension,"add"));
  THROW1 (N != src.n(), Error(Error::Dimension,"add"));
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  val[i][j] += s*src(i,j);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::add_diag(double s,const FMatrix<T,M,N>& src)
{
  THROW1 (M != src.m(), Error(Error::Dimension,"sadd"));
  THROW1 (N != src.n(), Error(Error::Dimension,"sadd"));
  for (int i=0;i<M;i++)
	  val[i][i] += s*src(i,i);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::Tadd(double s,const FMatrix<T,M,N>& src)
{
  THROW1 (M != n(), Error(Error::Dimension,"Tsadd"));
  THROW1 (N != src.m(), Error(Error::Dimension,"Tsadd"));
  THROW1 (N != src.n(), Error(Error::Dimension,"Tsadd"));
  for (int i=0;i<M;i++)
      for(int j=0;j<N;j++)
	  val[i][j] += s*src(j,i);
}


template<class T,int M, int N>
void FMatrix<T,M,N>::gauss_jordan()
{
  THROW1(n()!=m(), Error(Error::Dimension,"gauss_jordan"));
  iVector p(n());

  int i,j,k,r;
  double max, hr;

  for (i=0;i<n();i++) p(i) = i;

  for (j=0;j<n();j++)
    {
      // Pivotsuche
      max = ABS(el(j,j));
      r = j;
      for (i=j+1;i<n();i++)
	{
	  if (ABS(el(i,j)) > max)
	    {
	      max = ABS(el(i,j));
	      r = i;
	    }
	}
      THROW1(max<1.e-16,Error(Error::MatrixNotRegular, "gauss_jordan"));
      // Zeilentausch
      if (r>j)
	{
	  for (k=0;k<n();k++)
	    {
	      hr = el(j,k) ; el(j,k) = el(r,k) ; el(r,k) = hr;
	    }
	  i = p(j) ; p(j) = p(r) ; p(r) = i;
	}

      // Transformation
      hr = 1./el(j,j);
      el(j,j) = hr;
      for (k=0;k<n();k++)
	{
	  if (k==j) continue;
	  for (i=0;i<n();i++)
	    {
	      if (i==j) continue;
	      el(i,k) -= el(i,j)*el(j,k)*hr;
	    }
	}
      for (i=0;i<n();i++)
	{
	  el(i,j) *= hr;
	  el(j,i) *= -hr;
	}
      el(j,j) = hr;
    }
  // Spaltentausch
  dVector hv(n());
  for (i=0;i<n();i++)
    {
      for (k=0;k<n();k++) hv(p(k)) = el(i,k);
      for (k=0;k<n();k++) el(i,k) = hv(k);
    }
}

// QR-Transformation nach Stoer 1 4.8.2 (Seite 191)


template<class T,int M, int N>
void FMatrix<T,M,N>::householder(dVector& y)
{
  // m > n, y.n() = m

  for (int j=0 ; j<n() ; j++)
  {
    double sigma = 0;
    int i;
    for (i=j ; i<m() ; i++) sigma += el(i,j)*el(i,j);
    if (ABS(sigma) < 1.e-15) return;
    double s = el(j,j);
    s = (s<0) ? sqrt(sigma) : -sqrt(sigma);
    double dj = s;

    double beta = 1./(s*el(j,j)-sigma);
    el(j,j) -= s;

    for (int k=j+1 ; k<n() ; k++)
    {
      double sum = 0.;
      for (i=j ; i<m() ; i++) sum += el(i,j)*el(i,k);
      sum *= beta;

      for (i=j ; i<m() ; i++) el(i,k) += sum*el(i,j);
    }

    double sum = 0.;
    for (i=j ; i<m() ; i++) sum += el(i,j)*y(i);
    sum *= beta;

    for (i=j ; i<m() ; i++) y(i) += sum*el(i,j);
    el(j,j) = dj;
  }
}

template<class T,int M, int N>
double FMatrix<T,M,N>::least_squares(dVector& dst, dVector& src)
{
  // m > n, m = src.n, n = dst.n

  householder(src);
  backward(dst, src);

  double sum = 0.;
  for (int i=n() ; i<m() ; i++) sum += src(i) * src(i);
  return sqrt(sum);
}

template<class T,int M, int N>
void FMatrix<T,M,N>::householder(Vector<T,N>& y)
{
  // m > n, y.n() = m

  for (int j=0 ; j<n() ; j++)
  {
    double sigma = 0;
    int i;
    for (i=j ; i<m() ; i++) sigma += el(i,j)*el(i,j);
    if (ABS(sigma) < 1.e-15) return;
    double s = el(j,j);
    s = (s<0) ? sqrt(sigma) : -sqrt(sigma);
    double dj = s;

    double beta = 1./(s*el(j,j)-sigma);
    el(j,j) -= s;

    for (int k=j+1 ; k<n() ; k++)
    {
      double sum = 0.;
      for (i=j ; i<m() ; i++) sum += el(i,j)*el(i,k);
      sum *= beta;

      for (i=j ; i<m() ; i++) el(i,k) += sum*el(i,j);
    }

    double sum = 0.;
    for (i=j ; i<m() ; i++) sum += el(i,j)*y(i);
    sum *= beta;

    for (i=j ; i<m() ; i++) y(i) += sum*el(i,j);
    el(j,j) = dj;
  }
}

template<class T,int M, int N>
double FMatrix<T,M,N>::least_squares(Vector<T,N>& dst, Vector<T,N>& src)
{
  // m > n, m = src.n, n = dst.n

  householder(src);
  backward(dst, src);

  double sum = 0.;
  for (int i=n() ; i<m() ; i++) sum += src(i) * src(i);
  return sqrt(sum);
}
#endif