/** @file psSpline.c
*
*  @brief Contains basic function allocation, deallocation, and evaluation
*         routines.
*
*  This file will hold the functions for allocated, freeing, and evaluating
*  splines.
*
*  @version $Revision: 1.123 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-09-11 22:18:40 $
*
*
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*
*/
/*****************************************************************************/
/*  INCLUDE FILES                                                            */
/*****************************************************************************/
#include <stdio.h>
#include <stdbool.h>
#include <float.h>
#include <math.h>

#include "psRandom.h"
#include "psMemory.h"
#include "psVector.h"
#include "psScalar.h"
#include "psTrace.h"
#include "psError.h"
#include "psLogMsg.h"
#include "psPolynomial.h"
#include "psSpline.h"
#include "psConstants.h"
#include "psErrorText.h"

/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/

/*****************************************************************************/
/* TYPE DEFINITIONS                                                          */
/*****************************************************************************/
static void spline1DFree(psSpline1D *tmpSpline);
static psS32 vectorBinDisectF32(psF32 *bins,psS32 numBins,psF32 x);
static psS32 vectorBinDisectS32(psS32 *bins,psS32 numBins,psS32 x);

/*****************************************************************************/
/* GLOBAL VARIABLES                                                          */
/*****************************************************************************/

// None

/*****************************************************************************/
/* FILE STATIC VARIABLES                                                     */
/*****************************************************************************/

// None

/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - LOCAL                                           */
/*****************************************************************************/

bool psMemCheckSpline1D(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree );
}

static void spline1DFree(psSpline1D *tmpSpline)
{
    psS32 i;

    if (tmpSpline == NULL) {
        return;
    }

    if (tmpSpline->spline != NULL) {
        for (i=0;i<tmpSpline->n;i++) {
            psFree((tmpSpline->spline)[i]);
        }
        psFree(tmpSpline->spline);
    }

    if (tmpSpline->p_psDeriv2 != NULL) {
        psFree(tmpSpline->p_psDeriv2);
    }
    psFree(tmpSpline->knots);

    return;
}

/*****************************************************************************
fullInterpolate1DF32(): This routine will take as input n-element floating
point arrays domain and range, and the x value, assumed to lie with the
domain vector.  It produces as output the (n-1)-order LaGrange interpolated
value of x.
 
XXX: do we error check for non-distinct domain values?
 *****************************************************************************/
#define FUNC_MACRO_FULL_INTERPOLATE_1D(TYPE) \
static psF32 fullInterpolate1D##TYPE(ps##TYPE *domain, \
                                     ps##TYPE *range, \
                                     psS32 n, \
                                     ps##TYPE x) \
{ \
    \
    psS32 i; \
    psS32 m; \
    static psVector *p = NULL; \
    p = psVectorRecycle(p, n, PS_TYPE_##TYPE); \
    p_psMemSetPersistent(p, true); \
    p_psMemSetPersistent(p->data.TYPE, true); \
    \
    psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 4, \
            "---- fullInterpolate1D##TYPE() begin (%d-order at x=%f) (%d data points)----\n", n-1, x, n); \
    \
    for (i=0;i<n;i++) { \
        psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \
                "domain/range is (%f %f)\n", domain[i], range[i]); \
    } \
    \
    for (i=0;i<n;i++) { \
        p->data.TYPE[i] = range[i]; \
        psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \
                "p->data.TYPE[%d] is %f\n", i, p->data.TYPE[i]); \
        \
    } \
    \
    /* From NR, during each iteration of the m loop, we are computing the \
       p_{i ... i+m} terms. \
    */ \
    for (m=1;m<n;m++) { \
        for (i=0;i<n-m;i++) { \
            /* From NR: we are computing P_{i ... i+m} \
             */ \
            p->data.TYPE[i] = (((x-domain[i+m]) * p->data.TYPE[i]) + \
                               ((domain[i]-x) * p->data.TYPE[i+1])) / \
                              (domain[i] - domain[i+m]); \
            /*printf("((%f-%f * %f) + (%f-%f * %f)) / (%f - %f)\n", x, domain[i+m], p->data.TYPE[i], domain[i], x, p->data.TYPE[i+1], domain[i], domain[i+m]); \
             */ \
            psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \
                    "p->data.TYPE[%d] is %f\n", i, p->data.TYPE[i]); \
        } \
    } \
    psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 4, \
            "---- fullInterpolate1D##TYPE() end ----\n"); \
    \
    return(p->data.TYPE[0]); \
} \

