/** @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.52 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2004-10-14 01:44:48 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 */
/*****************************************************************************/
/*  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 "psAbort.h"
#include "psLogMsg.h"

#include "psFunctions.h"
/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/
/*
 * The following macros take the vector IN as input.  If IN is not of
 * the correct type, then a new vector OUT is created of the correct
 * type, and the elements of IN and converted to OUT.
 *
 * If vector IN is the correct type, then OUT is simply set to IN.
 */
#define PS_CONVERT_VECTOR_F32(IN, OUT) \
if (IN->type.type != PS_TYPE_F32) { \
    psLogMsg(__func__, PS_LOG_WARN, \
             "Input vector has incorrect type (%d).\n", \
             IN->type); \
    OUT = psVectorAlloc(IN->n, PS_TYPE_F32); \
    \
    if (IN->type.type == PS_TYPE_F64) { \
        for (int i=0;i<IN->n;i++) { \
            OUT->data.F32[i] = (float) (IN->data.F64[i]); \
        } \
    }\
    else { \
        psAbort(__func__, "Wrong type.\n"); \
    } \
} else { \
    OUT = (psVector *) IN; \
}

#define PS_CONVERT_VECTOR_F64(IN, OUT) \
if (IN->type.type != PS_TYPE_F64) { \
    psLogMsg(__func__, PS_LOG_WARN, \
             "Input vector has incorrect type (%d).\n", \
             IN->type); \
    OUT = psVectorAlloc(IN->n, PS_TYPE_F64); \
    \
    if (IN->type.type == PS_TYPE_F32) { \
        for (int i=0;i<IN->n;i++) { \
            OUT->data.F64[i] = (float) (IN->data.F32[i]); \
        } \
    }\
    else { \
        psAbort(__func__, "Wrong type.\n"); \
    } \
} else { \
    OUT = (psVector *) IN; \
}

/*****************************************************************************/
/* 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);

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

// None

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

// None

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

/*****************************************************************************
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(int maxChebyPoly)
{
    psPolynomial1D **chebPolys = NULL;
    int i = 0;
    int j = 0;

    chebPolys = (psPolynomial1D **) psAlloc(maxChebyPoly * sizeof(psPolynomial1D *));
    for (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;
    chebPolys[1]->coeff[1] = 1;
    for (i = 2; i < maxChebyPoly; i++) {
        for (j = 0; j < chebPolys[i - 1]->n; j++) {
            chebPolys[i]->coeff[j + 1] = 2 * chebPolys[i - 1]->coeff[j];
        }
        for (j = 0; j < chebPolys[i - 2]->n; j++) {
            chebPolys[i]->coeff[j] -= chebPolys[i - 2]->coeff[j];
        }
    }

    return (chebPolys);
}

/*****************************************************************************/
/*  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]
 *****************************************************************************/
float psGaussian(float x, float mean, float sigma, bool normal)
{
    float tmp = 1.0;

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

    if (normal == true) {
        #ifdef DARWIN
        tmp = 1.0 / (float)sqrt(2.0 * M_PI * (sigma * sigma));
        #else

        tmp = 1.0 / sqrtf(2.0 * M_PI * (sigma * sigma));
        #endif

    }

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

/*****************************************************************************
    psGaussianDev()
 This 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.
 
 NOTE: XXX: There is no way to seed the random generator.
 *****************************************************************************/
psVector* psGaussianDev(float mean, float sigma, int Npts)
{
    psVector* gauss = NULL;
    const gsl_rng_type *T = NULL;
    gsl_rng *r = NULL;
    int 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);
    }

    // NOTE: Should I free r as well?
    return(gauss);
}

/*****************************************************************************
    This routine must allocate memory for the polynomial structures.
 *****************************************************************************/
psPolynomial1D* psPolynomial1DAlloc(int n,
                                    psPolynomialType type)
{
    int i = 0;
    psPolynomial1D* newPoly = NULL;

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

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

    return(newPoly);
}

