/** @file  psPolynomial.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.133 $ $Name: not supported by cvs2svn $
*  @date $Date: 2005-11-30 02:00:09 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*
*  XXX: Should the "coeffErr[]" be used as well?  Bug ???.  Ignore coeffErr
*
*  XXX: In the various polyAlloc(n) functions, n is really the order of the
*  polynomial plus 1.  To create a 2nd-order polynomial, n == 3.
*/
/*****************************************************************************/
/*  INCLUDE FILES                                                            */
/*****************************************************************************/
#include <stdio.h>
#include <stdbool.h>
#include <float.h>
#include <math.h>

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

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

/*****************************************************************************/
/* TYPE DEFINITIONS                                                          */
/*****************************************************************************/
static void polynomial1DFree(psPolynomial1D* poly);
static void polynomial2DFree(psPolynomial2D* poly);
static void polynomial3DFree(psPolynomial3D* poly);
static void polynomial4DFree(psPolynomial4D* poly);

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

// None

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

// None

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

bool psMemCheckPolynomial1D(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)polynomial1DFree );
}

bool psMemCheckPolynomial2D(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)polynomial2DFree );
}

bool psMemCheckPolynomial3D(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)polynomial3DFree );
}

bool psMemCheckPolynomial4D(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)polynomial4DFree );
}

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

