/** @file  psMinimize.c
 *  \brief basic minimization functions
 *  @ingroup Math
 *
 *  This file will contain functions to minimize an arbitrary function at
 *  a data point, fit an arbitrary function to a set of data points, and
 *  fit a 1-D polynomial to a set of data points.
 *
 *  @author GLG, MHPCC
 *  @author EAM, IfA
 *
 *  @version $Revision: 1.7 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-02-02 21:09:07 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 *
 *  XXX: psMatrixLUSolve() does not return error codes when the results are NANs.
 *
 *  XXX: For clip-fit functions, what should we do if the mask is NULL?
 *
 */
/*****************************************************************************/
/* INCLUDE FILES                                                             */
/*****************************************************************************/
#include <stdio.h>
#include <float.h>
#include <math.h>

#include "psMinimizePolyFit.h"
#include "psMinimizeLMM.h"  // For Gauss-Jordan routines
#include "psStats.h"
#include "psImage.h"
#include "psImageStructManip.h"
#include "psBinaryOp.h"
#include "psLogMsg.h"
#include "psMathUtils.h"
/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/

#define PS_VECTOR_GEN_CHEBY_INDEX(VEC, SIZE, TYPE) \
VEC = psVectorAlloc(SIZE, TYPE); \
if (TYPE == PS_TYPE_F64) { \
    for (psS32 i = 0 ; i < SIZE ; i++) { \
        VEC->data.F64[i] = ((2.0 / ((psF64) (SIZE - 1))) * ((psF64) i)) - 1.0; \
    }\
} else if (TYPE == PS_TYPE_F32){ \
    for (psS32 i = 0 ; i < SIZE ; i++) { \
        VEC->data.F32[i] = ((2.0 / ((psF32) (SIZE - 1))) * ((psF32) i)) - 1.0; \
    }\
}\

/*****************************************************************************/
/* TYPE DEFINITIONS                                                          */
/*****************************************************************************/

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

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

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

/******************************************************************************
BuildSums1D(sums, x, polyOrder, sums): this routine calculates the powers of
input parameter "x" between 0 and input parameter nTerms*2.  The result is
returned as a psVector sums.
*****************************************************************************/
static psVector *BuildSums1D(
    psVector* sums,
    psF64 x,
    psS32 nTerm)
{
    psS32 nSum = 0;
    psF64 xSum = 0.0;

    //
    // XXX: Why do we multiply by 2 here?  It's better to do it outside and
    // have the definition of this function remain sensible.
    //
    nSum = 2*nTerm;
    if (sums == NULL) {
        sums = psVectorAlloc(nSum, PS_TYPE_F64);
    } else if (nSum > sums->n) {
        sums = psVectorRealloc(sums, nSum);
    }

    xSum = 1.0;
    for (psS32 i = 0; i < nSum; i++) {
        sums->data.F64[i] = xSum;
        xSum *= x;
    }
    return (sums);
}

/******************************************************************************
BuildSums2D(sums, x, y, nXterm, nYterm): this routine calculates the powers of
input parameter "x" and "y" between 0 and input parameter nXterms*2 and
nYterm*2.  The result is returned as a psImage sums.
 *****************************************************************************/
static psImage *BuildSums2D(
    psImage *sums,
    psF64 x,
    psF64 y,
    psS32 nXterm,
    psS32 nYterm)
{
    psS32 nXsum = 0;
    psS32 nYsum = 0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;

    nXsum = 2*nXterm;
    nYsum = 2*nYterm;
    if (sums == NULL) {
        sums = psImageAlloc(nXsum, nYsum, PS_TYPE_F64);
    }
    if ((nXsum != sums->numCols) || (nYsum != sums->numRows)) {
        psFree (sums);
        sums = psImageAlloc(nXsum, nYsum, PS_TYPE_F64);
    }

    xSum = 1.0;
    for (psS32 i = 0; i < nXsum; i++) {
        ySum = xSum;
        for (psS32 j = 0; j < nYsum; j++) {
            sums->data.F64[i][j] = ySum;
            ySum *= y;
        }
        xSum *= x;
    }

    return (sums);
}

/******************************************************************************
BuildSums3D(sums, x, y, z, nXterm, nYterm, nZterm): this routine calculates
the powers of input parameter "x", "y", and "z" between 0 and input parameter
nXterms*2, nYterm*2, and nZterm*2.  The result is returned as a 3-D array sums.
 *****************************************************************************/
static psF64 ***BuildSums3D(
    psF64 ***sums,
    psF64 x,
    psF64 y,
    psF64 z,
    psS32 nXterm,
    psS32 nYterm,
    psS32 nZterm)
{
    psS32 nXsum = 0;
    psS32 nYsum = 0;
    psS32 nZsum = 0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;
    psF64 zSum = 1.0;

    nXsum = 2*nXterm;
    nYsum = 2*nYterm;
    nZsum = 2*nZterm;
    if (sums == NULL) {
        sums = (psF64 ***) psAlloc (nXsum*sizeof(psF64));
        for (psS32 i = 0; i < nXsum; i++) {
            sums[i] = (psF64 **) psAlloc (nYsum*sizeof(psF64));
            for (psS32 j = 0; j < nYsum; j++) {
                sums[i][j] = (psF64 *) psAlloc (nZsum*sizeof(psF64));
            }
        }
    }
    // careful with this function: there is no size checking and realloc for reuse

    if (1) {
        zSum = 1.0;
        for (psS32 k = 0; k < nZsum; k++) {
            ySum = zSum;
            for (psS32 j = 0; j < nYsum; j++) {
                xSum = ySum;
                for (psS32 i = 0; i < nXsum; i++) {
                    sums[i][j][k] = xSum;
                    xSum *= x;
                }
                ySum *= y;
            }
            zSum *= z;
        }
    } else {
        xSum = 1.0;
        for (psS32 i = 0; i < nXsum; i++) {
            ySum = xSum;
            for (psS32 j = 0; j < nYsum; j++) {
                zSum = ySum;
                for (psS32 k = 0; k < nZsum; k++) {
                    sums[i][j][k] = zSum;
                    zSum *= z;
                }
                ySum *= y;
            }
            xSum *= x;
        }
    }

    return (sums);
}

/******************************************************************************
    BuildSums4D(sums, x, y, z, t, nXterm, nYterm, nZterm, nTterm). equiv to
    BuildSums2D(). The result is returned as a psF64 ****
*****************************************************************************/
static psF64 ****BuildSums4D(
    psF64 ****sums,
    psF64 x,
    psF64 y,
    psF64 z,
    psF64 t,
    psS32 nXterm,
    psS32 nYterm,
    psS32 nZterm,
    psS32 nTterm)
{
    psS32 nXsum = 0;
    psS32 nYsum = 0;
    psS32 nZsum = 0;
    psS32 nTsum = 0;
    psF64 xSum = 1.0;
    psF64 ySum = 1.0;
    psF64 zSum = 1.0;
    psF64 tSum = 1.0;

    nXsum = 2*nXterm;
    nYsum = 2*nYterm;
    nZsum = 2*nZterm;
    nTsum = 2*nTterm;
    if (sums == NULL) {
        sums = (psF64 ****) psAlloc (nXsum*sizeof(psF64));
        for (psS32 i = 0; i < nXsum; i++) {
            sums[i] = (psF64 ***) psAlloc (nYsum*sizeof(psF64));
            for (psS32 j = 0; j < nYsum; j++) {
                sums[i][j] = (psF64 **) psAlloc (nZsum*sizeof(psF64));
                for (psS32 k = 0; k < nZsum; k++) {
                    sums[i][j][k] = (psF64 *) psAlloc (nTsum*sizeof(psF64));
                }
            }
        }
    }
    // careful with this function: there is no size checking and realloc for reuse

    tSum = 1.0;
    for (psS32 m = 0; m < nTsum; m++) {
        zSum = tSum;
        for (psS32 k = 0; k < nZsum; k++) {
            ySum = zSum;
            for (psS32 j = 0; j < nYsum; j++) {
                xSum = ySum;
                for (psS32 i = 0; i < nXsum; i++) {
                    sums[i][j][k][m] = xSum;
                    xSum *= x;
                }
                ySum *= y;
            }
            zSum *= z;
        }
        tSum *= t;
    }
    return (sums);
}

/******************************************************************************
 ******************************************************************************
 Analytical 1-D fitting routines.
 ******************************************************************************
 *****************************************************************************/


/******************************************************************************
 ******************************************************************************
 1-D Vector Poly Fitting Code.
 ******************************************************************************
 *****************************************************************************/

static psPolynomial1D* VectorFitPolynomial1DOrd(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x);

