/* least_square_fit_3d.c up to power 3                     */ 
/*           tailorable code, provide your input and setup */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#undef  abs
#define abs(x) (((x)<0.0)?(-(x)):(x))
#undef  max
#define max(x,y) (((x)>(y))?((x)):(y))
#undef  min
#define min(x,y) (((x)<(y))?((x)):(y))

static double A[40000];
static double C[200];
static double Y[200];
static int Apwr[200]; /* powers of each variable in a term */
static int Bpwr[200];
static int Cpwr[200];
static int debug = 0;

static void simeq(int n, double A[], double Y[], double X[]);

/*
 * The purpose of this package is to provide a reliable and convenient
 * means for fitting existing data by a few coefficients. The companion
 * package check_fit provides the means to use the coefficients for
 * interpolation and limited extrapolation.
 * 
 * This package implements the least square fit. 
 * 
 * The problem is stated as follows :
 *   Given measured data for values of Y based on values of X1,X2 and X3. e.g.
 * 
 *    Y_actual         X1      X2     X3  
 *    --------       -----   -----  -----
 *     32.5           1.0     2.5    3.7 
 *      7.2           2.0     2.5    3.6 
 *      6.9           3.0     2.7    3.5 
 *     22.4           2.2     2.1    3.1 
 *     10.4           1.5     2.0    2.6 
 *     11.3           1.6     2.0    3.1 
 * 
 *  Find a, b and c such that   Y_approximate =  a * X1 + b * X2 + c * X3
 *  and such that the sum of (Y_actual - Y_approximate) squared is minimized.
 * 
 * The method for determining the coefficients a, b and c follows directly
 * form the problem definition and mathematical analysis. (See more below)
 * 
 * Y is called the dependent variable and X1 .. Xn the independent variables.
 * The procedures below implements a few special cases and the general case.
 *    The number of independent variables can vary.
 *    The approximation equation may use powers of the independent variables
 *    The user may create additional independent variables e.g. X2 = SIN(X1)
 *    with the restriction that the independent variables are linearly
 *    independent.  e.g.  Xi not equal  p Xj + q  for all i,j,p,q
 * 
 * 
 * 
 * The mathematical derivation of the least square fit is as follows :
 * 
 * Given data for the independent variable Y in terms of the dependent
 * variables S,T,U and V  consider that there exists a function F
 * such that     Y = F(S,T,U,V)
 * The problem is to find coefficients a,b,c and d such that
 *            Y_approximate = a * S + b * T + c * U + d * V
 * and such that the sum of ( Y - Y_approximate ) squared is minimized.
 * 
 * Note: a, b, c, d are scalars. S, T, U, V, Y, Y_approximate are vectors.
 * 
 * To find the minimum of  SUM( Y - Y_approximate ) ** 2
 * the derivatives must be taken with respect to a,b,c and d and
 * all must equal zero simultaneously. The steps follow :
 * 
 *  SUM( Y - Y_approximate ) ** 2 = SUM( Y - a*S - b*T - c*U - d*V ) ** 2
 * 
 * d/da =  -2 * S * SUM( Y - A*S - B*T - C*U - D*V )
 * d/db =  -2 * T * SUM( Y - A*S - B*T - C*U - D*V )
 * d/dc =  -2 * U * SUM( Y - A*S - B*T - C*U - D*V )
 * d/dd =  -2 * V * SUM( Y - A*S - B*T - C*U - D*V )
 * 
 * Setting each of the above equal to zero and putting constant term on left
 *    the -2 is factored out,
 *    the independent variable is moved inside the summation
 * 
 *  SUM( a*S*S + b*S*T + c*S*U + d*S*V = S*Y )
 *  SUM( a*T*S + b*T*T + c*T*U + d*T*V = T*Y )
 *  SUM( a*U*S + b*U*T + c*U*U + d*U*V = U*Y )
 *  SUM( a*V*S + b*V*T + c*V*U + d*V*V = V*Y )
 * 
 * Distributing the SUM inside yields
 * 
 *  a * SUM(S*S) + b * SUM(S*T) + c * SUM(S*U) + d * SUM(S*V) = SUM(S*Y)
 *  a * SUM(T*S) + b * SUM(T*T) + c * SUM(T*U) + d * SUM(T*V) = SUM(T*Y)
 *  a * SUM(U*S) + b * SUM(U*T) + c * SUM(U*U) + d * SUM(U*V) = SUM(U*Y)
 *  a * SUM(V*S) + b * SUM(V*T) + c * SUM(V*U) + d * SUM(V*V) = SUM(V*Y)
 * 
 * To find the coefficients a,b,c and d solve the linear system of equations
 * 
 *    | SUM(S*S)  SUM(S*T)  SUM(S*U)  SUM(S*V) |   | a |   | SUM(S*Y) |
 *    | SUM(T*S)  SUM(T*T)  SUM(T*U)  SUM(T*V) | x | b | = | SUM(T*Y) |
 *    | SUM(U*S)  SUM(U*T)  SUM(U*U)  SUM(U*V) |   | c |   | SUM(U*Y) |
 *    | SUM(V*S)  SUM(V*T)  SUM(V*U)  SUM(V*V) |   | d |   | SUM(V*Y) |
 * 
 * Some observations :
 *     S,T,U and V must be linearly independent.
 *     There must be more data sets (Y, S, T, U, V) than variables.
 *     The analysis did not depend on the number of independent variables
 *     A polynomial fit results from the substitutions S=1, T=X, U=X**2, V=X**3
 *     The general case for any order polynomial follows, fit_pn.
 *     Any substitution such as three variables to various powers may be used.
 */