static void polynomial2DFree(psPolynomial2D* poly)
{
    unsigned int x = 0;

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

static void polynomial3DFree(psPolynomial3D* poly)
{
    unsigned int x = 0;
    unsigned int y = 0;

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

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

static void polynomial4DFree(psPolynomial4D* poly)
{
    unsigned int x = 0;
    unsigned int y = 0;
    unsigned int z = 0;

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

    psFree(poly->coeff);
    psFree(poly->coeffErr);
    psFree(poly->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.
 
XXX: From the poly nOrder/nTerm changem the maxChebyPoly here is still nTerms,
not nOrder.
 *****************************************************************************/
static psPolynomial1D **createChebyshevPolys(psS32 maxChebyPoly)
{
    PS_ASSERT_INT_NONNEGATIVE(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]->nX; j++) {
                chebPolys[i]->coeff[j + 1] = 2 * chebPolys[i - 1]->coeff[j];
            }
            for (psS32 j = 0; j < chebPolys[i - 2]->nX; j++) {
                chebPolys[i]->coeff[j] -= chebPolys[i - 2]->coeff[j];
            }
        }
    } else {
        // XXX: Code this.
        printf("WARNING: %u-order chebyshev polynomials not correctly implemented.\n", maxChebyPoly);
    }

    return (chebPolys);
}

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

    psTrace(".psLib.dataManip.psPolynomial.ordPolynomial1DEval", 4,
            "---- Calling ordPolynomial1DEval(%lf)\n", x);
    psTrace(".psLib.dataManip.psPolynomial.ordPolynomial1DEval", 4,
            "Polynomial order is %u\n", poly->nX);
    for (loop_x = 0; loop_x < poly->nX+1; loop_x++) {
        psTrace(".psLib.dataManip.psPolynomial.ordPolynomial1DEval", 4,
                "Polynomial coeff[%u] is %lf\n", loop_x, poly->coeff[loop_x]);
    }

    for (loop_x = 0; loop_x < poly->nX+1; loop_x++) {
        if (poly->mask[loop_x] == 0) {
            // XXX: If you set the tracelevel to 10 here, and later set the tracelevel to
            // 2 or higher in the test code, you get seg faults.
            psTrace(".psLib.dataManip.psPolynomial.ordPolynomial1DEval", 8,
                    "polysum+= sum*coeff [%lf+= (%lf * %lf)\n", polySum, xSum, poly->coeff[loop_x]);
            polySum += xSum * poly->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 psF64 chebPolynomial1DEval(psF64 x,
                                  const psPolynomial1D* poly)
{
    PS_ASSERT_DOUBLE_WITHIN_RANGE(x, -1.0, 1.0, 0.0);
    // XXX: Create a macro for this in psConstants.h
    if (poly->nX < 1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Error: Chebyshev polynomial is order %u.", poly->nX);
        return(NAN);
    }
    psVector *d;
    // XXX: n should be nTerms here (for clarity).  Or get rid of the variable.
    unsigned int n = 1 + poly->nX;
    unsigned int i;
    psF64 tmp = 0.0;

    // Special case where the Chebyshev poly is constant.
    if (n == 1) {
        if (poly->mask[0] == 0) {
            tmp += poly->coeff[0];
        }
        return(tmp);
    }

    // Special case where the Chebyshev poly is linear.
    if (n == 2) {
        if (poly->mask[0] == 0) {
            tmp+= poly->coeff[0];
        }
        if (poly->mask[1] == 0) {
            tmp+= poly->coeff[1] * x;
        }
        return(tmp);
    }

    // General case where the Chebyshev poly has 2 or more terms.
    d = psVectorAlloc(n, PS_TYPE_F64);
    if(poly->mask[n-1] == 0) {
        d->data.F64[n-1] = poly->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(poly->mask[n-2] == 0) {
        d->data.F64[n-2] += poly->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(poly->mask[i] == 0) {
            d->data.F64[i] += poly->coeff[i];
        }
    }

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

    /* This is old code that does not use Clenshaw's formula.  Get rid of it.

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

    chebPolys = createChebyshevPolys(1 + poly->nX);

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


    return(tmp);
    */
}

static psF64 ordPolynomial2DEval(psF64 x,
                                 psF64 y,
                                 const psPolynomial2D* poly)
{
    PS_ASSERT_POLY_NON_NULL(poly, NAN);

    unsigned int loop_x = 0;
    unsigned int loop_y = 0;
    psF64 polySum = 0.0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;

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

    return(polySum);
}

static psF64 chebPolynomial2DEval(psF64 x,
                                  psF64 y,
                                  const psPolynomial2D* poly)
{
    PS_ASSERT_DOUBLE_WITHIN_RANGE(x, -1.0, 1.0, 0.0);
    PS_ASSERT_DOUBLE_WITHIN_RANGE(y, -1.0, 1.0, 0.0);
    PS_ASSERT_POLY_NON_NULL(poly, NAN);

    unsigned int loop_x = 0;
    unsigned int loop_y = 0;
    unsigned int i = 0;
    psF64 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    unsigned int maxChebyPoly = 0;

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

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

static psF64 ordPolynomial3DEval(psF64 x,
                                 psF64 y,
                                 psF64 z,
                                 const psPolynomial3D* poly)
{
    unsigned int loop_x = 0;
    unsigned int loop_y = 0;
    unsigned int 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 < (1 + poly->nX); loop_x++) {
        ySum = xSum;
        for (loop_y = 0; loop_y < (1 + poly->nY); loop_y++) {
            zSum = ySum;
            for (loop_z = 0; loop_z < (1 + poly->nZ); loop_z++) {
                if (poly->mask[loop_x][loop_y][loop_z] == 0) {
                    polySum += zSum * poly->coeff[loop_x][loop_y][loop_z];
                }
                zSum *= z;
            }
            ySum *= y;
        }
        xSum *= x;
    }

    return(polySum);
}

static psF64 chebPolynomial3DEval(psF64 x,
                                  psF64 y,
                                  psF64 z,
                                  const psPolynomial3D* poly)
{
    PS_ASSERT_DOUBLE_WITHIN_RANGE(x, -1.0, 1.0, 0.0);
    PS_ASSERT_DOUBLE_WITHIN_RANGE(y, -1.0, 1.0, 0.0);
    PS_ASSERT_DOUBLE_WITHIN_RANGE(z, -1.0, 1.0, 0.0);
    unsigned int loop_x = 0;
    unsigned int loop_y = 0;
    unsigned int loop_z = 0;
    unsigned int i = 0;
    psF64 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    unsigned int maxChebyPoly = 0;

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

    for (loop_x = 0; loop_x < (1 + poly->nX); loop_x++) {
        for (loop_y = 0; loop_y < (1 + poly->nY); loop_y++) {
            for (loop_z = 0; loop_z < (1 + poly->nZ); loop_z++) {
                if (poly->mask[loop_x][loop_y][loop_z] == 0) {
                    polySum += poly->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+1;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);
    return(polySum);
}

static psF64 ordPolynomial4DEval(psF64 x,
                                 psF64 y,
                                 psF64 z,
                                 psF64 t,
                                 const psPolynomial4D* poly)
{
    unsigned int loop_x = 0;
    unsigned int loop_y = 0;
    unsigned int loop_z = 0;
    unsigned int loop_t = 0;
    psF64 polySum = 0.0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;
    psF64 zSum = 1.0;
    psF64 tSum = 1.0;

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

    return(polySum);
}

static psF64 chebPolynomial4DEval(psF64 x,
                                  psF64 y,
                                  psF64 z,
                                  psF64 t,
                                  const psPolynomial4D* poly)
{
    PS_ASSERT_DOUBLE_WITHIN_RANGE(x, -1.0, 1.0, 0.0);
    PS_ASSERT_DOUBLE_WITHIN_RANGE(y, -1.0, 1.0, 0.0);
    PS_ASSERT_DOUBLE_WITHIN_RANGE(z, -1.0, 1.0, 0.0);
    PS_ASSERT_DOUBLE_WITHIN_RANGE(t, -1.0, 1.0, 0.0);
    unsigned int loop_x = 0;
    unsigned int loop_y = 0;
    unsigned int loop_z = 0;
    unsigned int loop_t = 0;
    unsigned int i = 0;
    psF64 polySum = 0.0;
    psPolynomial1D* *chebPolys = NULL;
    unsigned int maxChebyPoly = 0;

    // Determine how many Chebyshev polynomials
    // are needed, then create them.
    maxChebyPoly = poly->nX;
    if (poly->nY > maxChebyPoly) {
        maxChebyPoly = poly->nY;
    }
    if (poly->nZ > maxChebyPoly) {
        maxChebyPoly = poly->nZ;
    }
    if (poly->nT > maxChebyPoly) {
        maxChebyPoly = poly->nT;
    }
    // XXX: Add 1 since createChebyshevPolys() takes nTerms, not nOrder.
    chebPolys = createChebyshevPolys(maxChebyPoly + 1);

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

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

/*****************************************************************************/
/*  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)
{
    psF32 tmp = 1.0;

    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);

    if (normal == true) {
        tmp = 1.0 / sqrtf(2.0 * M_PI * (sigma * sigma));
    }

    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    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.
 
XXX: It's possible to have a different seed everytime.  However, for now,
for testability, we use a common seed.
 *****************************************************************************/
#define PS_XXX_GAUSSIAN_SEED 1995
psVector* p_psGaussianDev(psF32 mean,
                          psF32 sigma,
                          unsigned int Npts)
{
    PS_ASSERT_INT_NONNEGATIVE(Npts, NULL);

    //    psRandom *r = psRandomAlloc(PS_RANDOM_TAUS, p_psRandomGetSystemSeed());
    psRandom *r = psRandomAlloc(PS_RANDOM_TAUS, PS_XXX_GAUSSIAN_SEED);
    psVector* gauss = psVectorAlloc(Npts, PS_TYPE_F32);
    for (unsigned int i = 0; i < Npts; i++) {
        gauss->data.F32[i] = mean + p_psRandomGaussian(r, sigma);
    }
    psFree(r);

    return(gauss);
}

/*****************************************************************************
    This routine must allocate memory for the polynomial structures.
    XXX: replaces nterms variables with nOrder.  Lots of potential for bugs
         here.  Probably should create separately named private variables
         in these functions.
 *****************************************************************************/
psPolynomial1D* psPolynomial1DAlloc(unsigned int n,
                                    psPolynomialType type)
{
    PS_ASSERT_INT_NONNEGATIVE(n, NULL);

    unsigned int i = 0;
    psPolynomial1D* newPoly = NULL;

    newPoly = (psPolynomial1D* ) psAlloc(sizeof(psPolynomial1D));
    psMemSetDeallocator(newPoly, (psFreeFunc) polynomial1DFree);

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

    return(newPoly);
}

psPolynomial2D* psPolynomial2DAlloc( unsigned int nX,
                                     unsigned int nY,
                                     psPolynomialType type)
{
    PS_ASSERT_INT_NONNEGATIVE(nX, NULL);
    PS_ASSERT_INT_NONNEGATIVE(nY, NULL);

    unsigned int x = 0;
    unsigned int y = 0;
    psPolynomial2D* newPoly = NULL;

    newPoly = (psPolynomial2D* ) psAlloc(sizeof(psPolynomial2D));
    psMemSetDeallocator(newPoly, (psFreeFunc) polynomial2DFree);

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

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

    return(newPoly);
}

psPolynomial3D* psPolynomial3DAlloc( unsigned int nX,
                                     unsigned int nY,
                                     unsigned int nZ,
                                     psPolynomialType type)
{
    PS_ASSERT_INT_NONNEGATIVE(nX, NULL);
    PS_ASSERT_INT_NONNEGATIVE(nY, NULL);
    PS_ASSERT_INT_NONNEGATIVE(nZ, NULL);

    unsigned int x = 0;
    unsigned int y = 0;
    unsigned int z = 0;
    psPolynomial3D* newPoly = NULL;

    newPoly = (psPolynomial3D* ) psAlloc(sizeof(psPolynomial3D));
    psMemSetDeallocator(newPoly, (psFreeFunc) polynomial3DFree);

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

    newPoly->coeff = psAlloc((nX + 1) * sizeof(psF64 **));
    newPoly->coeffErr = psAlloc((nX + 1) * sizeof(psF64 **));
    newPoly->mask = (psMaskType ***)psAlloc((nX + 1) * sizeof(psMaskType **));
    for (x = 0; x < (1 + nX); x++) {
        newPoly->coeff[x] = psAlloc((nY + 1) * sizeof(psF64 *));
        newPoly->coeffErr[x] = psAlloc((nY + 1) * sizeof(psF64 *));
        newPoly->mask[x] = (psMaskType **)psAlloc((nY + 1) * sizeof(psMaskType *));
        for (y = 0; y < (nY + 1); y++) {
            newPoly->coeff[x][y] = psAlloc((nZ + 1) * sizeof(psF64));
            newPoly->coeffErr[x][y] = psAlloc((nZ + 1) * sizeof(psF64));
            newPoly->mask[x][y] = (psMaskType *)psAlloc((nZ + 1) * sizeof(psMaskType));
        }
    }
    for (x = 0; x < (nX + 1); x++) {
        for (y = 0; y < (nY + 1); y++) {
            for (z = 0; z < (nZ + 1); 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( unsigned int nX,
                                     unsigned int nY,
                                     unsigned int nZ,
                                     unsigned int nT,
                                     psPolynomialType type)
{
    PS_ASSERT_INT_NONNEGATIVE(nX, NULL);
    PS_ASSERT_INT_NONNEGATIVE(nY, NULL);
    PS_ASSERT_INT_NONNEGATIVE(nZ, NULL);
    PS_ASSERT_INT_NONNEGATIVE(nT, NULL);

    unsigned int x = 0;
    unsigned int y = 0;
    unsigned int z = 0;
    unsigned int t = 0;
    psPolynomial4D* newPoly = NULL;

    newPoly = (psPolynomial4D* ) psAlloc(sizeof(psPolynomial4D));
    psMemSetDeallocator(newPoly, (psFreeFunc) polynomial4DFree);

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

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

    return(newPoly);
}

psF64 psPolynomial1DEval(const psPolynomial1D* poly,
                         psF64 x)
{
    PS_ASSERT_POLY_NON_NULL(poly, NAN);

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

// this function must accept F32 and F64 input x vectors
psVector *psPolynomial1DEvalVector(const psPolynomial1D *poly,
                                   const psVector *x)
{
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);

    psVector *tmp;

    switch (x->type.type) {
    case PS_TYPE_F64:
        tmp = psVectorAlloc(x->n, PS_TYPE_F64);
        for (unsigned int i=0;i<x->n;i++) {
            tmp->data.F64[i] = psPolynomial1DEval(poly, x->data.F64[i]);
        }
        break;
    case PS_TYPE_F32:
        tmp = psVectorAlloc(x->n, PS_TYPE_F32);
        for (unsigned int i=0;i<x->n;i++) {
            tmp->data.F32[i] = psPolynomial1DEval(poly, x->data.F32[i]);
        }
        break;
    default:
        psError(PS_ERR_UNKNOWN, false, "invalid input data type.\n");
        return (NULL);
    }
    return(tmp);
}

psF64 psPolynomial2DEval(const psPolynomial2D* poly,
                         psF64 x,
                         psF64 y)
{
    PS_ASSERT_POLY_NON_NULL(poly, NAN);

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

// this function must support input data types of F32 and F64
// all input vectors data types must match (all F32 or all F64)
psVector *psPolynomial2DEvalVector(const psPolynomial2D *poly,
                                   const psVector *x,
                                   const psVector *y)

{
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);

    psVector *tmp;
    unsigned int 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;
    }

    switch (x->type.type) {
    case PS_TYPE_F32:
        if (y->type.type != x->type.type) {
            psError(PS_ERR_UNKNOWN, true, "type mismatch in data vectors");
            return (NULL);
        }

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

        // Evaluate the polynomial at the specified points
        for (unsigned int i=0; i<vecLen; i++) {
            tmp->data.F32[i] = psPolynomial2DEval(poly,x->data.F32[i],y->data.F32[i]);
        }
        break;
    case PS_TYPE_F64:
        if (y->type.type != x->type.type) {
            psError(PS_ERR_UNKNOWN, true, "type mismatch in data vectors");
            return (NULL);
        }

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

        // Evaluate the polynomial at the specified points
        for (unsigned int i=0; i<vecLen; i++) {
            tmp->data.F64[i] = psPolynomial2DEval(poly,x->data.F64[i],y->data.F64[i]);
        }
        break;
    default:
        psError(PS_ERR_UNKNOWN, false, "invalid input data type.\n");
        return (NULL);
    }
    // Return output vector
    return(tmp);
}

psF64 psPolynomial3DEval(const psPolynomial3D* poly,
                         psF64 x,
                         psF64 y,
                         psF64 z)
{
    PS_ASSERT_POLY_NON_NULL(poly, NAN);

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

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

{
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL);

    psVector *tmp;
    unsigned int 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_F64);

    // Evaluate polynomial
    for (unsigned int i = 0; i < vecLen; i++) {
        tmp->data.F64[i] = psPolynomial3DEval(poly,
                                              x->data.F64[i],
                                              y->data.F64[i],
                                              z->data.F64[i]);
    }

    // Return output vector
    return(tmp);
}

psF64 psPolynomial4DEval(const psPolynomial4D* poly,
                         psF64 x,
                         psF64 y,
                         psF64 z,
                         psF64 t)
{
    PS_ASSERT_POLY_NON_NULL(poly, NAN);

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

psVector *psPolynomial4DEvalVector(const psPolynomial4D *poly,
                                   const psVector *x,
                                   const psVector *y,
                                   const psVector *z,
                                   const psVector *t)
{
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F64, NULL);

    psVector *tmp;
    unsigned int vecLen=x->n;

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

    // Allocoutput vector
    tmp = psVectorAlloc(vecLen, PS_TYPE_F64);

    // Evaluate polynomial
    for (unsigned int i = 0; i < vecLen; i++) {
        tmp->data.F64[i] = psPolynomial4DEval(poly,
                                              x->data.F64[i],
                                              y->data.F64[i],
                                              z->data.F64[i],
                                              t->data.F64[i]);
    }

    // Return output vector
    return(tmp);
}

