#include <stdio.h>
#include <stdlib.h>

/* Mass Air Flow meter calibration curve fitter.
** (c) 2001 Perry Harrington <pedward@apsoft.com>
**
** This program uses the 10 data points provided by Pro-Flow on the MAF calibration sheet to generate
** 30 data points that can be used in a Ford EEC MAF transfer function table.
** 
** The program currently uses only 10 input points and generates 30 output points.  Unknown data is
** interpolated from the available data.  Data is interpolated using a cubic spline function.  Even if
** the source data is incomplete, you can interpolate up to 5 volts output.
**
** Cubic spline functions taken from xlispstat 3.52, which are further borrowed from Numerical
** Recipes in C.
**
** Why use this program? Because your mass air meter isn't the same as the perfect spline calibration
** Pro-Flow puts on their website.  Additionally, the website flow data Pro-Flow provides is incomplete
** below .400 volts.
**
** Changelog
**
** 04/08/02 ph - added command line arguments, read custom dataset from STDIN, fixed some bugs that only
**		showed up when compiled for ARM, and changed everything to use float instead of double.
** 04/17/02 ph - added a command line argument for minimum voltage output.  This made the program a more
**		generic tool for doing cubic spline interpolation, such as for NTC thermistors.  The behavior
**		changed a bit too, now the vmin is 0 based, before it'd start at 0 + step.  This neccessitated
**		modification of how points is handled, now you must supply points + 1 to get the same dataset
**		as before, but now the vmin output value is always the least calculated value.
**
*/

/* natural cubic spline interpolation based on Numerical Recipes in C */

/* calculate second derivatives; assumes strictly increasing x values */
static void find_spline_derivs(float *x, float *y, int n, float *y2, float *u)
{
  int i, k;
  float p, sig;

  y2[0] = u[0] = 0.0;  /* lower boundary condition for natural spline */

  /* decomposition loop for the tridiagonal algorithm */
  for (i = 1; i < n - 1; i++) {
    y2[i] = u[i] = 0.0; /* set in case a zero test is failed */
    if (x[i - 1] < x[i] && x[i] < x[i + 1]) {
      sig = (x[i] - x[i - 1]) / (x[i + 1] - x[i - 1]);
      p = sig * y2[i - 1] + 2.0;
      if (p != 0.0) {
        y2[i] = (sig - 1.0) / p;
        u[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i])
             - (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
        u[i] = (6.0 * u[i] / (x[i + 1] - x[i - 1]) - sig * u[i - 1]) / p;
      }
    }
  }

  /* upper boundary condition for natural spline */
  y2[n - 1] = 0.0;

  /* backsubstitution loop of the tridiagonal algorithm */
  for (k = n - 2; k >= 0; k--)
    y2[k] = y2[k] * y2[k + 1] + u[k];
}

/* interpolate or extrapolate value at x using results of find_spline_derivs */
static void spline_interp(float *xa, float *ya, float *y2a, int n, float x, float *y)
{
  int klo, khi, k;
  float h, b, a;

  if (x <= xa[0]) {
    h = xa[1] - xa[0];
    b = (h > 0.0) ? (ya[1] - ya[0]) / h - h * y2a[1] / 6.0 : 0.0;
    *y = ya[0] + b * (x - xa[0]);
  }
  else if (x >= xa[n - 1]) {
    h = xa[n - 1] - xa[n - 2];
    b = (h > 0.0) ? (ya[n - 1] - ya[n - 2]) / h + h * y2a[n - 2] / 6.0 : 0.0;
    *y = ya[n - 1] + b * (x - xa[n - 1]);
  }
  else {
    /* try a linear interpolation for equally spaced x values */
    k = (n - 1) * (x - xa[0]) / (xa[n - 1] - xa[0]);

    /* make sure the range is right */
    if (k < 0) k = 0;
    if (k > n - 2) k = n - 2;

    /* bisect if necessary until the bracketing interval is found */
    klo = (x >= xa[k]) ? k : 0;
    khi = (x < xa[k + 1]) ? k + 1 : n - 1;
    while (khi - klo > 1) {
      k = (khi + klo) / 2;
      if (xa[k] > x) khi = k;
      else klo = k;
    }

    /* interpolate */
    h = xa[khi] - xa[klo];
    if (h > 0.0) {
      a = (xa[khi] - x) / h;
      b = (x - xa[klo]) / h;
      *y = a * ya[klo] + b * ya[khi]
         + ((a * a * a - a) * y2a[klo] + (b * b * b - b) * y2a[khi]) * (h * h) / 6.0;
    }
    else *y = (ya[klo] + ya[khi]) / 2.0; /* should not be needed */
  }
}

