/** @file psSpline.c
*
*  @brief Contains basic spline allocation, deallocation, fitting, 
*         and evaluation routines.
*
*  This file will hold the functions for allocated, freeing, and evaluating
*  splines.
*
*  @version $Revision: 1.132 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-11-15 20:10:32 $
*
*  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 "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                                                          */
/*****************************************************************************/

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

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

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

static void spline1DFree(psSpline1D *tmpSpline)
{
    if (tmpSpline == NULL) {
        return;
    }

    if (tmpSpline->spline != NULL) {
        for (psS32 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;
}

/*****************************************************************************
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 were previously undefined in
the SDR.  From bugzilla #???, they are required to be 0.0, impementing natural
splines.
 
Endpoints are defined by
    PS_LEFT_SPLINE_DERIV
    PS_RIGHT_SPLINE_DERIV
 
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?
 *****************************************************************************/
#define PS_LEFT_SPLINE_DERIV 0.0
#define PS_RIGHT_SPLINE_DERIV 0.0
static psF32 *calculateSecondDerivs(
    const psVector* x,                  ///< Ordinates
    const psVector* y)                  ///< Coordinates
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    if (psTraceGetLevel(__func__) >= 6) {
        p_psVectorPrint(1, (psVector *) x, "x");
        p_psVectorPrint(1, (psVector *) y, "y");
    }
    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]);
    //
    // 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 (psS32 i=1;i<=(n-2);i++) {
        psF32 sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
        psF32 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(__func__, 6, "X[%d] is %f\n", i, X[i]);
        psTrace(__func__, 6, "Y[%d] is %f\n", i, Y[i]);
        psTrace(__func__, 6, "u[%d] is %f\n", i, u[i]);
    }

    psF32 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 (psS32 k=(n-2);k>=0;k--) {
        derivs2[k] = derivs2[k] * derivs2[k+1] + u[k];
        psTrace(__func__, 6, "derivs2[%d] is %f\n", k, derivs2[k]);
    }
    psFree(u);
    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return(derivs2);
}


/*****************************************************************************
vectorBinDisectTYPE(): 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(__func__, 4, "---- %s() begin ----\n", __func__); \
    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(__func__, 6, "(min, mid, max) is (%u, %u, %u): (x, bins) is (%f, %f)\n", min, mid, max, x, bins[mid]); \
        \
        if (x == bins[mid]) { \
            psTrace(__func__, 4, "---- %s(%d) end ----\n", __func__, mid); \
            return(mid); \
        } else if (x < bins[mid]) { \
            max = mid-1; \
        } else { \
            min = mid; \
        } \
        mid = ((max+1)+min)/2; \
    } \
    psTrace(__func__, 4, "---- %s(%d) end ----\n", __func__, 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().
  *****************************************************************************/
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);
}

/*****************************************************************************
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?
 
XXX: This is a somewhat general function.  There's no reason to put it in this
file.
 
XXX: Sort all these interpolation functions out.  Why are there so many?
 
XXX: The fullInterpolate1D macros are not used anywhere.
 *****************************************************************************/
#define FUNC_MACRO_FULL_INTERPOLATE_1D(TYPE) \
static psF32 fullInterpolate1D##TYPE(ps##TYPE *domain, \
                                     ps##TYPE *range, \
                                     unsigned int n, \
                                     ps##TYPE x) \
{ \
    \
    unsigned int i; \
    unsigned int m; \
    static psVector *p = NULL; \
    p = psVectorRecycle(p, n, PS_TYPE_##TYPE); \
    p_psMemSetPersistent(p, true); \
    p_psMemSetPersistent(p->data.TYPE, true); \
    \
    psTrace(__func__, 4, "---- %s() begin %u-order at x=%f) (%d data points) ----\n", __func__, n-1, x, n); \
    for (i=0;i<n;i++) { \
        psTrace(__func__, 6, "domain/range is (%f %f)\n", domain[i], range[i]); \
    } \
    \
    for (i=0;i<n;i++) { \
        p->data.TYPE[i] = range[i]; \
        psTrace(__func__, 6, "p->data.TYPE[%u] 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]); \
            psTrace(__func__, 6, "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \
        } \
    } \
    psTrace(__func__, 4, "---- %s(....) end ----\n", __func__); \
    return(p->data.TYPE[0]); \
} \

/* XXX: Do this correctly.
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,
                              unsigned int n,
                              unsigned int order,
                              psF32 x)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    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;

    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(__func__, 4, "---- %s(....) end ----\n", __func__);
    return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x));
}


/*****************************************************************************
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 (so you don't have to
     allocate it each time).
 
XXX: The VectorCopy routines seg fault when I declare range32 as static.
 *****************************************************************************/
psScalar *p_psVectorInterpolate(
    psVector *domain,
    psVector *range,
    psS32 order,
    psScalar *x)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    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;
    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(__func__, 4, "---- %s(NULL) end ----\n", __func__);
        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(__func__, 4, "---- %s() end ----\n", __func__);
        // 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(__func__, 4, "---- %s(NULL) end ----\n", __func__);
    return(NULL);
}


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

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