/******************************************************************************
vectorFitPolynomial1DCheb():  This routine will fit a Chebyshev
polynomial of degree myPoly->nX to the data points (x, y) and return the
coefficients of that polynomial.
 
    NOTE: We currently have implemented three algorithms.  This one is
    non-standard.  It ignores the orthogonal properties of the Chebyshev
    polys, and their known zero values.  Instead, we first fit a regular
    ordinary polynomial to the data.  This produces the coefficients for the
    various x^i terms.  We then "reverse-engineer" those coefficients to
    determine the coefficients of the Chebyshev polys: basically for each
    c_ix^i term in the ordinary polynomial, we sum all x^i terms in the
    Chebshev polys: these must be equal.  This creates a set of nOrder+1
    linear equations which can be easily solved.  The resulting vector is the
    coefficients of the Chebyshev polys.
    
    This method is significantly slower than the standard NR algorithm.  It
    was explicitly requested that we not use the NR algorithm.
 
 Matrix A[[][]:
     Element A[i][j] is the coefficient of the x^i term in the j-th cheby poly.
 
    XXX: This can be improved significantly, performance-wise.  The second set
    of linear equations which must be "solved" are already in upper-triangular
    form.  One must only perform the reverse-substitution, LUD decomposition.
 
    XXX: Also, we don't really need to generate the chebPolys data structure.
    We simply need the matrix which corresponds to a transpose of each Cheby
    polys coefficients.
*****************************************************************************/
static psPolynomial1D *vectorFitPolynomial1DCheb(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector* y,
    const psVector* yErr,
    const psVector* x)
{
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(myPoly->nX, 0, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    if (yErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, yErr, NULL);
        PS_ASSERT_VECTOR_TYPE(yErr, PS_TYPE_F64, NULL);
    }
    if (x != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, x, NULL);
        PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    }
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    psS32 polyOrder = myPoly->nX;
    psS32 numPoly = polyOrder + 1;

    //
    // We first fit an ordinary polynomial to the data.
    //
    psPolynomial1D *ordPoly = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, polyOrder);
    psPolynomial1D *rc = VectorFitPolynomial1DOrd(ordPoly, mask, maskValue, y, yErr, x);
    if (rc == NULL) {
        psError(PS_ERR_UNKNOWN, false, "Could not fit a preliminary polynomial to the data vector.  Returning NULL.\n");
        psFree(myPoly);
        return(NULL);
    }

    //
    // Create the A-matrix and B-vector which correspond to the linear equations
    // which will be solved and will then yield the Cheby poly coefficients.
    //
    psPolynomial1D **chebPolys = p_psCreateChebyshevPolys(numPoly);
    psImage *A = psImageAlloc(numPoly, numPoly, PS_TYPE_F64);
    psVector *B = psVectorAlloc(numPoly, PS_TYPE_F64);
    for (psS32 i = 0 ; i < numPoly ; i++) {
        for (psS32 j = 0 ; j < numPoly ; j++) {
            A->data.F64[i][j] = 0.0;
        }
        B->data.F64[i] = ordPoly->coeff[i];
    }

    for (psS32 i = 0 ; i < numPoly ; i++) {
        for (psS32 j = 0 ; j < numPoly ; j++) {
            if (i <= chebPolys[j]->nX)
                A->data.F64[i][j]+= chebPolys[j]->coeff[i];
        }
    }
    // The following statement is essential.  It derives from (5.8.8) NR.
    A->data.F64[0][0] = 0.5;
    psFree(ordPoly);
    if (psTraceGetLevel(__func__) >= 6) {
        PS_IMAGE_PRINT_F64(A);
        PS_VECTOR_PRINT_F64(B);
    }

    if (0) {
        // GaussJordan version
        if (false == psGaussJordan(A, B)) {
            psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            // the first nTerm entries in B correspond directly to the desired
            // polynomial coefficients.  this is only true for the 1D case
            for (psS32 k = 0; k < numPoly; k++) {
                myPoly->coeff[k] = B->data.F64[k];
            }
        }
    } else {
        // LUD version of the fit
        psImage *ALUD = NULL;
        psVector* outPerm = NULL;
        psVector* coeffs = NULL;

        ALUD = psImageAlloc(numPoly, numPoly, PS_TYPE_F64);
        ALUD = psMatrixLUD(ALUD, &outPerm, A);
        if (ALUD == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not do LUD decomposition on matrix.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
            if (coeffs == NULL) {
                psError(PS_ERR_UNKNOWN, false, "Could not solve LUD matrix.  Returning NULL.\n");
                psFree(myPoly);
                myPoly = NULL;
            } else {
                for (psS32 k = 0; k < numPoly; k++) {
                    myPoly->coeff[k] = coeffs->data.F64[k];
                }
            }
        }


        psFree(ALUD);
        psFree(coeffs);
        psFree(outPerm);
    }

    psFree(A);
    psFree(B);
    for (psS32 i=0;i<numPoly;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);

    return(myPoly);
}


/******************************************************************************
vectorFitPolynomial1DChebSlow():  This routine will fit a Chebyshev polynomial
of degree myPoly to the data points (x, y) and return the coefficients of that
polynomial.
 
    NOTE: We currently have implemented three algorithms.  This one is
    non-standard.  It ignores the orthogonal properties of the Chebyshev
    polys, and their known zero values.  Instead, we do build a system of
    linear equations based on minimizing the chi-squared for all data points
    and we then solve those equations.  This method is significantly slower
    than the other algorithms.  It was explicitly requested that we implement
    this algorithm.
*****************************************************************************/
static psPolynomial1D *vectorFitPolynomial1DChebySlow(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector* y,
    const psVector* yErr,
    const psVector* x)
{
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(myPoly->nX, 0, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    if (yErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, yErr, NULL);
        PS_ASSERT_VECTOR_TYPE(yErr, PS_TYPE_F64, NULL);
    }
    if (x != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, x, NULL);
        PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    }
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    psS32 NUM_POLY = myPoly->nX+1;
    psS32 NUM_DATA = 0;
    if (mask != NULL) {
        for (psS32 d = 0 ; d < mask->n  ; d++) {
            if (!(maskValue & mask->data.U8[d])) {
                NUM_DATA++;
            }
        }
    } else {
        NUM_DATA = x->n;
    }

    psPolynomial1D **chebPolys = p_psCreateChebyshevPolys(NUM_POLY);
    if (0) {
        for (psS32 j = 0; j < NUM_POLY; j++) {
            PS_POLY_PRINT_1D(chebPolys[j]);
        }
    }

    // Pre-compute the various Chebyshev polys T_i(x[j]) for all x[]
    psImage *tMatrix = psImageAlloc(NUM_DATA, NUM_POLY, PS_TYPE_F64);
    for (psS32 p = 0 ; p < NUM_POLY ; p++) {
        if (mask == NULL) {
            for (psS32 d = 0 ; d < NUM_DATA ; d++) {
                tMatrix->data.F64[p][d] = psPolynomial1DEval(chebPolys[p], x->data.F64[d]);
            }
        } else {
            psS32 dPtr = 0;
            for (psS32 d = 0 ; d < mask->n ; d++) {
                if (!(maskValue & mask->data.U8[d])) {
                    tMatrix->data.F64[p][dPtr] = psPolynomial1DEval(chebPolys[p], x->data.F64[d]);
                    dPtr++;
                }
            }
        }
    }

    // Compute the A matrix
    psImage *A = psImageAlloc(NUM_POLY, NUM_POLY, PS_TYPE_F64);
    for (psS32 i = 0 ; i < NUM_POLY ; i++) {
        for (psS32 j = 0 ; j < NUM_POLY ; j++) {
            A->data.F64[i][j] = 0.0;
            for (psS32 d = 0 ; d < NUM_DATA ; d++) {
                A->data.F64[i][j]+= (tMatrix->data.F64[j][d] * tMatrix->data.F64[i][d]);
            }
        }
        // This is because of the last term in: f(x) = SUM[c_i * T_i(x)]  -  c_0/2
        for (psS32 d = 0 ; d < NUM_DATA ; d++) {
            A->data.F64[i][0] -= (tMatrix->data.F64[i][d]/2.0);
        }
    }

    // Compute the B vector
    psVector *B = psVectorAlloc(NUM_POLY, PS_TYPE_F64);
    for (psS32 i = 0 ; i < NUM_POLY ; i++) {
        B->data.F64[i] = 0.0;
        if (mask == NULL) {
            for (psS32 d = 0 ; d < NUM_DATA ; d++) {
                B->data.F64[i] += (y->data.F64[d] * tMatrix->data.F64[i][d]);
            }
        } else {
            psS32 dPtr = 0;
            for (psS32 d = 0 ; d < mask->n ; d++) {
                if (!(maskValue & mask->data.U8[d])) {
                    B->data.F64[i] += (y->data.F64[d] * tMatrix->data.F64[i][dPtr++]);
                }
            }
        }
    }

    if (0) {
        // GaussJordan version
        if (false == psGaussJordan(A, B)) {
            psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            // the first nTerm entries in B correspond directly to the desired
            // polynomial coefficients.  this is only true for the 1D case
            for (psS32 k = 0; k < NUM_POLY; k++) {
                myPoly->coeff[k] = B->data.F64[k];
            }
        }
    } else {
        // LUD version of the fit
        psImage *ALUD = NULL;
        psVector* outPerm = NULL;
        psVector* coeffs = NULL;

        ALUD = psImageAlloc(NUM_POLY, NUM_POLY, PS_TYPE_F64);
        ALUD = psMatrixLUD(ALUD, &outPerm, A);
        if (ALUD == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not do LUD decomposition on matrix.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
            if (coeffs == NULL) {
                psError(PS_ERR_UNKNOWN, false, "Could not solve LUD matrix.  Returning NULL.\n");
                psFree(myPoly);
                myPoly = NULL;
            } else {
                for (psS32 k = 0; k < NUM_POLY; k++) {
                    myPoly->coeff[k] = coeffs->data.F64[k];
                }
            }
        }

        psFree(ALUD);
        psFree(coeffs);
        psFree(outPerm);
    }

    psFree(A);
    psFree(B);
    psFree(tMatrix);
    for (psS32 i=0;i<NUM_POLY;i++) {
        psFree(chebPolys[i]);
    }
    psFree(chebPolys);

    return(myPoly);
}

