/* nuderiv.c  non uniformly spaced derivative coefficients */
/*    given  x0, x1, x2, x3  non uniformly spaced
 *    find the coefficients of y0, y1, y2, y3
 *    in order to compute the derivatives:
 *      y'(x0) = c00*y0 + c01*y1 + c02*y2 + c03*y3
 *      y'(x1) = c10*y0 + c11*y1 + c12*y2 + c13*y3
 *      y'(x2) = c20*y0 + c21*y1 + c22*y2 + c23*y3
 *      y'(x3) = c30*y0 + c31*y1 + c32*y2 + c33*y3
 *
 *  Method:  fit data to  y(x)    = a + b*x + c*x^2 + d*x^3
 *                        y'(x)   =     b   + 2*c*x + 3*d*x^2
 *                        y''(x)  =           2*c   + 6*d*x
 *                        y'''(x) =                   6*d
 *
 *           with y0, y1, y2 and y3 variables:
 *           (Remember the y values are unknown at this time.
 *            The y values are the unknowns in the PDE.
 *            We are finding the c coefficients in order to
 *            build a system of simultaneous equations.)
 *
 *                              y0  y1  y2  y3 
 *  form:  | 1  x0  x0^2  x0^3   1   0   0   0 | = a
 *         | 1  x1  x1^2  x1^3   0   1   0   0 | = b
 *         | 1  x2  x2^2  x2^3   0   0   1   0 | = c
 *         | 1  x3  x3^2  x3^3   0   0   0   1 | = d
 *
 * reduce  | 1   0     0     0  a0  a1  a2  a3 | = a
 *         | 0   1     0     0  b0  b1  b2  b3 | = b
 *         | 0   0     1     0  c0  c1  c2  c3 | = c
 *         | 0   0     0     1  d0  d1  d2  d3 | = d
 *
 *                              this is just the inverse
 *
 *   y'(x0) =  b0*y0        + b1*y1        + b2*y2        + b3*y3        +
 *             2*c0*y0*x0   + 2*c1*y1*x0   + 2*c2*y2*x0   + 2*c3*y3*x0   +
 *             3*d0*y0*x0^2 + 3*d1*y1*x0^2 + 3*d2*y2*x0^2 + 3*d3*y3*x0^2
 *
 *   or  c00 = b0 + 2*c0*x0 + 3*d0*x0^2
 *       c01 = b1 + 2*c1*x0 + 3*d1*x0^2
 *       c02 = b2 + 2*c2*x0 + 3*d2*x0^2
 *       c03 = b3 + 2*c3*x0 + 3*d3*x0^2
 *
 *   y'(x0) = c00*y0 + c01*y1 + c02*y2 +c03*y3
 *
 *   y'(x1) =  b0*y0        + b1*y1        + b2*y2        + b3*y3        +
 *             2*c0*y0*x1   + 2*c1*y1*x1   + 2*c2*y2*x1   + 2*c3*y3*x1   +
 *             3*d0*y0*x1^2 + 3*d1*y1*x1^2 + 3*d2*y2*x1^2 + 3*d3*y3*x1^2
 *
 *   or  c10 = b0 + 2*c0*x1 + 3*d0*x1^2
 *       c11 = b1 + 2*c1*x1 + 3*d1*x1^2
 *       c12 = b2 + 2*c2*x1 + 3*d2*x1^2
 *       c13 = b3 + 2*c3*x1 + 3*d3*x1^2
 *
 *       cij = bj + 2*cj*xi + 3*dj*xi^2
 *
 *
 *   y'(x1) = c10*y0 + c11*y1 + c12*y2 +c13*y3
 *
 *   y''(x0) = 2*c0*y0    + 2*c1*y1    + 2*c2*y2    + 2*c3*y3    +
 *             6*d0*y0*x0 + 6*d1*y1*x0 + 6*d2*y2*x0 + 6*d3*y3*x0
 *
 *   or  c00 = 2*c0 + 6*d0*x0
 *       c01 = 2*c1 + 6*d1*x0
 *       c02 = 2*c2 + 6*d2*x0
 *       c03 = 2*c3 + 6*d3*x0
 *
 *   y'''(x0) = 6*d0*y0 + 6*d1*y1 + 6*d2*y2 + 6*d3*y3
 *
 *   or  c00 = 6*d0    independent of x
 *       c01 = 6*d1
 *       c02 = 6*d2
 *       c03 = 6*d3
 *
 */

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

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 */