/*
FUNC_MACRO_FULL_INTERPOLATE_1D(U8)
FUNC_MACRO_FULL_INTERPOLATE_1D(U16)
FUNC_MACRO_FULL_INTERPOLATE_1D(U32)
FUNC_MACRO_FULL_INTERPOLATE_1D(U64)
FUNC_MACRO_FULL_INTERPOLATE_1D(S8)
FUNC_MACRO_FULL_INTERPOLATE_1D(S16)
FUNC_MACRO_FULL_INTERPOLATE_1D(S32)
FUNC_MACRO_FULL_INTERPOLATE_1D(S64)
FUNC_MACRO_FULL_INTERPOLATE_1D(F64)
*/
FUNC_MACRO_FULL_INTERPOLATE_1D(F32)


/*****************************************************************************
interpolate1DF32(): this is the base 1-D flat memory routine to perform
LaGrange interpolation.
 *****************************************************************************/
static psF32 interpolate1DF32(psF32 *domain,
                              psF32 *range,
                              psS32 n,
                              psS32 order,
                              psF32 x)
{
    PS_ASSERT_PTR_NON_NULL(domain, NAN)
    PS_ASSERT_PTR_NON_NULL(range, NAN)
    // XXX: Check valid values for n, order, and x?

    psS32 binNum;
    psS32 numIntPoints = order+1;
    psS32 origin;

    psTrace(".psLib.dataManip.psSpline.interpolate1DF32", 4,
            "---- interpolate1DF32() begin ----\n");

    binNum = vectorBinDisectF32(domain, n, x);

    if (0 == numIntPoints%2) {
        origin = binNum - ((numIntPoints/2) - 1);
    } else {
        origin = binNum - (numIntPoints/2);
        if ((x-domain[binNum]) > (domain[binNum+1]-x)) {
            // x is closer to binNum+1.
            origin = 1 + (binNum - (numIntPoints/2));
        }
    }
    if (origin < 0) {
        origin = 0;
    }
    if ((origin + numIntPoints) > n) {
        origin = n - numIntPoints;
    }

    psTrace(".psLib.dataManip.psSpline.interpolate1DF32", 4,
            "---- interpolate1DF32() end ----\n");
    return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x));
}


/*****************************************************************************
CalculateSecondDerivs(): Given a set of x/y vectors corresponding to a
tabulated function at n points, this routine calculates the second
derivatives of the interpolating cubic splines at those n points.
 
The first and second derivatives at the endpoints, undefined in the SDR, are
here defined to be 0.0.  They can be modified via ypo and yp1.
 
This routine assumes that vectors x and y are of the appropriate types/sizes
(F32).
 
XXX: This algorithm is derived from the Numerical Recipes.
XXX: use recycled vectors for internal data.
XXX: do an F64 version?
 *****************************************************************************/
static psF32 *calculateSecondDerivs(const psVector* x,        ///< Ordinates (or NULL to just use the indices)
                                    const psVector* y)        ///< Coordinates
{
    psTrace(".psLib.dataManip.calculateSecondDerivs", 4,
            "---- calculateSecondDerivs() begin ----\n");

    psS32 i;
    psS32 k;
    psF32 sig;
    psF32 p;
    psS32 n = y->n;
    psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32));
    psF32 *derivs2 = (psF32 *) psAlloc(n * sizeof(psF32));
    psF32 *X = (psF32 *) & (x->data.F32[0]);
    psF32 *Y = (psF32 *) & (y->data.F32[0]);
    psF32 qn;

    // XXX: The second derivatives at the endpoints, undefined in the SDR,
    // are set in psConstants.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV.
    derivs2[0] = -0.5;
    u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_LEFT_SPLINE_DERIV);

    for (i=1;i<=(n-2);i++) {
        sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
        p = sig * derivs2[i-1] + 2.0;
        derivs2[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;

        psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
                "X[%d] is %f\n", i, X[i]);
        psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
                "Y[%d] is %f\n", i, Y[i]);
        psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
                "u[%d] is %f\n", i, u[i]);
    }

    qn = 0.5;
    u[n-1] = (3.0/(X[n-1]-X[n-2])) * (PS_RIGHT_SPLINE_DERIV - (Y[n-1]-Y[n-2])/(X[n-1]-X[n-2]));
    derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0);

    for (k=(n-2);k>=0;k--) {
        derivs2[k] = derivs2[k] * derivs2[k+1] + u[k];

        psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
                "derivs2[%d] is %f\n", k, derivs2[k]);
    }

    psFree(u);
    psTrace(".psLib.dataManip.calculateSecondDerivs", 4,
            "---- calculateSecondDerivs() end ----\n");
    return(derivs2);
}

/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - PUBLIC                                          */
/*****************************************************************************/