/******************************************************************************
vectorFitPolynomial1DChebFast():  This routine will fit a Chebyshev polynomial
of degree myPoly to the data points (x, y) and return the coefficients of that
polynomial.
 
    NOTE: We currently have three algorithms.  This is standard method which
    uses the orthogonal properties of the Chebyshev polys, and their known
    zero values.  This is significantly faster than the chi-squared
    approaches.
 
    NOTE: This function will not work properly if the x-vector does not fully span
    the [-1:1] interval.
 
    NOTE: mask, maskValue, yErr are ignored with this function.
*****************************************************************************/
static psPolynomial1D *vectorFitPolynomial1DChebyFast(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector* y,
    const psVector* yErr,
    const psVector* x)
{
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    if (yErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, yErr, NULL);
        PS_ASSERT_VECTOR_TYPE(yErr, PS_TYPE_F64, NULL);
    }
    if (x != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, x, NULL);
        PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    }

    psS32 j;
    psS32 k;
    psS32 n = y->n;
    psF64 fac;
    psF64 sum;
    PS_VECTOR_GEN_STATIC_RECYCLED(f, n, PS_TYPE_F64);
    psScalar *fScalar;
    psScalar tmpScalar;
    tmpScalar.type.type = PS_TYPE_F64;

    // These assignments appear too simple to warrant code and
    // variable declarations.  I retain them here to maintain coherence
    // with the NR code.
    psF64 min = -1.0;
    psF64 max = 1.0;
    psF64 bma = 0.5 * (max-min);  // 1
    psF64 bpa = 0.5 * (max+min);  // 0

    // In this loop, we first calculate the values of X for which the
    // Chebyshev polynomials are zero (see NR, section 5.4).  Then we
    // calculate the value of the function we are fitting the Chebyshev
    // polynomials to at those values of X.  This is a bit tricky since
    // we don't know that function.  So, we instead do 3-order LaGrange
    // interpolation at the point X for the psVectors x,y for which we
    // are fitting this ChebyShev polynomial to.

    for (psS32 i=0;i<n;i++) {
        // NR 5.8.4
        // NR 5.8.4
        psF64 Y = cos(M_PI * (0.5 + ((psF32) i)) / ((psF32) n));
        psF64 X = (Y + bma + bpa) - 1.0;
        tmpScalar.data.F64 = X;

        // We interpolate against the tabluated x,y vectors to determine the
        // function value at X.
        // XXX: This is somewhat of a hack to handle cases where the x vector does
        // not fully span the [-1.0:1.0] interval.  We set the values of f[] to the
        // values of y[] at those endpoints.
        // XXX: This only works if x[] is increasing.

        if (X <= x->data.F64[0]) {
            f->data.F64[i] = y->data.F64[0];
        } else if (X >= x->data.F64[x->n-1]) {
            f->data.F64[i] = y->data.F64[x->n-1];
        } else {
            fScalar = p_psVectorInterpolate(NULL, (psVector *) x, (psVector *) y, 3, &tmpScalar);
            if (fScalar == NULL) {
                psError(PS_ERR_UNKNOWN, false, "Could not perform vector interpolation.  Returning NULL.\n");
                psFree(myPoly)
                return(NULL);
            }

            f->data.F64[i] = fScalar->data.F64;
            psFree(fScalar);
        }

        psTrace(__func__, 6, "(x, X, y, f(X)) is (%f, %f, %f, %f)\n",
                x->data.F64[i], X, y->data.F64[i], f->data.F64[i]);
    }

    // We have the values for f() at the zero points, we now calculate the
    // coefficients of the Chebyshev polynomial: NR 5.8.7.

    fac = 2.0/((psF32) n);
    for (j=0;j<myPoly->nX+1;j++) {
        sum = 0.0;
        for (k=0;k<n;k++) {
            sum+= f->data.F64[k] *
                  cos(M_PI * ((psF32) j) * (0.5 + ((psF32) k)) / ((psF32) n));
        }

        myPoly->coeff[j] = fac * sum;
    }

    return(myPoly);
}



/******************************************************************************
VectorFitPolynomial1DOrd(myPoly, *mask, maskValue, *y, *yErr, *x): This is a
private routine which will fit a 1-D polynomial to a set of (x, f) pairs.  The
x and fErr vectors may be NULL.  All non-NULL vectors must be of type
PS_TYPE_F64.
 *****************************************************************************/
static psPolynomial1D* VectorFitPolynomial1DOrd(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    if (x != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
        PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    }
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL);
    }

    psImage* A = NULL;
    psVector* B = NULL;
    psVector* xSums = NULL;
    psS32 nTerm;
    psS32 nOrder;
    psF64 wt;

    if (psTraceGetLevel(__func__) >= 6) {
        psTrace(__func__, 6, "VectorFitPolynomial1D()\n");
        for (psS32 i = 0; i < f->n; i++) {
            psTrace(__func__, 6, "(x, f, fErr) is (");
            if (x != NULL) {
                psTrace(__func__, 6, "%f, %f, ", x->data.F64[i], f->data.F64[i]);
            } else {
                psTrace(__func__, 6, "%f, %f, ", (psF64) i, f->data.F64[i]);
            }
            if (fErr != NULL) {
                psTrace(__func__, 6, "%f)\n", fErr->data.F64[i]);
            } else {
                psTrace(__func__, 6, "NULL)\n");
            }
        }
    }

    nTerm = 1 + myPoly->nX;
    nOrder = nTerm - 1;

    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
    B = psVectorAlloc(nTerm, PS_TYPE_F64);
    // Initialize data structures.
    if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) {
        psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B.  Returning NULL.\n");
        psFree(myPoly);
        psFree(A);
        psFree(B);
        psTrace(__func__, 4, "---- %s() End ----\n", __func__);
        return(NULL);
    }

    // xSums look like: 1, x, x^2, ... x^(2n+1)
    // Build the B and A data structs.
    // XXX EAM : use temp pointers eg vB = B->data.F64 to save redirects
    for (psS32 k = 0; k < f->n; k++) {
        if ((mask != NULL) && (mask->data.U8[k] && maskValue)) {
            continue;
        }
        if (x != NULL) {
            xSums = BuildSums1D(xSums, x->data.F64[k], nTerm);
        } else {
            xSums = BuildSums1D(xSums, (psF64) k, nTerm);
        }

        if (fErr == NULL) {
            wt = 1.0;
        } else {
            // this filters fErr == 0 values
            wt = (fErr->data.F64[k] == 0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
        }
        for (psS32 i = 0; i < nTerm; i++) {
            B->data.F64[i] += f->data.F64[k] * xSums->data.F64[i] * wt;
        }

        // we could skip half of the array and assign at the end
        // we must handle masked orders
        for (psS32 i = 0; i < nTerm; i++) {
            for (psS32 j = 0; j < nTerm; j++) {
                A->data.F64[i][j] += xSums->data.F64[i + j] * wt;
            }
        }
    }

    if (0) {
        // GaussJordan version
        if (false == psGaussJordan(A, B)) {
            psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            // the first nTerm entries in B correspond directly to the desired
            // polynomial coefficients.  this is only true for the 1D case
            for (psS32 k = 0; k < nTerm; k++) {
                myPoly->coeff[k] = B->data.F64[k];
            }
        }

    } else {
        // LUD version of the fit
        psImage *ALUD = NULL;
        psVector* outPerm = NULL;
        psVector* coeffs = NULL;

        ALUD = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
        ALUD = psMatrixLUD(ALUD, &outPerm, A);
        if (ALUD == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not do LUD decomposition on matrix.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
            if (coeffs == NULL) {
                psError(PS_ERR_UNKNOWN, false, "Could not solve LUD matrix.  Returning NULL.\n");
                psFree(myPoly);
                myPoly = NULL;
            } else {
                for (psS32 k = 0; k < nTerm; k++) {
                    myPoly->coeff[k] = coeffs->data.F64[k];
                }
            }
        }

        psFree(ALUD);
        psFree(coeffs);
        psFree(outPerm);
    }


    psFree(A);
    psFree(B);
    psFree(xSums);

    psTrace(__func__, 4, "---- %s() End ----\n", __func__);
    return (myPoly);
}


/******************************************************************************
psVectorFitPolynomial1D():  This routine fits a polynomial of arbitrary degree
(specified in poly) to the data points (x, y) and return that polynomial.
Types F32 and F64 are supported, however, type F32 is done via vector
conversion only.
 *****************************************************************************/