static int data_set3d(double *u1, double *a1, double *b1, double *c1)
{
  static int i=0;
  static int j=0;
  static int k=0;
  static int first=1;
  int n = 3;
  double u, a, b, c;

  if(first)
  {
    first = 0;
    printf("the function generating the data set is \n");
    printf("u = 1.0 + 2.0*a + 3.0*b + 4.0*c + 5.0*a*a + 6.0*b*b +\n");
    printf("    7.0*c*c +8.0*a*a*a + 9.0*b*b*b + 10.0*c*c*c \n"
    );
  }
  i++;
  if(i>n) {i=0; j++;}
  if(j>n) {j=0; k++;}
  if(k>n) {i=0; j=0; k=0; return 0;}
  a = (double)i;
  b = (double)j;
  c = (double)k;
  u = 1.0 + 2.0*a + 3.0*b + 4.0*c + 5.0*a*a + 6.0*a*b + 7.0*a*c +
      8.0*b*b + 9.0*b*c + 10.0*c*c + 11.0*a*a*a + 12.0*a*a*b +
      13.0*a*a*c + 14.0*a*b*b + 15.0*a*b*c + 16.0*a*c*c +
      17.0*b*b*b + 18.0*b*b*c + 19.0*b*c*c + 20.0*c*c*c;
  if(debug>1) printf("data_set u=%g, a=%g, b=%g, c=%g\n", u, a, b, c);
  *u1=u;
  *a1=a;
  *b1=b;
  *c1=c;
  return 1;
}

static gen_3d_powers(int n, int m, int *nnn, int a[], int b[], int c[])
{
  int i,j,k; /* need more, or generalize, for more variables */
  int ii = 0; /* pointer to next available a[ii], b[ii], c[ii] */  
  int nn=1;  /* power being generated */

  if(m != 3) printf("ERROR, this only good for m=3 independent variables\n");
  a[0]=0;
  b[0]=0;
  c[0]=0;
  printf("terms used to find fit \n");
  printf("a^0 b^0 c^0 \n\n");
  
  i=nn;
  j=0;
  k=0;
  
  while(nn<=n) /* n is highest sum of powers */
  {
    ii++;
    a[ii] = i;
    b[ii] = j;
    c[ii] = k;
    printf("a^%d b^%d c^%d \n", i, j, k);
    if(i==0 && j==0 && k==nn) /* increment nn, set i,j,k */
    {
      nn++;
      i = nn;
      j = 0;
      k = 0;
      printf("\n");
    }
    else if(i==nn)
    {
      i--;
      j=1; /* k should be zero */
    }
    else if(j==nn)
    {
      j--; /* i should be zero */
      k=1;
    }
    else if(j>0)
    {
      j--;
      k++;
    }
    else if(j==0)
    {
      i--;
      j=nn-i;
      k=0;
    }
  }
  *nnn = ii+1;
} /* end gen_3d_powers */