psPolynomial2D* psPolynomial2DAlloc(int nX, int nY,
                                    psPolynomialType type)
{
    int x = 0;
    int 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 = (float **)psAlloc(nX * sizeof(float *));
    newPoly->coeffErr = (float **)psAlloc(nX * sizeof(float *));
    newPoly->mask = (char **)psAlloc(nX * sizeof(char *));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (float *)psAlloc(nY * sizeof(float));
        newPoly->coeffErr[x] = (float *)psAlloc(nY * sizeof(float));
        newPoly->mask[x] = (char *)psAlloc(nY * sizeof(char));
    }
    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(int nX, int nY, int nZ,
                                    psPolynomialType type)
{
    int x = 0;
    int y = 0;
    int 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 = (float ***)psAlloc(nX * sizeof(float **));
    newPoly->coeffErr = (float ***)psAlloc(nX * sizeof(float **));
    newPoly->mask = (char ***)psAlloc(nX * sizeof(char **));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (float **)psAlloc(nY * sizeof(float *));
        newPoly->coeffErr[x] = (float **)psAlloc(nY * sizeof(float *));
        newPoly->mask[x] = (char **)psAlloc(nY * sizeof(char *));
        for (y = 0; y < nY; y++) {
            newPoly->coeff[x][y] = (float *)psAlloc(nZ * sizeof(float));
            newPoly->coeffErr[x][y] = (float *)psAlloc(nZ * sizeof(float));
            newPoly->mask[x][y] = (char *)psAlloc(nZ * sizeof(char));
        }
    }
    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(int nW, int nX, int nY, int nZ,
                                    psPolynomialType type)
{
    int w = 0;
    int x = 0;
    int y = 0;
    int 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 = (float ****)psAlloc(nW * sizeof(float ***));
    newPoly->coeffErr = (float ****)psAlloc(nW * sizeof(float ***));
    newPoly->mask = (char ****)psAlloc(nW * sizeof(char ***));
    for (w = 0; w < nW; w++) {
        newPoly->coeff[w] = (float ***)psAlloc(nX * sizeof(float **));
        newPoly->coeffErr[w] = (float ***)psAlloc(nX * sizeof(float **));
        newPoly->mask[w] = (char ***)psAlloc(nX * sizeof(char **));
        for (x = 0; x < nX; x++) {
            newPoly->coeff[w][x] = (float **)psAlloc(nY * sizeof(float *));
            newPoly->coeffErr[w][x] = (float **)psAlloc(nY * sizeof(float *));
            newPoly->mask[w][x] = (char **)psAlloc(nY * sizeof(char *));
            for (y = 0; y < nY; y++) {
                newPoly->coeff[w][x][y] = (float *)psAlloc(nZ * sizeof(float));
                newPoly->coeffErr[w][x][y] = (float *)psAlloc(nZ * sizeof(float));
                newPoly->mask[w][x][y] = (char *)psAlloc(nZ * sizeof(char));
            }
        }
    }
    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);
}

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

static void polynomial2DFree(psPolynomial2D* myPoly)
{
    int 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)
{
    int x = 0;
    int 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)
{
    int w = 0;
    int x = 0;
    int 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);
}

/*****************************************************************************
    Polynomial coefficients will be accessed in [w][x][y][z] fashion.
 
    XXX: Should the "coeffErr[]" should be used as well?
 *****************************************************************************/
