//   =====================< FACTQRLQ.CPP >======================
//   * Class FactoredQRLQ derived from Factored                *
//   * Class FactoredQR derived from FactoredQRLQ              *
//   * Class FactoredLQ derived from FactoredQRLQ              *
//   * 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 "right.hpp"
#include "left.hpp"

#include "factored.hpp"
#include "factqrlq.hpp"

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

//   *********************< FurtherInit >***********************
//   * Purpose: Terminating initialisation                     *
//   * Description: Adds specific elements                     *
//   * Example: Internal use                                   *
//   ***********************************************************
void FactoredQRLQ::FurtherInit(void)
   {
   indx = 0;
   dqr = bqr = 0;  //for LQ and QR
   }

//   *****************< SpecificInitialize >********************
//   * Purpose: Initialising specific elements                 *
//   * Description: Adds specific elements                     *
//   ***********************************************************
void FactoredQRLQ::SpecificInitialize(void)
   {
   int minRowsColumns = Min(numRows,numColumns);
   indx = new int[numRows + 1];
   if(!indx)
      Error("%s%s%sSpecificInitialize",
      Factored::ERROR,ERR_SPACE,ERR_FUNCTION);
   dqr = new float[minRowsColumns + 1];
   if(!dqr)
      Error("%s%s%sSpecificInitialize",
      Factored::ERROR,ERR_SPACE,ERR_FUNCTION);
   for(int j=1;j <= minRowsColumns;j++)dqr[j] = 0.;
   bqr=new float[numRows + 1];
   if(!bqr)
      Error("%s%s%sSpecificInitialize",
      Factored::ERROR,ERR_SPACE,ERR_FUNCTION);
   for(int i = 1;i <= numRows;i++) bqr[i] = 0.;
   }

//   ****************< SpecificDeinitialize >*******************
//   * Purpose: Deinitialisation of specific elements          *
//   ***********************************************************
void FactoredQRLQ::SpecificDeinitialize(void)
   {
   delete indx;
   delete dqr;
   delete bqr;
   indx = 0;
   dqr = bqr = 0;  //for LQ and QR
   }

//   ===========================================================
//   =================   public functions   ====================
//   ===========================================================
//   ********************< constructor >************************
//   * Purpose: Sizing and initialising                        *
//   * Example: FactoredQR A(3,2,1.,2.,3.,4.,5.,6.);           *
//   ***********************************************************
FactoredQR::FactoredQR
              (int rows,int columns,double a11,...)
              : FactoredQRLQ('*',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);
   }