static void fit_3d(int n, int m, int *nnn, double A[], double Y[], double X[])
{
  int i, j, k, nn;
  double Av[10]; /* at least n */
  double Bv[10]; /* powers of variables */
  double Cv[10];
  double u1, a1, b1, c1;
  double term_i, term_j;
  
  gen_3d_powers(n, m, &nn, Apwr, Bpwr, Cpwr);
  *nnn = nn;
  
  for(i=0; i<nn; i++)
  {
    for(j=0; j<nn; j++)
    {
      A[i*nn+j] = 0.0;
    }
    Y[i] = 0.0;
  }
  while(data_set3d(&u1, &a1, &b1, &c1))
  {
    Av[0] = 1.0;
    Bv[0] = 1.0;
    Cv[0] = 1.0;
    for(i=1; i<=n; i++)
    {
      Av[i] = Av[i-1]*a1;
      Bv[i] = Bv[i-1]*b1;
      Cv[i] = Cv[i-1]*c1;
      if(debug>4) printf("Av[%d]=%g, Bv[%d]=%g, Cv[%d]=%g \n",
                          i, Av[i], i, Bv[i], i, Cv[i]);
    }
    for(i=0; i<nn; i++)
    {
      term_i = Av[Apwr[i]] * Bv[Bpwr[i]] * Cv[Cpwr[i]];
      for(j=0; j<nn; j++)
      {
        term_j = Av[Apwr[j]] * Bv[Bpwr[j]] * Cv[Cpwr[j]];
        A[i*nn+j] = A[i*nn+j] + term_i * term_j;
      }
      Y[i] = Y[i] + u1 * term_i;
    }
  }
  if(debug>2)
  {
    for(i=0; i<nn; i++)
    {
      for(j=0; j<nn; j++)
      {
        printf("A[%d][%d]=%g \n", i, j, A[i*nn+j]);
      }
      printf("Y[%d]=%g \n", i, Y[i]);
    }
  }
  simeq(nn, A, Y, X);
} /* end fit_3d */

static void check_3d(int n, int m, int nn, double X[],
                     double *rms_err, double *avg_err, double *max_err)
{
  double a1, b1, c1, u1, ua, diff;
  double sumsq = 0.0;
  double sum = 0.0;
  double maxe = 0.0;
  double amin, amax, bmin, bmax, cmin, cmax, umin, umax;
  double Av[10]; /* at least n */
  double Bv[10]; /* powers of variables */
  double Cv[10];
  int i, k;
  
  k = 0;
  while(data_set3d(&u1, &a1, &b1, &c1))
  {
    Av[0] = 1.0;
    Bv[0] = 1.0;
    Cv[0] = 1.0;
    for(i=1; i<=n; i++)
    {
      Av[i] = Av[i-1]*a1;
      Bv[i] = Bv[i-1]*b1;
      Cv[i] = Cv[i-1]*c1;
    }

    if(k==0)
    {
      amin=a1; amax=a1;
      bmin=b1; bmax=b1;
      cmin=c1; cmax=c1;
      umin=u1; umax=u1;
    }
    if(a1>amax) amax=a1;
    if(a1<amin) amin=a1;
    if(b1>bmax) bmax=b1;
    if(b1<bmin) bmin=b1;
    if(c1>cmax) cmax=c1;
    if(c1<cmin) cmin=c1;
    if(u1>umax) umax=u1;
    if(u1<umin) umin=u1;

    ua = 0.0;
    for(i=0; i<nn; i++)
    {
      ua = ua + Av[Apwr[i]] * Bv[Bpwr[i]] * Cv[Cpwr[i]] * X[i];
    }
    diff = abs(u1-ua);
    if(debug) printf("sample=%d, u_fit=%g, u_data=%g, diff=%g \n",
                     k, ua, u1, u1-ua);
    if(diff>maxe) maxe=diff;
    sum = sum + diff;
    sumsq = sumsq + diff*diff;
    k++;
  }
  printf("check_3d k=%d, amin=%g, amax=%g, bmin=%g, bmax=%g \n",
          k, amin, amax, bmin, bmax);
  printf("               cmin=%g, cmax=%g, umin=%g, umax=%g \n",
             cmin, cmax, umin, umax);
  *max_err = maxe;
  *avg_err = sum/(double)k;
  *rms_err = sqrt(sumsq/(double)k);
} /* end check_3d */

