/* 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>
#undef  abs
#define abs(x) (((x)<0.0)?(-(x)):(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  : tsimeq(np,n,A,Y,X);  A not modified                 */
/*               tsimeqb(np,n,B,X)    B=A with Y last column, changed*/
/*               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                                        */

#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 double * 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;

  id = (long int)threadID;
  for(k=0; k<nn; k++)
  {
    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++)
        {
          BB[ROW[i]*mm+j] = BB[ROW[i]*mm+j] - BB[ROW[i]*mm+k] * BB[ROW[k]*mm+j];
        }
      }
    }
    barrier(); /* for master to know all threads done */
  }
  /* pthread_exit(NULL); */
  return threadID; /* checked for correct finish */
}

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

  B = (double *)calloc(n*(n+1), sizeof(double));
  m = n+1;
  /* BUILD WORKING DATA STRUCTURE */
  for(i=0; i<n; i++){
    for(j=0; j<n; j++){
      B[i*m+j] = A[i*n+j];
    }
    B[i*m+n] = Y[i];
  }
  tsimeqb(np, n, B, X);
  free(B);
} /* end tsimeq */

void tsimeqb(int np, int n, double B[], double 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 */
  double PIVOT;        /* PIVOT ELEMENT VALUE */
  double ABS_PIVOT;
  int i,j,k,m;

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

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

    /* CHECK FOR NEAR SINGULAR */
    if( ABS_PIVOT < 1.0E-64 )
    {
      for(j=k+1; j<n+1; j++)
      {
        B[ROW[k]*m+j] = 0.0;
      }
      printf("redundant row (singular) %d \n", ROW[k]);
    } /* singular, delete row */
    else
    {

      /* REDUCE ABOUT PIVOT */
      for(j=k+1; j<n+1; j++)
      {
        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++)
  {
    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 tsimeqb */