/** @file  psPolynomial.c
*
*  @brief Contains basic function allocation, deallocation, and evaluation
*         routines.
*
*  This file will hold the routiness for allocating, freeing, and evaluating
*  polynomials.  It also contains a Gaussian functions.
*
*  @version $Revision: 1.152 $ $Name: not supported by cvs2svn $
*  @date $Date: 2006-10-24 22:52:56 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*
*  XXX: Should the "coeffErr[]" be used as well?  Bug ???.  Ignore coeffErr
*
*/
/*****************************************************************************/
/*  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 "psAssert.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)
{
    PS_ASSERT_PTR(ptr, false);
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)polynomial1DFree );
}

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

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

bool psMemCheckPolynomial4D(psPtr ptr)
{
    PS_ASSERT_PTR(ptr, false);
    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);
}

/*****************************************************************************
p_psCreateChebyshevPolys(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.
 *****************************************************************************/
psPolynomial1D **p_psCreateChebyshevPolys(psS32 numPolys)
{
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(numPolys, 1, NULL);

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

    // Create the Chebyshev polynomials.
    // Polynomial i has i-th order.
    chebPolys[0]->coeff[0] = 1.0;
    if (numPolys >= 2) {
        chebPolys[1]->coeff[1] = 1.0;

        for (psS32 i = 2; i < numPolys; i++) {
            for (psS32 j = 0; j < chebPolys[i - 1]->nX+1; j++) {
                chebPolys[i]->coeff[j + 1] = 2.0 * chebPolys[i - 1]->coeff[j];
            }
            for (psS32 j = 0; j < chebPolys[i - 2]->nX+1; j++) {
                chebPolys[i]->coeff[j] -= chebPolys[i - 2]->coeff[j];
            }
        }
    }

    if (psTraceGetLevel("psLib.math") >= 6) {
        for (psS32 j = 0; j < numPolys; j++) {
            PS_POLY_PRINT_1D(chebPolys[j]);
        }
    }
    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.math", 4, "---- %s() begin ----\n", __func__);
    psTrace("psLib.math", 4, "Polynomial order is %u\n", poly->nX);
    for (loop_x = 0; loop_x < poly->nX+1; loop_x++) {
        psTrace("psLib.math", 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) {
            psTrace("psLib.math", 8,
                    "polysum+= sum*coeff [%lf+= (%lf * %lf)\n", polySum, xSum, poly->coeff[loop_x]);
            polySum += xSum * poly->coeff[loop_x];
        }
        xSum *= x;
    }

    psTrace("psLib.math", 4, "---- %s() end ----\n", __func__);
    return(polySum);
}

