/* Factor unsymmetric square or rectangular matrix, add fills.
   Purpose: Factor Unsymmetric Matrix, add fills and
            Generate Permutation Vectors.
            Order according to min Degree rows / max Value OR min degree cols.
            Order within row partitions if partition vector is given. */

#include <stdlib.h>
//#ifndef WINDOWS
//#include <stdio.h>
//#else
#include "pfwstdio.h"
//#endif
#include <math.h>
#ifdef ANSIPROTO
#include <float.h>
#endif
#include "constant.h"
#include "param.h"
#include "sparse.h"

#define DeltaValence 3
        /* This is the only "trick": If row seems singular,
           defer factorization by increasing its valence.
           For square matrices, this happens only if singular.
           For rectangular factorization, this can happen. */

#ifdef ANSIPROTO
void InitializeLSWR(void);
void AllocateOrderArrays(void);
void LoadUnloadWorkingRow(INDEX i,BOOLEAN SetReset);
SparseMatrixElement *PutMatrix(INDEX PutRow,INDEX PutCol,ELEMENTVALUETYPE PutVal,
      SparseMatrixElement *PutRNext,SparseMatrixElement *PutCNext);
void SimElim(INDEX Ipivot,INDEX Jpivot,ELEMENTVALUETYPE *DiagVal);
void LinkVal(INDEX Ival,VALENCE valVal);
void UNLinkVal(INDEX Iunl);
void FindVal(INDEX Ifnd);
void FormClass(INDEX Ibeg,INDEX Iend);
INDEX GetLowval(void);
VALENCE ColumnValence(INDEX Col);
INDEX GetHighest(INDEX Ipivot);
void UpdateVals(INDEX Jpivot);
int OrderMatrix(INDEX Ibeg,INDEX Iend);
void InvertNormalize(void);
void FinishVectors(void);
int factorns(SparseMatrix *Mptr,double Param,IntegerVector *PartRow,IntegerVector *PartCol,
             IntegerVector *P1Row,IntegerVector *P1Col,IntegerVector *P2Row,IntegerVector *P2Col);
#else
void InitializeLSWR();
void AllocateOrderArrays();
void LoadUnloadWorkingRow();
SparseMatrixElement *PutMatrix();
void SimElim();
void LinkVal();
void UNLinkVal();
void FindVal();
void FormClass();
INDEX GetLowval();
VALENCE ColumnValence();
INDEX GetHighest();
void UpdateVals();
int OrderMatrix();
void InvertNormalize();
void FinishVectors();
int factorns();
#endif

/* Global array and variable definitions */
SparseMatrix *Matrix;
LONGINT Tau,Nfills,Nmult;
VALENCE maxVal = maxN;

BOOLEAN *Lswr; /* array 0..maxN */
ELEMENTVALUETYPE *Swr;    /* array 0..maxN */
IntegerVector *RowNew,*RowOld,*ColNew,*ColOld;
INDEX *PartitionAt,*PartitionCol;  /* array 0..maxPart */

double Alpha;
int LinkCount,Iorder,Nclasses;
INDEX Nstop,Ibeg,Iend,Ipart,Nparts;
INDEX CurrentColPart,NpartCol;

/* Following definitions global only within OrderMatrix routines */
        VALENCE *Valence; /* array 0..maxN */
        INDEX *FwdLink; /* array 0..maxN */
        INDEX *BckLink; /* array 0..maxN */
        INDEX *ValClass; /* array 0..maxVal */
        ELEMENTVALUETYPE PivotV,DiagVal;

/* ================= InitializeLSWR =================================== */
void InitializeLSWR()
{
  int i;

#ifdef WINDOWS
  Lswr = new BOOLEAN[Matrix->n1+1];
  Swr =  new ELEMENTVALUETYPE[Matrix->n1+1];
#else
  Lswr = (BOOLEAN *) calloc((Matrix->n1+1),sizeof(BOOLEAN));
  Swr =  (ELEMENTVALUETYPE *) calloc((Matrix->n1+1),sizeof(ELEMENTVALUETYPE));
  if (Lswr==NULL || Swr==NULL) {ErrorHalt("Insufficient memory for arrays"); stopExecute(ERROREXIT);}
#endif
  for(i=1;i<=Matrix->n1;i++) Lswr[i]=FALSE;
}