psPolynomial1D *psVectorFitPolynomial1D(
    psPolynomial1D *poly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x)
{
    // Internal pointers for possibly NULL or mis-typed vectors.
    psVector *x64 = NULL;
    psVector *f64 = NULL;
    psVector *fErr64 = NULL;

    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_INT_NONNEGATIVE(poly->nX, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_NON_EMPTY(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    if (x != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    }
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
    }

    if (f->type.type != PS_TYPE_F64) {
        f64 = psVectorCopy(NULL, f, PS_TYPE_F64);
    } else {
        f64 = (psVector *) f;
    }

    if (x != NULL) {
        if (x->type.type != PS_TYPE_F64) {
            x64 = psVectorCopy(NULL, x, PS_TYPE_F64);
        } else {
            x64 = (psVector *) x;
        }
    }

    if (fErr != NULL) {
        if (fErr->type.type != PS_TYPE_F64) {
            fErr64 = psVectorCopy(NULL, fErr, PS_TYPE_F64);
        } else {
            fErr64 = (psVector *) fErr;
        }
    }

    if (poly->type == PS_POLYNOMIAL_ORD) {
        poly = VectorFitPolynomial1DOrd(poly, mask, maskValue, f64, fErr64, x64);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not fit polynomial.  Returning NULL.\n");
            return(NULL);
        }
    } else if (poly->type == PS_POLYNOMIAL_CHEB) {
        if (mask != NULL) {
            //            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
        }
        if (fErr != NULL) {
            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring error vector with Chebyshev polynomials.\n");
        }
        if (x == NULL) {
            // If x==NULL, create an x64 vector with x values set to (-1:1).
            PS_VECTOR_GEN_CHEBY_INDEX(x64, f64->n, PS_TYPE_F64);
        }

        if (1) {
            poly = vectorFitPolynomial1DCheb(poly, mask, maskValue, f64, fErr64, x64);
        } else {
            if (0) {
                poly = vectorFitPolynomial1DChebySlow(poly, mask, maskValue, f64, fErr64, x64);
                poly = vectorFitPolynomial1DChebyFast(poly, mask, maskValue, f64, fErr64, x64);
            }
        }
        if (x == NULL) {
            psFree(x64);
        }
    } else {
        psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type (%d).  Returning NULL.\n", poly->type);
        poly = NULL;
    }

    // Free psVectors that were created for NULL arguments.
    if (f->type.type != PS_TYPE_F64) {
        psFree(f64);
    }

    if ((x != NULL) && (x->type.type != PS_TYPE_F64)) {
        psFree(x64);
    }

    if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
        psFree(fErr64);
    }

    return(poly);
}

// This function accepts F32 and F64 input vectors.
psPolynomial1D *psVectorClipFitPolynomial1D(
    psPolynomial1D *poly,
    psStats *stats,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *xIn)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_PTR_NON_NULL(stats, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    PS_ASSERT_VECTOR_NON_NULL(mask, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(mask, f, NULL);
    PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, f, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL);
    }

    // Internal pointers for possibly NULL vectors.
    psVector *x = NULL;
    if (xIn != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(xIn, f, NULL);
        PS_ASSERT_VECTOR_TYPE(xIn, f->type.type, NULL);
        x = (psVector *) xIn;
    } else {
        if (poly->type == PS_POLYNOMIAL_ORD) {
            x = psVectorCreate(NULL, 0, f->n, 1, f->type.type);
        } else if (poly->type == PS_POLYNOMIAL_CHEB) {
            if (f->type.type == PS_TYPE_F32) {
                PS_VECTOR_GEN_CHEBY_INDEX(x, f->n, PS_TYPE_F32);
            } else if (f->type.type == PS_TYPE_F64) {
                PS_VECTOR_GEN_CHEBY_INDEX(x, f->n, PS_TYPE_F64);
            }
        } else {
            psError(PS_ERR_UNKNOWN, false, "Error, bad poly type.\n");
            return(NULL);
        }
    }

    // clipping range defined by min and max and/or clipSigma
    psF32 minClipSigma;
    psF32 maxClipSigma;
    if (isfinite(stats->max)) {
        maxClipSigma = fabs(stats->max);
    } else {
        maxClipSigma = fabs(stats->clipSigma);
    }
    if (isfinite(stats->min)) {
        minClipSigma = fabs(stats->min);
    } else {
        minClipSigma = fabs(stats->clipSigma);
    }

    psVector *fit   = NULL;
    psVector *resid = psVectorAlloc(f->n, PS_TYPE_F64);

    // eventual expansion: user supplies one of various stats option pairs,
    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
    // evaluate the clipping sigma
    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
    psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter);
    psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma);

    //
    for (psS32 N = 0; N < stats->clipIter; N++) {
        psTrace(__func__, 6, "Loop iteration %d.  Calling psVectorFitPolynomial1D()\n");
        psS32 Nkeep = 0;
        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    psTrace(__func__, 6,  "mask[%d] is %d\n", i, mask->data.U8[i]);
                }
            }
        }
        poly = psVectorFitPolynomial1D(poly, mask, maskValue, f, fErr, x);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial.  Returning NULL.\n");
            if (xIn == NULL) {
                psFree(x);
            }
            return(NULL);
        }

        fit = psPolynomial1DEvalVector(poly, x);
        if (fit == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector().  Returning NULL.\n");
            psFree(resid)
            return(NULL);
        }
        for (psS32 i = 0 ; i < f->n ; i++) {
            if (f->type.type == PS_TYPE_F64) {
                resid->data.F64[i] = f->data.F64[i] - fit->data.F64[i];
            } else {
                resid->data.F64[i] = (psF64) (f->data.F32[i] - fit->data.F32[i]);
            }
        }
        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) {
                        psTrace(__func__, 6,  "(f, fit)[%d] is (%f, %f).  resid is (%f)\n",
                                i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]);
                    }
                }
            }
        }

        stats  = psVectorStats(stats, resid, NULL, mask, maskValue);
        psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian);
        psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev);
        psF32 minClipValue = -minClipSigma*stats->sampleStdev;
        psF32 maxClipValue = +maxClipSigma*stats->sampleStdev;

        // set mask if pts are not valid
        // we are masking out any point which is out of range
        // recovery is not allowed with this scheme
        for (psS32 i = 0; i < resid->n; i++) {
            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
                continue;
            }

            if ((resid->data.F64[i] - stats->sampleMedian > maxClipValue) ||
                    (resid->data.F64[i] - stats->sampleMedian < minClipValue)) {
                if (f->type.type == PS_TYPE_F64) {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F64[i], i, resid->data.F64[i]);
                } else {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F32[i], i, resid->data.F64[i]);
                }

                if (mask != NULL) {
                    mask->data.U8[i] |= 0x01;
                }
                continue;
            }
            Nkeep++;
        }

        //
        // We should probably exit this loop if no new elements were masked
        // since the polynomial fit won't change.
        //
        psTrace(__func__, 6, "keeping %d of %d pts for fit\n", Nkeep, x->n);
        psFree(fit);
    }

    // Free psVectors that were created for NULL arguments.
    if (xIn == NULL) {
        psFree(x);
    }
    // Free other local temporary variables
    psFree(resid);

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


/******************************************************************************
 ******************************************************************************
 2-D Vector Code.
 ******************************************************************************
 *****************************************************************************/

/******************************************************************************
VectorFitPolynomial2DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y): This is
a private routine which will fit a 2-D polynomial to a set of (x, y)-(f)
pairs.  All non-NULL vectors must be of type PS_TYPE_F64.
 
 *****************************************************************************/
static psPolynomial2D* VectorFitPolynomial2DOrd(
    psPolynomial2D* myPoly,
    const psVector* mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    // I think this is 1 dimension down
    psImage*     A = NULL;
    psVector*    B = NULL;
    psImage*   Sums = NULL;
    psF64 wt;
    psS32 nTerm;

    psS32 nXterm = 1 + myPoly->nX;
    psS32 nYterm = 1 + myPoly->nY;
    nTerm = nXterm * nYterm;

    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
    B = psVectorAlloc(nTerm, PS_TYPE_F64);
    // Initialize data structures.
    if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) {
        psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B.  Returning NULL.\n");
        psFree(myPoly);
        psFree(A);
        psFree(B);
        psTrace(__func__, 4, "---- %s() End ----\n", __func__);
        return(NULL);
    }

    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)

    // Build the B and A data structs.
    for (psS32 k  = 0; k < x->n; k++) {
        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
            continue;
        }

        Sums = BuildSums2D(Sums, x->data.F64[k], y->data.F64[k], nXterm, nYterm);

        if (fErr == NULL) {
            wt = 1.0;
        } else {
            // this filters fErr == 0 values
            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
        }

        // we could skip half of the array and assign at the end
        // we must handle masked orders
        for (psS32 n = 0; n < nXterm; n++) {
            for (psS32 m = 0; m < nYterm; m++) {
                B->data.F64[n+m*nXterm] += f->data.F64[k] * Sums->data.F64[n][m] * wt;
            }
        }

        for (psS32 i = 0; i < nXterm; i++) {
            for (psS32 j = 0; j < nYterm; j++) {
                for (psS32 n = 0; n < nXterm; n++) {
                    for (psS32 m = 0; m < nYterm; m++) {
                        A->data.F64[i+j*nXterm][n+m*nXterm] += Sums->data.F64[i+n][j+m] * wt;
                    }
                }
            }
        }
    }

    if (false == psGaussJordan(A, B)) {
        psError(PS_ERR_UNKNOWN, false, "Could not solve linear equations.  Returning NULL.\n");
        psFree(myPoly);
        myPoly = NULL;
    } else {
        // select the appropriate solution entries
        for (psS32 n = 0; n < nXterm; n++) {
            for (psS32 m = 0; m < nYterm; m++) {
                myPoly->coeff[n][m] = B->data.F64[n+m*nXterm];
            }
        }
    }

    psFree(A);
    psFree(B);
    psFree(Sums);

    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return (myPoly);
}