psSpline1D *psSpline1DAlloc()
{
    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
    tmpSpline->n = 0;
    tmpSpline->spline = NULL;
    tmpSpline->knots = NULL;
    tmpSpline->p_psDeriv2 = NULL;
    psMemSetDeallocator(tmpSpline, (psFreeFunc) spline1DFree);

    return(tmpSpline);
}

/*****************************************************************************
psVectorFitSpline1D(): given 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: What types must be supported?
 *****************************************************************************/
psSpline1D *psVectorFitSpline1D(
    const psVector* x,                  ///< Ordinates.
    const psVector* y)                  ///< Coordinates.
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(y->n, 2, NULL);
    psS32 numSplines = (y->n)-1;
    psTrace(__func__, 5, "numSplines is %d\n", numSplines);

    //
    // Create the psSpline1D struct.
    //
    psSpline1D *spline = psSpline1DAlloc();
    spline->n = numSplines;
    spline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    for (psS32 i=0;i<numSplines;i++) {
        spline->spline[i] = psPolynomial1DAlloc(3, PS_POLYNOMIAL_ORD);
    }

    //
    // The following code ensures that xPtr and yPtr points to a psF32 psVector.
    //
    // XXX: When you debug, use the vector copy and create routines here:
    //

    spline->knots = psVectorAlloc(y->n, PS_TYPE_F32);
    if (x != NULL) {
        PS_ASSERT_VECTOR_NON_NULL(x, NULL);
        PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
        if (x->type.type == PS_TYPE_F32) {
            for (psS32 i = 0 ; i < x->n ; i++) {
                spline->knots->data.F32[i] = x->data.F32[i];
            }
        } else if (x->type.type == PS_TYPE_F64) {
            for (psS32 i = 0 ; i < x->n ; i++) {
                spline->knots->data.F32[i] = (psF32) x->data.F64[i];
            }
        }
    } else {
        for (psS32 i = 0 ; i < y->n ; i++) {
            spline->knots->data.F32[i] = (psF32) i;
        }
    }
    psVector *xPtr = spline->knots;

    psVector *yPtr = NULL;
    // Convert y to F32 if necessary.
    if (PS_TYPE_F64 == y->type.type) {
        yPtr = psVectorCopy(NULL, y, PS_TYPE_F32);
    } else {
        yPtr = (psVector *) y;
    }

    //
    // Generate the second derivatives at each data point.
    //
    spline->p_psDeriv2 = calculateSecondDerivs(xPtr, yPtr);

    //
    // 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 (psS32 i=0 ; i < numSplines ; i++) {
        psF32 H = xPtr->data.F32[i+1] - xPtr->data.F32[i];
        if (fabs(H) <= FLT_EPSILON) {
            psError(PS_ERR_UNKNOWN, false, "x data points are not distinct (%d %d) (%f %f).\n",
                    i, i+1, xPtr->data.F32[i], xPtr->data.F32[i+1]);
        }
        psTrace(__func__, 6, "x data (%f - %f) (%f)\n", xPtr->data.F32[i], xPtr->data.F32[i+1], H);
        //
        // ******** Calculate 0-order term ********
        //
        // From (1)
        spline->spline[i]->coeff[0] = yPtr->data.F32[i] * xPtr->data.F32[i+1]/H;
        // From (2)
        spline->spline[i]->coeff[0]-= (yPtr->data.F32[i+1] * xPtr->data.F32[i])/H;
        // From (3)
        psF32 tmp = (xPtr->data.F32[i+1] * xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H);
        tmp-= xPtr->data.F32[i+1] / H;
        tmp*= spline->p_psDeriv2[i] * H * H / 6.0;
        spline->spline[i]->coeff[0]+= tmp;
        // From (4)
        tmp = -(xPtr->data.F32[i] * xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H);
        tmp+= xPtr->data.F32[i] / H;
        tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0;
        spline->spline[i]->coeff[0]+= tmp;

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

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

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

        psTrace(__func__, 6, "(spline->spline[%u])->coeff[0] is %f\n", i, spline->spline[i]->coeff[0]);
        psTrace(__func__, 6, "(spline->spline[%u])->coeff[1] is %f\n", i, spline->spline[i]->coeff[1]);
        psTrace(__func__, 6, "(spline->spline[%u])->coeff[2] is %f\n", i, spline->spline[i]->coeff[2]);
        psTrace(__func__, 6, "(spline->spline[%u])->coeff[3] is %f\n", i, spline->spline[i]->coeff[3]);
    }

    if (PS_TYPE_F64 == y->type.type) {
        psFree(yPtr);
    }
    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(spline);
}