/*****************************************************************************
psVectorFitSpline1D(): given a psSpline1D data structure and a set of x/y
vectors, this routine generates the linear or cublic splines which satisfy
those data points.
 
The formula for calculating the spline polynomials is derived from Numerical
Recipes in C.  The basic idea is that the polynomial is
 (1)     y = (A * y[0]) +
 (2)         (B * y[1]) +
 (3)         ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 +
 (4)         ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0
Where:
 H = x[1]-x[0]
 A = (x[1]-x)/H
 B = (x-x[0])/H
The bulk of the code in this routine is the expansion of the above equation
into a polynomial in terms of x, and then saving the coefficients of the
powers of x in the spline polynomials.  This gets pretty complicated.
 
XXX: usage of yErr is not specified in IfA documentation.
 
XXX: Is the x argument redundant?  What do we do if the x argument is
supplied, but does not equal the knots specified in mySpline?
 
XXX: can psSpline be NULL?
 
XXX: reimplement this assuming that mySpline is NULL?
 
XXX: What happens if X is NULL, then an index vector is generated for X, but
that index vector lies outside the range vectors in mySpline?
 
XXX: Assumes mySpline->knots is psF32.  Must add psU32 and psF64.
 *****************************************************************************/
psSpline1D *psVectorFitSpline1D(psSpline1D *mySpline,     ///< The spline which will be generated.
                                const psVector* x,        ///< Ordinates (or NULL to just use the indices)
                                const psVector* y,        ///< Coordinates
                                const psVector* yErr)     ///< Errors in coordinates, or NULL
{
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    if (mySpline != NULL) {
        PS_ASSERT_VECTOR_TYPE(mySpline->knots, PS_TYPE_F32, NULL);
    }
    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
            "---- psVectorFitSpline1D() begin ----\n");
    psS32 numSplines = (y->n)-1;
    psF32 tmp;
    psF32 H;
    psS32 i;
    psF32 slope;
    psVector *x32 = NULL;
    psVector *y32 = NULL;
    psVector *yErr32 = NULL;
    static psVector *x32Static = NULL;
    static psVector *y32Static = NULL;
    static psVector *yErr32Static = NULL;

    PS_VECTOR_CONVERT_F64_TO_F32_STATIC(y, y32, y32Static);

    // If yErr==NULL, set all errors equal.
    if (yErr == NULL) {
        PS_VECTOR_GEN_YERR_STATIC_F32(yErr32Static, y->n);
        yErr32 = yErr32Static;
    } else {
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(yErr, NULL);
        PS_VECTOR_CONVERT_F64_TO_F32_STATIC(yErr, yErr32, yErr32Static);
    }

    // If x==NULL, create an x32 vector with x values set to (0:n).
    if (x == NULL) {
        PS_VECTOR_GEN_X_INDEX_STATIC_F32(x32Static, y->n);
        x32 = x32Static;
    } else {
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
        PS_VECTOR_CONVERT_F64_TO_F32_STATIC(x, x32, x32Static);
    }
    PS_ASSERT_VECTORS_SIZE_EQUAL(x32, y32, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(yErr32, y32, NULL);

    /*
        XXX:
        This can not be implemented until SDR states what order spline should be
        created.
        Should we error if mySpline is not NULL?
        Should we error if mySPline is not NULL?
    */
    if (mySpline == NULL) {
        mySpline = psSpline1DAllocGeneric(x32, 3);
    }
    PS_ASSERT_PTR_NON_NULL(mySpline, NULL);
    PS_ASSERT_INT_NONNEGATIVE(mySpline->n, NULL);

    if (y32->n != (1 + mySpline->n)) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "data size / spline size mismatch (%d %d)\n",
                y32->n, mySpline->n);
        return(NULL);
    }

    // If these are linear splines, which means their polynomials will have
    // two coefficients, then we do the simple calculation.
    if (2 == (mySpline->spline[0])->n) {
        for (i=0;i<mySpline->n;i++) {
            slope = (y32->data.F32[i+1] - y32->data.F32[i]) /
                    (mySpline->knots->data.F32[i+1] - mySpline->knots->data.F32[i]);
            (mySpline->spline[i])->coeff[0] = y32->data.F32[i] -
                                              (slope * mySpline->knots->data.F32[i]);

            (mySpline->spline[i])->coeff[1] = slope;
            psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
                    "---- mySpline %d coeffs are (%f, %f)\n", i,
                    (mySpline->spline[i])->coeff[0],
                    (mySpline->spline[i])->coeff[1]);
        }
        psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
                "---- Exiting psVectorFitSpline1D()()\n");
        return((psSpline1D *) mySpline);
    }

    // Check if these are cubic splines (n==4).  If not, psError.
    if (4 != (mySpline->spline[0])->n) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Don't know how to generate %d-order splines.",
                (mySpline->spline[0])->n-1);
        return(NULL);
    }

    // If we get here, then we know these are cubic splines.  We first
    // generate the second derivatives at each data point.
    mySpline->p_psDeriv2 = calculateSecondDerivs(x32, y32);
    for (i=0;i<y32->n;i++)
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "Second deriv[%d] is %f\n", i, mySpline->p_psDeriv2[i]);

    // We generate the coefficients of the spline polynomials.  I can't
    // concisely explain how this code works.  See above function comments
    // and Numerical Recipes in C.
    for (i=0;i<numSplines;i++) {
        H = x32->data.F32[i+1] - x32->data.F32[i];
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
                "x data (%f - %f) (%f)\n",
                x32->data.F32[i],
                x32->data.F32[i+1], H);
        //
        // ******** Calculate 0-order term ********
        //
        // From (1)
        (mySpline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H);
        // From (2)
        ((mySpline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H);
        // From (3)
        tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
        tmp-= (x32->data.F32[i+1] / H);
        tmp*= (mySpline->p_psDeriv2)[i] * H * H / 6.0;
        ((mySpline->spline[i])->coeff[0])+= tmp;
        // From (4)
        tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
        tmp+= (x32->data.F32[i] / H);
        tmp*= (mySpline->p_psDeriv2)[i+1] * H * H / 6.0;
        ((mySpline->spline[i])->coeff[0])+= tmp;

        //
        // ******** Calculate 1-order term ********
        //
        // From (1)
        (mySpline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H;
        // From (2)
        ((mySpline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H);
        // From (3)
        tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
        tmp+= (1.0 / H);
        tmp*= ((mySpline->p_psDeriv2)[i]) * H * H / 6.0;
        ((mySpline->spline[i])->coeff[1])+= tmp;
        // From (4)
        tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
        tmp-= (1.0 / H);
        tmp*= ((mySpline->p_psDeriv2)[i+1]) * H * H / 6.0;
        ((mySpline->spline[i])->coeff[1])+= tmp;

        //
        // ******** Calculate 2-order term ********
        //
        // From (3)
        (mySpline->spline[i])->coeff[2] = ((mySpline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H);
        // From (4)
        ((mySpline->spline[i])->coeff[2])-= (((mySpline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H));

        //
        // ******** Calculate 3-order term ********
        //
        // From (3)
        (mySpline->spline[i])->coeff[3] = -((mySpline->p_psDeriv2)[i]) / (6.0 * H);
        // From (4)
        ((mySpline->spline[i])->coeff[3])+=  ((mySpline->p_psDeriv2)[i+1]) / (6.0 * H);

        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[3] is %f\n", i, (mySpline->spline[i])->coeff[3]);

    }

    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
            "---- psVectorFitSpline1D() end ----\n");
    return(mySpline);
}









/*****************************************************************************
psVectorFitSpline1DNEW(): given a psSpline1D data structure and a set of x/y
 
xF32 and yF32 are internal psVectors which are used to hold the psF32 versions
of the input data, if necessary.  xPtr and yPtr are pointers to either xF32 or
the x argument.  All computation is done on xPtr and yPtr.  xF32 and yF32 will
simply be psFree() at the end.
 
XXX: nKnots makes no sense.  This number is always equal to the size of the x
an y vectors.
 
XXX: How do we specify the spline order?  For now, order=3.
 *****************************************************************************/
#define PS_XXX_SPLINE_ORDER 3
psSpline1D *psVectorFitSpline1DNEW(const psVector* x,        ///< Ordinates (or NULL to just use the indices)
                                   const psVector* y,        ///< Coordinates
                                   int nKnots)
{
    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, "---- psVectorFitSpline1DNEW() begin ----\n");
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    //    PS_ASSERT_INT_EQUAL(y->n, nKnots);

    //
    // The following code ensures that xPtr points to a psF32 version of the
    // ordinate data.
    //
    psVector *xF32 = NULL;
    psVector *xPtr = NULL;
    if (x != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
        if (PS_TYPE_F64 == x->type.type) {
            xF32 = psVectorAlloc(y->n, PS_TYPE_F32);
            for (psS32 i = 0 ; i < x->n ; i++) {
                xF32->data.F32[i] = (psF32) x->data.F64[i];
            }
            xPtr = xF32;
        } else if (PS_TYPE_F32 == x->type.type) {
            xPtr = (psVector *) x;
        } else {
            psError(PS_ERR_UNKNOWN, true, "psVector x is wrong type.\n");
            return(NULL);
        }
    } else {
        // If x==NULL, create an x32 vector with x values set to (0:n).
        xF32 = psVectorAlloc(y->n, PS_TYPE_F32);
        for (psS32 i = 0 ; i < x->n ; i++) {
            xF32->data.F32[i] = (psF32) i;
        }
        xPtr = xF32;
    }

    //
    // If y is of type psF64, then create a new vector yF32 and convert the
    // y elements.  Regardless of y's type, we create a yPtr which will be
    // used in the remainder of this function.
    //
    psVector *yF32 = NULL;
    psVector *yPtr = NULL;
    if (PS_TYPE_F64 == y->type.type) {
        yF32 = psVectorAlloc(y->n, PS_TYPE_F32);
        for (psS32 i = 0 ; i < y->n ; i++) {
            yF32->data.F32[i] = (psF32) y->data.F64[i];
        }
        yPtr = yF32;
    } else {
        yPtr = (psVector *) y;
    }

    psSpline1D *mySpline = psSpline1DAllocGeneric(xPtr, PS_XXX_SPLINE_ORDER);

    psS32 numSplines = nKnots - 1;
    psF32 tmp;
    psF32 H;
    psS32 i;
    psF32 slope;
    // XXX: get rid of x32 and y32 (this is from old code)
    psVector *x32 = xPtr;
    psVector *y32 = yPtr;

    // If these are linear splines, which means their polynomials will have
    // two coefficients, then we do the simple calculation.
    if (1 == PS_XXX_SPLINE_ORDER) {
        for (i=0;i<mySpline->n;i++) {
            slope = (y32->data.F32[i+1] - y32->data.F32[i]) /
                    (mySpline->knots->data.F32[i+1] - mySpline->knots->data.F32[i]);
            (mySpline->spline[i])->coeff[0] = y32->data.F32[i] -
                                              (slope * mySpline->knots->data.F32[i]);

            (mySpline->spline[i])->coeff[1] = slope;
            psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
                    "---- mySpline %d coeffs are (%f, %f)\n", i,
                    (mySpline->spline[i])->coeff[0],
                    (mySpline->spline[i])->coeff[1]);
        }
        psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
                "---- Exiting psVectorFitSpline1D()()\n");
        return((psSpline1D *) mySpline);
    }

    //
    // Check if these are cubic splines (n==4).  If not, psError.
    //
    if (3 != PS_XXX_SPLINE_ORDER) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Don't know how to generate %d-order splines.", PS_XXX_SPLINE_ORDER);
        return(NULL);
    }

    //
    // If we get here, then we know these are cubic splines.  We first
    // generate the second derivatives at each data point.
    //
    mySpline->p_psDeriv2 = calculateSecondDerivs(x32, y32);
    for (i=0;i<y32->n;i++)
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "Second deriv[%d] is %f\n", i, mySpline->p_psDeriv2[i]);

    //
    // We generate the coefficients of the spline polynomials.  I can't
    // concisely explain how this code works.  See above function comments
    // and Numerical Recipes in C.
    //
    for (i=0;i<numSplines;i++) {
        H = x32->data.F32[i+1] - x32->data.F32[i];
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
                "x data (%f - %f) (%f)\n",
                x32->data.F32[i],
                x32->data.F32[i+1], H);
        //
        // ******** Calculate 0-order term ********
        //
        // From (1)
        (mySpline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H);
        // From (2)
        ((mySpline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H);
        // From (3)
        tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
        tmp-= (x32->data.F32[i+1] / H);
        tmp*= (mySpline->p_psDeriv2)[i] * H * H / 6.0;
        ((mySpline->spline[i])->coeff[0])+= tmp;
        // From (4)
        tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
        tmp+= (x32->data.F32[i] / H);
        tmp*= (mySpline->p_psDeriv2)[i+1] * H * H / 6.0;
        ((mySpline->spline[i])->coeff[0])+= tmp;

        //
        // ******** Calculate 1-order term ********
        //
        // From (1)
        (mySpline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H;
        // From (2)
        ((mySpline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H);
        // From (3)
        tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
        tmp+= (1.0 / H);
        tmp*= ((mySpline->p_psDeriv2)[i]) * H * H / 6.0;
        ((mySpline->spline[i])->coeff[1])+= tmp;
        // From (4)
        tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
        tmp-= (1.0 / H);
        tmp*= ((mySpline->p_psDeriv2)[i+1]) * H * H / 6.0;
        ((mySpline->spline[i])->coeff[1])+= tmp;

        //
        // ******** Calculate 2-order term ********
        //
        // From (3)
        (mySpline->spline[i])->coeff[2] = ((mySpline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H);
        // From (4)
        ((mySpline->spline[i])->coeff[2])-= (((mySpline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H));

        //
        // ******** Calculate 3-order term ********
        //
        // From (3)
        (mySpline->spline[i])->coeff[3] = -((mySpline->p_psDeriv2)[i]) / (6.0 * H);
        // From (4)
        ((mySpline->spline[i])->coeff[3])+=  ((mySpline->p_psDeriv2)[i+1]) / (6.0 * H);

        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[3] is %f\n", i, (mySpline->spline[i])->coeff[3]);

    }

    psFree(xF32);
    psFree(yF32);

    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
            "---- psVectorFitSpline1D() end ----\n");

    return(mySpline);
}





/*****************************************************************************/
/*  FUNCTION IMPLEMENTATION - PUBLIC                                         */
/*****************************************************************************/

//typedef struct {
//    psS32 n;
//    psPolynomial1D **spline;
//    psF32 *p_psDeriv2;
//    psVector *knots;
//} psSpline1D;

/*****************************************************************************
    NOTE: "n" specifies the number of spline polynomials.  Therefore, there
    must exist n+1 points in "knots".
 
XXX: Is this really needed anymore?
 
XXX: Ensure that knots[i+1] != knots[i]
 
XXX: What should be the default type for knots be?  psF32 is assumed.
 *****************************************************************************/
psSpline1D *psSpline1DAlloc(int numSplines,
                            int order,
                            float min,
                            float max)
{
    PS_ASSERT_INT_NONNEGATIVE(numSplines, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order, NULL);
    PS_ASSERT_FLOAT_NON_EQUAL(max, min, NULL);

    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
    tmpSpline->n = numSplines;

    //
    // XXX: We might have to allocate single or double polynomials depending on the type
    // of the psVector bounds.  For now, all knots and spline polynomials are 32-bit.
    //
    tmpSpline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    for (psS32 i=0;i<numSplines;i++) {
        (tmpSpline->spline)[i] = psPolynomial1DAlloc(order+1, PS_POLYNOMIAL_ORD);
    }

    // This will be computed by psVectorFitSpline1D()
    tmpSpline->p_psDeriv2 = NULL;

    //
    // XXX:Ensure that the knots are distinct, and monotonic.
    //
    tmpSpline->knots = psVectorAlloc(numSplines+1, PS_TYPE_F32);
    psF32 width = (max - min) / ((psF32) numSplines);
    tmpSpline->knots->data.F32[0] = min;
    for (psS32 i=1;i<numSplines;i++) {
        tmpSpline->knots->data.F32[i] = min + (width * (psF32) i);
    }
    tmpSpline->knots->data.F32[numSplines] = max;

    psMemSetDeallocator(tmpSpline, (psFreeFunc)spline1DFree);
    return(tmpSpline);
}

/*****************************************************************************
XXX: Is there a psLib function for this?
 *****************************************************************************/
psVector *PsVectorDup2(psVector *in)
{
    psVector *out = psVectorAlloc(in->n, in->type.type);

    if (in->type.type == PS_TYPE_F32) {
        for (psS32 i = 0 ; i < in->n ; i++) {
            out->data.F32[i] = in->data.F32[i];
        }
    } else if (in->type.type == PS_TYPE_F64) {
        for (psS32 i = 0 ; i < in->n ; i++) {
            out->data.F64[i] = in->data.F64[i];
        }
    } else {
        psError(PS_ERR_UNKNOWN, true, "psVector in has an incorrect type.\n");
        return(NULL);
    }
    return(out);
}

/*****************************************************************************
XXX: What should be the default type for knots, spline polys?  psF32 is assumed.
 *****************************************************************************/
psSpline1D *psSpline1DAllocGeneric(const psVector *bounds,
                                   int order)
{
    PS_ASSERT_VECTOR_NON_NULL(bounds, NULL);
    PS_ASSERT_VECTOR_NON_EMPTY(bounds, NULL);
    PS_ASSERT_VECTOR_TYPE(bounds, PS_TYPE_F32, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order, NULL);

    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
    psS32 numSplines = bounds->n - 1;
    tmpSpline->n = numSplines;

    //
    // XXX: We might have to allocate single or double polynomials depending on the type
    // of the psVector bounds.  For now, all knots and spline polynomials are 32-bit.
    //
    tmpSpline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    for (psS32 i=0;i<numSplines;i++) {
        (tmpSpline->spline)[i] = psPolynomial1DAlloc(order+1, PS_POLYNOMIAL_ORD);
    }

    // This will be computed by psVectorFitSpline1D()
    tmpSpline->p_psDeriv2 = NULL;

    //
    // Ensure that all knots are distinct.
    // XXX:Ensure that the knots are monotonic.
    //
    for (psS32 i=0;i<bounds->n-1;i++) {
        if (FLT_EPSILON >= fabs(bounds->data.F32[i+1]-bounds->data.F32[i])) {
            psError(PS_ERR_UNKNOWN, true, "data points must be distinct ([%d] %f %f)\n", i, bounds->data.F32[i], bounds->data.F32[i+1]);
            return(NULL);
        }
    }
    tmpSpline->knots = PsVectorDup2((psVector *) bounds);

    psMemSetDeallocator(tmpSpline, (psFreeFunc)spline1DFree);
    return(tmpSpline);
}

/*****************************************************************************
vectorBinDisectF32(): This is a macro for a private function which takes as
input a vector an array of data as well as a single value for that data.  The
input vector values are assumed to be non-decreasing (v[i-1] <= v[i] for all
i).  This routine does a binary disection of the vector and returns "i" such
that (v[i] <= x <= v[i+1).  If x lies outside the range of v[], then this
routine prints a warning message and returns (-2 or -1).
 *****************************************************************************/
#define FUNC_MACRO_VECTOR_BIN_DISECT(TYPE) \
static psS32 vectorBinDisect##TYPE(ps##TYPE *bins, \
                                   psS32 numBins, \
                                   ps##TYPE x) \
{ \
    psS32 min; \
    psS32 max; \
    psS32 mid; \
    \
    psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
            "---- Calling vectorBinDisect##TYPE(%f)\n", x); \
    \
    if (x < bins[0]) { \
        psLogMsg(__func__, PS_LOG_WARN, \
                 "vectorBinDisect%s(): ordinate %f is outside vector range (%f - %f).", \
                 #TYPE, x, bins[0], bins[numBins-1]); \
        return(-2); \
    } \
    \
    if (x > bins[numBins-1]) { \
        psLogMsg(__func__, PS_LOG_WARN, \
                 "vectorBinDisect%s(): ordinate %f is outside vector range (%f - %f).", \
                 #TYPE, x, bins[0], bins[numBins-1]); \
        return(-1); \
    } \
    \
    min = 0; \
    max = numBins-2; \
    mid = ((max+1)-min)/2; \
    \
    while (min != max) { \
        psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
                "(min, mid, max) is (%d, %d, %d): (x, bins) is (%f, %f)\n", \
                min, mid, max, x, bins[mid]); \
        \
        if (x == bins[mid]) { \
            psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
                    "---- Exiting vectorBinDisect##TYPE(): bin %d\n", mid); \
            return(mid); \
        } else if (x < bins[mid]) { \
            max = mid-1; \
        } else { \
            min = mid; \
        } \
        mid = ((max+1)+min)/2; \
    } \
    \
    psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
            "---- Exiting vectorBinDisect##TYPE(): bin %d\n", min); \
    return(min); \
} \