int fit_spline(int n, float *x, float *y, int ns, float *xs, float *ys, float *work)
{
  int i;
  float *y2, *u;

  y2 = work; u = work + n;

  if (n < 2 || ns < 1) return (1); /* signal an error */

  for (i = 1; i < n; i++) {
    if (x[i - 1] >= x[i]) return(1); /* signal an error */
  }

  find_spline_derivs(x, y, n, y2, u);

  for (i = 0; i < ns; i++)
    spline_interp(x, y, y2, n, xs[i], &ys[i]);

  return(0);
}

int main(int argc, char *argv[])
{
	float	*known_volts;
	float	*known_flow;
	float	*volts;
	float	*flow;
	float	*work;
	float	vmin, vmax;
	int	known_points, i, points = 30;
	float	f,g;
	int	retval;

	if (argc < 5) {
		printf("Too few arguments: maf_cub <input points> <output points> <min x output value> <max x output value> < <data file>\n");
		exit(1);
	}

	known_points = atoi(argv[1]);
	points = atoi(argv[2]);
	sscanf(argv[3],"%f",&vmin);
	sscanf(argv[4],"%f",&vmax);

	if (known_points < 0 || known_points > 500) {
		printf("Desired input data points out of range, value must be between 1 and 500!\n");
	}
		
	if (points < 0 || points > 500) {
		printf("Desired output data points out of range, value must be between 1 and 500!\n");
	}
		
	if (vmax < vmin) {
		printf("Desired max output voltage must be > min output!\n");
	}

	if (vmin > vmax) {
		printf("Desired min output voltage must be < max output!\n");
	}
		
	/* calculate a linear voltage table - note that vmax - vmin is zero origin, whereas points is 1
	so we must reduce points by 1 to make it zero origin and get a correct step value */

	f = ((vmax-vmin)/(points - 1));

	known_volts = calloc(known_points, sizeof(float));
	known_flow = calloc(known_points, sizeof(float));
	volts = calloc(points, sizeof(float));
	flow = calloc(points, sizeof(float));
	work = calloc(points * 3, sizeof(float));

	i=0;

	do {
		retval = fscanf(stdin, "%f, %f\n", &known_volts[i], &known_flow[i]);
		if (retval == EOF) break;

		i++;
	} while (retval != EOF);

	fprintf(stderr, "vmin = %f vmax = %f step = %f points = %d\n",vmin,vmax,f,points);

	g = vmin;

	/* this handles the output range population, the algorithm will base it's calculations on these points */
	for (i=0; i < points; i++) {
		volts[i] = g;
		g += f;
	}

	if (fit_spline(known_points, known_volts, known_flow, points + 1, volts, flow, work) == 1) {
		printf("Error while doing conversion, check that your dataset x values are ordered correctly\n");
	}
	
	printf("/" "* ( Voltage, Flow ) *" "/\n\n");

	/* this does greatest to least zero origin dump of the interpolated dataset, tag names are vestigial */
	for (i = points - 1; i > -1; i--) {
		printf("( %5.4f, %5.4f )\t", volts[i], flow[i]);
		if (i%3 == 0) printf("\n");
	}
	printf("\n");

	return(0);
}