/** @file  psFunctions.c
 *
 *  @brief Contains basic function allocation, deallocation, and evaluation
 *         routines.
 *
 *  This file will hold the functions for allocated, freeing, and evaluating
 *  polynomials.  It also contains a Gaussian functions.
 *
 *  @version $Revision: 1.94 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-03-10 22:33:57 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 *
 *  XXX: What happens if the polyEVal functions are called with data of the wrong
 *       type?
 *  XXX: Should the "coeffErr[]" be used as well?  Bug ???.  Ignore coeffErr
 *
 */
/*****************************************************************************/
/*  INCLUDE FILES                                                            */
/*****************************************************************************/
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#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 "psFunctions.h"
#include "psConstants.h"

#include "psDataManipErrors.h"

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

/*****************************************************************************/
/* TYPE DEFINITIONS                                                          */
/*****************************************************************************/
static void polynomial1DFree(psPolynomial1D* myPoly);
static void polynomial2DFree(psPolynomial2D* myPoly);
static void polynomial3DFree(psPolynomial3D* myPoly);
static void polynomial4DFree(psPolynomial4D* myPoly);
static void dPolynomial1DFree(psDPolynomial1D* myPoly);
static void dPolynomial2DFree(psDPolynomial2D* myPoly);
static void dPolynomial3DFree(psDPolynomial3D* myPoly);
static void dPolynomial4DFree(psDPolynomial4D* myPoly);
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                                           */
/*****************************************************************************/

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;
}