float p_psOrdPolynomial1DEval(float x, const psPolynomial1D* myPoly)
{
    int loop_x = 0;
    float polySum = 0.0;
    float xSum = 1.0;

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


    if (NULL == myPoly) {
        psAbort(__func__, "psPolynomial1DEval(): myPoly is NULL\n");
    }

    // NOTE: Do we want to flag this case?
    if (myPoly->n == 0) {
        return(1.0);
    }

    if (NULL == myPoly->coeff) {
        psAbort(__func__, "psPolynomial1DEval(): myPoly->coeff is NULL\n");
    }

    for (loop_x = 0; loop_x < myPoly->n; loop_x++) {
        if (myPoly->mask[loop_x] == 0) {
            psTrace(".psLib.dataManip.psFunctions.p_psOrdPolynomial1DEval", 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?
float p_psChebPolynomial1DEval(float x, const psPolynomial1D* myPoly)
{
    psVector *d;
    int n;
    int i;
    float tmp;

    n = myPoly->n;
    d = psVectorAlloc(n, PS_TYPE_F32);
    d->data.F32[n-1] = myPoly->coeff[n-1];
    d->data.F32[n-2] = (2.0 * x * d->data.F32[n-1]) + 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]) +
                         (myPoly->coeff[i]);
    }

    tmp = (x * d->data.F32[1]) -
          (d->data.F32[2]) +
          (0.5 * myPoly->coeff[0]);

    psFree(d);
    return(tmp);
    /*

    int n;
    int i;
    float 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]));
        //            printf("HMMM: psPolynomial1DEval(%f, chebPolys[%d]) is %f\n", x, i, psPolynomial1DEval(x, chebPolys[i]));
    }
    tmp-= (myPoly->coeff[0]/2.0);


    return(tmp);
    */
}

float psPolynomial1DEval(float x, const psPolynomial1D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psOrdPolynomial1DEval(x, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psChebPolynomial1DEval(x, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psPolynomial1DEvalVector(const psVector *x,
                                   const psPolynomial1D *myPoly)
{
    psVector *tmp;
    psVector *myX;
    int i;

    PS_CONVERT_VECTOR_F32(x, myX);

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

    if (x->type.type != PS_TYPE_F32) {
        psFree(myX);
    }
    return(tmp);
}


float p_psOrdPolynomial2DEval(float x, float y, const psPolynomial2D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    float polySum = 0.0;
    float xSum = 1.0;
    float 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);
}

float p_psChebPolynomial2DEval(float x, float y, const psPolynomial2D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    int i = 0;
    float polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    int 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(x, chebPolys[loop_x]) *
                           psPolynomial1DEval(y, chebPolys[loop_y]);
            }
        }
    }
    for (i=0;i<maxChebyPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

float psPolynomial2DEval(float x, float y, const psPolynomial2D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psOrdPolynomial2DEval(x, y, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psChebPolynomial2DEval(x, y, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}


psVector *psPolynomial2DEvalVector(const psVector *x,
                                   const psVector *y,
                                   const psPolynomial2D *myPoly)
{
    psVector *tmp;
    psVector *myX;
    psVector *myY;
    int i;
    int vecLen=x->n;

    PS_CONVERT_VECTOR_F32(x, myX);
    PS_CONVERT_VECTOR_F32(y, myY);
    vecLen=x->n;
    if (y->n < vecLen) {
        vecLen = y->n;
    }

    tmp = psVectorAlloc(vecLen, PS_TYPE_F32);
    for (i=0;i<vecLen;i++) {
        tmp->data.F32[i] = psPolynomial2DEval(myX->data.F32[i],
                                              myY->data.F32[i],
                                              myPoly);
    }

    if (x->type.type != PS_TYPE_F32) {
        psFree(myX);
    }
    if (y->type.type != PS_TYPE_F32) {
        psFree(myY);
    }
    return(tmp);
}



float p_psOrdPolynomial3DEval(float x, float y, float z, const psPolynomial3D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    float polySum = 0.0;
    float xSum = 1.0;
    float ySum = 1.0;
    float 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);
}

float p_psChebPolynomial3DEval(float x, float y, float z, const psPolynomial3D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    int i = 0;
    float polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    int 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(x, chebPolys[loop_x]) *
                               psPolynomial1DEval(y, chebPolys[loop_y]) *
                               psPolynomial1DEval(z, chebPolys[loop_z]);
                }
            }
        }
    }

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

float psPolynomial3DEval(float x, float y, float z, const psPolynomial3D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psOrdPolynomial3DEval(x, y, z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psChebPolynomial3DEval(x, y, z, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psPolynomial3DEvalVector(const psVector *x,
                                   const psVector *y,
                                   const psVector *z,
                                   const psPolynomial3D *myPoly)
{
    psVector *tmp;
    psVector *myX;
    psVector *myY;
    psVector *myZ;
    int i;
    int vecLen=x->n;

    PS_CONVERT_VECTOR_F32(x, myX);
    PS_CONVERT_VECTOR_F32(y, myY);
    PS_CONVERT_VECTOR_F32(z, myZ);
    vecLen=x->n;
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    tmp = psVectorAlloc(vecLen, PS_TYPE_F32);
    for (i=0;i<vecLen;i++) {
        tmp->data.F32[i] = psPolynomial3DEval(myX->data.F32[i],
                                              myY->data.F32[i],
                                              myZ->data.F32[i],
                                              myPoly);
    }

    if (x->type.type != PS_TYPE_F32) {
        psFree(myX);
    }
    if (y->type.type != PS_TYPE_F32) {
        psFree(myY);
    }
    if (z->type.type != PS_TYPE_F32) {
        psFree(myZ);
    }
    return(tmp);
}






float p_psOrdPolynomial4DEval(float w, float x, float y, float z, const psPolynomial4D* myPoly)
{
    int loop_w = 0;
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    float polySum = 0.0;
    float wSum = 1.0;
    float xSum = 1.0;
    float ySum = 1.0;
    float 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);
}

float p_psChebPolynomial4DEval(float w, float x, float y, float z, const psPolynomial4D* myPoly)
{
    int loop_w = 0;
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    int i = 0;
    float polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    int 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(w, chebPolys[loop_w]) *
                                   psPolynomial1DEval(x, chebPolys[loop_x]) *
                                   psPolynomial1DEval(y, chebPolys[loop_y]) *
                                   psPolynomial1DEval(z, chebPolys[loop_z]);
                    }
                }
            }
        }
    }

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

