/* mpf_inverse.c  compute matrix inverse  AA = A^-1 */
#include <stdlib.h>
#include "gmp.h"

void mpf_inverse(int n, int digits, mpf_t A[], mpf_t AA[])
{
  int *ROW;            /* Row interchange indicies */
  int *COL;            /* Column interchange indicies */
  mpf_t *TEMP;
  int HOLD , I_pivot, J_pivot;  /* pivot INDICIES */
  mpf_t pivot;         /* pivot ELEMENT VALUE */
  mpf_t ABS_pivot, T, D, small, zero, one;
  int i,j,k;

  mpf_set_default_prec(digits*3.32); /* digits, not bits */
  ROW = (int *)calloc(n, sizeof(int));
  COL = (int *)calloc(n, sizeof(int));
  TEMP = (mpf_t *)malloc(n*sizeof(mpf_t)); 
  mpf_init(pivot);
  mpf_init(ABS_pivot);
  mpf_init(T);
  mpf_init(D);
  mpf_init(small);
  mpf_init(zero);
  mpf_init(one);

  mpf_set_str(small, "1.0E-550", 10); /* should be function of digits */
  mpf_set_si(zero, 0);
  mpf_set_si(one, 1);

  /* copy A to working inverse AA */
  for(i=0; i<n; i++){
    for(j=0; j<n; j++){
      mpf_init(AA[i*n+j]);
      mpf_set(AA[i*n+j], A[i*n+j]);
    }
  }

  /* set up ROW and COL  interchange vectors */
  for(k=0; k<n; k++){
    ROW[k] = k;
    COL[k] = k;
    mpf_init(TEMP[k]);
  }

  /* begin main reduction loop */
  for(k=0; k<n; k++){

    /* find largest element for pivot */
    mpf_set(pivot, AA[ROW[k]*n+COL[k]]);
    mpf_abs(ABS_pivot, pivot);
    I_pivot = k;
    J_pivot = k;
    for(i=k; i<n; i++){
      for(j=k; j<n; j++){  
        mpf_abs(T, AA[ROW[i]*n+COL[j]]);
        if( mpf_cmp(T, ABS_pivot)>0){
          I_pivot = i;
          J_pivot = j;
          mpf_set(pivot, AA[ROW[i]*n+COL[j]]);
          mpf_abs(ABS_pivot, pivot);
        }
      }
    }

    /* gmp_printf("%d,%d ABS_pivot=%Fg \n", I_pivot, J_pivot, ABS_pivot); */
    /* check for near singular */
    if(mpf_cmp(small, ABS_pivot)>0){
      gmp_printf("redundant row (singular) %d \n", ROW[k]);
      gmp_printf("diagonal=%Fg \n", ABS_pivot);
    }

    /* have pivot, interchange ROW and COL pointers */
    HOLD = ROW[k];
    ROW[k] = ROW[I_pivot];
    ROW[I_pivot] = HOLD;
    HOLD = COL[k];
    COL[k] = COL[J_pivot];
    COL[J_pivot] = HOLD;

    /*               reduce about pivot */
    mpf_div(AA[ROW[k]*n+COL[k]], one, pivot);
    for (j=0; j<n; j++){
      if( j != k ){
        mpf_mul(AA[ROW[k]*n+COL[j]], AA[ROW[k]*n+COL[j]], AA[ROW[k]*n+COL[k]]);
      }
    }

    /*               inner reduction loop */
    for (i=0; i<n; i++){
      if( k != i ){
        for (j=0; j<n; j++){
          if( k != j ){
              mpf_mul(T, AA[ROW[i]*n+COL[k]], AA[ROW[k]*n+COL[j]]);
              mpf_sub(AA[ROW[i]*n+COL[j]], AA[ROW[i]*n+COL[j]], T); 
          }
        }
        mpf_mul(T, AA[ROW[i]*n+COL[k]], AA[ROW[k]*n+COL[k]]);
        mpf_sub(AA[ROW[i]*n+COL[k]], zero, T); 
      }
    }
  }
  /*      end of main reduction loop */

  /*      unscramble rows */
  for (j=0; j<n; j++) {
    for (i=0; i<n; i++) {
      mpf_set(TEMP[COL[i]], AA[ROW[i]*n+j]);
    }
    for (i=0; i<n; i++) {
      mpf_set(AA[i*n+j], TEMP[i]);
    }
  }

  /*      unscramble columns */
  for (i=0; i<n; i++)
  {
    for (j=0; j<n; j++)
    {
      mpf_set(TEMP[ROW[j]], AA[i*n+COL[j]]);
    }
    for (j=0; j<n; j++)
    {
      mpf_set(AA[i*n+j], TEMP[j]);
    }
  }
  free(ROW);
  free(COL);
  free(TEMP);
} /* end mpf_inverse */