static void polynomial1DFree(psPolynomial1D* myPoly)
{
    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void polynomial2DFree(psPolynomial2D* myPoly)
{
    psS32 x = 0;

    for (x = 0; x < myPoly->nX; x++) {
        psFree(myPoly->coeff[x]);
        psFree(myPoly->coeffErr[x]);
        psFree(myPoly->mask[x]);
    }
    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void polynomial3DFree(psPolynomial3D* myPoly)
{
    psS32 x = 0;
    psS32 y = 0;

    for (x = 0; x < myPoly->nX; x++) {
        for (y = 0; y < myPoly->nY; y++) {
            psFree(myPoly->coeff[x][y]);
            psFree(myPoly->coeffErr[x][y]);
            psFree(myPoly->mask[x][y]);
        }
        psFree(myPoly->coeff[x]);
        psFree(myPoly->coeffErr[x]);
        psFree(myPoly->mask[x]);
    }

    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void polynomial4DFree(psPolynomial4D* myPoly)
{
    psS32 w = 0;
    psS32 x = 0;
    psS32 y = 0;

    for (w = 0; w < myPoly->nW; w++) {
        for (x = 0; x < myPoly->nX; x++) {
            for (y = 0; y < myPoly->nY; y++) {
                psFree(myPoly->coeff[w][x][y]);
                psFree(myPoly->coeffErr[w][x][y]);
                psFree(myPoly->mask[w][x][y]);
            }
            psFree(myPoly->coeff[w][x]);
            psFree(myPoly->coeffErr[w][x]);
            psFree(myPoly->mask[w][x]);
        }
        psFree(myPoly->coeff[w]);
        psFree(myPoly->coeffErr[w]);
        psFree(myPoly->mask[w]);
    }

    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void dPolynomial1DFree(psDPolynomial1D* myPoly)
{
    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void dPolynomial2DFree(psDPolynomial2D* myPoly)
{
    psS32 x = 0;

    for (x = 0; x < myPoly->nX; x++) {
        psFree(myPoly->coeff[x]);
        psFree(myPoly->coeffErr[x]);
        psFree(myPoly->mask[x]);
    }
    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void dPolynomial3DFree(psDPolynomial3D* myPoly)
{
    psS32 x = 0;
    psS32 y = 0;

    for (x = 0; x < myPoly->nX; x++) {
        for (y = 0; y < myPoly->nY; y++) {
            psFree(myPoly->coeff[x][y]);
            psFree(myPoly->coeffErr[x][y]);
            psFree(myPoly->mask[x][y]);
        }
        psFree(myPoly->coeff[x]);
        psFree(myPoly->coeffErr[x]);
        psFree(myPoly->mask[x]);
    }

    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

static void dPolynomial4DFree(psDPolynomial4D* myPoly)
{
    psS32 w = 0;
    psS32 x = 0;
    psS32 y = 0;

    for (w = 0; w < myPoly->nW; w++) {
        for (x = 0; x < myPoly->nX; x++) {
            for (y = 0; y < myPoly->nY; y++) {
                psFree(myPoly->coeff[w][x][y]);
                psFree(myPoly->coeffErr[w][x][y]);
                psFree(myPoly->mask[w][x][y]);
            }
            psFree(myPoly->coeff[w][x]);
            psFree(myPoly->coeffErr[w][x]);
            psFree(myPoly->mask[w][x]);
        }
        psFree(myPoly->coeff[w]);
        psFree(myPoly->coeffErr[w]);
        psFree(myPoly->mask[w]);
    }

    psFree(myPoly->coeff);
    psFree(myPoly->coeffErr);
    psFree(myPoly->mask);
}

/*****************************************************************************
createChebyshevPolys(n): this routine takes as input the required order n,
and returns as output as a pointer to an array of n psPolynomial1D
structures, corresponding to the first n Chebyshev polynomials.
 
XXX: The output should be static since the Chebyshev polynomials might be
used frequently and the data structure created here does not contain the
outer coefficients of the Chebyshev polynomials.
 *****************************************************************************/
static psPolynomial1D **createChebyshevPolys(psS32 maxChebyPoly)
{
    PS_INT_CHECK_NON_NEGATIVE(maxChebyPoly, NULL);

    psPolynomial1D **chebPolys = NULL;

    chebPolys = (psPolynomial1D **) psAlloc(maxChebyPoly * sizeof(psPolynomial1D *));
    for (psS32 i = 0; i < maxChebyPoly; i++) {
        chebPolys[i] = psPolynomial1DAlloc(i + 1, PS_POLYNOMIAL_ORD);
    }

    // Create the Chebyshev polynomials.
    // Polynomial i has i-th order.
    chebPolys[0]->coeff[0] = 1;

    // XXX: Bug 296
    if (maxChebyPoly > 1) {
        chebPolys[1]->coeff[1] = 1;
    }
    for (psS32 i = 2; i < maxChebyPoly; i++) {
        for (psS32 j = 0; j < chebPolys[i - 1]->n; j++) {
            chebPolys[i]->coeff[j + 1] = 2 * chebPolys[i - 1]->coeff[j];
        }
        for (psS32 j = 0; j < chebPolys[i - 2]->n; j++) {
            chebPolys[i]->coeff[j] -= chebPolys[i - 2]->coeff[j];
        }
    }

    return (chebPolys);
}

/*****************************************************************************
    Polynomial coefficients will be accessed in [w][x][y][z] fashion.
 *****************************************************************************/
static psF32 ordPolynomial1DEval(psF32 x, const psPolynomial1D* myPoly)
{
    psS32 loop_x = 0;
    psF32 polySum = 0.0;
    psF32 xSum = 1.0;

    psTrace(".psLib.dataManip.psFunctions.ordPolynomial1DEval", 4,
            "---- Calling ordPolynomial1DEval(%f)\n", x);
    psTrace(".psLib.dataManip.psFunctions.ordPolynomial1DEval", 4,
            "Polynomial order is %d\n", myPoly->n);
    for (loop_x = 0; loop_x < myPoly->n; loop_x++) {
        psTrace(".psLib.dataManip.psFunctions.ordPolynomial1DEval", 4,
                "Polynomial coeff[%d] is %f\n", loop_x, myPoly->coeff[loop_x]);
    }

    for (loop_x = 0; loop_x < myPoly->n; loop_x++) {
        if (myPoly->mask[loop_x] == 0) {
            psTrace(".psLib.dataManip.psFunctions.ordPolynomial1DEval", 10,
                    "polysum+= sum*coeff [%f+= (%f * %f)\n", polySum, xSum, myPoly->coeff[loop_x]);
            polySum += xSum * myPoly->coeff[loop_x];
        }
        xSum *= x;
    }

    return(polySum);
}

// XXX: You can do this without having to psAlloc() vector d.
// XXX: How does the mask vector effect Crenshaw's formula?
// XXX: We assume that x is scaled between -1.0 and 1.0;
static psF32 chebPolynomial1DEval(psF32 x, const psPolynomial1D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    psVector *d;
    psS32 n;
    psS32 i;
    psF32 tmp;

    n = myPoly->n;
    d = psVectorAlloc(n, PS_TYPE_F32);
    if(myPoly->mask[n-1] == 0) {
        d->data.F32[n-1] = myPoly->coeff[n-1];
    } else {
        d->data.F32[n-1] = 0.0;
    }
    d->data.F32[n-2] = (2.0 * x * d->data.F32[n-1]);
    if(myPoly->mask[n-2] == 0) {
        d->data.F32[n-2] += myPoly->coeff[n-2];
    }
    for (i=n-3;i>=1;i--) {
        d->data.F32[i] = (2.0 * x * d->data.F32[i+1]) -
                         (d->data.F32[i+2]);
        if(myPoly->mask[i] == 0) {
            d->data.F32[i] += myPoly->coeff[i];
        }
    }

    tmp = (x * d->data.F32[1]) -
          (d->data.F32[2]);
    if(myPoly->mask[0] == 0) {
        tmp += (0.5 * myPoly->coeff[0]);
    }
    psFree(d);
    return(tmp);

    /*

    psS32 n;
    psS32 i;
    psF32 tmp;
    psPolynomial1D **chebPolys = NULL;

    n = myPoly->n;
    chebPolys = createChebyshevPolys(n);

    tmp = 0.0;
    for (i=0;i<myPoly->n;i++) {
        tmp+= (myPoly->coeff[i] * psPolynomial1DEval(x, chebPolys[i]));
    }
    tmp-= (myPoly->coeff[0]/2.0);


    return(tmp);
    */
}

static psF32 ordPolynomial2DEval(psF32 x,
                                 psF32 y,
                                 const psPolynomial2D* myPoly)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psF32 polySum = 0.0;
    psF32 xSum = 1.0;
    psF32 ySum = 1.0;

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        ySum = xSum;
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            if (myPoly->mask[loop_x][loop_y] == 0) {
                polySum += ySum * myPoly->coeff[loop_x][loop_y];
            }
            ySum *= y;
        }
        xSum *= x;
    }

    return(polySum);
}

static psF32 chebPolynomial2DEval(psF32 x, psF32 y, const psPolynomial2D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(y, -1.0, 1.0, 0.0);
    PS_POLY_CHECK_NULL(myPoly, NAN);

    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 i = 0;
    psF32 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    psS32 maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = myPoly->nX;
    if (myPoly->nY > maxChebyPoly) {
        maxChebyPoly = myPoly->nY;
    }
    chebPolys = createChebyshevPolys(maxChebyPoly);

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            if (myPoly->mask[loop_x][loop_y] == 0) {
                polySum += myPoly->coeff[loop_x][loop_y] *
                           psPolynomial1DEval(chebPolys[loop_x], x) *
                           psPolynomial1DEval(chebPolys[loop_y], y);
            }
        }
    }
    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

static psF32 ordPolynomial3DEval(psF32 x, psF32 y, psF32 z, const psPolynomial3D* myPoly)
{
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psF32 polySum = 0.0;
    psF32 xSum = 1.0;
    psF32 ySum = 1.0;
    psF32 zSum = 1.0;

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        ySum = xSum;
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            zSum = ySum;
            for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                if (myPoly->mask[loop_x][loop_y][loop_z] == 0) {
                    polySum += zSum * myPoly->coeff[loop_x][loop_y][loop_z];
                }
                zSum *= z;
            }
            ySum *= y;
        }
        xSum *= x;
    }

    return(polySum);
}

static psF32 chebPolynomial3DEval(psF32 x, psF32 y, psF32 z, const psPolynomial3D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(y, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(z, -1.0, 1.0, 0.0);
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psS32 i = 0;
    psF32 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    psS32 maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = myPoly->nX;
    if (myPoly->nY > maxChebyPoly) {
        maxChebyPoly = myPoly->nY;
    }
    if (myPoly->nZ > maxChebyPoly) {
        maxChebyPoly = myPoly->nZ;
    }
    chebPolys = createChebyshevPolys(maxChebyPoly);

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                if (myPoly->mask[loop_x][loop_y][loop_z] == 0) {
                    polySum += myPoly->coeff[loop_x][loop_y][loop_z] *
                               psPolynomial1DEval(chebPolys[loop_x], x) *
                               psPolynomial1DEval(chebPolys[loop_y], y) *
                               psPolynomial1DEval(chebPolys[loop_z], z);
                }
            }
        }
    }

    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