float psPolynomial4DEval(float w, float x, float y, float z, const psPolynomial4D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psOrdPolynomial4DEval(w,x,y,z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psChebPolynomial4DEval(w,x,y,z, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psPolynomial4DEvalVector(const psVector *w,
                                   const psVector *x,
                                   const psVector *y,
                                   const psVector *z,
                                   const psPolynomial4D *myPoly)
{
    psVector *tmp;
    psVector *myW;
    psVector *myX;
    psVector *myY;
    psVector *myZ;
    int i;
    int vecLen=x->n;

    PS_CONVERT_VECTOR_F32(w, myW);
    PS_CONVERT_VECTOR_F32(x, myX);
    PS_CONVERT_VECTOR_F32(y, myY);
    PS_CONVERT_VECTOR_F32(z, myZ);
    vecLen=w->n;
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (x->n < vecLen) {
        vecLen = x->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    tmp = psVectorAlloc(vecLen, PS_TYPE_F32);
    for (i=0;i<vecLen;i++) {
        tmp->data.F32[i] = psPolynomial4DEval(myW->data.F32[i],
                                              myX->data.F32[i],
                                              myY->data.F32[i],
                                              myZ->data.F32[i],
                                              myPoly);
    }

    if (w->type.type != PS_TYPE_F32) {
        psFree(myW);
    }
    if (x->type.type != PS_TYPE_F32) {
        psFree(myX);
    }
    if (y->type.type != PS_TYPE_F32) {
        psFree(myY);
    }
    if (z->type.type != PS_TYPE_F32) {
        psFree(myZ);
    }
    return(tmp);
}





psDPolynomial1D* psDPolynomial1DAlloc(int n,
                                      psPolynomialType type)
{
    int i = 0;
    psDPolynomial1D* newPoly = NULL;

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

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

    return(newPoly);
}

psDPolynomial2D* psDPolynomial2DAlloc(int nX, int nY,
                                      psPolynomialType type)
{
    int x = 0;
    int 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 = (double **)psAlloc(nX * sizeof(double *));
    newPoly->coeffErr = (double **)psAlloc(nX * sizeof(double *));
    newPoly->mask = (char **)psAlloc(nX * sizeof(char *));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (double *)psAlloc(nY * sizeof(double));
        newPoly->coeffErr[x] = (double *)psAlloc(nY * sizeof(double));
        newPoly->mask[x] = (char *)psAlloc(nY * sizeof(char));
    }
    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(int nX, int nY, int nZ,
                                      psPolynomialType type)
{
    int x = 0;
    int y = 0;
    int 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 = (double ***)psAlloc(nX * sizeof(double **));
    newPoly->coeffErr = (double ***)psAlloc(nX * sizeof(double **));
    newPoly->mask = (char ***)psAlloc(nX * sizeof(char **));
    for (x = 0; x < nX; x++) {
        newPoly->coeff[x] = (double **)psAlloc(nY * sizeof(double *));
        newPoly->coeffErr[x] = (double **)psAlloc(nY * sizeof(double *));
        newPoly->mask[x] = (char **)psAlloc(nY * sizeof(char *));
        for (y = 0; y < nY; y++) {
            newPoly->coeff[x][y] = (double *)psAlloc(nZ * sizeof(double));
            newPoly->coeffErr[x][y] = (double *)psAlloc(nZ * sizeof(double));
            newPoly->mask[x][y] = (char *)psAlloc(nZ * sizeof(char));
        }
    }
    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(int nW, int nX, int nY, int nZ,
                                      psPolynomialType type)
{
    int w = 0;
    int x = 0;
    int y = 0;
    int 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 = (double ****)psAlloc(nW * sizeof(double ***));
    newPoly->coeffErr = (double ****)psAlloc(nW * sizeof(double ***));
    newPoly->mask = (char ****)psAlloc(nW * sizeof(char ***));
    for (w = 0; w < nW; w++) {
        newPoly->coeff[w] = (double ***)psAlloc(nX * sizeof(double **));
        newPoly->coeffErr[w] = (double ***)psAlloc(nX * sizeof(double **));
        newPoly->mask[w] = (char ***)psAlloc(nX * sizeof(char **));
        for (x = 0; x < nX; x++) {
            newPoly->coeff[w][x] = (double **)psAlloc(nY * sizeof(double *));
            newPoly->coeffErr[w][x] = (double **)psAlloc(nY * sizeof(double *));
            newPoly->mask[w][x] = (char **)psAlloc(nY * sizeof(char *));
            for (y = 0; y < nY; y++) {
                newPoly->coeff[w][x][y] = (double *)psAlloc(nZ * sizeof(double));
                newPoly->coeffErr[w][x][y] = (double *)psAlloc(nZ * sizeof(double));
                newPoly->mask[w][x][y] = (char *)psAlloc(nZ * sizeof(char));
            }
        }
    }
    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);
}

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

static void dPolynomial2DFree(psDPolynomial2D* myPoly)
{
    int 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)
{
    int x = 0;
    int 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)
{
    int w = 0;
    int x = 0;
    int 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);
}

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

    // NOTE: Do we want to flag this case?
    if (myPoly->n == 0) {
        return(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?
double p_psDChebPolynomial1DEval(double x, const psDPolynomial1D* myPoly)
{
    psVector *d;
    int n;
    int i;
    double tmp;

    n = myPoly->n;
    d = psVectorAlloc(n, PS_TYPE_F64);
    d->data.F64[n-1] = myPoly->coeff[n-1];
    d->data.F64[n-2] = (2.0 * x * d->data.F64[n-1]) + 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]) +
                         (myPoly->coeff[i]);
    }

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

    psFree(d);
    return(tmp);
}

double psDPolynomial1DEval(double x, const psDPolynomial1D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psDOrdPolynomial1DEval(x, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psDChebPolynomial1DEval(x, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psDPolynomial1DEvalVector(const psVector *x,
                                    const psDPolynomial1D *myPoly)
{
    psVector *tmp;
    psVector *myX;
    int i;

    PS_CONVERT_VECTOR_F64(x, myX);

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

    if (x->type.type != PS_TYPE_F64) {
        psFree(myX);
    }
    return(tmp);
}



double p_psDOrdPolynomial2DEval(double x, double y, const psDPolynomial2D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    double polySum = 0.0;
    double xSum = 1.0;
    double 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);
}

double p_psDChebPolynomial2DEval(double x, double y, const psDPolynomial2D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    int i = 0;
    double polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    int 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(x, chebPolys[loop_x]) *
                           psPolynomial1DEval(y, chebPolys[loop_y]);
            }
        }
    }

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

double psDPolynomial2DEval(double x, double y, const psDPolynomial2D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psDOrdPolynomial2DEval(x, y, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psDChebPolynomial2DEval(x, y, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psDPolynomial2DEvalVector(const psVector *x,
                                    const psVector *y,
                                    const psDPolynomial2D *myPoly)
{
    psVector *tmp;
    psVector *myX;
    psVector *myY;
    int i;
    int vecLen=x->n;

    PS_CONVERT_VECTOR_F64(x, myX);
    PS_CONVERT_VECTOR_F64(y, myY);
    vecLen=x->n;
    if (y->n < vecLen) {
        vecLen = y->n;
    }

    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);
    for (i=0;i<vecLen;i++) {
        tmp->data.F64[i] = psDPolynomial2DEval(myX->data.F64[i],
                                               myY->data.F64[i],
                                               myPoly);
    }

    if (x->type.type != PS_TYPE_F64) {
        psFree(myX);
    }
    if (y->type.type != PS_TYPE_F64) {
        psFree(myY);
    }
    return(tmp);
}



double p_psDOrdPolynomial3DEval(double x, double y, double z, const psDPolynomial3D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    double polySum = 0.0;
    double xSum = 1.0;
    double ySum = 1.0;
    double 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);
}

double p_psDChebPolynomial3DEval(double x, double y, double z, const psDPolynomial3D* myPoly)
{
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    int i = 0;
    double polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    int 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(x, chebPolys[loop_x]) *
                               psPolynomial1DEval(y, chebPolys[loop_y]) *
                               psPolynomial1DEval(z, chebPolys[loop_z]);
                }
            }
        }
    }

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