void PS_POLY1D_PRINT(
    psPolynomial1D *poly)
{
    printf("-------------- PS_POLY1D_PRINT() --------------\n");
    printf("poly->nX is %d\n", poly->nX);
    for (psS32 i = 0 ; i < (1 + poly->nX) ; i++) {
        printf("poly->coeff[%d] is %f\n", i, poly->coeff[i]);
    }
}

void PS_PRINT_SPLINE2(psSpline1D *mySpline)
{
    printf("-------------- PS_PRINT_SPLINE2() --------------\n");
    printf("mySpline->n is %d\n", mySpline->n);
    for (psS32 i = 0 ; i < mySpline->n ; i++) {
        PS_POLY1D_PRINT(mySpline->spline[i]);
    }
    PS_VECTOR_PRINT_F32(mySpline->knots);
}





































/*****************************************************************************
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 spline->knots if psF32.  Must we add support for psU32 and
psF64?
 *****************************************************************************/
float psSpline1DEval(
    const psSpline1D *spline,
    float x)
{
    psTrace(__func__, 3, "---- %s(%f) begin ----\n", __func__, 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);

    psS32 n = spline->n;
    psS32 binNum = vectorBinDisectF32(spline->knots->data.F32, (spline->n)+1, x);
    if (binNum < 0) {
        //
        // If x is outside the range of spline->knots, generate a warning
        // message, then return the left, or right, endpoint.
        //
        psLogMsg(__func__, PS_LOG_WARN,
                 "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f) (%d).",
                 x, spline->knots->data.F32[0], spline->knots->data.F32[n-1], n);

        if (x < spline->knots->data.F32[0]) {
            psF32 rcF32 = psPolynomial1DEval(spline->spline[0], x);
            psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32);
            return(rcF32);
        } else if (x > spline->knots->data.F32[n-1]) {
            psF32 rcF32 = psPolynomial1DEval(spline->spline[n-1], x);
            psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32);
            return(rcF32);
        }
    }

    psF32 rcF32 = psPolynomial1DEval(spline->spline[binNum], x);
    psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32);
    return(rcF32);
}


/*****************************************************************************
 *****************************************************************************/
psVector *psSpline1DEvalVector(
    const psSpline1D *spline,
    const psVector *x)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_PTR_NON_NULL(spline, NULL);
    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    if (psTraceGetLevel(__func__) >= 6) {
        PS_VECTOR_PRINT_F32(x);
        // XXX: print spline
    }

    psVector *tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
    if (x->type.type == PS_TYPE_F32) {
        for (psS32 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 (psS32 i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
        }
    }

    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    return(tmpVector);
}
