//$$ newmat7.cxx     Invert, solve, binary operations

// Copyright (C) 1991: R B Davies and DSIR

#include "include.hxx"

#include "newmat.hxx"
#include "newmatrc.hxx"

//#define REPORT { static ExeCounter ExeCount(__LINE__,7); ExeCount++; }

#define REPORT {}


/***************************** solve routines ******************************/

GeneralMatrix* GeneralMatrix::MakeSolver()
{
   REPORT
   GeneralMatrix* gm = new CroutMatrix(*this);
   MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
}

GeneralMatrix* Matrix::MakeSolver()
{
   REPORT
   GeneralMatrix* gm = new CroutMatrix(*this);
   MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
}

void CroutMatrix::Solver(MatrixRowCol& mcout, const MatrixRowCol& mcin)
{
   REPORT
   real* el = mcin.store; int i = mcin.skip;
   while (i--) *el++ = 0.0;
   el += mcin.storage; i = nrows - mcin.skip - mcin.storage;
   while (i--) *el++ = 0.0;
   lubksb(mcin.store, mcout.skip);
}


// Do we need check for entirely zero output?

void UpperTriangularMatrix::Solver(MatrixRowCol& mcout,
   const MatrixRowCol& mcin)
{
   REPORT
   real* elx = mcin.store+mcout.skip; int i = mcin.skip-mcout.skip;
   while (i-- > 0) *elx++ = 0.0;
   int nr = mcin.skip+mcin.storage; elx = mcin.store+nr; real* el = elx;
   int j = mcout.skip+mcout.storage-nr; int nc = ncols-nr; i = nr-mcout.skip;
   while (j-- > 0) *elx++ = 0.0;
   real* Ael = store + (nr*(2*ncols-nr+1))/2; j = 0;
   while (i-- > 0)
   {
      elx = el; real sum = 0.0; int jx = j++; Ael -= nc;
      while (jx--) sum += *(--Ael) * *(--elx);
      elx--; *elx = (*elx - sum) / *(--Ael);
   }
}

void LowerTriangularMatrix::Solver(MatrixRowCol& mcout,
   const MatrixRowCol& mcin)
{
   REPORT
   real* elx = mcin.store+mcout.skip; int i = mcin.skip-mcout.skip;
   while (i-- > 0) *elx++ = 0.0;
   int nc = mcin.skip; i = nc+mcin.storage; elx = mcin.store+i;
   int nr = mcout.skip+mcout.storage; int j = nr-i; i = nr-nc;
   while (j-- > 0) *elx++ = 0.0;
   real* el = mcin.store+nc; real* Ael = store + (nc*(nc+1))/2; j = 0;
   while (i-- > 0)
   {
      elx = el; real sum = 0.0; int jx = j++; Ael += nc;
      while (jx--) sum += *Ael++ * *elx++;
      *elx = (*elx - sum) / *Ael++;
   }
}

/******************* carry out binary operations *************************/

static void Add(GeneralMatrix*,GeneralMatrix*, GeneralMatrix*);
static void Add(GeneralMatrix*,GeneralMatrix*);

static void Subtract(GeneralMatrix*,GeneralMatrix*, GeneralMatrix*);
static void Subtract(GeneralMatrix*,GeneralMatrix*);
static void ReverseSubtract(GeneralMatrix*,GeneralMatrix*);

static GeneralMatrix* GeneralAdd(GeneralMatrix*,GeneralMatrix*,MatrixType);
static GeneralMatrix* GeneralSub(GeneralMatrix*,GeneralMatrix*,MatrixType);
static GeneralMatrix* GeneralMult(GeneralMatrix*,GeneralMatrix*,MatrixType);
static GeneralMatrix* GeneralSolv(GeneralMatrix*,GeneralMatrix*,MatrixType);

GeneralMatrix* AddedMatrix::Evaluate(MatrixType mt)
{ REPORT return GeneralAdd(bm1->Evaluate(), bm2->Evaluate(), mt); }

GeneralMatrix* SubtractedMatrix::Evaluate(MatrixType mt)
{ REPORT return GeneralSub(bm1->Evaluate(), bm2->Evaluate(), mt); }

GeneralMatrix* MultipliedMatrix::Evaluate(MatrixType mt)
{
   REPORT
   GeneralMatrix* gm2 = bm2->Evaluate();
   MatrixType mtsym(MatrixType::Sym);
   if (gm2->Type() == mtsym) gm2 = gm2->Evaluate(MatrixType::Rect);
   return GeneralMult(bm1->Evaluate(), gm2, mt);
}

GeneralMatrix* SolvedMatrix::Evaluate(MatrixType mt)
{ REPORT return GeneralSolv(bm1->Evaluate(), bm2->Evaluate(), mt); }

// routines for adding or subtracting matrices of identical storage structure