/* =================== AllocateOrderArrays ========================== */
void AllocateOrderArrays()
{
  INDEX i,n0;
  /* BEGIN */
  if (Matrix->n1 > Matrix->n2) n0 = Matrix->n1; else n0 = Matrix->n2;
  maxVal = n0; /* This could be larger yet */
#ifdef WINDOWS
  Valence = new VALENCE[n0+1];  /* array 0..maxN */
  FwdLink = new INDEX[n0+1];  /* array 0..maxN */
  BckLink = new INDEX[n0+1];  /* array 0..maxN */
  ValClass = new INDEX[maxVal+1]; /* array 0..maxVal */
#else
  Valence = (VALENCE *) malloc((n0+1)*sizeof(VALENCE));  /* array 0..maxN */
  FwdLink = (INDEX *) malloc((n0+1)*sizeof(INDEX));  /* array 0..maxN */
  BckLink = (INDEX *) malloc((n0+1)*sizeof(INDEX));  /* array 0..maxN */
  ValClass = (INDEX *) calloc((maxVal+1),sizeof(INDEX)); /* array 0..maxVal */
  if (ValClass==NULL) {ErrorHalt("Insufficient memory to allocate order vectors"); stopExecute(ERROREXIT);}
#endif
  for(i=0;i<n0+1;i++) { Valence[i]=0; FwdLink[i]=BckLink[i]=0; }
  for(i=0;i<maxVal+1;i++) ValClass[i]=0;
}

/* ====================== LoadUnloadWorkingRow ======================== */
#ifdef ANSIPROTO
  void LoadUnloadWorkingRow(INDEX i,BOOLEAN SetReset)
#else
  void LoadUnloadWorkingRow(i,SetReset)
  INDEX i;
  BOOLEAN SetReset;
#endif
  {
    SparseMatrixElement *Ptr2;
    /* BEGIN */
    Ptr2 = Matrix->RowHead[i];
    while (Ptr2 != NULL) {
      if (SetReset) Tau++;
      Lswr[Ptr2->Col] = SetReset;
      Ptr2 = Ptr2->RowNext;
    }
  }

/* ========================== PutMatrix =============================== */
#ifdef ANSIPROTO
  SparseMatrixElement *PutMatrix(INDEX PutRow,INDEX PutCol,
           ELEMENTVALUETYPE PutVal,
           SparseMatrixElement *PutRNext,SparseMatrixElement *PutCNext)
#else
  SparseMatrixElement
         *PutMatrix(PutRow,PutCol,PutVal,PutRNext,PutCNext)
  INDEX PutRow,PutCol;
  ELEMENTVALUETYPE PutVal;
  SparseMatrixElement *PutRNext;
  SparseMatrixElement *PutCNext;
#endif
  { /* PutMatrix */
    SparseMatrixElement *PutPtr;
         /* BEGIN */
#ifdef WINDOWS
         PutPtr = new SparseMatrixElement;
#else
         PutPtr = (SparseMatrixElement *) malloc(sizeof(*PutPtr));
#endif
         if (PutPtr != NULL) {
                PutPtr->Row = PutRow;
                PutPtr->Col = PutCol;
                PutPtr->Value = PutVal;
                PutPtr->RowNext = PutRNext;
                PutPtr->ColNext = PutCNext;
    } else {
      ErrorHalt("Insufficient memory for fills");
      stopExecute(ERROREXIT);
    }
    return(PutPtr);
  } /* PutMatrix */

/* =========================== SimElim ============================== */
#ifdef ANSIPROTO
        void SimElim(INDEX Ipivot,INDEX Jpivot,ELEMENTVALUETYPE *DiagVal)
#else
        void SimElim(Ipivot,Jpivot,DiagVal)
        INDEX Ipivot,Jpivot;
        ELEMENTVALUETYPE *DiagVal;