static psF32 ordPolynomial4DEval(psF32 w, psF32 x, psF32 y, psF32 z, const psPolynomial4D* myPoly)
{
    psS32 loop_w = 0;
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psF32 polySum = 0.0;
    psF32 wSum = 1.0;
    psF32 xSum = 1.0;
    psF32 ySum = 1.0;
    psF32 zSum = 1.0;

    for (loop_w = 0; loop_w < myPoly->nW; loop_w++) {
        xSum = wSum;
        for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
            ySum = xSum;
            for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
                zSum = ySum;
                for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                    if (myPoly->mask[loop_w][loop_x][loop_y][loop_z] == 0) {
                        polySum += zSum * myPoly->coeff[loop_w][loop_x][loop_y][loop_z];
                    }
                    zSum *= z;
                }
                ySum *= y;
            }
            xSum *= x;
        }
        wSum *= w;
    }

    return(polySum);
}

static psF32 chebPolynomial4DEval(psF32 w, psF32 x, psF32 y, psF32 z, const psPolynomial4D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(w, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(y, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(z, -1.0, 1.0, 0.0);
    psS32 loop_w = 0;
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psS32 i = 0;
    psF32 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    psS32 maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = myPoly->nW;
    if (myPoly->nX > maxChebyPoly) {
        maxChebyPoly = myPoly->nX;
    }
    if (myPoly->nY > maxChebyPoly) {
        maxChebyPoly = myPoly->nY;
    }
    if (myPoly->nZ > maxChebyPoly) {
        maxChebyPoly = myPoly->nZ;
    }
    chebPolys = createChebyshevPolys(maxChebyPoly);

    for (loop_w = 0; loop_w < myPoly->nW; loop_w++) {
        for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
            for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
                for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                    if (myPoly->mask[loop_w][loop_x][loop_y][loop_z] == 0) {
                        polySum += myPoly->coeff[loop_w][loop_x][loop_y][loop_z] *
                                   psPolynomial1DEval(chebPolys[loop_w], w) *
                                   psPolynomial1DEval(chebPolys[loop_x], x) *
                                   psPolynomial1DEval(chebPolys[loop_y], y) *
                                   psPolynomial1DEval(chebPolys[loop_z], z);
                    }
                }
            }
        }
    }

    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

/*****************************************************************************
    Polynomial coefficients will be accessed in [w][x][y][z] fashion.
 *****************************************************************************/
static psF64 dOrdPolynomial1DEval(psF64 x, const psDPolynomial1D* myPoly)
{
    psS32 loop_x = 0;
    psF64 polySum = 0.0;
    psF64 xSum = 1.0;

    for (loop_x = 0; loop_x < myPoly->n; loop_x++) {
        if (myPoly->mask[loop_x] == 0) {
            polySum += xSum * myPoly->coeff[loop_x];
        }
        xSum *= x;
    }

    return(polySum);
}

// XXX: You can do this without having to psAlloc() vector d.
// XXX: How does the mask vector effect Crenshaw's formula?
static psF64 dChebPolynomial1DEval(psF64 x, const psDPolynomial1D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    psVector *d;
    psS32 n;
    psS32 i;
    psF64 tmp;

    n = myPoly->n;
    d = psVectorAlloc(n, PS_TYPE_F64);
    if(myPoly->mask[n-1] == 0) {
        d->data.F64[n-1] = myPoly->coeff[n-1];
    } else {
        d->data.F64[n-1] = 0.0;
    }
    d->data.F64[n-2] = (2.0 * x * d->data.F64[n-1]);
    if(myPoly->mask[n-2] == 0) {
        d->data.F64[n-2] += myPoly->coeff[n-2];
    }
    for (i=n-3;i>=1;i--) {
        d->data.F64[i] = (2.0 * x * d->data.F64[i+1]) -
                         (d->data.F64[i+2]);
        if(myPoly->mask[i] == 0) {
            d->data.F64[i] += myPoly->coeff[i];
        }
    }

    tmp = (x * d->data.F64[1]) -
          (d->data.F64[2]);
    if(myPoly->mask[0] == 0) {
        tmp += (0.5 * myPoly->coeff[0]);
    }

    psFree(d);
    return(tmp);
}

static psF64 dOrdPolynomial2DEval(psF64 x, psF64 y, const psDPolynomial2D* myPoly)
{
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psF64 polySum = 0.0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        ySum = xSum;
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            if (myPoly->mask[loop_x][loop_y] == 0) {
                polySum += ySum * myPoly->coeff[loop_x][loop_y];
            }
            ySum *= y;
        }
        xSum *= x;
    }

    return(polySum);
}

static psF64 dChebPolynomial2DEval(psF64 x, psF64 y, const psDPolynomial2D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(y, -1.0, 1.0, 0.0);
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 i = 0;
    psF64 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    psS32 maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = myPoly->nX;
    if (myPoly->nY > maxChebyPoly) {
        maxChebyPoly = myPoly->nY;
    }
    chebPolys = createChebyshevPolys(maxChebyPoly);

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            if (myPoly->mask[loop_x][loop_y] == 0) {
                polySum += myPoly->coeff[loop_x][loop_y] *
                           psPolynomial1DEval(chebPolys[loop_x], x) *
                           psPolynomial1DEval(chebPolys[loop_y], y);
            }
        }
    }

    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

static psF64 dOrdPolynomial3DEval(psF64 x, psF64 y, psF64 z, const psDPolynomial3D* myPoly)
{
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psF64 polySum = 0.0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;
    psF64 zSum = 1.0;

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        ySum = xSum;
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            zSum = ySum;
            for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                if (myPoly->mask[loop_x][loop_y][loop_z] == 0) {
                    polySum += zSum * myPoly->coeff[loop_x][loop_y][loop_z];
                }
                zSum *= z;
            }
            ySum *= y;
        }
        xSum *= x;
    }

    return(polySum);
}

static psF64 dChebPolynomial3DEval(psF64 x, psF64 y, psF64 z, const psDPolynomial3D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(y, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(z, -1.0, 1.0, 0.0);
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psS32 i = 0;
    psF64 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    psS32 maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = myPoly->nX;
    if (myPoly->nY > maxChebyPoly) {
        maxChebyPoly = myPoly->nY;
    }
    if (myPoly->nZ > maxChebyPoly) {
        maxChebyPoly = myPoly->nZ;
    }
    chebPolys = createChebyshevPolys(maxChebyPoly);

    for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
        for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
            for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                if (myPoly->mask[loop_x][loop_y][loop_z] == 0) {
                    polySum += myPoly->coeff[loop_x][loop_y][loop_z] *
                               psPolynomial1DEval(chebPolys[loop_x], x) *
                               psPolynomial1DEval(chebPolys[loop_y], y) *
                               psPolynomial1DEval(chebPolys[loop_z], z);
                }
            }
        }
    }

    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