//   ********************< constructor >************************
//   * Purpose: Sizing and initialising                        *
//   * Example: FactoredLQ A(2,3,1.,2.,3.,4.,5.,6.);           *
//   ***********************************************************
FactoredLQ::FactoredLQ
              (int rows,int columns,double a11,...)
              : FactoredQRLQ('*',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   ***********************
//   ===========================================================

//   ********************< destructor >*************************
//   * Purpose: Freeing the specific memory of QRLQ            *
//   ***********************************************************
FactoredQRLQ::~FactoredQRLQ(void)
   {
   delete indx;
   delete dqr;
   delete bqr;
   }

//   ********************< destructor >*************************
//   * Description: Not having any further memory to free      *
//   ***********************************************************
FactoredQR::~FactoredQR(void)
   { }

//   ********************< destructor >*************************
//   * Description: Not having any further memory to free      *
//   ***********************************************************
FactoredLQ::~FactoredLQ(void)
   { }

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

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

//   *************************< = >*****************************
//   * Purpose: Assignment of a Matrix                         *
//   ***********************************************************
FactoredQR &FactoredQR::operator = (const Matrix &rval)
   {
   CopyFromMatrix(rval);
   return *this;
   }

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

//   *************************< = >*****************************
//   * Purpose: Assignment of a Matrix                         *
//   ***********************************************************
FactoredLQ &FactoredLQ::operator = (const Matrix &rval)
   {
   CopyFromMatrix(rval);
   return *this;
   }

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

//   ******************< Factorization >************************
//   * Purpose: Factorising with QR                            *
//   ***********************************************************
void FactoredQR::Factorization(void)
   {
   if(numRows >= numColumns)
      singular = QRFactorization
               (numRows,numColumns,factored,dqr);
   else
      Error("%s%s%sFactorization QR",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   ******************< Factorization >************************
//   * Purpose: Factorising with LQ                            *
//   ***********************************************************
void FactoredLQ::Factorization(void)
   {
   if(numRows <= numColumns)
      singular = LQFactorization
               (numRows,numColumns,factored,dqr);
   else
      Error("%s%s%sFactorization LQ",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with QR                 *
//   ***********************************************************
void FactoredQR::Solution(Vector *bx)
   {
   Vector x(numColumns);
   for(int i=1;i <= numRows;i++)bqr[i] = (*bx)[i];
   if(numRows >= numColumns)
      {
      QRSolution(numRows,numColumns,factored,
               dqr,bqr,x.vector);
      *bx = x;
      }
   else
      Error("%s%s%sSolution QR",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with QR                 *
//   ***********************************************************
void FactoredQR::Solution(const Vector &b,Vector *x)
   {
   ChangeDimensions(numColumns,x);
   for(int i=1;i <= numRows;i++)bqr[i] = b.GetValue(i);
   if(numRows >= numColumns)
      QRSolution(numRows,numColumns,
               factored,dqr,bqr,x->vector);
   else
      Error("%s%s%sSolution QR",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with LQ                 *
//   ***********************************************************
void FactoredLQ::Solution(Vector *bx)
   {
   Vector x(numColumns);
   for(int i=1;i <= numRows;i++)bqr[i] = (*bx)[i];
   if(numRows <= numColumns)
      {
      LQSolution(numRows,numColumns,factored,
               dqr,bqr,x.vector);
      *bx = x;
      }
   else
      Error("%s%s%sSolution LQ",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with LQ                 *
//   ***********************************************************
void FactoredLQ::Solution(const Vector &b,Vector *x)
   {
   ChangeDimensions(numColumns,x);
   for(int i=1;i <= numRows;i++)bqr[i] = b.GetValue(i);
   if(numRows <= numColumns)
      LQSolution(numRows,numColumns,
               factored,dqr,bqr,x->vector);
   else
      Error("%s%s%sSolution LQ",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with QR                 *
//   ***********************************************************
void FactoredQR::Solution(Matrix *BX)
   {
   Vector x(numColumns);
   if(numRows == numColumns)
      {
      for(int j = 1;j <= BX->Columns();j++)
        {
        for(int i=1;i <= numRows;i++)bqr[i] = (*BX)[i][j];
        QRSolution(numRows,numColumns,factored,
                  dqr,bqr,x.vector);
        BX->SetColumn(j,x);
        }
      }
   else if(numRows > numColumns)
      {
      Matrix X(numColumns,BX->Columns());
      for(int j = 1;j <= BX->Columns();j++)
        {
        for(int i=1;i <= numRows;i++)bqr[i] = (*BX)[i][j];
        QRSolution(numRows,numColumns,factored,
                  dqr,bqr,x.vector);
        X.SetColumn(j,x);
        }
      (*BX) = X;
      }
   else
      Error("%s%s%sSolution QR",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with QR                 *
//   ***********************************************************
void FactoredQR::Solution(const Matrix &B,Matrix *X)
   {
   Vector x(numColumns);
   ChangeDimensions(numColumns,B.Columns(),X);
   if(numRows >= numColumns)
      {
      for(int j = 1;j <= B.Columns();j++)
        {
        for(int i=1;i <= numRows;i++)
            bqr[i] = B.GetValue(i,j);
        QRSolution(numRows,numColumns,factored,
                  dqr,bqr,x.vector);
        X->SetColumn(j,x);
        }
      }
   else
      Error("%s%s%sSolution QR",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with LQ                 *
//   ***********************************************************
void FactoredLQ::Solution(Matrix *BX)
   {
   Vector x(numColumns);
   if(numRows == numColumns)
      {
      for(int j = 1;j <= BX->Columns();j++)
        {
        for(int i=1;i <= numRows;i++)bqr[i] = (*BX)[i][j];
        LQSolution(numRows,numColumns,factored,
                  dqr,bqr,x.vector);
        BX->SetColumn(j,x);
        }
      }
   else if(numRows < numColumns)
      {
      Matrix X(numColumns,BX->Columns());
      for(int j = 1;j <= BX->Columns();j++)
        {
        for(int i=1;i <= numRows;i++)
           bqr[i] = (*BX)[i][j];
        LQSolution(numRows,numColumns,factored,
                  dqr,bqr,x.vector);
        X.SetColumn(j,x);
        }
        (*BX) = X;
       }
   else
      Error("%s%s%sSolution LQ",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   **********************< Solution >*************************
//   * Purpose: Linear system solution with  LQ                *
//   ***********************************************************
void FactoredLQ::Solution(const Matrix &B,Matrix *X)
   {
   Vector x(numColumns);
   ChangeDimensions(numColumns,B.Columns(),X);
   if(numRows <= numColumns)
      {
      for(int j = 1;j <= B.Columns();j++)
        {
        for(int i=1;i <= numRows;i++)
           bqr[i] = B.GetValue(i,j);
        LQSolution(numRows,numColumns,factored,
                  dqr,bqr,x.vector);
        X->SetColumn(j,x);
        }
      }
   else
      Error("%s%s%sSolution LQ",
      Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   }

//   *****************< TransposeSolution >*********************
//   * Description: Not implemented                            *
//   ***********************************************************
void FactoredQR::TransposeSolution(const Vector &b,Vector *x)
   {
   Error("%s%s",Factored::ERROR,ERR_IMPLEMENTATION);
   b.GetValue(1); (*x)[1] = 0;
   }

//   **************< TransposeSolution >************************
//   * Description: Not implemented                            *
//   ***********************************************************
void FactoredQR::TransposeSolution(Vector *bx)
   {
   Error("%s%s",Factored::ERROR,ERR_IMPLEMENTATION);
   (*bx)[1] = 0;
   }

//   **************< TransposeSolution >************************
//   * Description: Not implemented                            *
//   ***********************************************************
void FactoredLQ::TransposeSolution(const Vector &b,Vector *x)
   {
   Error("%s%s",Factored::ERROR,ERR_IMPLEMENTATION);
   b.GetValue(1); (*x)[1] = 0;
   }

//   **************< TransposeSolution >************************
//   * Description: Not implemented                            *
//   ***********************************************************
void FactoredLQ::TransposeSolution(Vector *bx)
   {
   Error("%s%s",Factored::ERROR,ERR_IMPLEMENTATION);
   (*bx)[1] = 0;
   }

//   *********************< Condition >*************************
//   * Purpose: Calculating the condition number QR            *
//   ***********************************************************
float FactoredQR::Condition(void)
   {
   return QRCondition(numRows,factored,dqr);
   }

//   *********************< Condition >*************************
//   * Purpose: Calculating the condition number for QL        *
//   ***********************************************************
float FactoredLQ::Condition(void)
   {
   return LQCondition(numRows,factored,dqr);
   }

//   ****************< DeterminantEvaluation >******************
//   * Purpose: Calculating the determinant separately         *
//   *          from the sign                                  *
//   ***********************************************************
double FactoredQRLQ::DeterminantEvaluation(void)
   {
   double determinant = 1.;
   for(int i = 1;i <= numRows;i++)
      determinant *= dqr[i];
   return determinant;
   }

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

//   ******************< QRFactorization >**********************
//   * Purpose: Factorising QR with Householder                *
//   * Description: For matrices with m >= n                   *
//   ***********************************************************
char QRFactorization(int m,int n,float **a,float *d)
   {
   if(m < n)
      Error("m<n in QRDec");
   const float SIN_TINY = 10.*MACH_EPS;
   int i,j;
   float *aux = new float[m+1];
   float *norm=new float[n+1];
   for(j = 1;j <= n;j++)
      {
      for(i = 1;i <= m;i++)
        aux[i] = a[i][j];
      norm[j] = SqrtSumSqr(n,aux+1);
      }
   char sing = 0;
   for(j = 1;j <= n;j++)
      {
      for(i = j;i <= m;i++)
        aux[i] = a[i][j];
      Householder(j,m,aux,&d[j]);
      if(Abs(d[j]) <= SIN_TINY*norm[j] || norm[j] == 0.)
        {
        sing = 1;
        d[j] = 0.;
        Message("\nSingular matrix in QRDec");
        for(i = j;i <= m;i++)
           { aux[i] = a[i][j] = 0.;}
        aux[j] = a[j][j] = 1.;
        }
      else
        for(i = j;i <= m;i++)
           a[i][j]=aux[i];
      HouseholderApplyLeft(j,m,j+1,n,a,aux);
      }
   delete aux;
   delete norm;
   return sing;
   }

//   ********************< QRSolution >*************************
//   * Purpose: Solving a QR factorised system                 *
//   ***********************************************************
void QRSolution(int m,int n,float **a,
              float *d,float *b,float *x)
   {
   int j,k,i;
   double s;
   if(m < n)Error("m<n in QRSolution");
   for(j=1;j<=n;j++)
      {
      s=0.;
      for(k = j;k <= m;k++)
        s -= a[k][j]*b[k];
      s += s;
      for(k = j;k <= m;k++)
        b[k] = Control(b[k] + s*a[k][j]);
      }
   for(i = n;i >= 1;i--)
      {
      if(d[i]==0.)
        {
        Message("\nSingular matrix in QRSolution!");
        x[i]=0.;
        }
      else
        {
        s = b[i];
        for(k = i+1;k <= n;k++)
           s -= a[i][k]*x[k];
        x[i] = Control(s/d[i]);
        }
      }
   }

//   ******************< LQFactorization >**********************
//   * Purpose: Factorising LQ with Householder                *
//   * Description: For matrices with m <= n                   *
//   ***********************************************************
char LQFactorization(int m,int n,float **a,float *d)
   {
   if(m > n)
      Error("m>n LQ");
   float *aux=new float[n+1];
   float *norm=new float[m+1];
   char sing = 0;
   int i,j,k;
   const float SIN_TINY = 10.*MACH_EPS;
   for(i = 1;i <= m;i++)
      {
      for(j = 1;j <= n;j++)
        aux[j] = a[i][j];
      norm[i] = SqrtSumSqr(n,aux+1);
      }
   for(i=1,k=1;i<=m;i++,k++)
      {
      for(j = k;j <= n;j++)
        aux[j] = a[i][j];
      Householder(k,n,aux,&d[i]);
      // Checks whether the sine of the angle between
      // the new vector and the preceding space
      // is greater than SIN_TINY
      if(Abs(d[i]) <= SIN_TINY*norm[i] || norm[i] == 0.)
        {
        sing = 1;
        d[i] = 0.;
        k--;     // Eliminates that row
        Message
        ("\nSingular matrix in LQFactorization");
        }
      else
        {
        for(j = k;j <= n;j++)
           a[i][j]=aux[j];
        HouseholderApplyRight(i+1,m,k,n,a,aux);
        }
      }
   delete aux;
   delete norm;
   return sing;
   }

//   ********************< LQSolution >*************************
//   * Description: For use with LQFactorization               *
//   ***********************************************************
void LQSolution(int m,int n,float **a,
              float *d,float *b,float *x)
   {
   double s;
   int l=0;
   if(m > n)Error("m > n in LQSolution");
   for(int i = 1;i <= n;i++)
      x[i] = 0.;
   for(i = 1;i <= m;i++)
      {
      if(d[i] != 0.)
        {
        s = b[i];
        for(int k = 1;k < i-l;k++)
           s -= a[i][k]*x[k];
        x[i-l]= Control(s/d[i]);
        }
      else l++;
      }
   for(int j = m;j >= 1;j--)
      {
      if(d[j] != 0.)
        {
        s = 0.;
        for(int k = j-l;k <= n;k++)
           s -= a[j][k]*x[k];
        s += s;
        for(k = j-l;k <= n;k++)
           x[k] = Control(x[k]+s*a[j][k]);
        }
      else l--;
      }
   }

//   *******************< QRCondition >*************************
//   * Purpose: Estimation of the condition number for a       *
//   *          square matrix factorised with QR               *
//   ***********************************************************
float QRCondition(int n,float **a,float *d)
   {
   float yp,ym,tempp,tempm,temp;
   float ynorm,znorm,condition;
   int i,j,k;
   for(j = 1;j <= n;j++)
      if(d[j] == 0.)
        return BIG_FLOAT;
   Vector y(n),pp(n),pm(n);
   condition = Abs(d[1]);
   for(j = 2;j <= n;j++)
      {
      temp = Abs(d[j]);
      for(i = 1;i < j;i++)
        temp += Abs(a[i][j]);
      if(temp > condition)
        condition = temp;
      }
   y[1] = 1./d[1];
   for(i = 2;i <= n;i++)
      pp[i] = a[1][i]*y[1];
   for(j = 2;j <= n;j++)
      {
      yp = (1.-pp[j])/d[j];
      ym = (-1.-pp[j])/d[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]/d[i]);
        pp[i] += a[j][i]*yp;
        tempp += Abs(pp[i]/d[i]);
        }
      if(tempp >= tempm)
        y[j] = yp;
      else
        {
        y[j] = ym;
        for(i = j+1;i <= n;i++)
           pp[i] = pm[i];
        }
      }
// norm 1 of y
   for(i = 1,ynorm=0.;i <= n;i++)
      ynorm += Abs(y[i]);
   if(ynorm == 0.)
      {
    return BIG_FLOAT;
      }
   condition /= ynorm;
   y[n] /= d[n];
   for(i = n-1;i >= 1;i--)
      {
      double tmp = y[i];
      for(j = i+1;j <= n;j++)
        tmp -= a[i][j]*y[j];
      y[i] = Control(tmp/d[i]);
      }
// norm 1
   for(i = 1,znorm = 0.;i <= n;i++)
      znorm += Abs(y[i]);
   condition *= znorm;
   return condition;
   }

//   *******************< LQCondition >*************************
//   * Purpose: Estimation of the condition number for a       *
//   *          square matrix factorised with LQ               *
//   ***********************************************************
float LQCondition(int n,float **a,float *d)
   {
   float yp,ym,tempp,tempm,temp;
   float ynorm,znorm,condition;
   int i,j,k;
   for(j = 1;j <= n;j++)
      if(d[j] == 0.)
        return BIG_FLOAT;
   Vector y(n),pp(n),pm(n);
   condition = Abs(d[1]);
   for(j = 2;j <= n;j++)
      {
      temp = Abs(d[j]);
      for(i = 1;i < j;i++) temp += Abs(a[j][i]);
      if(temp > condition)
        condition = temp;
      }
   y[1] = 1./d[1];
   for(i = 2;i <= n;i++)
      pp[i] = a[i][1]*y[1];
   for(j = 2;j <= n;j++)
      {
      yp = (1.-pp[j])/d[j];
      ym = (-1.-pp[j])/d[j];
      tempp = Abs(yp);
      tempm = Abs(ym);
      for(i = j+1;i <= n;i++)
        {
        pm[i] = pp[i] + a[i][j]*ym;
        tempm += Abs(pm[i]/d[i]);
        pp[i] += a[i][j]*yp;
        tempp += Abs(pp[i]/d[i]);
        }
      if(tempp >= tempm)
        y[j] = yp;
      else
        {
        y[j] = ym;
        for(i = j+1;i <= n;i++)
           pp[i] = pm[i];
        }
      }
// norm 1 of y
   for(i = 1,ynorm = 0.;i <= n;i++)
      ynorm += Abs(y[i]);
   if(ynorm == 0.)
      return BIG_FLOAT;
   condition /= ynorm;
   y[n] /= d[n];
   for(i = n-1;i >= 1;i--)
      {
      double tmp = y[i];
      for(j = i+1;j <= n;j++)
        tmp -= a[j][i]*y[j];
      y[i] = Control(tmp/d[i]);
      }
// norm 1
   for(i = 1,znorm=0.;i <= n;i++)
      znorm += Abs(y[i]);
   condition = Control(condition*znorm);
   return condition;
   }

//   ===========================================================
//   ======   Functions which modify the matrix  ===============
//   ===========================================================

//   ********************< GetMatrixP >*************************
//   * Purpose: Calculating the matrix P from the              *
//   *          factorisation QR                               *
//   * Description: PA = R                                     *
//   * Example: A.GetMatrixP(&P);                              *
//   ***********************************************************
void FactoredQR::GetMatrixP(Matrix *P)
   {
   int j,k,i;
   double s;
   if(numRows < numColumns)
      Error("%s%s%s GetMatrixP",
        Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   PrepSolve();
   ChangeDimensions(numRows,numRows,P);
   for(i = 1;i <= numRows;i++)
      (*P)[i][i]=1.;

      for(j = numColumns;j >= 1;j--)
      {
      for(i = j;i <= numRows;i++)
        {
        s = 0.;
        for(k = j;k <= numRows;k++)
           s += factored[k][j]*(*P)[i][k];
        s += s;
        for(k = j;k <= numRows;k++)
           (*P)[i][k] -= Control(s*factored[k][j]);
        }
      }
   }

//   ********************< GetMatrixQ >*************************
//   * Purpose: Calculating the matrix Q from                  *
//   *          the factorisation QR                           *
//   * Description: A = QR                                     *
//   * Example: A.GetMatrixQ(&Q);                              *
//   ***********************************************************
void FactoredQR::GetMatrixQ(Matrix *Q)
   {
   int j,k,i;
   double s;
   if(numRows < numColumns)
      Error("%s%s%s GetMatrixQ",
        Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   PrepSolve();
   ChangeDimensions(numRows,numColumns,Q);
   for(i=1;i<=numColumns;i++)
      (*Q)[i][i] = 1.;

   for(j = numColumns;j >= 1;j--)
      {
      for(i = j;i <= numColumns;i++)
        {
        s = 0.;
        for(k = j;k <= numRows;k++)
           s += factored[k][j]*(*Q)[k][i];
        s += s;
        for(k = j;k <= numRows;k++)
           (*Q)[k][i] -= Control(s*factored[k][j]);
        }
      }
   }

//   ********************< GetMatrixR >*************************
//   * Purpose: Calculating the matrix R from                  *
//   *          the factorisation QR                           *
//   * Description: PA = R  or  A = QR                         *
//   * Example: A.GetMatrixR(&R);                              *
//   ***********************************************************
void FactoredQR::GetMatrixR(MatrixRight *R)
   {
   PrepSolve();
   ChangeDimensions(numColumns,numColumns,R);
   for(int i = 1;i<= numColumns;i++)
      {
      (*R)[i][i] = dqr[i];
      for(int j = i+1;j <= numColumns;j++)
        (*R)[i][j] = factored[i][j];
      }
   }

//   *******************< GetResiduals >************************
//   * Purpose: Calculating the residuals                      *
//   *          when numRows > numColumns                      *
//   ***********************************************************
void FactoredQR::GetResiduals(Vector *residuals)
   {
   ChangeDimensions(numRows,residuals);
   if(factorizationStatus == UNFACTORED ||
      matrixStatus == MODIFIED)
        {
        Message("%s%sResiduals\nMust have "
        "solved a system in order to have residuals!",
        Factored::ERROR,ERR_FUNCTION);
        return;
        }
   double s;
   if(numRows <= numColumns)
      {
      Message("\nWith numRows == numColumns"
             " The residuals are not significant");
      for(int i=1;i<=numRows;i++)(*residuals)[i]=0.;
      return;
      }
   for(int i = 1;i <= numColumns;i++)
      (*residuals)[i]=0.;
   for(i = numColumns+1;i<=numRows;i++)
      (*residuals)[i] = bqr[i];
   for(int j = numColumns;j >= 1;j--)
      {
      s = 0.;
      for(int k = j;k <= numRows;k++)
        s -= factored[k][j]*(*residuals)[k];
      s += s;
      for(k = j;k <= numRows;k++)
        (*residuals)[k] += s*factored[k][j];
      }
   return;
   }


//   ********************< GetMatrixP >*************************
//   * Purpose: Calculating the matrix P from the              *
//   *          factorisation LQ                               *
//   * Description: PA = L                                     *
//   * Example: A.GetMatrixP(&P);                              *
//   ***********************************************************
void FactoredLQ::GetMatrixP(Matrix *P)
   {
   int j,k,i;
   double s;
   if(numColumns < numRows)
      Error("%s%s%s GetMatrixP",
        Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   PrepSolve();
   for(i = 1; i <= numRows;i++)
      if(dqr[i] == 0.)
        Error("\nSingular LeftMatrix");
   ChangeDimensions(numColumns,numColumns,P);

   for(i = 1;i <= numColumns;i++)
      (*P)[i][i]=1.;

   for(j = numRows;j >= 1;j--)
      {
      for(i = j;i <= numColumns;i++)
         {
         s = 0.;
         for(k = j;k <= numColumns;k++)
            s += factored[j][k]*(*P)[k][i];
         s += s;
         for(k = j;k <= numColumns;k++)
            (*P)[k][i] -= Control(s*factored[j][k]);
         }
      }
   }

//   ********************< GetMatrixQ >*************************
//   * Purpose: Calculating the matrix Q from                  *
//   *          the factorisation LQ                           *
//   * Description: A = LQ                                     *
//   * Example: A.GetMatrixQ(&Q);                              *
//   ***********************************************************
void FactoredLQ::GetMatrixQ(Matrix *Q)
   {
   int j,k,i;
   double s;
   if(numColumns < numRows)
      Error("%s%s%s GetMatrixQ",
        Factored::ERROR,ERR_CHECK_DIMENSION,ERR_FUNCTION);
   PrepSolve();
   for(i = 1; i <= numRows;i++)
      if(dqr[i] == 0.)
         Error("\nSingular LeftMatrix");
   ChangeDimensions(numRows,numColumns,Q);
   for(i = 1;i <= numRows;i++)
      (*Q)[i][i] = 1.;

   for(j = numRows;j >= 1;j--)
      {
      for(i = j;i <= numRows;i++)
         {
         s = 0.;
         for(k = j;k <= numColumns;k++)
            s += factored[j][k]*(*Q)[i][k];
         s += s;
         for(k = j;k <= numColumns;k++)
            (*Q)[i][k] -= Control(s*factored[j][k]);
         }
      }
   }

//   ********************< GetMatrixL >*************************
//   * Purpose: Calculating the matrix L from                  *
//   *          the factorisation LQ                           *
//   * Description: PA = L  or  A = LQ                         *
//   * Example: A.GetMatrixL(&L);                              *
//   ***********************************************************
void FactoredLQ::GetMatrixL(MatrixLeft *L)
   {
   PrepSolve();
   ChangeDimensions(numRows,numRows,L);
   for(int i = 1;i<= numRows;i++)
      {
      (*L)[i][i] = dqr[i];
      for(int j = 1;j < i;j++)
       (*L)[i][j] = factored[i][j];
      }
   }


//   ===========================================================
//   * Utilities for FactoredQR and FactoredLQ                      *
//   ===========================================================

//   *********************< Householder >***********************
//   * Purpose: Creating coefficients of the matrix Householder*
//   * Description: Given j,k  j start k end modification      *
//   *              vector aux                                 *
//   *     substitutes it with the coefficients of Householder *
//   *     and places the modified value of aux[j] in *d       *
//   ***********************************************************
void Householder(int j, int k,float *aux,float *d)
   {
   float s;
   if(j > k)Error("%s Householder",ERR_CHECK_DIMENSION);
   if(j == k) s = Abs(aux[j]);
   else  s = SqrtSumSqr(k-j+1,aux+j);
   if(s==0.)
      {
      *d=0.;
      for(int i=j;i<=k;i++) aux[i]=0.;
      return;
      }
   double h,w;
   w = sqrt((1. + Abs(aux[j])/s)*.5);
   if(aux[j] >= 0.)
      { h = 2.*s*w; *d = -s; }
   else
      { h = -2.*s*w; *d = s;}
   aux[j] = Control(w);
   for(int i = j+1;i <= k;i++)
      aux[i] = Control(aux[i]/h);
   }

//   ******************< HouseholderApplyLeft >*****************
//   * Purpose: Modifying matrix a                             *
//   *          from row ri to row rf                          *
//   *          and from column ci up to cf                    *
//   *          replacing with the coefficients of Householder *
//   *          using the coefficients of Householder in aux   *
//   ***********************************************************
void HouseholderApplyLeft(int ri, int rf,int ci,int cf,
           float **a,float *aux)
   {
   int k;
   for(int l = ci;l <= cf;l++)
      {
      double s = 0.;
      for(k = ri;k <= rf;k++)s += aux[k]*a[k][l];
      s += s;
      for(k = ri;k <= rf;k++)
        {
        float *temp = &a[k][l];
        *temp = Control(*temp - s*aux[k]);
        }
      }
   }

//   *****************< HouseholderApplyRight >*****************
//   * Purpose: Modifying matrix a                             *
//   *          from row ri to row rf                          *
//   *          and from column ci up to cf                    *
//   *          replacing with the coefficients of Householder *
//   *          using the coefficients of Householder in aux   *
//   ***********************************************************
void HouseholderApplyRight(int ri, int rf,int ci,int cf,
           float **a,float *aux)
   {
   int k,l;
   double s;
   for(l = ri;l <= rf;l++)
      {
      s = 0.;
      for(k = ci;k <= cf;k++) s += aux[k]*a[l][k];
      s += s;
      for(k = ci;k <= cf;k++)
        {
        float *temp = &a[l][k];
        *temp = Control(*temp - s*aux[k]);
        }
      }
   }