/* mpf_sparse.c Sparse storage of a matrix, mpf precision
 *              specifically designed for solving PDE
 *              simultaneous equations.
 *              zero based subscripting
 *              column(nrow) is right hand side
 */

#include "mpf_sparse.h"
#include <stdio.h>

static int nrow;
static int cell_count;

/* newCell(j, val, next)  cell stuff, abstraction */
/* Cell operations, uses "private" part of header */

static link newCell(int aj, mpf_t aval, link anext)
{
  link next;

  cell_count++;
  next = (link)malloc(sizeof(struct cell));
  next->j = aj;
  mpf_init_set(next->val, aval);
  next->next = anext;
  return next;
} /* end newCell */

int sparseGetCellCount()
{
  return cell_count;
}

static void Setval(link* C, int aj, mpf_t aval) /* creates aj if not in list*/
{
  link* P;

  if(*C == NULL)
  {
    *C = newCell(aj, aval, NULL);
    return;
  }
  P = C; /* walk the list */
  while(1)
  {
    if(aj == (*P)->j)
    {
      mpf_set((*P)->val, aval);
      return;
    }
    if(aj < (*P)->j) /* we passed it, insert before */
    {
      (*P)->next = newCell((*P)->j, (*P)->val, (*P)->next);
      (*P)->j = aj;
      mpf_set((*P)->val, aval);
      return ;
    }
    if((*P)->next == NULL) /* insert at end */
    {
      (*P)->next = newCell(aj, aval, NULL);
      return;
    }
    P = &((*P)->next); /* move along list */
  } /* end while */
} /* end sparseSetval */

static void Addval(link* C, int aj, mpf_t aval) /* creates aj if not in list*/
{
  link* P;

  if(*C == NULL)
  {
    *C = newCell(aj, aval, NULL);
    return;
  }
  P = C; /* walk the list */
  while(1)
  {
    if(aj == (*P)->j)
    {
      mpf_add((*P)->val, (*P)->val, aval);
      return;
    }
    if(aj < (*P)->j) /* we passed it, insert before */
    {
      (*P)->next = newCell((*P)->j, (*P)->val, (*P)->next);
      (*P)->j = aj;
      mpf_set((*P)->val, aval);
      return ;
    }
    if((*P)->next == NULL) /* insert at end */
    {
      (*P)->next = newCell(aj, aval, NULL);
      return;
    }
    P = &((*P)->next); /* move along list */
  } /* end while */
} /* end Addval  aval added */

static void Getval(mpf_t result, link C, int aj)
{
  link P;

  mpf_set_si(result, 0);
  P = C; /* walk the list */
  while(P != NULL)
  {
    if(aj == P->j)
    { 
      mpf_set(result, P->val);
      return;
    }
    if(aj <  P->j)
    {
      mpf_set_si(result, 0);  /* we passed it */
      return;
    }
    P = P->next; /* move along list */
  } /* end while */
} /* end Getval */


/* exported via sparse.h */

void sparseInit(struct sparse* A, int nrow)
{
  int i;

  cell_count = 0;
  A->nrow = nrow;
  A->A = (link *)malloc(nrow*sizeof(link));
  for(i=0; i<nrow; i++)
  {
    A->A[i] = NULL;
  }
} /* end sparseinit */

void sparseWrite_All(struct sparse A)
{
  link P;
  int i;

  nrow = A.nrow;
  printf("SparseWrite_All nrow=%d \n", nrow);
  for(i=0; i<nrow; i++)
  {
    P = A.A[i];
    while(P != NULL)
    {
      gmp_printf("i=%d, j=%d, val=%Fg \n", i, P->j, P->val );
      P = P->next;
    }
  }
} /* end sparseWrite_All */

void sparseImport(struct sparse A, mpf_t M[]) /* single index i*nrow+j */
{
  int i, j;

  nrow = A.nrow;
  for(i=0; i<nrow; i++)
    for(j=0; j<=nrow; j++)
      if(mpf_cmp_ui(M[i*nrow+j],0)!=0) Setval(&(A.A[i]),j,M[i*nrow+j]);
} /* end sparseimport */

void sparseExport(struct sparse A, mpf_t M[]) /* size nrow*(nrow-1) */
{
  link P;
  int i, j;

  nrow = A.nrow;
  for(i=0; i<nrow; i++)
    for(j=0; j<=nrow; j++)
      mpf_set_si(M[i*nrow+j], 0);

  for(i=0; i<nrow; i++)
  {
    P = A.A[i];
    while(P != NULL)
    {
      mpf_set(M[i*nrow+P->j], P->val);
      P = P->next;
    }
  }
} /* end sparseExport */