/******************************************************************************
psVectorFitPolynomial2D():  This routine fits a 2D polynomial of arbitrary
degree (specified in poly) to the data points (x, y)-(f) and returns that
polynomial.  Types F32 and F64 are supported, however, type F32 is done via
vector conversion only.
 *****************************************************************************/
psPolynomial2D *psVectorFitPolynomial2D(
    psPolynomial2D *poly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y)
{
    // Internal pointers for possibly NULL or mis-typed vectors.
    psVector *x64 = NULL;
    psVector *y64 = NULL;
    psVector *f64 = NULL;
    psVector *fErr64 = NULL;

    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL);

    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);

    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
    }

    //
    // Convert input vectors to F64 if necessary.
    //
    if (f->type.type != PS_TYPE_F64) {
        f64 = psVectorCopy(NULL, f, PS_TYPE_F64);
    } else {
        f64 = (psVector *) f;
    }

    if (x->type.type != PS_TYPE_F64) {
        x64 = psVectorCopy(NULL, x, PS_TYPE_F64);
    } else {
        x64 = (psVector *) x;
    }

    if (y->type.type != PS_TYPE_F64) {
        y64 = psVectorCopy(NULL, y, PS_TYPE_F64);
    } else {
        y64 = (psVector *) y;
    }

    //
    // fErr
    //
    if (fErr != NULL) {
        if (fErr->type.type != PS_TYPE_F64) {
            fErr64 = psVectorCopy(NULL, fErr, PS_TYPE_F64);
        } else {
            fErr64 = (psVector *) fErr;
        }
    }

    if (poly->type == PS_POLYNOMIAL_ORD) {
        poly = VectorFitPolynomial2DOrd(poly, mask, maskValue, f64, fErr64, x64, y64);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial.  Returning NULL.\n");
            // Free psVectors that were created for NULL arguments.
            if (f->type.type != PS_TYPE_F64) {
                psFree(f64);
            }

            if (x->type.type != PS_TYPE_F64) {
                psFree(x64);
            }

            if (y->type.type != PS_TYPE_F64) {
                psFree(y64);
            }

            if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
                psFree(fErr64);
            }
            return(NULL);
        }
    } else if (poly->type == PS_POLYNOMIAL_CHEB) {
        if (mask != NULL) {
            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
        }
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: 2-D Chebyshev polynomial vector fitting has not been implemented.  Returning NULL.\n");
        psFree(poly);
        poly = NULL;
    } else {
        // Free psVectors that were created for NULL arguments.
        if (f->type.type != PS_TYPE_F64) {
            psFree(f64);
        }

        if (x->type.type != PS_TYPE_F64) {
            psFree(x64);
        }

        if (y->type.type != PS_TYPE_F64) {
            psFree(y64);
        }

        if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
            psFree(fErr64);
        }
        psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type.  Returning NULL.\n");
    }


    // Free psVectors that were created for NULL arguments.
    if (f->type.type != PS_TYPE_F64) {
        psFree(f64);
    }

    if (x->type.type != PS_TYPE_F64) {
        psFree(x64);
    }

    if (y->type.type != PS_TYPE_F64) {
        psFree(y64);
    }

    if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
        psFree(fErr64);
    }

    return(poly);
}

psPolynomial2D *psVectorClipFitPolynomial2D(
    psPolynomial2D *poly,
    psStats *stats,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL);
    PS_ASSERT_PTR_NON_NULL(stats, NULL);
    PS_ASSERT_VECTOR_NON_NULL(mask, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL);

    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL);

    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL);
    }

    // clipping range defined by min and max and/or clipSigma
    psF32 minClipSigma;
    psF32 maxClipSigma;
    if (isfinite(stats->max)) {
        maxClipSigma = fabs(stats->max);
    } else {
        maxClipSigma = fabs(stats->clipSigma);
    }
    if (isfinite(stats->min)) {
        minClipSigma = fabs(stats->min);
    } else {
        minClipSigma = fabs(stats->clipSigma);
    }
    psVector *resid = psVectorAlloc(f->n, PS_TYPE_F64);

    // eventual expansion: user supplies one of various stats option pairs,
    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
    // evaluate the clipping sigma
    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
    psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter);
    psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma);

    for (psS32 N = 0; N < stats->clipIter; N++) {
        psTrace(__func__, 6, "Loop iteration %d.  Calling psVectorFitPolynomial1D()\n");
        psS32 Nkeep = 0;
        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    psTrace(__func__, 6,  "mask[%d] is %d\n", i, mask->data.U8[i]);
                }
            }
        }

        poly = psVectorFitPolynomial2D(poly, mask, maskValue, f, fErr, x, y);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data.  Returning NULL.\n");
            psFree(resid)
            return(NULL);
        }

        psVector *fit = psPolynomial2DEvalVector(poly, x, y);
        if (fit == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector().  Returning NULL.\n");
            psFree(resid)
            return(NULL);
        }

        for (psS32 i = 0 ; i < f->n ; i++) {
            if (f->type.type == PS_TYPE_F64) {
                resid->data.F64[i] = f->data.F64[i] - fit->data.F64[i];
            } else {
                resid->data.F64[i] = (psF64) (f->data.F32[i] - fit->data.F32[i]);
            }
        }

        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) {
                        psTrace(__func__, 6,  "(f, fit)[%d] is (%f, %f).  resid is (%f)\n",
                                i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]);
                    }
                }
            }
        }

        stats  = psVectorStats(stats, resid, NULL, mask, maskValue);
        if (stats == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not compute statistics on the resid vector.  Returning NULL.\n");
            psFree(resid)
            psFree(fit)
            return(NULL);
        }
        psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian);
        psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev);
        psF32 minClipValue = -minClipSigma*stats->sampleStdev;
        psF32 maxClipValue = +maxClipSigma*stats->sampleStdev;

        // set mask if pts are not valid
        // we are masking out any point which is out of range
        // recovery is not allowed with this scheme
        for (psS32 i = 0; i < resid->n; i++) {
            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
                continue;
            }

            if ((resid->data.F64[i] - stats->sampleMedian > maxClipValue) ||
                    (resid->data.F64[i] - stats->sampleMedian < minClipValue)) {
                if (fit->type.type == PS_TYPE_F64) {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F64[i], i, resid->data.F64[i]);
                } else {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F32[i], i, resid->data.F64[i]);
                }

                if (mask != NULL) {
                    mask->data.U8[i] |= 0x01;
                }
                continue;
            }
            Nkeep++;
        }

        psTrace(__func__, 6, "keeping %d of %d pts for fit\n", Nkeep, x->n);
        psFree(fit);
    }
    // Free local temporary variables
    psFree(resid);

    if (poly == NULL) {
        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
        return(NULL);
    }

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


/******************************************************************************
 ******************************************************************************
 3-D Vector Code.
 ******************************************************************************
 *****************************************************************************/

/******************************************************************************
VectorFitPolynomial3DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y, *z):
This is a private routine which will fit a 3-D polynomial to a set of (x,
y, z)-(f) pairs.  All non-NULL vectors must be of type PS_TYPE_F64.
 
 *****************************************************************************/