FUNC_MACRO_VECTOR_BIN_DISECT(S8)
FUNC_MACRO_VECTOR_BIN_DISECT(S16)
FUNC_MACRO_VECTOR_BIN_DISECT(S32)
FUNC_MACRO_VECTOR_BIN_DISECT(S64)
FUNC_MACRO_VECTOR_BIN_DISECT(U8)
FUNC_MACRO_VECTOR_BIN_DISECT(U16)
FUNC_MACRO_VECTOR_BIN_DISECT(U32)
FUNC_MACRO_VECTOR_BIN_DISECT(U64)
FUNC_MACRO_VECTOR_BIN_DISECT(F32)
FUNC_MACRO_VECTOR_BIN_DISECT(F64)

/*****************************************************************************
p_psVectorBinDisect(): A wrapper to the above p_psVectorBinDisect().
 
XXX: Assert that the psVector and psScalar have the same type.
 *****************************************************************************/
psS32 p_psVectorBinDisect(psVector *bins,
                          psScalar *x)
{
    PS_ASSERT_VECTOR_NON_NULL(bins, -4);
    PS_ASSERT_VECTOR_NON_EMPTY(bins, -4);
    PS_ASSERT_PTR_NON_NULL(x, -6);
    PS_ASSERT_PTR_TYPE_EQUAL(x, bins, -3);
    char* strType;

    switch (x->type.type) {
    case PS_TYPE_U8:
        return(vectorBinDisectU8(bins->data.U8, bins->n, x->data.U8));
    case PS_TYPE_U16:
        return(vectorBinDisectU16(bins->data.U16, bins->n, x->data.U16));
    case PS_TYPE_U32:
        return(vectorBinDisectU32(bins->data.U32, bins->n, x->data.U32));
    case PS_TYPE_U64:
        return(vectorBinDisectU64(bins->data.U64, bins->n, x->data.U64));
    case PS_TYPE_S8:
        return(vectorBinDisectS8(bins->data.S8, bins->n, x->data.S8));
    case PS_TYPE_S16:
        return(vectorBinDisectS16(bins->data.S16, bins->n, x->data.S16));
    case PS_TYPE_S32:
        return(vectorBinDisectS32(bins->data.S32, bins->n, x->data.S32));
    case PS_TYPE_S64:
        return(vectorBinDisectS64(bins->data.S64, bins->n, x->data.S64));
    case PS_TYPE_F32:
        return(vectorBinDisectF32(bins->data.F32, bins->n, x->data.F32));
    case PS_TYPE_F64:
        return(vectorBinDisectF64(bins->data.F64, bins->n, x->data.F64));
    case PS_TYPE_C32:
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED,
                strType);
        return 0;
    case PS_TYPE_C64:
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED,
                strType);
        return 0;
    case PS_TYPE_BOOL:
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED,
                strType);
        return 0;
    }
    return(-3);
}