double psDPolynomial3DEval(double x, double y, double z, const psDPolynomial3D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psDOrdPolynomial3DEval(x, y, z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psDChebPolynomial3DEval(x, y, z, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psDPolynomial3DEvalVector(const psVector *x,
                                    const psVector *y,
                                    const psVector *z,
                                    const psDPolynomial3D *myPoly)
{
    psVector *tmp;
    psVector *myX;
    psVector *myY;
    psVector *myZ;
    int i;
    int vecLen=x->n;

    PS_CONVERT_VECTOR_F64(x, myX);
    PS_CONVERT_VECTOR_F64(y, myY);
    PS_CONVERT_VECTOR_F64(z, myZ);
    vecLen=x->n;
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);
    for (i=0;i<vecLen;i++) {
        tmp->data.F64[i] = psDPolynomial3DEval(myX->data.F64[i],
                                               myY->data.F64[i],
                                               myZ->data.F64[i],
                                               myPoly);
    }

    if (x->type.type != PS_TYPE_F64) {
        psFree(myX);
    }
    if (y->type.type != PS_TYPE_F64) {
        psFree(myY);
    }
    if (z->type.type != PS_TYPE_F64) {
        psFree(myZ);
    }
    return(tmp);
}








double p_psDOrdPolynomial4DEval(double w, double x, double y, double z, const psDPolynomial4D* myPoly)
{
    int loop_w = 0;
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    double polySum = 0.0;
    double wSum = 1.0;
    double xSum = 1.0;
    double ySum = 1.0;
    double 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);
}

double p_psDChebPolynomial4DEval(double w, double x, double y, double z, const psDPolynomial4D* myPoly)
{
    int loop_w = 0;
    int loop_x = 0;
    int loop_y = 0;
    int loop_z = 0;
    int i = 0;
    double polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    int 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(w, chebPolys[loop_w]) *
                                   psPolynomial1DEval(x, chebPolys[loop_x]) *
                                   psPolynomial1DEval(y, chebPolys[loop_y]) *
                                   psPolynomial1DEval(z, chebPolys[loop_z]);
                    }
                }
            }
        }
    }

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