static psPolynomial3D* VectorFitPolynomial3DOrd(
    psPolynomial3D* myPoly,
    const psVector* mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, NULL);

    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    psImage    *A = NULL;
    psVector   *B = NULL;
    psF64 ***Sums = NULL;
    psF64 wt;
    psS32 nTerm;

    psS32 nXterm = 1 + myPoly->nX;
    psS32 nYterm = 1 + myPoly->nY;
    psS32 nZterm = 1 + myPoly->nZ;
    nTerm = nXterm * nYterm * nZterm;

    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
    B = psVectorAlloc(nTerm, PS_TYPE_F64);
    // Initialize data structures.
    if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) {
        psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B.  Returning NULL.\n");
        psFree(myPoly);
        psFree(A);
        psFree(B);
        psTrace(__func__, 4, "---- %s() End ----\n", __func__);
        return(NULL);
    }

    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...

    // Build the B and A data structs.
    for (psS32 k  = 0; k < x->n; k++) {
        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
            continue;
        }

        Sums = BuildSums3D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], nXterm, nYterm, nZterm);

        if (fErr == NULL) {
            wt = 1.0;
        } else {
            // this filters fErr == 0 values
            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
        }

        // we could skip half of the array and assign at the end
        // we must handle masked orders
        for (psS32 ix = 0; ix < nXterm; ix++) {
            for (psS32 iy = 0; iy < nYterm; iy++) {
                for (psS32 iz = 0; iz < nZterm; iz++) {
                    if (myPoly->mask[ix][iy][iz]) {
                        continue;
                    } else {
                        psS32 nx = ix + iy*nXterm + iz*nXterm*nYterm;
                        B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz] * wt;
                    }
                }
            }
        }

        for (psS32 ix = 0; ix < nXterm; ix++) {
            for (psS32 iy = 0; iy < nYterm; iy++) {
                for (psS32 iz = 0; iz < nZterm; iz++) {
                    if (myPoly->mask[ix][iy][iz])
                        continue;
                    psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm;
                    for (psS32 jx = 0; jx < nXterm; jx++) {
                        for (psS32 jy = 0; jy < nYterm; jy++) {
                            for (psS32 jz = 0; jz < nZterm; jz++) {
                                if (myPoly->mask[jx][jy][jz])
                                    continue;
                                psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm;
                                A->data.F64[nx][ny] += Sums[ix+jx][iy+jy][iz+jz] * wt;
                            }
                        }
                    }
                }
            }
        }
    }

    for (psS32 ix = 0; ix < nXterm; ix++) {
        for (psS32 iy = 0; iy < nYterm; iy++) {
            for (psS32 iz = 0; iz < nZterm; iz++) {
                if (!myPoly->mask[ix][iy][iz])
                    continue;
                psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm;
                B->data.F64[nx] = 0;
                for (psS32 jx = 0; jx < nXterm; jx++) {
                    for (psS32 jy = 0; jy < nYterm; jy++) {
                        for (psS32 jz = 0; jz < nZterm; jz++) {
                            psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm;
                            A->data.F64[nx][ny] = (nx == ny) ? 1 : 0;
                        }
                    }
                }
            }
        }
    }

    if (0) {
        // does the solution in place
        // The matrices were overflowing, so I switched to LUD.
        if (false == psGaussJordan(A, B)) {
            psFree(A);
            psFree(B);

            for (psS32 ix = 0; ix < 2*nXterm; ix++) {
                for (psS32 iy = 0; iy < 2*nYterm; iy++) {
                    psFree(Sums[ix][iy]);
                }
                psFree(Sums[ix]);
            }
            psFree(Sums);
            psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n");
            return(NULL);
        }
        // select the appropriate solution entries
        for (psS32 ix = 0; ix < nXterm; ix++) {
            for (psS32 iy = 0; iy < nYterm; iy++) {
                for (psS32 iz = 0; iz < nZterm; iz++) {
                    psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm;
                    myPoly->coeff[ix][iy][iz] = B->data.F64[nx];
                    myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[nx][nx]);
                }
            }
        }
    } else {
        // LUD version of the fit
        psImage *ALUD = NULL;
        psVector* outPerm = NULL;
        psVector* coeffs = NULL;

        ALUD = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
        ALUD = psMatrixLUD(ALUD, &outPerm, A);
        if (ALUD == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not do LUD decomposition on matrix.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
            if (coeffs == NULL) {
                psError(PS_ERR_UNKNOWN, false, "Could not solve LUD matrix.  Returning NULL.\n");
                psFree(myPoly);
                myPoly = NULL;
            } else {
                // select the appropriate solution entries
                for (psS32 ix = 0; ix < nXterm; ix++) {
                    for (psS32 iy = 0; iy < nYterm; iy++) {
                        for (psS32 iz = 0; iz < nZterm; iz++) {
                            psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm;
                            myPoly->coeff[ix][iy][iz] = coeffs->data.F64[nx];
                            myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[nx][nx]);
                        }
                    }
                }
            }
        }

        psFree(ALUD);
        psFree(coeffs);
        psFree(outPerm);
    }
    psFree(A);
    psFree(B);

    for (psS32 ix = 0; ix < 2*nXterm; ix++) {
        for (psS32 iy = 0; iy < 2*nYterm; iy++) {
            psFree(Sums[ix][iy]);
        }
        psFree(Sums[ix]);
    }
    psFree(Sums);

    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return (myPoly);
}

/******************************************************************************
psVectorFitPolynomial3D():  This routine fits a 3D polynomial of arbitrary
degree (specified in poly) to the data points (x, y, z)-(f) and returns that
polynomial.  Types F32 and F64 are supported, however, type F32 is done via
vector conversion only.
 *****************************************************************************/
psPolynomial3D *psVectorFitPolynomial3D(
    psPolynomial3D *poly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z)
{
    // Internal pointers for possibly NULL or mis-typed vectors.
    psVector *x64 = NULL;
    psVector *y64 = NULL;
    psVector *z64 = NULL;
    psVector *f64 = NULL;
    psVector *fErr64 = NULL;

    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL);

    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        //        PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL);
    }

    //
    // Convert input vectors to F64 if necessary.
    //
    if (f->type.type != PS_TYPE_F64) {
        f64 = psVectorCopy(NULL, f, PS_TYPE_F64);
    } else {
        f64 = (psVector *) f;
    }
    if (x->type.type != PS_TYPE_F64) {
        x64 = psVectorCopy(NULL, x, PS_TYPE_F64);
    } else {
        x64 = (psVector *) x;
    }
    if (y->type.type != PS_TYPE_F64) {
        y64 = psVectorCopy(NULL, y, PS_TYPE_F64);
    } else {
        y64 = (psVector *) y;
    }

    if (z->type.type != PS_TYPE_F64) {
        z64 = psVectorCopy(NULL, z, PS_TYPE_F64);
    } else {
        z64 = (psVector *) z;
    }

    if (fErr != NULL) {
        if (fErr->type.type != PS_TYPE_F64) {
            fErr64 = psVectorCopy(NULL, fErr, PS_TYPE_F64);
        } else {
            fErr64 = (psVector *) fErr;
        }
    }

    if (poly->type == PS_POLYNOMIAL_ORD) {
        poly = VectorFitPolynomial3DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial.  Returning NULL.\n");
            // Free psVectors that were created for NULL arguments.
            if (f->type.type != PS_TYPE_F64) {
                psFree(f64);
            }

            if (x->type.type != PS_TYPE_F64) {
                psFree(x64);
            }

            if (y->type.type != PS_TYPE_F64) {
                psFree(y64);
            }

            if (z->type.type != PS_TYPE_F64) {
                psFree(z64);
            }

            if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
                psFree(fErr64);
            }
            return(NULL);
        }
    } else if (poly->type == PS_POLYNOMIAL_CHEB) {
        if (mask != NULL) {
            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
        }
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: 3-D Chebyshev polynomial vector fitting has not been implemented.  Returning NULL.\n");
        psFree(poly);
        poly = NULL;
    } else {
        // Free psVectors that were created for NULL arguments.
        if (f->type.type != PS_TYPE_F64) {
            psFree(f64);
        }

        if (x->type.type != PS_TYPE_F64) {
            psFree(x64);
        }

        if (y->type.type != PS_TYPE_F64) {
            psFree(y64);
        }

        if (z->type.type != PS_TYPE_F64) {
            psFree(z64);
        }

        if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
            psFree(fErr64);
        }
        psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type.  Returning NULL.\n");
    }


    // Free psVectors that were created for NULL arguments.
    if (f->type.type != PS_TYPE_F64) {
        psFree(f64);
    }

    if (x->type.type != PS_TYPE_F64) {
        psFree(x64);
    }

    if (y->type.type != PS_TYPE_F64) {
        psFree(y64);
    }

    if (z->type.type != PS_TYPE_F64) {
        psFree(z64);
    }

    if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
        psFree(fErr64);
    }

    return(poly);
}

