/* least_square_fit_2d.c third power                                 */ 
/*                     tailorable code, provide your input and setup */
/*  fit  u(x,t)  given values u(0,0)   u(h,0)   u(2h,0)  ...         */
/*                            u(0,k)   u(h,k)   u(2h,k)  ...         */
/*                            u(0,2k)  u(h,2k)  u(2h,2k) ...         */
/* find c_i   u(x,t) = c_0       + c_1*x     + c_2*y     +           */
/*                     c_3*x*x   + c_4*x*y   + c_5*y*y   +           */
/*                     c_6*x*x*x + c_7*x*x*y + c_8*x*y*y + c_9*y*y*y */
/* given value for  n  the highest power                             */

#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 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_set2d(double *u1, double *x1, double *t1)
{ /* read file and return  u1,x1,t1 for u(x1, t1) =  u1   */
  /* function value returned 1=good data, 0=bad, no data  */
  static int first=1;
  static int i, j;
  double u, x, t;
  double hx = 0.5;
  double ht = 0.2;

  if(first)
  {
    first = 0;
    printf("the function generating data set  \n");
    i = 0;
    j = 0;
  }
  
  x = (double)i*hx;
  t = (double)j*ht;
  u = 1.0+2.0*x+3.0*t+4.0*x*x+5.0*x*t+6.0*t*t+7.0*x*x*x+
      8.0*x*x*t+9.0*x*t*t+10.0*t*t*t;
  i++;
  if(i>5) {i=0; j++;}
  if(j>5) {first=1; return 0;}
  *x1 = x;
  *t1 = t;
  *u1 = u;
  return 1;
}

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

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

static void fit_2d(int n, int m, int *nnn, double A[], double Y[], double X[])
{
  int i, j, nn;
  double Av[10]; /* at least n */
  double Bv[10]; /* powers of variables */
  double u1, a1, b1;
  double term_i, term_j;
  
  if(debug) printf("fit_2d n=%d, m=%d \n", n, m);
  gen_2d_powers(n, m, &nn, Apwr, Bpwr);
  if(debug) printf("after gen_2d_powers n=%d, m=%d, nn=%d \n", n, m, nn);
  *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_set2d(&u1, &a1, &b1))
  {
    Av[0] = 1.0;
    Bv[0] = 1.0;
    if(debug>4) printf("Av[0]=%g, Bv[0]=%g \n",
                        Av[0], Bv[0]);
    for(i=1; i<=n; i++)
    {
      Av[i] = Av[i-1]*a1;
      Bv[i] = Bv[i-1]*b1;
      if(debug>4) printf("Av[%d]=%g, Bv[%d]=%g \n",
                          i, Av[i], i, Bv[i]);
    }
    for(i=0; i<nn; i++)
    {
      term_i = Av[Apwr[i]] * Bv[Bpwr[i]];
      for(j=0; j<nn; j++)
      {
        term_j = Av[Apwr[j]] * Bv[Bpwr[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_2d */

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

    if(k==0)
    {
      amin=a1; amax=a1;
      bmin=b1; bmax=b1;
      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(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]] * 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_2d k=%d, amin=%g, amax=%g, bmin=%g, bmax=%g \n",
          k, amin, amax, bmin, bmax);
  printf("               umin=%g, umax=%g \n",
             umin, umax);
  *max_err = maxe;
  *avg_err = sum/(double)k;
  *rms_err = sqrt(sumsq/(double)k);
} /* end check_2d */

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 = 2;    /* number of variables, only 2 allowed */
  int nn;       /* number of terms in fit equation */
  double rms_err, avg_err, max_err;
  
  printf("least_square_fit.c\n");
  printf("fitting u=1.0+2.0*x+3.0*t+4.0*x*x+5.0*x*t+6.0*t*t+7.0*x*x*x+ \n");
  printf("          8.0*x*x*t+9.0*x*t*t+10.0*t*t*t; \n");
  /* fit 2D surface u(a,b) */
  n=1; /* terms will be   constant, a, b  first order*/
  fit_2d(n, m, &nn, A, Y, C);
  printf("fit with %d variables to %d power using %d terms \n", m, n, nn);
  check_2d(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  \n",
                             C[i], Apwr[i], Bpwr[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 x*t  second order*/
  fit_2d(n, m, &nn, A, Y, C);
  printf("fit with %d variables to %d power using %d terms \n", m, n, nn);
  check_2d(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  \n",
                             C[i], Apwr[i], Bpwr[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  x^2*t  third order*/
  fit_2d(n, m, &nn, A, Y, C);
  printf("fit with %d variables to %d power using %d terms \n", m, n, nn);
  check_2d(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  \n",
                             C[i], Apwr[i], Bpwr[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_2d.c */