/* mpf_simeq.c  solve simultaneous equations AX=Y */
#include <stdio.h>
#include <stdlib.h>
#include "mpf_simeq.h" /* includes gmp and digits */

void mpf_simeq(int n, mpf_t A[], mpf_t Y[], mpf_t X[])
{

/*      PURPOSE : SOLVE THE LINEAR SYSTEM OF EQUATIONS WITH REAL     */
/*                COEFFICIENTS   [A] * |X| = |Y|                     */
/*                                                                   */
/*      INPUT  : THE NUMBER OF EQUATIONS  n                          */
/*               THE REAL MATRIX  A   should be A[i][j] but A[i*n+j] */
/*               THE REAL VECTOR  Y                                  */
/*      OUTPUT : THE REAL VECTOR  X                                  */
/*                                                                   */
/*      METHOD : GAUSS-JORDAN ELIMINATION USING MAXIMUM ELEMENT      */
/*               FOR PIVOT.                                          */
/*                                                                   */
/*      USAGE  :     mpf_simeq(n,A,Y,X);                             */
/*                                                                   */
/*                                                                   */
/*    WRITTEN BY : JON SQUIRE , 28 MAY 1983                          */
/*    ORIGONAL DEC 1959 for IBM 650, TRANSLATED TO OTHER LANGUAGES   */
/*    e.g. FORTRAN converted to Ada converted to C etc.              */

  mpf_t *B;            /* [n][n+1]  WORKING MATRIX */
  int i, j, m;

  mpf_set_default_prec(digits*3.32);
  B = (mpf_t *)calloc((n+1)*(n+1), sizeof(mpf_t));
  m = n+1;

  /* BUILD WORKING DATA STRUCTURE */
  for(i=0; i<n; i++)
  {
    for(j=0; j<n; j++)
    {
      mpf_init(B[i*m+j]);
      mpf_set(B[i*m+j], A[i*n+j]);
    }
    mpf_init(B[i*m+n]);
    mpf_set(B[i*m+n], Y[i]);
  }

  mpf_simeqb(n, B, X);
} /* end mpf_simeqb */

void mpf_simeqb(int n, mpf_t B[], mpf_t X[])
{
  /* B[] must be exactly B[n][n+1] */
  int *ROW;            /* ROW INTERCHANGE INDICIES */
  int HOLD , I_PIVOT;  /* PIVOT INDICIES */
  mpf_t PIVOT;         /* PIVOT ELEMENT VALUE */
  mpf_t ABS_PIVOT, T, D, small, zero;
  int i,j,k,m;

  ROW = (int *)calloc(n, sizeof(int));
  m = n+1;
  mpf_init(PIVOT);
  mpf_init(ABS_PIVOT);
  mpf_init(T);
  mpf_init(D);
  mpf_init(small);
  mpf_init(zero);

  mpf_set_str(small, "1.0E-50", 10);
  mpf_set_str(zero, "0.0", 10);

  /* 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 */
    mpf_set(PIVOT, B[ROW[k]*m+k]);
    mpf_abs(ABS_PIVOT, PIVOT);
    I_PIVOT = k;
    for(i=k; i<n; i++)
    {
      mpf_abs(T, B[ROW[i]*m+k]);
      if( mpf_cmp(T, ABS_PIVOT)>0)
      {
        I_PIVOT = i;
        mpf_set(PIVOT, B[ROW[i]*m+k]);
        mpf_abs(ABS_PIVOT, PIVOT);
      }
    }

    /* HAVE PIVOT, INTERCHANGE ROW POINTERS */
    HOLD = ROW[k];
    ROW[k] = ROW[I_PIVOT];
    ROW[I_PIVOT] = HOLD;

    /* CHECK FOR NEAR SINGULAR */
    if(mpf_cmp(small, ABS_PIVOT)>0)
    {
      for(j=k+1; j<m; j++)
      {
        mpf_set(B[ROW[k]*m+j], zero);
      }
      printf("redundant row (singular) %d \n", ROW[k]);
    } /* singular, delete row */
    else
    {
      /* REDUCE ABOUT PIVOT */
      for(j=k+1; j<n+1; j++)
      {
        mpf_div(B[ROW[k]*m+j], B[ROW[k]*m+j], B[ROW[k]*m+k]);
      }

      /* INNER REDUCTION LOOP */
      for(i=0; i<n; i++)
      {
        if( i != k)
        {
          for(j=k+1; j<m; j++)
          {
            mpf_mul(T, B[ROW[i]*m+k], B[ROW[k]*m+j]);
            mpf_sub(B[ROW[i]*m+j], B[ROW[i]*m+j], T); 
          }
        }
      }
    }
    /* FINISHED INNER REDUCTION */
  }

  /* END OF MAIN REDUCTION LOOP */
  /* BUILD  X  FOR RETURN, UNSCRAMBLING ROWS */
  for(i=0; i<n; i++)
  {
    mpf_set(X[i], B[ROW[i]*m+n]);
  }
  free(ROW);
} /* end mpf_simeqb */

void mpf_simeqd(int n, double A[], double Y[], double X[])
{
  mpf_t *B;            /* [n][n+1]  working matrix */
  mpf_t *Xm;           /* [n]  temp array */
  int i, j, m;

  mpf_set_default_prec(digits*3.32);
  B = (mpf_t *)calloc((n+1)*(n+1), sizeof(mpf_t));
  Xm = (mpf_t *)calloc(n, sizeof(mpf_t));
  m = n+1;

  /* BUILD WORKING DATA STRUCTURE */
  for(i=0; i<n; i++)
  {
    for(j=0; j<n; j++)
    {
      mpf_init(B[i*m+j]);
      mpf_set_d(B[i*m+j], A[i*n+j]); /* double to mpf */
    }
    mpf_init(B[i*m+n]);
    mpf_init(Xm[i]);
    mpf_set_d(B[i*m+n], Y[i]); /* double to mpf */
  }
  mpf_simeqb(n, B, Xm);
  for(i=0; i<n; i++)
  {
    X[i] = mpf_get_d(Xm[i]); /* mpf to double */
  }
} /* end mpf_simeqd */