/*****************************************************************************
p_psVectorInterpolate(): This routine will take as input psVectors domain and
range, and the x value, assumed to lie with the domain vector.  It produces
as output the LaGrange interpolated value of a polynomial of the specified
order around the point x.
 
XXX: This stuff does not currently work with a mask.
 
XXX: add another psScalar argument for the result.
 
XXX: The VectorCopy routines seg fault when I declare range32 as static.
 *****************************************************************************/
psScalar *p_psVectorInterpolate(psVector *domain,
                                psVector *range,
                                int order,
                                psScalar *x)
{
    PS_ASSERT_VECTOR_NON_NULL(domain, NULL);
    PS_ASSERT_VECTOR_NON_NULL(range, NULL);
    PS_ASSERT_PTR_NON_NULL(x, NULL);
    PS_ASSERT_INT_NONNEGATIVE(order, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(domain, range, NULL);
    PS_ASSERT_PTR_TYPE_EQUAL(domain, range, NULL);
    PS_ASSERT_PTR_TYPE_EQUAL(domain, x, NULL);

    psVector *range32 = NULL;
    psVector *domain32 = NULL;
    psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
            "---- p_psVectorInterpolate() begin ----\n");

    if (order > (domain->n - 1)) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                PS_ERRORTEXT_psSpline_NOT_ENOUGH_DATAPOINTS,
                order);
        return(NULL);
    }

    if (x->type.type == PS_TYPE_F32) {
        psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
                "---- p_psVectorInterpolate() end ----\n");
        return(psScalarAlloc(interpolate1DF32(domain->data.F32,
                                              range->data.F32,
                                              domain->n,
                                              order,
                                              x->data.F32), PS_TYPE_F32));
    } else if (x->type.type == PS_TYPE_F64) {
        // XXX: use recycled vectors here.
        range32 = psVectorCopy(range32, range, PS_TYPE_F32);
        domain32 = psVectorCopy(domain32, domain, PS_TYPE_F32);

        psScalar *tmpScalar = psScalarAlloc((psF64)
                                            interpolate1DF32(domain32->data.F32,
                                                             range32->data.F32,
                                                             domain32->n,
                                                             order,
                                                             (psF32) x->data.F64), PS_TYPE_F64);
        psFree(range32);
        psFree(domain32);

        psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
                "---- p_psVectorInterpolate() end ----\n");
        // XXX: Convert data type to F64?
        return(tmpScalar);

    } else {
        char* strType;
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED,
                strType);
    }

    psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
            "return(NULL)\n");
    psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
            "---- p_psVectorInterpolate() end ----\n");

    return(NULL);
}