double psDPolynomial4DEval(double w, double x, double y, double z, const psDPolynomial4D* myPoly)
{
    if (myPoly->type == PS_POLYNOMIAL_ORD) {
        return(p_psDOrdPolynomial4DEval(w,x,y,z, myPoly));
    } else if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        return(p_psDChebPolynomial4DEval(w,x,y,z, myPoly));
    } else {
        psAbort(__func__, "Unknown polynomial type 0x%x\n", myPoly->type);
    }
    return(0.0);
}

psVector *psDPolynomial4DEvalVector(const psVector *w,
                                    const psVector *x,
                                    const psVector *y,
                                    const psVector *z,
                                    const psDPolynomial4D *myPoly)
{
    psVector *tmp;
    psVector *myW;
    psVector *myX;
    psVector *myY;
    psVector *myZ;
    int i;
    int vecLen=x->n;

    PS_CONVERT_VECTOR_F64(w, myW);
    PS_CONVERT_VECTOR_F64(x, myX);
    PS_CONVERT_VECTOR_F64(y, myY);
    PS_CONVERT_VECTOR_F64(z, myZ);
    vecLen=w->n;
    if (y->n < vecLen) {
        vecLen = y->n;
    }
    if (x->n < vecLen) {
        vecLen = x->n;
    }
    if (z->n < vecLen) {
        vecLen = z->n;
    }

    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);
    for (i=0;i<vecLen;i++) {
        tmp->data.F64[i] = psDPolynomial4DEval(myW->data.F64[i],
                                               myX->data.F64[i],
                                               myY->data.F64[i],
                                               myZ->data.F64[i],
                                               myPoly);
    }

    if (w->type.type != PS_TYPE_F64) {
        psFree(myW);
    }
    if (x->type.type != PS_TYPE_F64) {
        psFree(myX);
    }
    if (y->type.type != PS_TYPE_F64) {
        psFree(myY);
    }
    if (z->type.type != PS_TYPE_F64) {
        psFree(myZ);
    }
    return(tmp);
}




//typedef struct {
//    int n;
//    psPolynomial1D **spline;
//    float *p_psDeriv2;
//    float *domains;
//} psSpline1D;

/*****************************************************************************
    NOTE: "n" specifies the number of spline polynomials.  Therefore, there
    must exist n+1 points in "domains".
 
XXX: Ensure that domain[i+1] != domain[i]
 *****************************************************************************/
psSpline1D *psSpline1DAlloc(int numSplines,
                            int order,
                            float min,
                            float max)
{
    psSpline1D *tmp = NULL;
    int i;
    float tmpDomain;
    float width;

    if (fabs(max-min) < FLT_EPSILON) {
        psLogMsg(__func__, PS_LOG_WARN, "min and max are the same.\n");
        return(NULL);
    }

    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->domains = (float *) psAlloc((numSplines+1) * sizeof(float));
    width = (max - min) / ((float) numSplines);

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

    return(tmp);
}

// XXX: Have Robert put the dealocator in the memory file.
int p_psSpline1DFree(psSpline1D *tmpSpline)
{
    int i;

    if (tmpSpline == NULL) {
        return(0);
    }

    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->domains);
    psFree(tmpSpline);

    return(0);
}

/*****************************************************************************
 *****************************************************************************/