static psF64 dOrdPolynomial4DEval(psF64 w, psF64 x, psF64 y, psF64 z, const psDPolynomial4D* myPoly)
{
    psS32 loop_w = 0;
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psF64 polySum = 0.0;
    psF64 wSum = 1.0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;
    psF64 zSum = 1.0;

    for (loop_w = 0; loop_w < myPoly->nW; loop_w++) {
        xSum = wSum;
        for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
            ySum = xSum;
            for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
                zSum = ySum;
                for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                    if (myPoly->mask[loop_w][loop_x][loop_y][loop_z] == 0) {
                        polySum += zSum * myPoly->coeff[loop_w][loop_x][loop_y][loop_z];
                    }
                    zSum *= z;
                }
                ySum *= y;
            }
            xSum *= x;
        }
        wSum *= w;
    }

    return(polySum);
}

static psF64 dChebPolynomial4DEval(psF64 w, psF64 x, psF64 y, psF64 z, const psDPolynomial4D* myPoly)
{
    PS_FLOAT_CHECK_RANGE(w, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(x, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(y, -1.0, 1.0, 0.0);
    PS_FLOAT_CHECK_RANGE(z, -1.0, 1.0, 0.0);
    psS32 loop_w = 0;
    psS32 loop_x = 0;
    psS32 loop_y = 0;
    psS32 loop_z = 0;
    psS32 i = 0;
    psF64 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    psS32 maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = myPoly->nW;
    if (myPoly->nX > maxChebyPoly) {
        maxChebyPoly = myPoly->nX;
    }
    if (myPoly->nY > maxChebyPoly) {
        maxChebyPoly = myPoly->nY;
    }
    if (myPoly->nZ > maxChebyPoly) {
        maxChebyPoly = myPoly->nZ;
    }
    chebPolys = createChebyshevPolys(maxChebyPoly);

    for (loop_w = 0; loop_w < myPoly->nW; loop_w++) {
        for (loop_x = 0; loop_x < myPoly->nX; loop_x++) {
            for (loop_y = 0; loop_y < myPoly->nY; loop_y++) {
                for (loop_z = 0; loop_z < myPoly->nZ; loop_z++) {
                    if (myPoly->mask[loop_w][loop_x][loop_y][loop_z] == 0) {
                        polySum += myPoly->coeff[loop_w][loop_x][loop_y][loop_z] *
                                   psPolynomial1DEval(chebPolys[loop_w], w) *
                                   psPolynomial1DEval(chebPolys[loop_x], x) *
                                   psPolynomial1DEval(chebPolys[loop_y], y) *
                                   psPolynomial1DEval(chebPolys[loop_z], z);
                    }
                }
            }
        }
    }

    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}


/*****************************************************************************
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.psFunctions.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.psFunctions.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.psFunctions.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.psFunctions.fullInterpolate1D##TYPE", 6, \
                    "p->data.TYPE[%d] is %f\n", i, p->data.TYPE[i]); \
        } \
    } \
    psTrace(".psLib.dataManip.psFunctions.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_PTR_CHECK_NULL(domain, NAN)
    PS_PTR_CHECK_NULL(range, NAN)
    // XXX: Check valid values for n, order, and x?

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

    psTrace(".psLib.dataManip.psFunctions.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.psFunctions.interpolate1DF32", 4,
            "---- interpolate1DF32() end ----\n");
    return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x));
}

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

/*****************************************************************************
    Evaluate a non-normalized Gaussian with the given mean and sigma at the
    given coordianate.  Note that this is not a Gaussian deviate.  The
    evaluated Gaussian is: \f[ exp(-\frac{(x-mean)^2}{2\sigma^2}) \f]
 *****************************************************************************/
psF32 psGaussian(psF32 x, psF32 mean, psF32 sigma, psBool normal)
{
    psF32 tmp = 1.0;

    psTrace(".psLib.dataManip.psFunctions.psGaussian", 4,
            "---- psGaussian() begin ----\n");

    if (normal == true) {
        tmp = 1.0 / PS_SQRT_F32(2.0 * PS_PI * (sigma * sigma));
    }

    psTrace(".psLib.dataManip.psFunctions.psGaussian", 4,
            "---- psGaussian() end ----\n");
    return(tmp * exp(-((x - mean) * (x - mean)) / (2.0 * sigma * sigma)));
}

/*****************************************************************************
    p_psGaussianDev()
 This private routine (formerly a psLib API routine) creates a psVector of the
 specified size and type F32 and fills it with a random Gaussian distribution
 of numbers with the specified mean and sigma.  This routine makes use of the
 GSL routines for generating both uniformly distributed numbers and the
 Gaussian distribution as well.
 
XXX: There is no way to seed the random generator.
 *****************************************************************************/
psVector* p_psGaussianDev(psF32 mean, psF32 sigma, psS32 Npts)
{
    PS_INT_CHECK_NON_NEGATIVE(Npts, NULL);

    psVector* gauss = NULL;
    const gsl_rng_type *T = NULL;
    gsl_rng *r = NULL;
    psS32 i = 0;


    gauss = psVectorAlloc(Npts, PS_TYPE_F32);
    gauss->n = Npts;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc(T);

    for (i = 0; i < Npts; i++) {
        gauss->data.F32[i] = mean + gsl_ran_gaussian(r, sigma);
    }

    // XXX: Should I free r, T as well?  This is a memory leak.
    return(gauss);
}

/*****************************************************************************
    This routine must allocate memory for the polynomial structures.
 *****************************************************************************/
psPolynomial1D* psPolynomial1DAlloc(psS32 n,
                                    psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(n, NULL);

    psS32 i = 0;
    psPolynomial1D* newPoly = NULL;

    newPoly = (psPolynomial1D* ) psAlloc(sizeof(psPolynomial1D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) polynomial1DFree);

    newPoly->type = type;
    newPoly->n = n;
    newPoly->coeff = (psF32 *)psAlloc(n * sizeof(psF32));
    newPoly->coeffErr = (psF32 *)psAlloc(n * sizeof(psF32));
    newPoly->mask = (psU8 *)psAlloc(n * sizeof(psU8));
    for (i = 0; i < n; i++) {
        newPoly->coeff[i] = 0.0;
        newPoly->coeffErr[i] = 0.0;
        newPoly->mask[i] = 0;
    }

    return(newPoly);
}

psPolynomial2D* psPolynomial2DAlloc(psS32 nX, psS32 nY,
                                    psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(nX, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nY, NULL);

    psS32 x = 0;
    psS32 y = 0;
    psPolynomial2D* newPoly = NULL;

    newPoly = (psPolynomial2D* ) psAlloc(sizeof(psPolynomial2D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) polynomial2DFree);

    newPoly->type = type;
    newPoly->nX = nX;
    newPoly->nY = nY;

    newPoly->coeff = (psF32 **)psAlloc(nX * sizeof(psF32 *));
    newPoly->coeffErr = (psF32 **)psAlloc(nX * sizeof(psF32 *));
    newPoly->mask = (psU8 **)psAlloc(nX * sizeof(psU8 *));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (psF32 *)psAlloc(nY * sizeof(psF32));
        newPoly->coeffErr[x] = (psF32 *)psAlloc(nY * sizeof(psF32));
        newPoly->mask[x] = (psU8 *)psAlloc(nY * sizeof(psU8));
    }
    for (x = 0; x < nX; x++) {
        for (y = 0; y < nY; y++) {
            newPoly->coeff[x][y] = 0.0;
            newPoly->coeffErr[x][y] = 0.0;
            newPoly->mask[x][y] = 0;
        }
    }

    return(newPoly);
}

psPolynomial3D* psPolynomial3DAlloc(psS32 nX, psS32 nY, psS32 nZ,
                                    psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(nX, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nY, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nZ, NULL);

    psS32 x = 0;
    psS32 y = 0;
    psS32 z = 0;
    psPolynomial3D* newPoly = NULL;

    newPoly = (psPolynomial3D* ) psAlloc(sizeof(psPolynomial3D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) polynomial3DFree);

    newPoly->type = type;
    newPoly->nX = nX;
    newPoly->nY = nY;
    newPoly->nZ = nZ;

    newPoly->coeff = (psF32 ***)psAlloc(nX * sizeof(psF32 **));
    newPoly->coeffErr = (psF32 ***)psAlloc(nX * sizeof(psF32 **));
    newPoly->mask = (psU8 ***)psAlloc(nX * sizeof(psU8 **));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (psF32 **)psAlloc(nY * sizeof(psF32 *));
        newPoly->coeffErr[x] = (psF32 **)psAlloc(nY * sizeof(psF32 *));
        newPoly->mask[x] = (psU8 **)psAlloc(nY * sizeof(psU8 *));
        for (y = 0; y < nY; y++) {
            newPoly->coeff[x][y] = (psF32 *)psAlloc(nZ * sizeof(psF32));
            newPoly->coeffErr[x][y] = (psF32 *)psAlloc(nZ * sizeof(psF32));
            newPoly->mask[x][y] = (psU8 *)psAlloc(nZ * sizeof(psU8));
        }
    }
    for (x = 0; x < nX; x++) {
        for (y = 0; y < nY; y++) {
            for (z = 0; z < nZ; z++) {
                newPoly->coeff[x][y][z] = 0.0;
                newPoly->coeffErr[x][y][z] = 0.0;
                newPoly->mask[x][y][z] = 0;
            }
        }
    }

    return(newPoly);
}