void sparseGet(mpf_t result, struct sparse A, int i, int j)
{
  nrow = A.nrow;
  if(i<0 || i>=nrow || j<0 || j>nrow)
  {
    printf("sparseGet error i=%d, j=%d \n", i, j);
    mpf_set_si(result,  0);
  }
  Getval(result, A.A[i],j);
} /* end sparseGet */

void sparsePut(struct sparse A, int i, int j, mpf_t val)
{
  nrow = A.nrow;
  if(i<0 || i>=nrow || j<0 || j>nrow)
  {
    printf("sparsePut error i=%d, j=%d \n", i, j);
    return;
  }
  Setval(&(A.A[i]), j, val);
} /* end sparse Put */

void sparseAdd(struct sparse A, int i, int j, mpf_t val)
{
  nrow = A.nrow;
  if(i<0 || i>=nrow || j<0 || j>nrow)
  {
    printf("sparseAdd error i=%d, j=%d \n", i, j);
    return;
  }
  Addval(&(A.A[i]), j, val);
} /* end sparseAdd */

void sparseGetRHS(struct sparse A, mpf_t Y[])
{
  int i;

  nrow = A.nrow;
  for(i=0; i<nrow; i++)
  {
    Getval(Y[i], A.A[i], nrow);
  }
} /* end sparseGetRHS */

void sparseSetRHS(struct sparse A, mpf_t Y[])
{
  int i;

  nrow = A.nrow;
  for(i=0; i<nrow; i++) Setval(&(A.A[i]),nrow,Y[i]);
} /* end sparseSetRHS */

void sparseMul(struct sparse A, mpf_t X[], mpf_t Y[]) /* Y output */
{
  link P;
  int i;
  mpf_t tmp;

  mpf_init(tmp);
  nrow = A.nrow;
  for(i=0; i<nrow; i++) mpf_set_si(Y[i], 0);
  for(i=0; i<nrow; i++)
  {
    P = A.A[i];
    while(P != NULL)
    {
      if(P->j >= nrow) break;
      mpf_mul(tmp, P->val, X[P->j]);
      mpf_add(Y[i], Y[i], tmp);
      P = P->next;
    }
  }
} /* end sparseMul */

/* sparseSimeq      solve simultaneous equations AX=Y
 * solve real linear equations for X where Y = A * X
 * method: Gauss-Jordan elimination using maximum pivot
 *    Translated to Sparse by : Jon Squire , 25 March 2009
 *    First written by Jon Squire December 1959 for IBM 650, translated to
 *    other languages  e.g. Fortran converted to Ada converted to C
 *   ) converted to java,) to sparse.c
 */