psPolynomial3D *psVectorClipFitPolynomial3D(
    psPolynomial3D *poly,
    psStats *stats,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL);
    PS_ASSERT_PTR_NON_NULL(stats, NULL);
    PS_ASSERT_VECTOR_NON_NULL(mask, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL);

    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL);

    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    PS_ASSERT_VECTOR_TYPE(z, f->type.type, NULL);

    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL);
    }

    // clipping range defined by min and max and/or clipSigma
    psF32 minClipSigma;
    psF32 maxClipSigma;
    if (isfinite(stats->max)) {
        maxClipSigma = fabs(stats->max);
    } else {
        maxClipSigma = fabs(stats->clipSigma);
    }
    if (isfinite(stats->min)) {
        minClipSigma = fabs(stats->min);
    } else {
        minClipSigma = fabs(stats->clipSigma);
    }
    psVector *fit   = NULL;
    psVector *resid = psVectorAlloc(f->n, PS_TYPE_F64);

    // eventual expansion: user supplies one of various stats option pairs,
    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
    // evaluate the clipping sigma
    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
    psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter);
    psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma);

    for (psS32 N = 0; N < stats->clipIter; N++) {
        psTrace(__func__, 6, "Loop iteration %d.  Calling psVectorFitPolynomial1D()\n");
        psS32 Nkeep = 0;
        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    psTrace(__func__, 6,  "mask[%d] is %d\n", i, mask->data.U8[i]);
                }
            }
        }

        poly = psVectorFitPolynomial3D(poly, mask, maskValue, f, fErr, x, y, z);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data.  Returning NULL.\n");
            psFree(resid)
            psFree(fit)
            return(NULL);
        }
        fit = psPolynomial3DEvalVector(poly, x, y, z);
        if (fit == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector().  Returning NULL.\n");
            psFree(resid)
            return(NULL);
        }
        for (psS32 i = 0 ; i < f->n ; i++) {
            if (f->type.type == PS_TYPE_F64) {
                resid->data.F64[i] = f->data.F64[i] - fit->data.F64[i];
            } else {
                resid->data.F64[i] = ((psF64) f->data.F32[i]) - fit->data.F64[i];
            }
        }

        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) {
                        psTrace(__func__, 6,  "(f, fit)[%d] is (%f, %f).  resid is (%f)\n",
                                i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]);
                    }
                }
            }
        }

        stats  = psVectorStats(stats, resid, NULL, mask, maskValue);
        if (stats == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not compute statistics on the resid vector.  Returning NULL.\n");
            psFree(resid)
            psFree(fit)
            return(NULL);
        }

        psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian);
        psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev);
        psF32 minClipValue = -minClipSigma*stats->sampleStdev;
        psF32 maxClipValue = +maxClipSigma*stats->sampleStdev;

        // set mask if pts are not valid
        // we are masking out any point which is out of range
        // recovery is not allowed with this scheme
        for (psS32 i = 0; i < resid->n; i++) {
            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
                continue;
            }

            if ((resid->data.F64[i] - stats->sampleMedian > maxClipValue) ||
                    (resid->data.F64[i] - stats->sampleMedian < minClipValue))  {
                if (f->type.type == PS_TYPE_F64) {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F64[i], i, resid->data.F64[i]);
                } else {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F32[i], i, resid->data.F64[i]);
                }

                if (mask != NULL) {
                    mask->data.U8[i] |= 0x01;
                }
                continue;
            }
            Nkeep++;
        }

        psTrace(__func__, 6, "keeping %d of %d pts for fit\n", Nkeep, x->n);
        psFree(fit);
    }
    // Free local temporary variables
    psFree(resid);

    if (poly == NULL) {
        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
        return(NULL);
    }

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

/******************************************************************************
 ******************************************************************************
 4-D Vector Code.
 ******************************************************************************
 *****************************************************************************/
/******************************************************************************
VectorFitPolynomial4DOrd(myPoly, *mask, maskValue, *f, *fErr, *x, *y, *z, *t):
This is a private routine which will fit a 4-D polynomial to a set of (x,
y, z, t)-(f) pairs.  All non-NULL vectors must be of type PS_TYPE_F64.
 
 *****************************************************************************/
static psPolynomial4D* VectorFitPolynomial4DOrd(
    psPolynomial4D* myPoly,
    const psVector* mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z,
    const psVector *t)
{
    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nY, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nZ, NULL);
    PS_ASSERT_INT_NONNEGATIVE(myPoly->nT, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F64, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F64, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTOR_TYPE(z, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    // I think this is 1 dimension down
    psImage     *A = NULL;
    psVector    *B = NULL;
    psF64 ****Sums = NULL;
    psF64 wt;
    psS32 nTerm;

    psS32 nXterm = 1 + myPoly->nX;
    psS32 nYterm = 1 + myPoly->nY;
    psS32 nZterm = 1 + myPoly->nZ;
    psS32 nTterm = 1 + myPoly->nZ;
    nTerm = nXterm * nYterm * nZterm * nTterm;

    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
    B = psVectorAlloc(nTerm, PS_TYPE_F64);
    // Initialize data structures.
    if (!psImageInit(A, 0.0) || !psVectorInit(B, 0.0)) {
        psError(PS_ERR_UNKNOWN, false, "Could initialize data structures A, B.  Returning NULL.\n");
        psFree(myPoly);
        psFree(A);
        psFree(B);
        psTrace(__func__, 4, "---- %s() End ----\n", __func__);
        return(NULL);
    }

    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...

    // Build the B and A data structs.
    for (psS32 k  = 0; k < x->n; k++) {
        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
            continue;
        }

        Sums = BuildSums4D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], t->data.F64[k], nXterm, nYterm, nZterm, nTterm);

        if (fErr == NULL) {
            wt = 1.0;
        } else {
            // this filters fErr == 0 values
            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
        }

        // we could skip half of the array and assign at the end
        // we must handle masked orders
        for (psS32 ix = 0; ix < nXterm; ix++) {
            for (psS32 iy = 0; iy < nYterm; iy++) {
                for (psS32 iz = 0; iz < nZterm; iz++) {
                    for (psS32 it = 0; it < nTterm; it++) {
                        if (myPoly->mask[ix][iy][iz][it])
                            continue;
                        psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
                        B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz][it] * wt;
                    }
                }
            }
        }

        for (psS32 ix = 0; ix < nXterm; ix++) {
            for (psS32 iy = 0; iy < nYterm; iy++) {
                for (psS32 iz = 0; iz < nZterm; iz++) {
                    for (psS32 it = 0; it < nTterm; it++) {
                        if (myPoly->mask[ix][iy][iz][it])
                            continue;
                        psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
                        for (psS32 jx = 0; jx < nXterm; jx++) {
                            for (psS32 jy = 0; jy < nYterm; jy++) {
                                for (psS32 jz = 0; jz < nZterm; jz++) {
                                    for (psS32 jt = 0; jt < nTterm; jt++) {
                                        if (myPoly->mask[jx][jy][jz][jt])
                                            continue;
                                        psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm;
                                        A->data.F64[nx][ny]+= Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    for (psS32 ix = 0; ix < nXterm; ix++) {
        for (psS32 iy = 0; iy < nYterm; iy++) {
            for (psS32 iz = 0; iz < nZterm; iz++) {
                for (psS32 it = 0; it < nTterm; it++) {
                    if (!myPoly->mask[ix][iy][iz][it])
                        continue;
                    psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
                    B->data.F64[nx] = 0;
                    for (psS32 jx = 0; jx < nXterm; jx++) {
                        for (psS32 jy = 0; jy < nYterm; jy++) {
                            for (psS32 jz = 0; jz < nZterm; jz++) {
                                for (psS32 jt = 0; jt < nTterm; jt++) {
                                    psS32 ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm;
                                    A->data.F64[nx][ny] = (nx == ny) ? 1 : 0;
                                }
                            }
                        }
                    }
                }
            }
        }
    }


    if (0) {
        // does the solution in place
        // The GaussJordan version was overflowing, so I'm using LUD.
        if (false == psGaussJordan(A, B)) {
            psFree(A);
            psFree(B);
            for (psS32 ix = 0; ix < 2*nXterm; ix++) {
                for (psS32 iy = 0; iy < 2*nYterm; iy++) {
                    for (psS32 iz = 0; iz < 2*nZterm; iz++) {
                        psFree(Sums[ix][iy][iz]);
                    }
                    psFree(Sums[ix][iy]);
                }
                psFree(Sums[ix]);
            }
            psFree(Sums);
            psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n");
            return(NULL);
        }

        // select the appropriate solution entries
        for (psS32 ix = 0; ix < nXterm; ix++) {
            for (psS32 iy = 0; iy < nYterm; iy++) {
                for (psS32 iz = 0; iz < nZterm; iz++) {
                    for (psS32 it = 0; it < nTterm; it++) {
                        psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
                        myPoly->coeff[ix][iy][iz][it] = B->data.F64[nx];
                        myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[nx][nx]);
                    }
                }
            }
        }
    } else {
        // LUD version of the fit
        psImage *ALUD = NULL;
        psVector* outPerm = NULL;
        psVector* coeffs = NULL;

        ALUD = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
        ALUD = psMatrixLUD(ALUD, &outPerm, A);
        if (ALUD == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not do LUD decomposition on matrix.  Returning NULL.\n");
            psFree(myPoly);
            myPoly = NULL;
        } else {
            coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
            if (coeffs == NULL) {
                psError(PS_ERR_UNKNOWN, false, "Could not solve LUD matrix.  Returning NULL.\n");
                psFree(myPoly);
                myPoly = NULL;
            } else {
                // select the appropriate solution entries
                for (psS32 ix = 0; ix < nXterm; ix++) {
                    for (psS32 iy = 0; iy < nYterm; iy++) {
                        for (psS32 iz = 0; iz < nZterm; iz++) {
                            for (psS32 it = 0; it < nTterm; it++) {
                                psS32 nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
                                myPoly->coeff[ix][iy][iz][it] = coeffs->data.F64[nx];
                                myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[nx][nx]);
                            }
                        }
                    }
                }
            }
        }

        psFree(ALUD);
        psFree(coeffs);
        psFree(outPerm);

    }

    psFree(A);
    psFree(B);

    for (psS32 ix = 0; ix < 2*nXterm; ix++) {
        for (psS32 iy = 0; iy < 2*nYterm; iy++) {
            for (psS32 iz = 0; iz < 2*nZterm; iz++) {
                psFree(Sums[ix][iy][iz]);
            }
            psFree(Sums[ix][iy]);
        }
        psFree(Sums[ix]);
    }
    psFree(Sums);

    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    return (myPoly);
}

/******************************************************************************
psVectorFitPolynomial4D():  This routine fits a 4D polynomial of arbitrary
degree (specified in poly) to the data points (x, y, z, t)-(f) and returns
that polynomial.  Types F32 and F64 are supported, however, type F32 is done
via vector conversion only.
 *****************************************************************************/
psPolynomial4D *psVectorFitPolynomial4D(
    psPolynomial4D *poly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z,
    const psVector *t)
{
    // Internal pointers for possibly NULL or mis-typed vectors.
    psVector *x64 = NULL;
    psVector *y64 = NULL;
    psVector *z64 = NULL;
    psVector *t64 = NULL;
    psVector *f64 = NULL;
    psVector *fErr64 = NULL;

    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL);

    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
    }


    //
    // Convert input vector to F64 if necessary.
    //
    if (f->type.type != PS_TYPE_F64) {
        f64 = psVectorCopy(NULL, f, PS_TYPE_F64);
    } else {
        f64 = (psVector *) f;
    }
    if (x->type.type != PS_TYPE_F64) {
        x64 = psVectorCopy(NULL, x, PS_TYPE_F64);
    } else {
        x64 = (psVector *) x;
    }
    if (y->type.type != PS_TYPE_F64) {
        y64 = psVectorCopy(NULL, y, PS_TYPE_F64);
    } else {
        y64 = (psVector *) y;
    }
    if (z->type.type != PS_TYPE_F64) {
        z64 = psVectorCopy(NULL, z, PS_TYPE_F64);
    } else {
        z64 = (psVector *) z;
    }
    if (t->type.type != PS_TYPE_F64) {
        t64 = psVectorCopy(NULL, t, PS_TYPE_F64);
    } else {
        t64 = (psVector *) t;
    }
    //
    // fErr
    //
    if (fErr != NULL) {
        if (fErr->type.type != PS_TYPE_F64) {
            fErr64 = psVectorCopy(NULL, fErr, PS_TYPE_F64);
        } else {
            fErr64 = (psVector *) fErr;
        }
    }

    if (poly->type == PS_POLYNOMIAL_ORD) {
        poly = VectorFitPolynomial4DOrd(poly, mask, maskValue, f64, fErr64, x64, y64, z64, t64);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, true, "Could not fit polynomial.  Returning NULL.\n");
            // Free psVectors that were created for NULL arguments.
            if (f->type.type != PS_TYPE_F64) {
                psFree(f64);
            }

            if (x->type.type != PS_TYPE_F64) {
                psFree(x64);
            }

            if (y->type.type != PS_TYPE_F64) {
                psFree(y64);
            }

            if (z->type.type != PS_TYPE_F64) {
                psFree(z64);
            }

            if (t->type.type != PS_TYPE_F64) {
                psFree(t64);
            }

            if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
                psFree(fErr64);
            }
            return(NULL);
        }
    } else if (poly->type == PS_POLYNOMIAL_CHEB) {
        if (mask != NULL) {
            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
        }
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: 4-D Chebyshev polynomial vector fitting has not been implemented.  Returning NULL.\n");
        psFree(poly);
        poly = NULL;
    } else {
        // Free psVectors that were created for NULL arguments.
        if (f->type.type != PS_TYPE_F64) {
            psFree(f64);
        }

        if (x->type.type != PS_TYPE_F64) {
            psFree(x64);
        }

        if (y->type.type != PS_TYPE_F64) {
            psFree(y64);
        }

        if (z->type.type != PS_TYPE_F64) {
            psFree(z64);
        }

        if (t->type.type != PS_TYPE_F64) {
            psFree(t64);
        }

        if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
            psFree(fErr64);
        }
        psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type.  Returning NULL.\n");
    }


    // Free psVectors that were created for NULL arguments.
    if (f->type.type != PS_TYPE_F64) {
        psFree(f64);
    }

    if (x->type.type != PS_TYPE_F64) {
        psFree(x64);
    }

    if (y->type.type != PS_TYPE_F64) {
        psFree(y64);
    }

    if (z->type.type != PS_TYPE_F64) {
        psFree(z64);
    }

    if (t->type.type != PS_TYPE_F64) {
        psFree(t64);
    }

    if ((fErr != NULL) && (fErr->type.type != PS_TYPE_F64)) {
        psFree(fErr64);
    }

    return(poly);
}