static void Add(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
{
   REPORT
   register real* s1=gm1->Store(); register real* s2=gm2->Store();
   register real* s=gm->Store(); register int i=gm->Storage();
   while (i--) *s++ = *s1++ + *s2++;
}
   
static void Add(GeneralMatrix* gm, GeneralMatrix* gm2)
{
   REPORT
   register real* s2=gm2->Store();
   register real* s=gm->Store(); register int i=gm->Storage();
   while (i--) *s++ += *s2++;
}

static void Subtract(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
{
   REPORT
   register real* s1=gm1->Store(); register real* s2=gm2->Store();
   register real* s=gm->Store(); register int i=gm->Storage();
   while (i--) *s++ = *s1++ - *s2++;
}

static void Subtract(GeneralMatrix* gm, GeneralMatrix* gm2)
{
   REPORT
   register real* s2=gm2->Store();
   register real* s=gm->Store(); register int i=gm->Storage();
   while (i--) *s++ -= *s2++;
}

static void ReverseSubtract(GeneralMatrix* gm, GeneralMatrix* gm2)
{
   REPORT
   register real* s2=gm2->Store();
   register real* s=gm->Store(); register int i=gm->Storage();
   while (i--) { *s = *s2++ - *s; s++; }
}

static GeneralMatrix* GeneralAdd(GeneralMatrix* gm1, GeneralMatrix* gm2,
   MatrixType mtx)
{
   int nr=gm1->Nrows(); int nc=gm1->Ncols();
   if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
      MatrixError("Incompatible dimensions");
   if (!mtx) mtx = gm1->Type() + gm2->Type();
   if (mtx == gm1->Type() && mtx == gm2->Type())
					     // modify when band or sparse
					     // matrices are included.
   {
      if (gm1->reuse()) { REPORT Add(gm1,gm2); gm2->tDelete(); return gm1; }
      else if (gm2->reuse()) { REPORT Add(gm2,gm1); return gm2; }
      else
      {
         REPORT
         GeneralMatrix* gmx = gm1->Type().New(nr,nc); gmx->ReleaseAndDelete();
         gmx->ReleaseAndDelete(); Add(gmx,gm1,gm2); return gmx;
      }
   }
   else
   {
      if (gm1->Type()==mtx && gm1->reuse() )    // must have type test first
      {
	 REPORT
	 MatrixRow mr1(gm1, StoreOnExit+LoadOnEntry+DirectPart);
	 MatrixRow mr2(gm2, LoadOnEntry);
	 while (nr--) { mr1.Add(mr2); mr1.Next(); mr2.Next(); }
         gm2->tDelete(); return gm1;
      }
      else if (gm2->Type()==mtx && gm2->reuse() )
      {
         REPORT
	 MatrixRow mr1(gm1, LoadOnEntry);
	 MatrixRow mr2(gm2, StoreOnExit+LoadOnEntry+DirectPart);
	 while (nr--) { mr2.Add(mr1); mr1.Next(); mr2.Next(); }
	 if (gm1->Type()!=mtx) gm1->tDelete();
         return gm2;
      }
      else
      {
         REPORT
	 GeneralMatrix* gmx = mtx.New(nr,nc);
         MatrixRow mr1(gm1, LoadOnEntry);
	 MatrixRow mr2(gm2, LoadOnEntry);
	 MatrixRow mrx(gmx, StoreOnExit+DirectPart);
	 while (nr--)
	 { mrx.Add(mr1,mr2); mr1.Next(); mr2.Next(); mrx.Next(); }
	 if (gm1->Type()!=mtx) gm1->tDelete();
	 if (gm2->Type()!=mtx) gm2->tDelete();
         gmx->ReleaseAndDelete(); return gmx;
      }
   }
}


static GeneralMatrix* GeneralSub(GeneralMatrix* gm1, GeneralMatrix* gm2,
   MatrixType mtx)
{
   if (!mtx) mtx = gm1->Type() - gm2->Type();
   int nr=gm1->Nrows(); int nc=gm1->Ncols();
   if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
      MatrixError("Incompatible dimensions");
   if (mtx == gm1->Type() && mtx == gm2->Type())
					     // modify when band or sparse
                                             // matrices are included.
   {
      if (gm1->reuse())
      { REPORT Subtract(gm1,gm2); gm2->tDelete(); return gm1; }
      else if (gm2->reuse()) { REPORT ReverseSubtract(gm2,gm1); return gm2; }
      else
      {
         REPORT
	 GeneralMatrix* gmx = gm1->Type().New(nr,nc); gmx->ReleaseAndDelete();
         gmx->ReleaseAndDelete(); Subtract(gmx,gm1,gm2); return gmx;
      }
   }
   else
   {
      if (gm1->Type() == mtx && gm1->reuse() )
      {
         REPORT
	 MatrixRow mr1(gm1, LoadOnEntry+StoreOnExit+DirectPart);
	 MatrixRow mr2(gm2, LoadOnEntry);
	 while (nr--) { mr1.Sub(mr2); mr1.Next(); mr2.Next(); }
         gm2->tDelete(); return gm1;
      }
      else if (gm2->Type() == mtx && gm2->reuse() )
      {
         REPORT
         MatrixRow mr1(gm1, LoadOnEntry);
	 MatrixRow mr2(gm2, LoadOnEntry+StoreOnExit+DirectPart);
	 while (nr--) { mr2.RevSub(mr1); mr1.Next(); mr2.Next(); }
	 if (gm1->Type()!=mtx) gm1->tDelete();
	 return gm2;
      }
      else
      {
         REPORT
	 GeneralMatrix* gmx = mtx.New(nr,nc);
         MatrixRow mr1(gm1, LoadOnEntry);
         MatrixRow mr2(gm2, LoadOnEntry);
	 MatrixRow mrx(gmx, StoreOnExit+DirectPart);
	 while (nr--)
	    { mrx.Sub(mr1,mr2); mr1.Next(); mr2.Next(); mrx.Next(); }
	 if (gm1->Type()!=mtx) gm1->tDelete();
	 if (gm2->Type()!=mtx) gm2->tDelete();
	 gmx->ReleaseAndDelete(); return gmx;
      }
   }
}

/*
static GeneralMatrix* GeneralMult(GeneralMatrix* gm1, GeneralMatrix* gm2,
   MatrixType mtx)
{
   REPORT
   if (!mtx) mtx = gm1->Type() * gm2->Type();
   int nr=gm1->Nrows(); int nc=gm2->Ncols();
   if (gm1->Ncols() !=gm2->Nrows()) MatrixError("Incompatible dimensions");
   GeneralMatrix* gmx = mtx.New(nr,nc);

   MatrixCol mcx(gmx, StoreOnExit+DirectPart);
   MatrixCol mc2(gm2, LoadOnEntry);
   while (nc--)
   {
      MatrixRow mr1(gm1, LoadOnEntry, mcx.Skip());
      real* el = mcx();                              // pointer to an element
      int n = mcx.Storage();
      while (n--) { *(el++) = DotProd(mr1,mc2); mr1.Next(); }
      mc2.Next(); mcx.Next();
   }
   gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
}
*/

static GeneralMatrix* GeneralMult(GeneralMatrix* gm1, GeneralMatrix* gm2,
   MatrixType mtx)
{
   REPORT
   if (!mtx) mtx = gm1->Type() * gm2->Type();
   int nr=gm1->Nrows(); int nc=gm2->Ncols();
   if (gm1->Ncols() !=gm2->Nrows()) MatrixError("Incompatible dimensions");
   GeneralMatrix* gmx = mtx.New(nr,nc);

   real* el = gmx->Store(); int n = gmx->Storage();
   while (n--) *el++ = 0.0;
   MatrixRow mrx(gmx, LoadOnEntry+StoreOnExit+DirectPart);
   MatrixRow mr1(gm1, LoadOnEntry);
   while (nr--)
   {
      MatrixRow mr2(gm2, LoadOnEntry, mr1.Skip());
      el = mr1();                              // pointer to an element
      n = mr1.Storage();
      while (n--) { mrx.AddScaled(mr2, *el++); mr2.Next(); }
      mr1.Next(); mrx.Next();
   }
   gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
}

static GeneralMatrix* GeneralSolv(GeneralMatrix* gm1, GeneralMatrix* gm2,
   MatrixType mtx)
{
   REPORT
   if (!mtx) mtx = gm1->Type().i()*gm2->Type();
   int nr=gm1->Nrows(); int nc=gm2->Ncols();
   if (gm1->Ncols() !=gm2->Nrows()) MatrixError("Incompatible dimensions");
   GeneralMatrix* gmx = mtx.New(nr,nc);

   real* r = new real [nr]; MatrixErrorNoSpace(r);
   MatrixCol mcx(gmx, r, StoreOnExit+DirectPart);   // copy to and from r
   MatrixCol mc2(gm2, r, LoadOnEntry);
   GeneralMatrix* gms = gm1->MakeSolver();
   while (nc--) { gms->Solver(mcx, mc2); mcx.Next(); mc2.Next(); }
   gms->tDelete(); gmx->ReleaseAndDelete(); gm2->tDelete();
   delete [nr] r;
   return gmx;
}

GeneralMatrix* InvertedMatrix::Evaluate(MatrixType mtx)
{
   // Matrix Inversion - use solve routines
   REPORT
   GeneralMatrix* gm=bm->Evaluate();
   int n = gm->Nrows(); DiagonalMatrix I(n); I=1.0;
   return GeneralSolv(gm,&I,mtx);
}


/*************************** norm functions ****************************/

real BaseMatrix::Norm1()
{
   // maximum of sum of absolute values of a column
   REPORT
   GeneralMatrix* gm = Evaluate(); int nc = gm->Ncols(); real value = 0.0;
   MatrixCol mc(gm, LoadOnEntry);
   while (nc--)
      { real v = mc.SumAbsoluteValue(); if (value < v) value = v; mc.Next(); }
   gm->tDelete(); return value;
}

real BaseMatrix::NormInfinity()
{
   // maximum of sum of absolute values of a row
   REPORT
   GeneralMatrix* gm = Evaluate(); int nr = gm->Nrows(); real value = 0.0;
   MatrixRow mr(gm, LoadOnEntry);
   while (nr--)
      { real v = mr.SumAbsoluteValue(); if (value < v) value = v; mr.Next(); }
   gm->tDelete(); return value;
}