static void simeq(int n, double A[], double Y[], double 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  :     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                   */

    double *B;           /* [n][n+1]  WORKING MATRIX */
    int *ROW;            /* ROW INTERCHANGE INDICIES */
    int HOLD , I_PIVOT;  /* PIVOT INDICIES */
    double PIVOT;        /* PIVOT ELEMENT VALUE */
    double ABS_PIVOT;
    int i,j,k,m;

    B = (double *)calloc((n+1)*(n+1), sizeof(double));
    ROW = (int *)calloc(n, sizeof(int));
    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];
    }
    /* 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 */
      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-8 ){
        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 */
        for(i=0; i<n; i++){
          if( i != k){
            for(j=k+1; j<n+1; j++){
              B[ROW[i]*m+j] = B[ROW[i]*m+j] - B[ROW[i]*m+k] * B[ROW[k]*m+j];
            }
          }
        }
      }
      /* 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(B);
    free(ROW);
} /* end simeq */

int main(int argc, char *argv[])
{
  /* array declarations in file space due to stack limitations */
  int i, n;
  int m = 3;    /* number of variables */
  int nn;       /* number of terms in fit equation */
  double rms_err, avg_err, max_err;
  
  printf("least_square_fit.c\n");
  printf("fit 1.0 + 2.0*a + 3.0*b + 4.0*c + 5.0*a*a + 6.0*a*b + 7.0*a*c +\n");
  printf("    8.0*b*b + 9.0*b*c + 10.0*c*c + 11.0*a*a*a + 12.0*a*a*b +\n");
  printf("    13.0*a*a*c + 14.0*a*b*b + 15.0*a*b*c + 16.0*a*c*c +\n");
  printf("    17.0*b*b*b + 18.0*b*b*c + 19.0*b*c*c + 20.0*c*c*c; \n");
  
  /* fit 3D surface u(a,b,c) */
  n=1; /* terms will be   constant, a, b, c */
  fit_3d(n, m, &nn, A, Y, C);
  printf("fit with %d variables to %d power using %d terms \n", m, n, nn);
  check_3d(n, m, nn, C, &rms_err, &avg_err, &max_err);
  printf("\nLeast Square Fit is u = \n");
  for(i=0; i<nn; i++) printf("   %g * a^%d * b^%d * c^%d \n",
                             C[i], Apwr[i], Bpwr[i], Cpwr[i]);
  printf("rms_err=%g, avg_err=%g, max_err=%g \n\n",
         rms_err, avg_err, max_err);

  n=2; /* terms will be up to x^2 */
  fit_3d(n, m, &nn, A, Y, C);
  printf("fit with %d variables to %d power using %d terms \n", m, n, nn);
  check_3d(n, m, nn, C, &rms_err, &avg_err, &max_err);
  printf("\nLeast Square Fit is u = \n");
  for(i=0; i<nn; i++) printf("   %g * a^%d * b^%d * c^%d \n",
                             C[i], Apwr[i], Bpwr[i], Cpwr[i]);
  printf("rms_err=%g, avg_err=%g, max_err=%g \n\n",
         rms_err, avg_err, max_err);

  n=3; /* terms will be up to x^3 */
  fit_3d(n, m, &nn, A, Y, C);
  printf("fit with %d variables to %d power using %d terms \n", m, n, nn);
  check_3d(n, m, nn, C, &rms_err, &avg_err, &max_err);
  printf("\nLeast Square Fit is u = \n");
  for(i=0; i<nn; i++) printf("   %g * a^%d * b^%d * c^%d \n",
                             C[i], Apwr[i], Bpwr[i], Cpwr[i]);
  printf("rms_err=%g, avg_err=%g, max_err=%g \n\n",
         rms_err, avg_err, max_err);
  return 0;
} /* end main for least_square_fit_3d.c */