/*
**  interp.c 1.3	Solaris 2.3 Unix	940906	SIO/ODF	fmd
**
**  Interpolate using linear, single or double quadratic interpolation.
*/
#include <math.h>
#include <odflib.h>

#define EPS 1.0e-9

#ifndef FORTRAN

/*
**  Interpolate a series using linear, single or double-quadratic
**  interpolation.
**
**  Given arrays of pairs of dependent and independent values,
**  and an array of dependent values to interpolate,
**  interpolate corresponding independent values.
**
**  Limitations:
**  ===========
**
**  The dependent values must be ordered in ascending sequence.
**  duplicate dependent values will cause problems.
*/
void
Interpolate(nPts, xp, incrX, yp, incrY, iPts, xi, incrXi, yi, incrYi)
	int	nPts;	/* -> number of data points in xp and yp */
	float	*xp;	/* -> dependent variable values, ordered ascending */
	int	incrX;	/* -> second dimension of xp */
	float	*yp;	/* -> independent variable values */
	int	incrY;	/* -> second dimension of yp */
	int	iPts;	/* -> number of points to interpolate */
	float	*xi;	/* -> dependent values to interpolate */
	int	incrXi;	/* -> second dimension of xi */
	float	*yi;	/* <- interpolated independent variable values */
	int	incrYi;	/* -> second dimension of yi */
{
	int	indx, iCnt, i, j, n, iStart, iEnd, iPoint;
	float	*x, *y;
	double	xd, yd, xid;
	double	a[2], b[2], c[2], a1, a2, a3, b1, b2, b3, c1, c2,
		c3, x1, x2, y1, y2, aa, bb, a12, cc, a13, a23, xs;
    /*
    **  Check for enough points for interpolation
    */
	if (nPts < 2)
	    return;
    /*
    **  Interpolate each dependent value.
    */
	for (iCnt=0; iCnt<iPts; iCnt++, xi += incrXi, yi += incrYi) {
	/*
	**  Find closest dependent values in given data,
	**  using a binary search.
	*/
	    for (iStart = 0, iEnd = nPts-1;;) {
		if (iStart > iEnd) {
		    iPoint = iStart;
		    break;
		}
		iPoint = (iStart+iEnd) >> 1;
		if ((x1 = *xi - xp[iPoint*incrX]) < 0.0) {
		    iEnd = iPoint - 1;
		    continue;
		} else if (fabs(x1) <= EPS)
		    break;
		else {
		    iStart = iPoint + 1;
		    continue;
		}
	    }
	    if (iPoint == 0)
		j = 0;
	    else if (iPoint >= nPts)
		j = nPts-2;
	    else
		j = iPoint - 1;
	/*
	**  Decide number of points to interpolate and type of
	**  interpolation.
	*/
	    if (j == 0) {		/* top of data set */
		n    = 0;
		indx = 0;
	    } else if (j < nPts - 2) {	/* middle of data set */
		n    = j - 1;
		indx = 1;
	    } else {			/* bottom of data set */
		n    = nPts - 4;
		indx = 2;
	    }
	    if (nPts > 3) {		/* single or double parabolic fit */
		x = xp + n*incrX;
		y = yp + n*incrY;
	    /*
	    **  If requested dependent value exists, use it.
	    */
		x1 = x[indx*incrX];
		x2 = x[(indx+1)*incrX];
		y1 = y[indx*incrY];
		y2 = y[(indx+1)*incrY];
		if (*xi < x1 || *xi > x2)
		    goto Linear;
		if (fabs(x1- *xi)<EPS) {
		    *yi = y1;
		    continue;
		} else if (fabs(x2- *xi)<EPS) {
		    *yi = y2;
		    continue;
		}
		for (i = 0; i < 2; ++i, x+=incrX, y+=incrY) {
		    if (i==0 && indx==2)
			continue;
		    a12 = x[0]     - x[incrX];
		    a13 = x[0]     - x[incrX+incrX];
		    a23 = x[incrX] - x[incrX+incrX];

		    a1  = a12 * a13;
		    a2  = a23 * a12;
		    a3  = a13 * a23;

		    if (a1 != 0.0) a1 = (double)y[0] / a1;
		    if (a2 != 0.0) a2 = (double)y[incrY] / (-a2);
		    if (a3 != 0.0) a3 = (double)y[incrY+incrY] / a3;

		    b1 = ((double)x[incrX] + (double)x[incrX+incrX]) * a1;
		    b2 = ((double)x[0]     + (double)x[incrX+incrX]) * a2;
		    b3 = ((double)x[0]     + (double)x[incrX]      ) * a3;

		    c1 = (double)x[incrX] * (double)x[incrX+incrX] * a1;
		    c2 = (double)x[0]     * (double)x[incrX+incrX] * a2;
		    c3 = (double)x[0]     * (double)x[incrX]       * a3;

		    a[i] = a1 + a2 + a3;
		    b[i] = b1 + b2 + b3;
		    c[i] = c1 + c2 + c3;
		    if (i==0 && indx==0)
			break;
		}
		switch (indx) {
		case 0:		/* top of data set: single quadratic */
		    aa =  a[0];
		    bb = -b[0];
		    cc =  c[0];
		    break;
		case 1:		/* middle of data set: double quadratic */
		    aa =  (a[0] + a[1]) * 0.5;
		    bb = -(b[0] + b[1]) * 0.5;
		    cc =  (c[0] + c[1]) * 0.5;
		    break;
		case 2:		/* bottom of data set: single quadratic */
		    aa =  a[1];
		    bb = -b[1];
		    cc =  c[1];
		    break;
		}
		if (fabs(aa) >= EPS) {
		    xs = -bb / (aa+aa);
		/*
		**  If the inflection point is not between x1 and x2,
		**  perform the quadratic fit.
		*/
		    if (x1 >= xs || xs >= x2) {
			*yi = (aa * (double)*xi + bb) * (double)*xi + cc;
			continue;
		    }
		}
	    } else {			/* linear fit */
		x   = xp+j*incrX; x1  = x[0]; x2  = x[incrX];
		y   = yp+j*incrY; y1  = y[0]; y2  = y[incrY];
	    }
	/*
	**  Otherwise, simply perform linear interpolation.
	*/
Linear:
	    xd = x2 - x1;
	    if (xd != 0.0) {
		yd  = y2 - y1;
		xid = x2 - *xi;
		*yi = y2 - xid * yd / xd;
	    } else
		*yi = (y2+y1)*0.5;
	}
	return;
} /* Interpolate() */

#else  /* FORTRAN */

intrp_(npts, x, y, ipts, xi, yi)
	int	*npts;
	float	*x, *y;
	int	*ipts;
	float	*xi, *yi;
{
	Interpolate(*npts,x,1,y,1,*ipts,xi,1,yi,1);
}
#endif /* FORTRAN */