/*****************************************************************************
psSpline1DEval(): this routine takes an existing spline of arbitrary order
and an independent x value.  Each determines which spline that x corresponds
to by doing a bracket disection on the knots of the spline data structure
(vectorBinDisectF32()).  Then it evaluates the spline at that x location
by a call to the 1D polynomial functions.
 
XXX: The spline eval functions require input and output to be F32.  however
     the spline fit functions require F32 and F64.
 
XXX: This only works if spline0>knots if psF32.  Must add support for psU32 and
psF64.
 *****************************************************************************/
float psSpline1DEval(
    const psSpline1D *spline,
    float x
)
{
    PS_ASSERT_PTR_NON_NULL(spline, NAN);
    PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN);
    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN);

    unsigned int binNum;
    unsigned int n;

    n = spline->n;
    //XXX    binNum = vectorBinDisectF32(spline->domains, (spline->n)+1, x);
    binNum = vectorBinDisectF32(spline->knots->data.F32, (spline->n)+1, x);
    if (binNum < 0) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f).",
                 x, spline->knots->data.F32[0],
                 spline->knots->data.F32[n-1]);

        if (x < spline->knots->data.F32[0]) {
            return(psPolynomial1DEval(spline->spline[0],
                                      x));
        } else if (x > spline->knots->data.F32[n-1]) {
            return(psPolynomial1DEval(spline->spline[n-1],
                                      x));
        }
    }

    return(psPolynomial1DEval(spline->spline[binNum],
                              x));
}

// XXX: The spline eval functions require input and output to be F32.
// however the spline fit functions require F32 and F64.
psVector *psSpline1DEvalVector(
    const psSpline1D *spline,
    const psVector *x
)
{
    PS_ASSERT_PTR_NON_NULL(spline, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);

    unsigned int i;
    psVector *tmpVector;

    tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
    if (x->type.type == PS_TYPE_F32) {
        for (i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(
                                         spline,
                                         x->data.F32[i]
                                     );
        }
    } else if (x->type.type == PS_TYPE_F64) {
        for (i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(
                                         spline,
                                         (psF32) x->data.F64[i]
                                     );
        }
    } else {
        char* strType;
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED,
                strType);
        return(NULL);
    }

    return(tmpVector);
}
