/* deriv.c includes deriv, rderiv, nuderiv                                 */
/*          order is order of derivative, 1 = first derivative, 2 = second */
/*          points is number of points where value of function is known    */
/*          f(x0), f(x1), f(x2) ... f(x points-1)                          */
/*          point is the term where derivative is computed                 */
/*          f'(x0) = (1/bh^order)*( a(0)*f(x0) + a(1)*f(x1)                */
/*                   + ... + a(points-1)*f(x points-1)                     */
/*          uniformly spaced points x1=x0+h, x2=x1+h=x1+2*h, ...           */
/*                                                                         */
/*          rderiv returns c[0]=(1/bh^order)*a[0]  ...                     */
/*                                                                         */
/*          nuderiv works for non-uniformly spaced points                  */
/*                                                                         */
/* algorithm: use divided differences to get polynomial p(x) that          */
/*            approximates  f(x).  f(x)=p(x)+error term                    */
/*            f'(x) = p'(x) + error term'                                  */
/*            substitute xj = x0 + j*h                                     */
/*            substitute x = x0 to get  p'(x0) etc                         */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#undef  abs
#define abs(x) (((x)<0.0)?(-(x)):(x))

static int gcd(int a, int b)
{
  int a1, b1, q, r;

  if(a==0 || b==0) return 1;
  if(abs(a)>abs(b)) { a1 = abs(a); b1 = abs(b); }
  else              { a1 = abs(b); b1 = abs(a); }
  r=1;
  while(r!=0)
  {
    q = a1 / b1;
    r = a1 - q * b1;
    a1 = b1;
    b1 = r;
  }
  return a1;
} /* end gcd */

void deriv(int order, int npoints, int point, int *a, int *bb)
{
  /* compute the exact coefficients to numerically compute a derivative */
  /* of order 'order' using 'npoints' at ordinate point,                */
  /* where order>=1, npoints>=order+1, 0 <= point < npoints,            */
  /* 'a' array returned with numerator of coefficients,                 */
  /* 'bb' returned with denominator of h^order                          */
  
  int h[100];          /* coefficients of h */
  int x[100];          /* coefficients of x, variable for differentiating */
  int numer[100][100]; /* numerator of a term numer[term][pos] */
  int denom[100];      /* denominator of coefficient */
  int i, j, k, term, b;
  int jorder, ipower, isum, iat, jterm, r;
  int debug = 0;
  
  if(npoints<=order)
  {
    printf("ERROR in call to deriv, npoints=%d < order=%d+1=%d \n\n",
           npoints, order, order+1);
    return;
  }
  for(term=0; term<npoints; term++)
  {
    denom[term] = 1;
    j = 0;
    for(i=0; i<npoints-1; i++)
    {
      if(j==term) j++; /* no index j in jth term */
      numer[term][i] = -j; /* from (x-x[j]) */
      denom[term] = denom[term]*(term-j);
      j++;
    }
  } /* end term setting up numerator and denominator */
  if(debug>0)
    for(i=0; i<npoints; i++)
    {
      printf("denom[%d]=%d \n", i, denom[i]);
      for(j=0; j<npoints-1; j++)
        printf("numer[%d][%d]=%d \n", i, j, numer[i][j]);
    }
        
  /* substitute xj = x0 + j*h, just keep j value in numer[term][i] */
  /* don't need to count x0 because same as x's */
  /* done by above setup */
      
  /* multiply out polynomial in x with coefficients of h */
  /* starting from (x+j1*h)*(x+j2*h) * ... */
  /* then differentiate d/dx */
  for(term=0; term<npoints; term++)
  {
    x[0] = 1; /* initialize */
    x[1] = 0;
    h[1] = 0;
    k = 1;
    for(i=0; i<npoints-1; i++)
    {
      for(j=0; j<k; j++)            /* multiply by (x + j h) */
      {
        h[j] = x[j]*numer[term][i]; /* multiply be constant x[j] */
      }
      for(j=0; j<k; j++)         /* multiply by x */
      {
        h[j+1] = h[j+1]+x[j]; /* multiply by x and add */
      }
      k++;
      for(j=0; j<k; j++) x[j] = h[j];
      x[k] = 0;
      h[k] = 0;
    }
    for(j=0; j<k; j++)
    {
      numer[term][j] = x[j];
      if(debug>0)printf("p(x)= %d x^%d at term=%d \n",
                        numer[term][j], j, term);
    } /* have p(x) for this 'term' */

    /* differentiate 'order' number of times */
    for(jorder=0; jorder<order; jorder++)
    {
      for(i=1; i<k; i++) /* differentiate p'(x) */
      {
        numer[term][i-1] = i*numer[term][i];
      }
      k--;
    } /* have p^(order) for this term */
    if(debug>0)
      for(i=0; i<k; i++)
        printf("p^(%d)(x)= %d x^%d at term=%d \n",
               order, numer[term][i], i, term);
  } /* end 'term' loop  for one 'order', for one 'npoints' */

  /* substitute each point for x, evaluate, get coefficients of f(x[j]) */
  iat = point; /* evaluate at x[iat], substitute */
  for(jterm=0; jterm<npoints; jterm++) /* get each term */
  {
    ipower = iat;
    isum = numer[jterm][0];
    for(j=1; j<k; j++)
    {
      isum = isum + ipower * numer[jterm][j];
      ipower = ipower * iat;
    }
    a[jterm] = isum;
    if(debug>0)
      printf("f^(%d)(x[%d]) = (1/h^%d) (%d/%d f(x[%d]) + \n",
             order, iat, order, a[jterm], denom[jterm], jterm);
  }
  if(debug>0) printf("\n");
  b = 0;
  for(jterm=0; jterm<npoints; jterm++) /* clean up each term */
  {
    if(a[jterm]==0) continue;
    r = gcd(a[jterm],denom[jterm]);
    a[jterm] = a[jterm] / r;
    denom[jterm] = denom[jterm] /r;
    if(denom[jterm]<0)
    {
      denom[jterm] = -denom[jterm];
      a[jterm] = - a[jterm];
    }
    if(denom[jterm]>b) b=denom[jterm]; /* largest denominator */
  }
  for(jterm=0; jterm<npoints; jterm++) /* check for divisor */
  {
    r = (a[jterm] * b) / denom[jterm];
    if(r * denom[jterm] != a[jterm] * b)
    {
      r = gcd(b, denom[jterm]);
      b = b * (denom[jterm] / r);
    }
  }    
  for(jterm=0; jterm<npoints; jterm++) /* adjust for divisor */
  {
    a[jterm] = (a[jterm] * b) / denom[jterm];
    if(debug>0)
      printf("f^(%d)(x[%d])=(1/%dh^%d)(%d f(x[%d]) + \n",
             order, iat, b, order, a[jterm], jterm);
  } /* end computing terms of coefficients */
  *bb = b;
} /* end deriv */