// XXX: You can do this without having to psAlloc() vector d.
// XXX: How does the mask vector effect Crenshaw's formula?
// NOTE: We assume that x is scaled between -1.0 and 1.0;
// XXX: Create a faster version for low-order Chebyshevs.
static psF64 chebPolynomial1DEval(
    psF64 x,
    const psPolynomial1D* poly)
{
    PS_ASSERT_DOUBLE_WITHIN_RANGE(x, -1.0, 1.0, NAN);
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(poly->nX, 0, NAN);
    psVector *d;

    unsigned int nTerms = 1 + poly->nX;
    unsigned int i;
    psF64 tmp = 0.0;

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

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

    if (1) {
        // General case where the Chebyshev poly has 2 or more terms.
        d = psVectorAlloc(nTerms, PS_TYPE_F64);
        if(poly->mask[nTerms-1] == 0) {
            d->data.F64[nTerms-1] = poly->coeff[nTerms-1];
        } else {
            d->data.F64[nTerms-1] = 0.0;
        }

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

        for (i=nTerms-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);
    } else {
        // XXX: This is old code that does not use Clenshaw's formula.  Get rid of it.
        psPolynomial1D **chebPolys = p_psCreateChebyshevPolys(1 + poly->nX);

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

        for (psS32 i=0;i<(1 + poly->nX);i++) {
            psFree(chebPolys[i]);
        }
        psFree(chebPolys);
    }

    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 = p_psCreateChebyshevPolys(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 = p_psCreateChebyshevPolys(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;
    }
    // Add 1 since p_psCreateChebyshevPolys() takes nTerms, not nOrder.
    chebPolys = p_psCreateChebyshevPolys(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("psLib.math", 4, "---- %s() begin ----\n", __func__);

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

    psTrace("psLib.math", 4, "---- %s() end ----\n", __func__);
    return(tmp * exp(-((x - mean) * (x - mean)) / (2.0 * sigma * sigma)));
}

/*****************************************************************************
    This routine must allocate memory for the polynomial structures.
 
    XXX: How do we check for an appropriate value for n?
 *****************************************************************************/
psPolynomial1D* psPolynomial1DAlloc(
    psPolynomialType type,
    unsigned int nX)
{
    //PS_ASSERT_INT_NONNEGATIVE(n, NULL);
    psU32 nOrder = nX;
    psPolynomial1D *newPoly = (psPolynomial1D* ) psAlloc(sizeof(psPolynomial1D));
    psMemSetDeallocator(newPoly, (psFreeFunc) polynomial1DFree);

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

    return(newPoly);
}

psPolynomial2D* psPolynomial2DAlloc(
    psPolynomialType type,
    unsigned int nX,
    unsigned int nY)
{
    //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(
    psPolynomialType type,
    unsigned int nX,
    unsigned int nY,
    unsigned int nZ)
{
    //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(
    psPolynomialType type,
    unsigned int nX,
    unsigned int nY,
    unsigned int nZ,
    unsigned int nT)
{
    //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,
                _("Unknown polynomial type 0x%x found.  Evaluation failed."),
                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,
                _("Unknown polynomial type 0x%x found.  Evaluation failed."),
                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,
                _("Unknown polynomial type 0x%x found.  Evaluation failed."),
                poly->type);
    }
    return(NAN);
}

// XXX: The output of this routine is always psF64 while 1D and 2D are
// dependent on the input vectors.
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_F32_OR_F64(x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(z, 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
    // XXX: Consult with IfA: is this how they want to handle multiple data types?
    if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F32)
            && (z->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i],
                                                  y->data.F32[i], z->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i],
                                                  y->data.F32[i], z->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i],
                                                  y->data.F64[i], z->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F32[i],
                                                  y->data.F64[i], z->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i],
                                                  y->data.F32[i], z->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i],
                                                  y->data.F32[i], z->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial3DEval(poly, x->data.F64[i],
                                                  y->data.F64[i], z->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F64)) {
        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,
                _("Unknown polynomial type 0x%x found.  Evaluation failed."),
                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_F32_OR_F64(x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(z, NULL);
    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(t, 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
    // XXX: Consult with IfA: is this how they want to handle multiple data types?
    if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F32)
            && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F32[i], z->data.F32[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F32[i], z->data.F32[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F32[i], z->data.F64[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F32[i], z->data.F64[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F64[i], z->data.F32[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F64[i], z->data.F32[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F64[i], z->data.F64[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F32) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F32[i],
                                                  y->data.F64[i], z->data.F64[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i],
                                                  y->data.F32[i], z->data.F32[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i],
                                                  y->data.F32[i], z->data.F32[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i],
                                                  y->data.F32[i], z->data.F64[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F32)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i],
                                                  y->data.F32[i], z->data.F64[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F32)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i],
                                                  y->data.F64[i], z->data.F32[i], t->data.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F32) && (t->type.type == PS_TYPE_F64)) {
        for (unsigned int i = 0; i < vecLen; i++) {
            tmp->data.F64[i] = psPolynomial4DEval(poly, x->data.F64[i],
                                                  y->data.F64[i], z->data.F32[i], t->data.F64[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F32)) {
        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.F32[i]);
        }
    } else if ((x->type.type == PS_TYPE_F64) && (y->type.type == PS_TYPE_F64)
               && (z->type.type == PS_TYPE_F64) && (t->type.type == PS_TYPE_F64)) {
        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]);
        }
    }


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