psPolynomial4D* psPolynomial4DAlloc(psS32 nW, psS32 nX, psS32 nY, psS32 nZ,
                                    psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(nW, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nX, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nY, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nZ, NULL);

    psS32 w = 0;
    psS32 x = 0;
    psS32 y = 0;
    psS32 z = 0;
    psPolynomial4D* newPoly = NULL;

    newPoly = (psPolynomial4D* ) psAlloc(sizeof(psPolynomial4D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) polynomial4DFree);

    newPoly->type = type;
    newPoly->nW = nW;
    newPoly->nX = nX;
    newPoly->nY = nY;
    newPoly->nZ = nZ;

    newPoly->coeff = (psF32 ****)psAlloc(nW * sizeof(psF32 ***));
    newPoly->coeffErr = (psF32 ****)psAlloc(nW * sizeof(psF32 ***));
    newPoly->mask = (psU8 ****)psAlloc(nW * sizeof(psU8 ***));
    for (w = 0; w < nW; w++) {
        newPoly->coeff[w] = (psF32 ***)psAlloc(nX * sizeof(psF32 **));
        newPoly->coeffErr[w] = (psF32 ***)psAlloc(nX * sizeof(psF32 **));
        newPoly->mask[w] = (psU8 ***)psAlloc(nX * sizeof(psU8 **));
        for (x = 0; x < nX; x++) {
            newPoly->coeff[w][x] = (psF32 **)psAlloc(nY * sizeof(psF32 *));
            newPoly->coeffErr[w][x] = (psF32 **)psAlloc(nY * sizeof(psF32 *));
            newPoly->mask[w][x] = (psU8 **)psAlloc(nY * sizeof(psU8 *));
            for (y = 0; y < nY; y++) {
                newPoly->coeff[w][x][y] = (psF32 *)psAlloc(nZ * sizeof(psF32));
                newPoly->coeffErr[w][x][y] = (psF32 *)psAlloc(nZ * sizeof(psF32));
                newPoly->mask[w][x][y] = (psU8 *)psAlloc(nZ * sizeof(psU8));
            }
        }
    }
    for (w = 0; w < nW; w++) {
        for (x = 0; x < nX; x++) {
            for (y = 0; y < nY; y++) {
                for (z = 0; z < nZ; z++) {
                    newPoly->coeff[w][x][y][z] = 0.0;
                    newPoly->coeffErr[w][x][y][z] = 0.0;
                    newPoly->mask[w][x][y][z] = 0;
                }
            }
        }
    }

    return(newPoly);
}

