/* mpf_tsimeq.c  solve simultaneous equations AX=Y  threaded version */
/* Linux with glibc:
 *   _REENTRANT to grab thread-safe libraries
 *   _POSIX_SOURCE to get POSIX semantics
 */
#ifdef __linux__
# define _REENTRANT
# define _POSIX_SOURCE
# define _P __P
#endif

#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include "mpf_tsimeq.h"

/*      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_tsimeq(np,n,A,Y,X);  A not modified             */
/*               mpf_tsimeqb(np,n,B,X)    B=A with Y last column,    */
/*               np is number of worker threads                      */
/*                                                                   */
/*    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, Java, parallel   */
/*    modified to have simeqb and multiple precision                 */

#include "tsimeq.h"
static int NP = 0; /* number of workers, not include master */
static int nb = 0; /* count to NP+1 to include master */
static pthread_cond_t go;
static pthread_mutex_t bar;
static mpf_t * BB;   /* B global */
static int nn, mm;    /* n, n+1 global */
static int *srow;     /* NP+1  starting row for each thread */
static int *ROW;      /* ROW INTERCHANGE INDICIES */

void barrier(void)
{
  fflush(stdout);

  pthread_mutex_lock(&bar);
  nb++;
  if(nb == NP+1) /* includes master allowing release */
  {
    nb = 0;
    pthread_cond_broadcast(&go);
  }
  else
  {
    pthread_cond_wait(&go, &bar);      
  }
  pthread_mutex_unlock(&bar);
} /* end barrier */

void * parallel(void * threadID)
{
  long int id;
  int i, j, k;
  mpf_t T;

  id = (long int)threadID;
  mpf_init(T);
  for(k=0; k<nn; k++) /* tracks loop */
  {
    barrier(); /* wait for time to reduce */
    /* INNER REDUCTION LOOP */
    for(i=srow[id]; i<srow[id+1]; i++)
    {
      if( i != k)
      {
        for(j=k+1; j<mm; j++)
        {
          mpf_mul(T, BB[ROW[i]*mm+k], BB[ROW[k]*mm+j]);
          mpf_sub(BB[ROW[i]*mm+j], BB[ROW[i]*mm+j], T);
        }
      }
    }
    /* END INNER REDUCTION */
    barrier(); /* for master to know all threads done */
  }
  /* pthread_exit(NULL); */
  return threadID; /* checked for correct finish */
}

void mpf_tsimeq(int np, int n, mpf_t A[], mpf_t Y[], mpf_t X[])
{
  mpf_t *B;           /* [n][n+1]  WORKING MATRIX */
  int i, j, m;

  B = (mpf_t *)calloc(n*(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_tsimeqb(np, n, B, X);
  free(B);
} /* end mpf_tsimeq */

void mpf_tsimeqb(int np, int n, mpf_t B[], mpf_t X[])
{
  long int worker;
  int ids[np];                          /* worker id number */
  pthread_t threads[np];                /* holds thread info */
  int errcode;                          /* holds pthread error code */
  int status;                           /* holds return code */
  int part, remain;
  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;

  mpf_init(PIVOT);
  mpf_init(ABS_PIVOT);
  mpf_init(T);
  mpf_init(D);
  mpf_init(small);
  mpf_init_set_si(zero, 0);
  mpf_set_str(small, "1.0E-50", 10);

  nn = n;   /* global for threads */
  mm = n+1;
  BB = B;

  NP = np;
  if(NP>n) NP=n;
  srow = (int *)malloc((NP+1)*sizeof(int)); /* starting row for each thread */
  
  // set up row range for each thread
  part = n/NP;
  remain = n-part*NP;
  srow[0]=0;
  for(i=1; i<NP; i++)
  {
    srow[i]=srow[i-1]+part;
    if(i<=remain) srow[i]++;
  }
  srow[NP]=n; /* last group may be one less */ 

  /* SET UP ROW  INTERCHANGE VECTORS */
  ROW = (int *)calloc(n, sizeof(int));
  m = n+1;
  for(k=0; k<n; k++)
    ROW[k] = k;

  /* create the threads */
  for (worker=0; worker<NP; worker++)
  {
    if (errcode=pthread_create(&threads[worker],/* thread struct             */
		       NULL,                    /* default thread attributes */
		       parallel,                /* start thread              */
		       (void *)worker))         /* arg to thread             */
    {
      printf("worker %ld failed create: %d \n", worker, strerror(errcode));
      exit(1);
    }
  }

  /* 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<n+1; 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 */
      barrier(); /* release threads */
      barrier(); /* all threads done this interation */
    }
    /* 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);

  /* wait for all threads to exit, optional error check */
  for (worker=0; worker<NP; worker++)
  {
    if (errcode=pthread_join(threads[worker],(void *) &status))
    { 
      printf("worker %ld failed join: %d \n", worker, strerror(errcode));
      exit(1);
    }
    /* check thread's exit status and release its resources */
    if (status != worker)
    {
      printf("worker %ld terminated abnormally \n", worker);
      exit(1);
    }
  }
} /* end mpf_tsimeqb */

void mpf_tsimeqd(int np, 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); in main */
  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_tsimeqb(np, n, B, Xm);
  for(i=0; i<n; i++)
  {
    X[i] = mpf_get_d(Xm[i]); /* mpf to double */
  }
} /* end mpf_tsimeqd */