psPolynomial4D *psVectorClipFitPolynomial4D(
    psPolynomial4D *poly,
    psStats *stats,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z,
    const psVector *t)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_POLY_NON_NULL(poly, NULL);
    PS_ASSERT_POLY_TYPE(poly, PS_POLYNOMIAL_ORD, NULL);
    PS_ASSERT_PTR_NON_NULL(stats, NULL);
    PS_ASSERT_VECTOR_NON_NULL(mask, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, f->type.type, NULL);

    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, f->type.type, NULL);

    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    PS_ASSERT_VECTOR_TYPE(z, f->type.type, NULL);

    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL);
    PS_ASSERT_VECTOR_TYPE(t, f->type.type, NULL);

    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, f->type.type, NULL);
    }

    // clipping range defined by min and max and/or clipSigma
    psF32 minClipSigma;
    psF32 maxClipSigma;
    if (isfinite(stats->max)) {
        maxClipSigma = fabs(stats->max);
    } else {
        maxClipSigma = fabs(stats->clipSigma);
    }
    if (isfinite(stats->min)) {
        minClipSigma = fabs(stats->min);
    } else {
        minClipSigma = fabs(stats->clipSigma);
    }
    psVector *fit   = NULL;
    psVector *resid = psVectorAlloc(f->n, PS_TYPE_F64);

    // eventual expansion: user supplies one of various stats option pairs,
    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
    // evaluate the clipping sigma
    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
    psTrace(__func__, 4, "stats->clipIter is %d\n", stats->clipIter);
    psTrace(__func__, 4, "(minClipSigma, maxClipSigma) is (%.2f, %.2f)\n", minClipSigma, maxClipSigma);

    for (psS32 N = 0; N < stats->clipIter; N++) {
        psTrace(__func__, 6, "Loop iteration %d.  Calling psVectorFitPolynomial1D()\n");
        psS32 Nkeep = 0;
        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    psTrace(__func__, 6,  "mask[%d] is %d\n", i, mask->data.U8[i]);
                }
            }
        }

        poly = psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t);
        if (poly == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not fit a polynomial to the data.  Returning NULL.\n");
            psFree(resid)
            psFree(fit)
            return(NULL);
        }

        fit = psPolynomial4DEvalVector (poly, x, y, z, t);
        if (fit == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not call psPolynomial3DEvalVector().  Returning NULL.\n");
            psFree(resid)
            return(NULL);
        }
        for (psS32 i = 0 ; i < f->n ; i++) {
            if (f->type.type == PS_TYPE_F64) {
                resid->data.F64[i] = f->data.F64[i] - fit->data.F64[i];
            } else {
                resid->data.F64[i] = ((psF64) f->data.F32[i]) - fit->data.F64[i];
            }
        }

        if (psTraceGetLevel(__func__) >= 6) {
            if (mask != NULL) {
                for (psS32 i = 0 ; i < mask->n ; i++) {
                    if (!((mask != NULL) && (mask->data.U8[i] & maskValue))) {
                        psTrace(__func__, 6,  "(f, fit)[%d] is (%f, %f).  resid is (%f)\n",
                                i, f->data.F32[i], fit->data.F32[i], resid->data.F64[i]);
                    }
                }
            }
        }

        stats  = psVectorStats (stats, resid, NULL, mask, maskValue);
        if (stats == NULL) {
            psError(PS_ERR_UNKNOWN, false, "Could not compute statistics on the resid vector.  Returning NULL.\n");
            psFree(resid)
            psFree(fit)
            return(NULL);
        }
        psTrace(__func__, 6, "Median is %f\n", stats->sampleMedian);
        psTrace(__func__, 6, "Stdev is %f\n", stats->sampleStdev);
        psF32 minClipValue = -minClipSigma*stats->sampleStdev;
        psF32 maxClipValue = +maxClipSigma*stats->sampleStdev;

        // set mask if pts are not valid
        // we are masking out any point which is out of range
        // recovery is not allowed with this scheme
        for (psS32 i = 0; i < resid->n; i++) {
            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
                continue;
            }

            if ((resid->data.F64[i] - stats->sampleMedian > maxClipValue) ||
                    (resid->data.F64[i] - stats->sampleMedian < minClipValue)) {
                if (f->type.type == PS_TYPE_F64) {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F64[i], i, resid->data.F64[i]);
                } else {
                    psTrace(__func__, 6, "Masking element %d (%f).  resid->data.F64[%d] is %f\n",
                            i, fit->data.F32[i], i, resid->data.F64[i]);
                }

                if (mask != NULL) {
                    mask->data.U8[i] |= 0x01;
                }
                continue;
            }

            Nkeep++;
        }

        psTrace(__func__, 6, "keeping %d of %d pts for fit\n", Nkeep, x->n);
        psFree (fit);
    }
    // Free local temporary variables
    psFree (resid);

    if (poly == NULL) {
        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
        return(NULL);
    }

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