//   =====================< FACTPLR.CPP >=======================
//   * Class FactoredPLR derived from Factored                 *
//   * Class FactoredGauss derived from FactoredPLR            *
//   * Class FactoredCrout derived from FactoredPLR            *
//   * Description: Chapter 15                                 *
//   * Scientific C++ Building Numerical Libraries             *
//   *                the Object-Oriented Way                  *
//   * by G. Buzzi-Ferraris                                    *
//   * Addison-Wesley (1993)                                   *
//   ===========================================================


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>

#include "utility.hpp"
#include "vector.hpp"
#include "matrix.hpp"
#include "factored.hpp"
#include "factplr.hpp"

//   ===========================================================
//   ================   protected functions   ==================
//   ===========================================================

//   *********************< FurtherInit >***********************
//   * Purpose: Terminating initialisation                     *
//   * Description: Adds specific elements                     *
//   * Example: Used internally                                *
//   ***********************************************************
void FactoredPLR::FurtherInit(void)
   {
   signd = 0;
   indx = 0;
   }

//   *****************< SpecificInitialize >********************
//   * Purpose: Initialising specific elements                 *
//   * Description: Adds specific elements                     *
//   * Example: Used internally                                *
//   ***********************************************************
void FactoredPLR::SpecificInitialize(void)
   {
   if(numRows != numColumns)
      Error("%s%s%sSpecificInitialize",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   indx = new int[numRows + 1];
   if(!indx)
      Error("%s%s%sSpecificInitialize",
      Factored::ERROR,ERR_SPACE,ERR_FUNCTION);
   }

//   ****************< SpecificDeinitialize >*******************
//   * Purpose: Deinitialisation of specific elements          *
//   ***********************************************************
void FactoredPLR::SpecificDeinitialize(void)
   {
   delete indx;
   indx = 0;
   signd = 0;
   }

//   ===========================================================
//   =================   public functions   ====================
//   ===========================================================

//   ===========================================================
//   ********************   destructor   ***********************
//   ===========================================================

//   ********************< destructor >*************************
//   * Purpose: Eliminating a specific part                    *
//   ***********************************************************
FactoredPLR::~FactoredPLR(void)
   {
   delete indx;
   }

//   ===========================================================
//   ==========   Functions for linear algebra   ===============
//   ===========================================================

//   **********************< Solution >*************************
//   * Purpose: Solving the system                             *
//   * Description: The matrix must be PLR factorised          *
//   ***********************************************************
void FactoredPLR::Solution(Vector *bx)
   {
   PLRSolution(numRows,factored,bx->vector,indx);
   }

//   **********************< Solution >*************************
//   * Purpose: Solving the system                             *
//   * Description: The matrix must be PLR factorised          *
//   ***********************************************************
void FactoredPLR::Solution(const Vector &b,Vector *x)
   {
   *x = b;
   PLRSolution(numRows,factored,x->vector,indx);
   }

//   *****************< TransposeSolution >*********************
//   * Purpose: Solving the system  for A transposed           *
//   * Description: The matrix must be PLR factorised          *
//   ***********************************************************
void FactoredPLR::TransposeSolution(Vector *bx)
   {
   PLRTransposeSolution(numRows,factored,bx->vector,indx);
   }

//   *****************< TransposeSolution >*********************
//   * Purpose: Solving the system  for A transposed           *
//   * Description: The matrix must be PLR factorised          *
//   ***********************************************************
void FactoredPLR::TransposeSolution
                (const Vector &b,Vector *x)
   {
   *x = b;
   PLRTransposeSolution(numRows,factored,x->vector,indx);
   }

//   **********************< Solution >*************************
//   * Purpose: Solving the system  for matrices               *
//   * Description: The matrix must be PLR factorised          *
//   ***********************************************************
void FactoredPLR::Solution(Matrix *BX)
   {
   Vector b(numColumns);
   for(int j = 1;j <= BX->Columns();j++)
      {
      b = BX->GetColumn(j);
      PLRSolution(numColumns,factored,b.vector,indx);
      BX->SetColumn(j,b);
      }
   }

//   **********************< Solution >*************************
//   * Purpose: Solving the system  for matrices               *
//   * Description: The matrix must be PLR factorised          *
//   ***********************************************************
void FactoredPLR::Solution(const Matrix &B,Matrix *X)
   {
   *X = B;
   Solution(X);
   }

//   *********************< Condition >*************************
//   * Purpose: Calculating the condition number               *
//   ***********************************************************
float FactoredPLR::Condition(void)
   {
   return Control((double)norm*PLRNormInvEst
                            (numRows,factored,indx));
   }

//   ****************< DeterminantEvaluation >******************
//   * Purpose: Calculating the determinant                    *
//   ***********************************************************
double FactoredPLR::DeterminantEvaluation(void)
   {
   double determinant = (double)signd;
   for(int i = 1;i <= numRows;i++)
      determinant *= factored[i][i];
   return determinant;
   }

//   ===========================================================
//   ===================   Algorithms   ========================
//   ===========================================================

//   *******************< PLRSolution >*************************
//   * Purpose: Solving a system with matrix factorisation     *
//   *          with the PLR method                            *
//   ***********************************************************
void PLRSolution(int n,float **a,float *b,int *indx)
   {
   int i,j,pivot;
   double sum;
   float *ai;
   for(i = 1;i <= n;i++)
      {
      pivot = indx[i];
      sum = b[pivot];
      b[pivot] = b[i];
      if(a[i][i] == 0.)
        {
        Message("\nSingular matrix");
        if(b[i] != 0.)
           Message(" and incompatible system");
        b[i] = 0.;
        }
      else
        {
        ai = a[i];
        for(j = 1;j <= i-1;j++)
           sum -= (*++ai)*b[j];
        b[i] = Control(sum);
        }
      }
   for(i = n;i >= 1;i--)
      {
      sum = b[i];
      ai = a[i] + i + 1;
      if(a[i][i] != 0.)
        {
        for(j = i+1;j <= n;j++)
           sum -= (*ai++)*b[j];
        b[i] = Control(sum/a[i][i]);
        }
      else
        b[i] = 0.;
      }
   }

//   ***************< PLRTransposeSolution >********************
//   * Purpose: Solving a system with factorised matrix        *
//   *          with the PLR method on the transposed matrix   *
//   ***********************************************************
void PLRTransposeSolution(int n,float **a,float *b,int *indx)
   {
   double temp;
   int i,k,j;
   for(k = 1;k <= n;k++)
      {
      if(a[k][k] != 0.)
        {
        temp = b[k];
        for(i = 1;i < k;i++) temp -= a[i][k]*b[i];
        b[k] = Control(temp/a[k][k]);
        }
      else
        {
        Message("\nSingular matrix");
        b[k] = 0.;
        }
      }
   for(k = n;k >= 1;k--)
      {
      temp = b[k];
      for(i = k+1;i <= n;i++)
        temp -= a[i][k]*b[i];
      b[k] = Control(temp);
      }
   for(k = n;k >= 1;k--)
      {
      j = indx[k];
      if(j != k)
        Swap(&b[j],&b[k]); // in UTILITY.CPP
      }
   }

//   *******************< PLRNormInvEst >***********************
//   * Purpose: Estimating the matrix inverse norm             *
//   * Description: Serves for the condition number            *
//   ***********************************************************
float PLRNormInvEst(int n,float **a,int *indx)
   {
   float yp,ym,tempp,tempm,temp;
   float ynorm,znorm,normaiest;
   int i,j,k;
   for(j = 1;j <= n;j++)
      if(a[j][j] == 0.)
        return BIG_FLOAT;
   float *y = new float [n + 1];
   Vector pp(n),pm(n);
   y[1] = 1./a[1][1];
   for(i = 2;i <= n;i++)
      pp[i] = a[1][i]*y[1];
   for(j = 2;j <= n;j++)
      {
      yp = (1.-pp[j])/a[j][j];
      ym = (-1.-pp[j])/a[j][j];
      tempp = Abs(yp);
      tempm = Abs(ym);
      for(i = j+1;i <= n;i++)
        {
        pm[i] = pp[i]+a[j][i]*ym;
        tempm += Abs(pm[i]/a[i][i]);
        pp[i] += a[j][i]*yp;
        tempp += Abs(pp[i]/a[i][i]);
        }
     if(tempp >= tempm)
       y[j] = yp;
     else
       {
       y[j] = ym;
       for(i = j+1;i <= n;i++)
          pp[i] = pm[i];
       }
     }
 // Lty = y 
   for(k=n;k>=1;k--)
      {
      double tmp = y[k];
      for(i = k+1;i <= n;i++)
        tmp -= a[i][k]*y[i];
      y[k] = Control(tmp);
      }
// Pty = y 
   for(k = n;k >= 1;k--)
      {
      j = indx[k];
      if(j != k)
        Swap(&y[j],&y[k]);
      }
// norm 1 of y
   for(i = 1,ynorm = 0.;i <= n;i++)
      ynorm += Abs(y[i]);
   if(ynorm==0.)
      {
      delete y;
      return BIG_FLOAT;
      }
   PLRSolution(n,a,y,indx);
// norm 1
   for(i = 1,znorm=0.;i <= n;i++)
      znorm += Abs(y[i]);
   normaiest = znorm/ynorm;
   delete y;
   return normaiest;
   }

//   ===========================================================
//   ===============   class FactoredGauss   ===================
//   ===========================================================

//   ********************< constructor >************************
//   * Purpose: Sizing and initialising                        *
//   * Example: FactoredGauss A(2,2,1.,2.,3.,4.);              *
//   ***********************************************************
FactoredGauss::FactoredGauss
              (int rows,int columns,double a11,...)
              : FactoredPLR('*',rows,columns)
   {
   float *w=matrix[0] + 1;
   va_list puntaList;
   va_start(puntaList,a11);
   int i;
   *w = Control(a11);
   for(i = 2;i < size;i++)
      *++w = Control(va_arg(puntaList,double));
   va_end(puntaList);
   }

//   ********************< destructor >*************************
//   * Purpose: Must not deinitialise anything                 *
//   ***********************************************************
FactoredGauss::~FactoredGauss(void)
   { }

//   ===========================================================
//   ***************   assignment operators   ******************
//   ===========================================================

//   *************************< = >*****************************
//   * Purpose: Assignment of a FactoredGauss                  *
//   * Description: Use forbidden                              *
//   ***********************************************************
void FactoredGauss::operator =
              (const FactoredGauss &rval)
   {
   Error("\n%sAn assignment is being used %s"
       "\n with Factored N. %d",Factored::ERROR,rval.whoAmI);
   }

//   *************************< = >*****************************
//   * Purpose: Assignment of a Matrix                         *
//   ***********************************************************

FactoredGauss &FactoredGauss::operator = (const Matrix &rval)
   {
   CopyFromMatrix(rval);
   return *this;
   }

//   ===========================================================
//   *************   Functions for linear algebra **************
//   ===========================================================

//   ******************< Factorization >************************
//   * Purpose: Factorisation with Gauss                       *
//   ***********************************************************
void FactoredGauss::Factorization(void)
   {
   singular = GaussFactorization
             (numRows,factored,indx,&signd);
   }

//   ===========================================================
//   ===================   Algorithms   ========================
//   ===========================================================

//   ****************< GaussFactorization >*********************
//   * Purpose: Factorisation with the Gauss method            *
//   ***********************************************************
char GaussFactorization(int n,float **a,int *indx,int *signd)
   {
   Vector d(n);
   double sum;
   char sing = 0;
   int i,r,j,k;
   const float EPS2 = 2.*MACH_EPS;
   *signd = 1.; // for the sign of the determinant

   for(i = 1;i <= n;i++)
      {
      d[i] = MaxAbs(n,a[i]+1); // for implicit balance
      if(d[i] == 0.)
	d[i] = BIG_FLOAT; // put at the end!
      }

   for(r = 1;r < n;r++) 
      {
      double max = Abs(a[r][r]/d[r]); // balanced pivot !
      int pivot = r;

      for(i = r + 1;i <= n;i++)
        {
        double temp = Abs(a[i][r]/d[i]);
        if(temp > max)
           { max = temp; pivot = i; }
        }

        if(r != pivot)
           {
           Swap(&a[pivot],&a[r]); // swaps pointers!
           Swap(&d[pivot],&d[r]); // in UTILITY.CPP
           *signd = -*signd; // changes sign
           indx[r] = pivot;
           }
        else
           indx[r] = r;
      float *ar = a[r];
      float *arr = ar + r;
      if(*arr == 0.)
        sing = 1;
      else
        {
        for(i = r + 1;i <=n;i++) 
           {
           float *air = a[i] + r;
           if(*air != 0.)
              {
              double temp = *air/(*arr);
              *air = Control(temp);
              for(j = r+1;j <= n;j++)
                {
                sum = *++air;
                sum -= temp * (*(ar + j));
                if(Abs(sum) < EPS2 * Abs(*air))
                   *air = 0.;
                else
                   *air = Control(sum);
                }
              }
           }
        }
      }
   indx[n] = n;
   if(a[n][n] == 0.)sing = 1;
   return sing;
   }

//   ===========================================================
//   ===============   class FactoredCrout   ===================
//   ===========================================================

//   ********************< constructor >************************
//   * Purpose: Sizing and initialising                        *
//   * Example: FactoredCrout A(2,2,1.,2.,3.,4.);              *
//   ***********************************************************
FactoredCrout::FactoredCrout
              (int rows,int columns,double a11,...)
              : FactoredPLR('*',rows,columns)
   {
   float *w=matrix[0] + 1;
   va_list puntaList;
   va_start(puntaList,a11);
   int i;
   *w = Control(a11);
   for(i = 2;i < size;i++)
      *++w = Control(va_arg(puntaList,double));
   va_end(puntaList);
   }

//   ********************< destructor >*************************
//   * Purpose: Must not deinitialise anything                 *
//   ***********************************************************
FactoredCrout::~FactoredCrout(void)
   { }

//   ===========================================================
//   ***************   assignment operators   ******************
//   ===========================================================

//   *************************< = >*****************************
//   * Purpose: Assignment of a FactoredCrout                  *
//   * Description: Use forbidden                              *
//   ***********************************************************
void FactoredCrout::operator =
              (const FactoredCrout &rval)
   {
   Error("\n%sAn assignment is used %s"
       "\n con Factored N. %d",Factored::ERROR,rval.whoAmI);
   }

//   *************************< = >*****************************
//   * Purpose: Assignment of a Matrix                         *
//   ***********************************************************

FactoredCrout &FactoredCrout::operator = (const Matrix &rval)
   {
   CopyFromMatrix(rval);
   return *this;
   }

//   ===========================================================
//   ************   Functions for linear algebra  **************
//   ===========================================================

//   ******************< Factorization >************************
//   * Purpose: Interface for factorisation                    *
//   ***********************************************************
void FactoredCrout::Factorization(void)
   {
   singular = CroutFactorization(numRows,factored,
                             indx,&signd);
   }

//   ===========================================================
//   ===================   Algorithms  =========================
//   ===========================================================

//   ****************< CroutFactorization >*********************
//   * Purpose: Factorisation with Crout method                *
//   ***********************************************************
char CroutFactorization(int n,float **a,int *indx,int *signd)
   {
   Vector d(n);
   double sum;
   char sing = 0;
   int i,r,j,k;
   *signd = 1.; // for the sign of the determinant

   for(i = 1;i <= n;i++)
      {
      d[i] = MaxAbs(n,a[i]+1); // for implicit balance
      if(d[i] == 0.)
        d[i] = BIG_FLOAT; //put at the end!
      }

   for(r = 1;r <= n;r++) 
      {
      for(i = r;i <= n;i++) // construct L
        {
        sum = a[i][r];
        float *ai = a[i];  // avoids double index
        for(k = 1;k <= r-1;k++)
           sum -= *++ai * a[k][r];  
        a[i][r] = Control(sum);
        }

      double max = Abs(a[r][r]/d[r]); // balanced pivot!
      int pivot = r;
      for(i = r + 1;i <= n;i++)
        {
        double temp = Abs(a[i][r]/d[i]);
        if(temp > max)
           { max = temp; pivot = i; }
        }
        if(r != pivot)
           {
           Swap(&a[pivot],&a[r]); // swap pointers!
           Swap(&d[pivot],&d[r]); // in UTILITY.CPP
           *signd = -*signd; // changes sign
           indx[r] = pivot;
           }
        else
           indx[r] = r;

      if(a[r][r] == 0.)
        sing = 1;
      else
        {
        for(j = r + 1;j <=n;j++) // constructs R
           {
           sum = a[r][j];
           float *ar = a[r];
           for(k = 1;k <= r -1;k++)
              sum -= *++ar * a[k][j];
           a[r][j] = Control(sum/a[r][r]); 
           }
        }
      }

// Transform in Doolittle
   for(r = 1;r < n;r++)
      {
      if(a[r][r] != 0.)
        {
        for(i = r + 1;i<=n;i++)
           {
           a[r][i] = Control(a[r][i]*a[r][r]);
           a[i][r] = Control(a[i][r]/a[r][r]);
           }
        }
      }
   return sing;
   }