psSpline1D *psSpline1DAllocGeneric(const psVector *bounds,
                                   int order)
{
    psSpline1D *tmp = NULL;
    int i;
    int 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);
    }

    tmp->domains = (float *) psAlloc((bounds->n) * sizeof(float));

    for (i=0;i<bounds->n;i++) {
        (tmp->domains)[i] = bounds->data.F32[i];
        if (i<(bounds->n-1)) {
            if (FLT_EPSILON < fabs(bounds->data.F32[i+1]-bounds->data.F32[i])) {
                // XXX: psErrorMsg(data points must be distinct)
            }
        }
    }

    return(tmp);
}

/*****************************************************************************
p_psVectorBinDisectF32(): This is a private function which takes as input a
vector of floating point data as well as a single floating point values.
The input vector values are assumed to be non-decreasing (v[i-1] <= v[j] for
all j>=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).
 
XXX: Macro this for a few different types.
 
XXX: name since we don't take psVectors as input.
 *****************************************************************************/
int p_psVectorBinDisectF32(float *bins,
                           int numBins,
                           float x)
{
    int min;
    int max;
    int mid;

    psTrace(".psLib.dataManip.psFunctions.p_psVectorBinDisectF32", 4,
            "---- Calling p_psVectorBinDisectF32(%f)\n", x);

    if (x < bins[0]) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "p_psVectorBinDisectF32(): ordinate %f is outside vector range (%f - %f).",
                 x, bins[0], bins[numBins-1]);
        return(-2);
    }

    if (x > bins[numBins-1]) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "p_psVectorBinDisectF32(): ordinate %f is outside vector range (%f - %f).",
                 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.p_psVectorBinDisectF32", 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.p_psVectorBinDisectF32", 4,
                    "---- Exiting p_psVectorBinDisectF32(): 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.p_psVectorBinDisectF32", 4,
            "---- Exiting p_psVectorBinDisectF32(): bin %d\n", min);
    return(min);
}

/*****************************************************************************
p_psVectorBinDisectS32(): integer version of above.
 *****************************************************************************/
int p_psVectorBinDisectS32(int *bins,
                           int numBins,
                           int x)
{
    int min;
    int max;
    int mid;

    psTrace(".psLib.dataManip.psFunctions.p_psVectorBinDisectS32", 4,
            "---- Calling p_psVectorBinDisectS32(%f)\n", x);

    if ((x < bins[0]) ||
            (x > bins[numBins-1])) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "p_psVectorBinDisectS32(): ordinate %f is outside vector range (%f - %f).",
                 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.p_psVectorBinDisectS32", 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.p_psVectorBinDisectS32", 4,
                    "---- Exiting p_psVectorBinDisectS32(): bin %d\n", min);
            return(min);
        } else if (x < bins[mid]) {
            max = mid-1;
        } else {
            min = mid;
        }
        mid = ((max+1)+min)/2;
    }

    psTrace(".psLib.dataManip.psFunctions.p_psVectorBinDisectS32", 4,
            "---- Exiting p_psVectorBinDisectS32(): bin %d\n", min);
    return(min);
}

/*****************************************************************************
p_psVectorBinDisect(): A wrapper to the above p_psVectorBinDisect().
 *****************************************************************************/
int p_psVectorBinDisect(psVector *bins,
                        psScalar *x)
{
    if (x->type.type != bins->type.type) {
        psError(__func__, "x->type.type != bins->type.type");
        return(-2);
    }

    if (x->type.type == PS_TYPE_S32) {
        return(p_psVectorBinDisectS32(bins->data.S32, bins->n, x->data.S32));
    } else if (x->type.type == PS_TYPE_F32) {
        return(p_psVectorBinDisectF32(bins->data.F32, bins->n, x->data.F32));
    } else {
        psError(__func__, "Unallowable data type.");
        return(-2);
    }
    return(-1);
}

/*****************************************************************************
p_psInterpolate1D(): 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?
 *****************************************************************************/