void sparseSimeq(struct sparse A, mpf_t X[]) /* X returned solution */
{
  int Hold, i_Pivot, i, k;
  mpf_t   Pivot, Abs_Pivot, tmp;
  int  N  = nrow;
  int row[N]; /* row interchange indices */

  mpf_init(Pivot);
  mpf_init(Abs_Pivot);
  mpf_init(tmp);

  /* set up row interchange vectors */
  for(k=0; k<N; k++) row[k] = k;

  /*  begin main reduction loop */
  for(k=0; k<N; k++)
  {
    /* find largest element for pivot */
    Getval(Pivot, A.A[row[k]],k); /* B(row(k))(k);  */
    mpf_abs(Abs_Pivot, Pivot);
    i_Pivot = k;
    gmp_printf("i_Pivot=%d, Abs_Pivot=%Fg\n",i_Pivot,Abs_Pivot);
    for(i=k+1; i<N; i++)
    {
      /* if A(row(i),k) = NULL) exit; end if; */
      Getval(Pivot, A.A[row[i]],k); /* B(row(i))(k)) */
      mpf_abs(tmp, Pivot);
      if(mpf_cmp(tmp, Abs_Pivot)>0) /* B(row(i))(k)) */
      {
        i_Pivot = i;
        mpf_abs(Abs_Pivot, Pivot);
        gmp_printf("i_Pivot=%d, bigger Abs_Pivot=%Fg\n",i_Pivot,Abs_Pivot);
      }
    }
    /* have pivot, interchange row indices */
    Hold = row[k];
    row[k] = row[i_Pivot];
    row[i_Pivot] = Hold;
    /* check for near singular */
    mpf_set_d(tmp, 1.0e-20);
    if(mpf_cmp(Abs_Pivot, tmp)<0)
    {
      gmp_printf("singular at k=%d redundant row=%d \n", k, row[k]);
      gmp_printf("Pivot=%Fg\n", Pivot);
      gmp_printf("Abs_Pivot=%Fg\n", Abs_Pivot);
    }
    else
    {
      /* can divide by pivot */
      /* reduce about pivot */
      /* for(int j=k+1; j<n+1; j++) */
      /* { */
      /* B(row(k))(j) = B(row(k))(j) / B(row(k))(k); */
      sparseDivide(A,row[k],k); /* j loop in here  should use pivot */
      /* } */
      /*  inner reduction loop */
      for(i=0; i<N; i++)
      {
        if(i!= k)
	{
          /* for(int j=k+1; j<n+1; j++) */
          /* { */
          /* B(row(i))(j) = B(row(i))(j) - B(row(i))(k) * B(row(k))(j); */
          sparseReduce(A,row[i],k,row[k]); /* j loop in here on both */
	                                   /* row(i) and row(k) */
        }
      }
      /*  finished inner reduction */
    }
  }  /*  end main reduction loop */
    /*  build  X  for return, unscrambling rows */
  for(i=0; i<N; i++) Getval(X[i], A.A[row[i]],N); 
                            /* this could be direct if matrix shrunk */
} /* end sparseSimeq */

void sparseDivide(struct sparse A, int krow, int k) 
                  /* works on a krow j=k+1..n */
{
  link* P;
  mpf_t Piv;

  mpf_init(Piv);
  P = &(A.A[krow]);
  while((*P) != NULL) /* find pivot, position to pivot */
  {
    if((*P)->j == k) /* found, thus must exists */
    {
      mpf_set(Piv, (*P)->val);
      P = &((*P)->next);
      break;
    }
    P = &((*P)->next);
  } /* end while that gets Piv and positions to next */
  while((*P) != NULL)
  {
    mpf_div((*P)->val, (*P)->val, Piv);
    P = &((*P)->next);
  }
} /*  end sparseDivide */

void sparseReduce(struct sparse A, int irow, int k, int krow)
{
  /* works on complete irow  j=k+1..n */
  /* using krow j=k+1..n  where it exists */
  /* B[row[i]][j] = B[row[i]][j] - B[row[i]][k] * B[row[k]][j]; */
  /*                               valik */
  link Pij, Pkj;
  mpf_t valik, tmp;

  mpf_init_set_si(valik, 0);
  mpf_init(tmp);
  Pkj = A.A[krow];
  Pij = A.A[irow];
  /* find Pik and valik  constant for  while loop on j */
  while(Pij != NULL)
  {
    if(Pij->j == k)
    {
      mpf_set(valik, Pij->val); /* have multiplier */
      Pij = Pij->next;
      break;
    }
    else if(Pij->j > k)
    {
      return; /* no multiplier */
    }
    else
    {
      Pij = Pij->next;
    }
  }
  if(mpf_cmp_ui(valik, 0)==0) return; /* no multiplier */
  while(Pkj != NULL) /* position Pkj */
  {
    if(Pkj->j > k)
    {
      break;
    }
    Pkj = Pkj->next;
  }
  while(Pij!=NULL && Pkj!= NULL)
  {
    /* find Pij->j = Pkj,j (both move in j) */
    if(Pij->j < Pkj->j)
    {
      Pij = Pij->next;
    }
    else if(Pij->j == Pkj->j)
    {
      mpf_mul(tmp, valik, Pkj->val);
      mpf_sub(Pij->val, Pij->val, tmp);
      Pij = Pij->next;
      Pkj = Pkj->next;
    }
    else if(Pij->j > Pkj->j) /* must insert negative value, Pij->val = 0 */
    {
      Pij->next = newCell(Pij->j, Pij->val, Pij->next);
      Pij->j = Pkj->j;
      mpf_mul(tmp, valik, Pkj->val);
      mpf_neg(Pij->val, tmp);
      Pij = Pij->next;
      Pkj = Pkj->next;
    }
    else
    {
      Pkj = Pkj->next;
    }
  }
} /* end sparseReduce */
/* end sparse.c */