#endif
        {
          INDEX Jsim,Ksim;
          SparseMatrixElement *OrdP1;
          SparseMatrixElement *OrdP2;
          /* BEGIN SimElim */
          OrdP1 = Matrix->ColHead[Jpivot];
          while (OrdP1 != NULL) {
            Jsim = OrdP1->Row;
            if (RowNew->p[Jsim] == 0) {
              OrdP2 = Matrix->RowHead[Jsim];
              while (OrdP2 != NULL) {
                Lswr[OrdP2->Col] = TRUE;
                Swr[OrdP2->Col] = OrdP2->Value;
                OrdP2 = OrdP2->RowNext;
              }

              /* DivideValues(&Swr[Jpivot],Swr[Jpivot],DiagVal); */
              Swr[Jpivot] = Swr[Jpivot] / *DiagVal;

              OrdP2 = Matrix->RowHead[Ipivot];
              while (OrdP2 != NULL) {
                Ksim = OrdP2->Col;
                if (ColNew->p[Ksim] == 0) {
                  if (Lswr[Ksim] != TRUE) {
                    Nfills++;
                    Matrix->RowHead[Jsim] = PutMatrix(Jsim,Ksim,0.0,
                       Matrix->RowHead[Jsim],Matrix->ColHead[Ksim]);
                    Matrix->ColHead[Ksim] = Matrix->RowHead[Jsim];
                    Lswr[Ksim] = TRUE;
                    Swr[Ksim] = 0.0;
                  } /* END IF */
                  /*
                  MultiplyValues(TempVal,Swr[Jpivot],OrdP2^.Value);
                  SubtractValues(Swr[Ksim],Swr[Ksim],TempVal);
                  */
                  Swr[Ksim] = Swr[Ksim] - Swr[Jpivot] * OrdP2->Value;
                  Nmult++;
                } /* END IF */
                OrdP2 = OrdP2->RowNext;
              } /* END WHILE */;
              OrdP2 = Matrix->RowHead[Jsim];
              while (OrdP2 != NULL) {
                Lswr[OrdP2->Col] = FALSE;
                OrdP2->Value = Swr[OrdP2->Col];
                Swr[OrdP2->Col] = 0.0;
                OrdP2 = OrdP2->RowNext;
              } /* END WHILE */
            } /* END IF */
            OrdP1 = OrdP1->ColNext;
          } /* END WHILE */
        } /* END SimElim */


/* ============== START OF ROUTINES LOCAL TO OrderMatrix ============= */

#ifdef ANSIPROTO
        void LinkVal(INDEX Ival,VALENCE valVal)
#else
        void LinkVal(Ival,valVal)
        INDEX Ival;
        VALENCE valVal;
#endif
        {
          if (ValClass[valVal] != 0) BckLink[ValClass[valVal]] = Ival;
          FwdLink[Ival] = ValClass[valVal];
          BckLink[Ival] = 0;
          ValClass[valVal] = Ival;
          LinkCount++;
        } /* END LinkVal */

#ifdef ANSIPROTO
        void UNLinkVal(INDEX Iunl)
#else
        void UNLinkVal(Iunl)
        INDEX Iunl;
#endif
        {
          if (BckLink[Iunl] != 0) FwdLink[BckLink[Iunl]] = FwdLink[Iunl];
          else ValClass[Valence[Iunl]] = FwdLink[Iunl];
          if (FwdLink[Iunl] != 0) BckLink[FwdLink[Iunl]] = BckLink[Iunl];
          LinkCount--;
        } /* END UNLinkVal */

#ifdef ANSIPROTO
        void FindVal(INDEX Ifnd)
#else
        void FindVal(Ifnd)
        INDEX Ifnd;
#endif
        {
          SparseMatrixElement *OrdP1;
          /* BEGIN */
          Valence[Ifnd] = 0;
          OrdP1 = Matrix->RowHead[Ifnd];
          while (OrdP1 != NULL) {
            if ((Valence[Ifnd] < maxVal) && (ColNew->p[OrdP1->Col]==0)) {
              Valence[Ifnd]++;
            } /* END IF */
            OrdP1 = OrdP1->RowNext;
          } /* END WHILE */
        } /* END FindVal */

#ifdef ANSIPROTO
        void FormClass(INDEX Ibeg,INDEX Iend)
#else
        void FormClass(Ibeg,Iend)
        INDEX Ibeg,Iend;
#endif
        {
          INDEX I;
          /* BEGIN */
          for (I=Ibeg; I<=Iend; I++) {
            RowOld->p[I] = 0;
            ColOld->p[I] = 0;
          }
          for (I=Ibeg; I<=Iend; I++) FindVal(I);
          for (I=0; I<=maxVal; I++) ValClass[I] = 0;
          for (I=Ibeg; I<=Iend; I++) LinkVal(I,Valence[I]);
        } /* END FormClass */