/* nuderiv.c  non uniformly spaced derivative coefficients */

static void inverse(int n, double A[], double AA[]);

void nuderiv(int order, int npoint, int point, double x[], double c[])
{
  double *A;
  double *AI;
  double *fct;
  double pwr;
  int i, j, k, n;

  n = npoint;
  A = (double *)calloc(n*n, sizeof(double));
  AI = (double *)calloc(n*n, sizeof(double));
  fct = (double *)calloc(n, sizeof(double));

  for(i=0; i<n; i++) /* build matrix that will be inverted */
  {
    pwr=1.0;
    for(j=0; j<n; j++)
    {
      A[i*n+j] = pwr;
      pwr = pwr*x[i];
    }
  }
  inverse(n, A, AI); /* invert the matrix */
  /* form derivative for order and point */
  for(i=0; i<n; i++) /* compute factors for order */
  {
    fct[i] = 1.0;
  }
  for(j=0; j<order; j++)
  {
    for(i=0; i<n; i++)
    {
      fct[i] = fct[i]*(double)(i-j);
    }
  }
  for(i=0; i<n; i++)
  {
    c[i] = 0.0;
    pwr = 1.0;
    for(j=order; j<n; j++)
    { 
      c[i] = c[i] + fct[j]*AI[j*n+i]*pwr;
      pwr = pwr * x[point];
    }
  }
} /* end nuderiv */

static void inverse(int n, double A[], double AA[])
{
    int *ROW, *COL;
    double *TEMP;
    int HOLD , I_PIVOT , J_PIVOT;
    double PIVOT, ABS_PIVOT;
    int i, j, k;

    ROW = (int *)calloc(n, sizeof(int));
    COL = (int *)calloc(n, sizeof(int));
    TEMP = (double *)calloc(n, sizeof(double));
    memcpy(AA,A,n*n*sizeof(double));

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

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

      /*        find largest element for pivot  */
      PIVOT = AA[ROW[k]*n+COL[k]] ;
      I_PIVOT = k;
      J_PIVOT = k;
      for(i=k; i<n; i++){
        for(j=k; j<n; j++){
          ABS_PIVOT = abs(PIVOT) ;
          if( abs(AA[ROW[i]*n+COL[j]]) > ABS_PIVOT ){
            I_PIVOT = i ;
            J_PIVOT = j ;
            PIVOT = AA[ROW[i]*n+COL[j]] ;
          }
        }
      }
      if(abs(PIVOT) < 1.0E-12){
        free(ROW);
        free(COL);
        free(TEMP);
        printf("nuderiv MATRIX is SINGULAR !!! \n");
        return;
      }

      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 */
      AA[ROW[k]*n+COL[k]] = 1.0 / PIVOT ;
      for (j=0; j<n; j++) {
        if( j != k ){
          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 ){
              AA[ROW[i]*n+COL[j]] = AA[ROW[i]*n+COL[j]] - AA[ROW[i]*n+COL[k]] *
                                   AA[ROW[k]*n+COL[j]] ;
            }
          }
          AA[ROW[i]*n+COL [k]] = - AA[ROW[i]*n+COL[k]] * AA[ROW[k]*n+COL[k]] ;
        }
      }
    }
    /*      end main reduction loop */

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

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

void rderiv(int order, int npoints, int point, double h, double c[])
{
  /* compute double precision coefficients to numerically compute */
  /* a derivative  of order 'order' using 'npoints'               */
  /* at ordinate point,                                           */
  /* where order>=1, npoints>=order+1, 0 <= point < npoints,      */
  /* 'c' array returned with the coefficients,                    */
  int i, bb[1], a[100];
  double hpower, aa[100];

  if(order+npoints<=15)
  {
    deriv(order, npoints, point, a, bb);
    hpower = h;
    for(i=1; i<order; i++) hpower *= h;
    hpower = 1.0/((double)bb[0]*hpower);
    for(i=0; i<npoints; i++) c[i]=hpower*(double)a[i];
  }
  else
  {
    for(i=0; i<npoints; i++) aa[i] = (double)i;
    nuderiv(order, npoints, point, aa, c);
    hpower = 1.0/h;
    for(i=1; i<order; i++) hpower /= h;
    for(i=0; i<npoints; i++) c[i]=hpower*c[i];
  }
} /* end rderiv */
/* end deriv.c */