float p_ps1DFullInterpolateF32(float *domain,
                               float *range,
                               int n,
                               float x)
{
    int i;
    int m;
    static psVector *p = NULL;
    p = psVectorRecycle(p, n, PS_TYPE_F32);
    p_psMemSetPersistent(p, true);
    p_psMemSetPersistent(p->data.F32, true);
    /*
        psVector *p = psVectorAlloc(n, PS_TYPE_F32);
        float tmp;
    */

    psTrace(".psLib.dataManip.psFunctions.p_ps1DFullInterpolateF32", 4,
            "---- p_ps1DFullInterpolateF32() begin (%d-order at x=%f) (%d data points)----\n", n-1, x, n);

    for (i=0;i<n;i++) {
        psTrace(".psLib.dataManip.psFunctions.p_ps1DFullInterpolateF32", 6,
                "domain/range is (%f %f)\n", domain[i], range[i]);
    }

    for (i=0;i<n;i++) {
        p->data.F32[i] = range[i];
        psTrace(".psLib.dataManip.psFunctions.p_ps1DFullInterpolateF32", 6,
                "p->data.F32[%d] is %f\n", i, p->data.F32[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.F32[i] = (((x-domain[i+m]) * p->data.F32[i]) +
                              ((domain[i]-x) * p->data.F32[i+1])) /
                             (domain[i] - domain[i+m]);
            //printf("((%f-%f * %f) + (%f-%f * %f)) / (%f - %f)\n", x, domain[i+m], p->data.F32[i], domain[i], x, p->data.F32[i+1], domain[i], domain[i+m]);
            psTrace(".psLib.dataManip.psFunctions.p_ps1DFullInterpolateF32", 6,
                    "p->data.F32[%d] is %f\n", i, p->data.F32[i]);
        }
    }
    psTrace(".psLib.dataManip.psFunctions.p_ps1DFullInterpolateF32", 4,
            "---- p_ps1DFullInterpolateF32() end ----\n");

    /*
        tmp = p->data.F32[0];
        psFree(p);
        return(tmp);
    */
    return(p->data.F32[0]);
}


/*****************************************************************************
p_ps1DInterpolateF32(): this is the base 1-D flat memory routine to perform
LaGrange interpolation.
 *****************************************************************************/
float p_ps1DInterpolateF32(float *domain,
                           float *range,
                           int n,
                           int order,
                           float x)
{
    int binNum;
    int numIntPoints = order+1;
    int origin;

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

    binNum = p_psVectorBinDisectF32(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.p_ps1DInterpolateF32", 4,
            "---- p_ps1DInterpolateF32() end ----\n");
    return(p_ps1DFullInterpolateF32(&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: nobody asked for us to generate this routine.
 
XXX: add another psScalar argument for the result.
 
XXX: The VectorCopy routines seg fault when I declare range32 as static.
 *****************************************************************************/
psScalar *p_psVectorInterpolate(psVector *domain,
                                psVector *range,
                                int order,
                                psScalar *x)
{
    psVector *range32 = NULL;
    psVector *domain32 = NULL;
    psTrace(".psLib.dataManip.psFunctions.p_psVectorInterpolate", 4,
            "---- p_psVectorInterpolate() begin ----\n");

    if ((domain->type.type != range->type.type) ||
            (domain->type.type != x->type.type)) {
        // XXX psError
        printf("p_psVectorInterpolate(): domain->type.type != range->type.type != x->type.type\n");
        printf("%d %d %d\n", domain->type.type, range->type.type, x->type.type);
        exit(1);
    }
    if (domain->n != range->n) {
        // XXX psError
        printf("domain->n != range->n\n");
        exit(1);
    }
    if (order > (domain->n - 1)) {
        // XXX psError: not enough data points for order-order interpolation.
        printf("not enough data points for order-order interpolation.\n");
        exit(1);
    }

    if (x->type.type == PS_TYPE_F32) {
        psTrace(".psLib.dataManip.psFunctions.p_psVectorInterpolate", 4,
                "---- p_psVectorInterpolate() end ----\n");
        return(psScalarAlloc(p_ps1DInterpolateF32(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((double)
                                            p_ps1DInterpolateF32(domain32->data.F32,
                                                                 range32->data.F32,
                                                                 domain32->n,
                                                                 order,
                                                                 (float) 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 {
        // XXX psError: type not supported
        printf("XXX psError: type not supported\n");
    }

    printf("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 domains of the spline data structure
(p_psVectorBinDisectF32()).  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.
 *****************************************************************************/
float psSpline1DEval(const psSpline1D *spline,
                     float x)
{
    int binNum;
    int n;

    n = spline->n;
    binNum = p_psVectorBinDisectF32(spline->domains, (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->domains)[0],
                 (spline->domains)[n-1]);

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

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

// XXX: The spline eval functions require input and output to be F32.
// however the spline fit functions require F32 and F64.
psVector *psSpline1DEvalVector(const psVector *x,
                               const psSpline1D *spline)
{
    int i;
    psVector *tmpVector;

    tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
    if (x->type.type == PS_TYPE_F32) {
        for (i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]);
        }
    } else if (x->type.type == PS_TYPE_F64) {
        for (i=0;i<x->n;i++) {
            tmpVector->data.F32[i] = psSpline1DEval(spline, (float) x->data.F64[i]);
        }
    } else {
        psError(__func__, "Unknown data type.\n");
        return(NULL);
    }

    return(tmpVector);
}