psF32 psPolynomial1DEval(const psPolynomial1D* myPoly, psF32 x)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(ordPolynomial1DEval(x, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(chebPolynomial1DEval(x, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psPolynomial1DEvalVector(const psPolynomial1D *myPoly,
                                   const psVector *x)
{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F32, NULL);

    psVector *tmp;

    tmp = psVectorAlloc(x->n, PS_TYPE_F32);
    for (psS32 i=0;i<x->n;i++) {
        tmp->data.F32[i] = psPolynomial1DEval(myPoly, x->data.F32[i]);
    }

    return(tmp);
}

psF32 psPolynomial2DEval(const psPolynomial2D* myPoly, psF32 x, psF32 y)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(ordPolynomial2DEval(x, y, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(chebPolynomial2DEval(x, y, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psPolynomial2DEvalVector(const psPolynomial2D *myPoly,
                                   const psVector *x,
                                   const psVector *y)

{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F32, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F32, NULL);

    psVector *tmp;
    psS32 vecLen=x->n;

    // Determine the length of the output vector to by the minimum of the x,y vectors
    if (y->n < vecLen) {
        vecLen = y->n;
    }

    // Create output vector to return
    tmp = psVectorAlloc(vecLen, PS_TYPE_F32);

    // Evaluate the polynomial at the specified points
    for (psS32 i=0; i<vecLen; i++) {
        tmp->data.F32[i] = psPolynomial2DEval(myPoly,x->data.F32[i],y->data.F32[i]);
    }

    // Return output vector
    return(tmp);
}

psF32 psPolynomial3DEval(const psPolynomial3D* myPoly, psF32 x, psF32 y, psF32 z)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(ordPolynomial3DEval(x, y, z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(chebPolynomial3DEval(x, y, z, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psPolynomial3DEvalVector(const psPolynomial3D *myPoly,
                                   const psVector *x,
                                   const psVector *y,
                                   const psVector *z)

{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F32, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F32, NULL);
    PS_VECTOR_CHECK_NULL(z, NULL);
    PS_VECTOR_CHECK_TYPE(z, PS_TYPE_F32, NULL);

    psVector *tmp;
    psS32 vecLen=x->n;

    // Determine the length of output vector from min of the input vectors
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    // Allocate output vector
    tmp = psVectorAlloc(vecLen, PS_TYPE_F32);

    // Evaluate polynomial
    for (psS32 i = 0; i < vecLen; i++) {
        tmp->data.F32[i] = psPolynomial3DEval(myPoly,
                                              x->data.F32[i],
                                              y->data.F32[i],
                                              z->data.F32[i]);
    }

    // Return output vector
    return(tmp);
}

psF32 psPolynomial4DEval(const psPolynomial4D* myPoly, psF32 w, psF32 x, psF32 y, psF32 z)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(ordPolynomial4DEval(w,x,y,z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(chebPolynomial4DEval(w,x,y,z, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psPolynomial4DEvalVector(const psPolynomial4D *myPoly,
                                   const psVector *w,
                                   const psVector *x,
                                   const psVector *y,
                                   const psVector *z)
{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(w, NULL);
    PS_VECTOR_CHECK_TYPE(w, PS_TYPE_F32, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F32, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F32, NULL);
    PS_VECTOR_CHECK_NULL(z, NULL);
    PS_VECTOR_CHECK_TYPE(z, PS_TYPE_F32, NULL);

    psVector *tmp;
    psS32 vecLen=w->n;

    // Determine output vector size from min of input vectors
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (x->n < vecLen) {
        vecLen = x->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    // Allocate output vector
    tmp = psVectorAlloc(vecLen, PS_TYPE_F32);

    // Evaluate polynomial
    for (psS32 i = 0; i < vecLen; i++) {
        tmp->data.F32[i] = psPolynomial4DEval(myPoly,
                                              w->data.F32[i],
                                              x->data.F32[i],
                                              y->data.F32[i],
                                              z->data.F32[i]);
    }

    // Return output vector
    return(tmp);
}


psDPolynomial1D* psDPolynomial1DAlloc(psS32 n,
                                      psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(n, NULL);

    psS32 i = 0;
    psDPolynomial1D* newPoly = NULL;

    newPoly = (psDPolynomial1D* ) psAlloc(sizeof(psDPolynomial1D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) dPolynomial1DFree);

    newPoly->type = type;
    newPoly->n = n;
    newPoly->coeff = (psF64 *)psAlloc(n * sizeof(psF64));
    newPoly->coeffErr = (psF64 *)psAlloc(n * sizeof(psF64));
    newPoly->mask = (psU8 *)psAlloc(n * sizeof(psU8));
    for (i = 0; i < n; i++) {
        newPoly->coeff[i] = 0.0;
        newPoly->coeffErr[i] = 0.0;
        newPoly->mask[i] = 0;
    }

    return(newPoly);
}

psDPolynomial2D* psDPolynomial2DAlloc(psS32 nX, psS32 nY,
                                      psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(nX, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nY, NULL);

    psS32 x = 0;
    psS32 y = 0;
    psDPolynomial2D* newPoly = NULL;

    newPoly = (psDPolynomial2D* ) psAlloc(sizeof(psDPolynomial2D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) dPolynomial2DFree);

    newPoly->type = type;
    newPoly->nX = nX;
    newPoly->nY = nY;

    newPoly->coeff = (psF64 **)psAlloc(nX * sizeof(psF64 *));
    newPoly->coeffErr = (psF64 **)psAlloc(nX * sizeof(psF64 *));
    newPoly->mask = (psU8 **)psAlloc(nX * sizeof(psU8 *));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (psF64 *)psAlloc(nY * sizeof(psF64));
        newPoly->coeffErr[x] = (psF64 *)psAlloc(nY * sizeof(psF64));
        newPoly->mask[x] = (psU8 *)psAlloc(nY * sizeof(psU8));
    }
    for (x = 0; x < nX; x++) {
        for (y = 0; y < nY; y++) {
            newPoly->coeff[x][y] = 0.0;
            newPoly->coeffErr[x][y] = 0.0;
            newPoly->mask[x][y] = 0;
        }
    }

    return(newPoly);
}

psDPolynomial3D* psDPolynomial3DAlloc(psS32 nX, psS32 nY, psS32 nZ,
                                      psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(nX, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nY, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nZ, NULL);

    psS32 x = 0;
    psS32 y = 0;
    psS32 z = 0;
    psDPolynomial3D* newPoly = NULL;

    newPoly = (psDPolynomial3D* ) psAlloc(sizeof(psDPolynomial3D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) dPolynomial3DFree);

    newPoly->type = type;
    newPoly->nX = nX;
    newPoly->nY = nY;
    newPoly->nZ = nZ;

    newPoly->coeff = (psF64 ***)psAlloc(nX * sizeof(psF64 **));
    newPoly->coeffErr = (psF64 ***)psAlloc(nX * sizeof(psF64 **));
    newPoly->mask = (psU8 ***)psAlloc(nX * sizeof(psU8 **));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (psF64 **)psAlloc(nY * sizeof(psF64 *));
        newPoly->coeffErr[x] = (psF64 **)psAlloc(nY * sizeof(psF64 *));
        newPoly->mask[x] = (psU8 **)psAlloc(nY * sizeof(psU8 *));
        for (y = 0; y < nY; y++) {
            newPoly->coeff[x][y] = (psF64 *)psAlloc(nZ * sizeof(psF64));
            newPoly->coeffErr[x][y] = (psF64 *)psAlloc(nZ * sizeof(psF64));
            newPoly->mask[x][y] = (psU8 *)psAlloc(nZ * sizeof(psU8));
        }
    }
    for (x = 0; x < nX; x++) {
        for (y = 0; y < nY; y++) {
            for (z = 0; z < nZ; z++) {
                newPoly->coeff[x][y][z] = 0.0;
                newPoly->coeffErr[x][y][z] = 0.0;
                newPoly->mask[x][y][z] = 0;
            }
        }
    }

    return(newPoly);
}

psDPolynomial4D* psDPolynomial4DAlloc(psS32 nW, psS32 nX, psS32 nY, psS32 nZ,
                                      psPolynomialType type)
{
    PS_INT_CHECK_NON_NEGATIVE(nW, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nX, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nY, NULL);
    PS_INT_CHECK_NON_NEGATIVE(nZ, NULL);

    psS32 w = 0;
    psS32 x = 0;
    psS32 y = 0;
    psS32 z = 0;
    psDPolynomial4D* newPoly = NULL;

    newPoly = (psDPolynomial4D* ) psAlloc(sizeof(psDPolynomial4D));
    p_psMemSetDeallocator(newPoly, (psFreeFcn) dPolynomial4DFree);

    newPoly->type = type;
    newPoly->nW = nW;
    newPoly->nX = nX;
    newPoly->nY = nY;
    newPoly->nZ = nZ;

    newPoly->coeff = (psF64 ****)psAlloc(nW * sizeof(psF64 ***));
    newPoly->coeffErr = (psF64 ****)psAlloc(nW * sizeof(psF64 ***));
    newPoly->mask = (psU8 ****)psAlloc(nW * sizeof(psU8 ***));
    for (w = 0; w < nW; w++) {
        newPoly->coeff[w] = (psF64 ***)psAlloc(nX * sizeof(psF64 **));
        newPoly->coeffErr[w] = (psF64 ***)psAlloc(nX * sizeof(psF64 **));
        newPoly->mask[w] = (psU8 ***)psAlloc(nX * sizeof(psU8 **));
        for (x = 0; x < nX; x++) {
            newPoly->coeff[w][x] = (psF64 **)psAlloc(nY * sizeof(psF64 *));
            newPoly->coeffErr[w][x] = (psF64 **)psAlloc(nY * sizeof(psF64 *));
            newPoly->mask[w][x] = (psU8 **)psAlloc(nY * sizeof(psU8 *));
            for (y = 0; y < nY; y++) {
                newPoly->coeff[w][x][y] = (psF64 *)psAlloc(nZ * sizeof(psF64));
                newPoly->coeffErr[w][x][y] = (psF64 *)psAlloc(nZ * sizeof(psF64));
                newPoly->mask[w][x][y] = (psU8 *)psAlloc(nZ * sizeof(psU8));
            }
        }
    }
    for (w = 0; w < nW; w++) {
        for (x = 0; x < nX; x++) {
            for (y = 0; y < nY; y++) {
                for (z = 0; z < nZ; z++) {
                    newPoly->coeff[w][x][y][z] = 0.0;
                    newPoly->coeffErr[w][x][y][z] = 0.0;
                    newPoly->mask[w][x][y][z] = 0;
                }
            }
        }
    }

    return(newPoly);
}


psF64 psDPolynomial1DEval(const psDPolynomial1D* myPoly, psF64 x)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(dOrdPolynomial1DEval(x, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(dChebPolynomial1DEval(x, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psDPolynomial1DEvalVector(const psDPolynomial1D *myPoly,
                                    const psVector *x)

{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F64, NULL);

    psVector *tmp;

    tmp = psVectorAlloc(x->n, PS_TYPE_F64);
    for (psS32 i=0;i<x->n;i++) {
        tmp->data.F64[i] = psDPolynomial1DEval(myPoly,
                                               x->data.F64[i]);
    }

    return(tmp);
}


psF64 psDPolynomial2DEval(const psDPolynomial2D* myPoly,
                          psF64 x,
                          psF64 y)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(dOrdPolynomial2DEval(x, y, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(dChebPolynomial2DEval(x, y, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psDPolynomial2DEvalVector(const psDPolynomial2D *myPoly,
                                    const psVector *x,
                                    const psVector *y)
{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F64, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F64, NULL);

    psVector *tmp;
    psS32 vecLen=x->n;

    // Determine the output vector length from minimum length of input vectors
    if (y->n < vecLen) {
        vecLen = y->n;
    }

    // Allocate output vector
    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);

    // Evaluate the polynomial
    for (psS32 i = 0; i < vecLen; i++) {
        tmp->data.F64[i] = psDPolynomial2DEval(myPoly,x->data.F64[i],y->data.F64[i]);
    }

    // Return output vector
    return(tmp);
}


psF64 psDPolynomial3DEval(const psDPolynomial3D* myPoly,
                          psF64 x,
                          psF64 y,
                          psF64 z)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(dOrdPolynomial3DEval(x, y, z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(dChebPolynomial3DEval(x, y, z, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psDPolynomial3DEvalVector(const psDPolynomial3D *myPoly,
                                    const psVector *x,
                                    const psVector *y,
                                    const psVector *z)

{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F64, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F64, NULL);
    PS_VECTOR_CHECK_NULL(z, NULL);
    PS_VECTOR_CHECK_TYPE(z, PS_TYPE_F64, NULL);

    psVector *tmp;
    psS32 vecLen=x->n;

    // Determine the size of output vector from min of input vectors
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    // Allocate output vector
    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);

    // Evaluate polynomial
    for (psS32 i = 0; i < vecLen; i++) {
        tmp->data.F64[i] = psDPolynomial3DEval(myPoly,
                                               x->data.F64[i],
                                               y->data.F64[i],
                                               z->data.F64[i]);
    }

    // Return output vector
    return(tmp);
}

psF64 psDPolynomial4DEval(const psDPolynomial4D* myPoly,
                          psF64 w,
                          psF64 x,
                          psF64 y,
                          psF64 z)
{
    PS_POLY_CHECK_NULL(myPoly, NAN);

    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(dOrdPolynomial4DEval(w,x,y,z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(dChebPolynomial4DEval(w,x,y,z, myPoly));
    } else {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psFunctions_INVALID_POLYNOMIAL_TYPE,
                myPoly->type);
    }
    return(NAN);
}

psVector *psDPolynomial4DEvalVector(const psDPolynomial4D *myPoly,
                                    const psVector *w,
                                    const psVector *x,
                                    const psVector *y,
                                    const psVector *z)
{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_VECTOR_CHECK_NULL(w, NULL);
    PS_VECTOR_CHECK_TYPE(w, PS_TYPE_F64, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F64, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F64, NULL);
    PS_VECTOR_CHECK_NULL(z, NULL);
    PS_VECTOR_CHECK_TYPE(z, PS_TYPE_F64, NULL);

    psVector *tmp;
    psS32 vecLen=w->n;

    // Determine the output vector size from min of input vectors
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (x->n < vecLen) {
        vecLen = x->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    // Allocate output vector
    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);

    // Evaluate the polynomial
    for (psS32 i = 0; i < vecLen; i++) {
        tmp->data.F64[i] = psDPolynomial4DEval(myPoly,
                                               w->data.F64[i],
                                               x->data.F64[i],
                                               y->data.F64[i],
                                               z->data.F64[i]);
    }

    // Return output vector
    return(tmp);
}




//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: Ensure that domain[i+1] != domain[i]
 
XXX: What should be the defualty type for knots be?  psF32 is assumed.
 *****************************************************************************/
psSpline1D *psSpline1DAlloc(psS32 numSplines,
                            psS32 order,
                            psF32 min,
                            psF32 max)
{
    PS_INT_CHECK_NON_NEGATIVE(numSplines, NULL);
    PS_INT_CHECK_NON_NEGATIVE(order, NULL);
    PS_FLOAT_CHECK_NON_EQUAL(max, min, NULL);

    psSpline1D *tmp = NULL;
    psS32 i;
    psF32 tmpDomain;
    psF32 width;

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

    tmp->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    for (i=0;i<numSplines;i++) {
        (tmp->spline)[i] = psPolynomial1DAlloc(order+1, PS_POLYNOMIAL_ORD);
    }

    // This should be set by the psVectorFitSpline1D()
    tmp->p_psDeriv2 = NULL;

    tmp->knots = psVectorAlloc(numSplines+1, PS_TYPE_F32);
    width = (max - min) / ((psF32) numSplines);

    tmp->knots->data.F32[0] = min;
    tmpDomain = min+width;
    for (i=1;i<numSplines+1;i++) {
        tmp->knots->data.F32[i] = tmpDomain;
        tmpDomain+= width;
    }
    tmp->knots->data.F32[numSplines] = max;

    p_psMemSetDeallocator(tmp,(psFreeFcn)spline1DFree);
    return(tmp);
}


/*****************************************************************************
XXX: What should be the defualty type for knots be?  psF32 is assumed.
 *****************************************************************************/
psSpline1D *psSpline1DAllocGeneric(const psVector *bounds,
                                   psS32 order)
{
    PS_VECTOR_CHECK_NULL(bounds, NULL);
    PS_VECTOR_CHECK_EMPTY(bounds, NULL);
    PS_INT_CHECK_NON_NEGATIVE(order, NULL);

    psSpline1D *tmp = NULL;
    psS32 i;
    psS32 numSplines;

    tmp = (psSpline1D *) psAlloc(sizeof(psSpline1D));

    numSplines = bounds->n - 1;
    tmp->n = numSplines;

    tmp->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    for (i=0;i<numSplines;i++) {
        (tmp->spline)[i] = psPolynomial1DAlloc(order+1, PS_POLYNOMIAL_ORD);
    }

    // This should be set by the psVectorFitSpline1D()
    tmp->p_psDeriv2 = NULL;

    tmp->knots = psVectorAlloc(bounds->n, PS_TYPE_F32);

    for (i=0;i<bounds->n;i++) {
        tmp->knots->data.F32[i] = bounds->data.F32[i];
        if (i<(bounds->n-1)) {
            if (FLT_EPSILON >= fabs(bounds->data.F32[i+1]-bounds->data.F32[i])) {
                psError(PS_ERR_UNKNOWN, true, "data points must be distinct\n");
            }
        }
    }

    p_psMemSetDeallocator(tmp,(psFreeFcn)spline1DFree);
    return(tmp);
}

/*****************************************************************************
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.psFunctions.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.psFunctions.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.psFunctions.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.psFunctions.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().
 *****************************************************************************/
psS32 p_psVectorBinDisect(psVector *bins,
                          psScalar *x)
{
    PS_VECTOR_CHECK_NULL(bins, -4);
    PS_VECTOR_CHECK_EMPTY(bins, -4);
    PS_PTR_CHECK_NULL(x, -6);
    PS_PTR_CHECK_TYPE_EQUAL(x, bins, -3);
    psS8* 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_psFunctions_TYPE_NOT_SUPPORTED,
                strType);
    case PS_TYPE_C64:
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psFunctions_TYPE_NOT_SUPPORTED,
                strType);
    case PS_TYPE_PTR:
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psFunctions_TYPE_NOT_SUPPORTED,
                strType);
    case PS_TYPE_BOOL:
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psFunctions_TYPE_NOT_SUPPORTED,
                strType);
    }
    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,
                                psS32 order,
                                psScalar *x)
{
    PS_VECTOR_CHECK_NULL(domain, NULL);
    PS_VECTOR_CHECK_NULL(range, NULL);
    PS_PTR_CHECK_NULL(x, NULL);
    PS_INT_CHECK_NON_NEGATIVE(order, NULL);
    PS_VECTOR_CHECK_SIZE_EQUAL(domain, range, NULL);
    PS_PTR_CHECK_TYPE_EQUAL(domain, range, NULL);
    PS_PTR_CHECK_TYPE_EQUAL(domain, x, NULL);

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

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

    if (x->type.type == PS_TYPE_F32) {
        psTrace(".psLib.dataManip.psFunctions.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.psFunctions.p_psVectorInterpolate", 4,
                "---- p_psVectorInterpolate() end ----\n");
        // XXX: Convert data type to F64?
        return(tmpScalar);

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

    psTrace(".psLib.dataManip.psFunctions.p_psVectorInterpolate", 4,
            "return(NULL)\n");
    psTrace(".psLib.dataManip.psFunctions.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.
 *****************************************************************************/
psF32 psSpline1DEval(
    const psSpline1D *spline,
    psF32 x
)
{
    PS_PTR_CHECK_NULL(spline, NAN);
    PS_INT_CHECK_NON_NEGATIVE(spline->n, NAN);
    PS_VECTOR_CHECK_TYPE(spline->knots, PS_TYPE_F32, NAN);

    psS32 binNum;
    psS32 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_PTR_CHECK_NULL(spline, NULL);
    PS_VECTOR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_TYPE_F32_OR_F64(x, NULL);
    PS_VECTOR_CHECK_TYPE(spline->knots, PS_TYPE_F32, NULL);

    psS32 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 {
        psS8* strType;
        PS_TYPE_NAME(strType,x->type.type);
        psError(PS_ERR_BAD_PARAMETER_TYPE,
                PS_ERRORTEXT_psFunctions_TYPE_NOT_SUPPORTED,
                strType);
        return(NULL);
    }

    return(tmpVector);
}
