/** @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.142 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-10-12 21:02:20 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 *
 *  XXX: must follow coding name standards on local functions.
 *  XXX: put local functions in front.
 * 
 */
/*****************************************************************************/
/* INCLUDE FILES                                                             */
/*****************************************************************************/
#include <stdio.h>
#include <float.h>
#include <math.h>

#include "psMinimize.h"
#include "psStats.h"
#include "psImage.h"
#include "psImageStructManip.h"
#include "psBinaryOp.h"
#include "psLogMsg.h"
/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/

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

/*****************************************************************************/
/* GLOBAL VARIABLES                                                          */
/* XXX: Do these conform to code standard?         */
/*****************************************************************************/
static psMinimizeChi2PowellFunc Chi2PowellFunc = NULL;
static psVector *myValue;
static psVector *myError;
/*****************************************************************************/
/* FILE STATIC VARIABLES                                                     */
/*****************************************************************************/

// None

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

/******************************************************************************
 ******************************************************************************
 Levenberg-Marquadt routines.
 ******************************************************************************
 *****************************************************************************/
// XXX EAM : can we use static copies of LUv, LUm, A?
psBool p_psMinLM_GuessABP(
    psImage  *Alpha,
    psVector *Beta,
    psVector *Params,
    const psImage  *alpha,
    const psVector *beta,
    const psVector *params,
    const psVector *paramMask,
    const psVector *beta_lim,
    const psVector *params_min,
    const psVector *params_max,
    psF64 lambda)
{
    # define USE_LU_DECOMP 1
    # if (USE_LU_DECOMP)
        psVector *LUv = NULL;
    psImage  *LUm = NULL;
    psImage  *A   = NULL;
    psF32    det;

    // LU decomposition version
    psTrace(__func__, 5, "using LUD version\n");

    // set new guess values (creates matrix A)
    A = psImageCopy(NULL, alpha, PS_TYPE_F64);
    for (int j = 0; j < params->n; j++) {
        if ((paramMask != NULL) && (paramMask->data.U8[j]))
            continue;
        A->data.F64[j][j] = alpha->data.F64[j][j] * (1.0 + lambda);
    }

    // solve A*beta = Beta (Alpha = 1/A)
    // these operations do not modify the input values (creates LUm, LUv)
    LUm   = psMatrixLUD(NULL, &LUv, A);
    Beta  = psMatrixLUSolve(Beta, LUm, beta, LUv);
    Alpha = psMatrixInvert(Alpha, A, &det);

    # else
        // gauss-jordan version
        psTrace(__func__, 5, "using Gauss-J version");

    // set new guess values (creates matrix A)
    Beta = psVectorCopy(Beta, beta, PS_TYPE_F64);
    Alpha = psImageCopy(Alpha, alpha, PS_TYPE_F64);
    for (int j = 0; j < params->n; j++) {
        if ((paramMask != NULL) && (paramMask->data.U8[j]))
            continue;
        Alpha->data.F64[j][j] = alpha->data.F64[j][j] * (1.0 + lambda);
    }

    psGaussJordan(Alpha, Beta);
    psFree(A);
    psFree(LUm);
    psFree(LUv);
    # endif

    // apply Beta to get new Params values
    for (int j = 0; j < params->n; j++) {
        if ((paramMask != NULL) && (paramMask->data.U8[j]))
            continue;
        // Params->data.F32[j] = params->data.F32[j] - Beta->data.F64[j];
        // compare Beta to beta limits
        if (beta_lim != NULL) {
            if (fabs(Beta->data.F64[j]) > fabs(beta_lim->data.F32[j])) {
                Beta->data.F64[j] = (Beta->data.F64[j] > 0) ? fabs(beta_lim->data.F32[j]) : -fabs(beta_lim->data.F32[j]);
            }
        }
        Params->data.F32[j] = params->data.F32[j] - Beta->data.F64[j];
        // compare new params to param limits
        if (params_max != NULL) {
            Params->data.F32[j] = PS_MIN (Params->data.F32[j], params_max->data.F32[j]);
        }
        if (params_min != NULL) {
            Params->data.F32[j] = PS_MAX (Params->data.F32[j], params_min->data.F32[j]);
        }
    }
    # if (USE_LU_DECOMP)
        psFree(A);
    psFree(LUm);
    psFree(LUv);
    # endif

    return(true);
}


bool psMinimizeGaussNewtonDelta(
    psVector *delta,
    const psVector *params,
    const psVector *paramMask,
    const psArray  *x,
    const psVector *y,
    const psVector *yWt,
    psMinimizeLMChi2Func func)
{

    // allocate internal arrays (current vs Guess)
    psImage  *alpha  = psImageAlloc (params->n, params->n, PS_TYPE_F64);
    psImage  *Alpha  = psImageAlloc (params->n, params->n, PS_TYPE_F64);
    psVector *beta   = psVectorAlloc(params->n, PS_TYPE_F64);
    psVector *Params = psVectorAlloc(params->n, PS_TYPE_F64);
    psVector *dy     = NULL;

    // the user provides the error or NULL.  we need to convert
    // to appropriate weights
    if (yWt != NULL) {
        dy = (psVector *) yWt;
    } else {
        dy = psVectorAlloc(y->n, PS_TYPE_F32);
        psVectorInit(dy, 1.0);
    }

    p_psMinLM_SetABX(alpha, beta, params, paramMask, x, y, dy, func);
    p_psMinLM_GuessABP(Alpha, delta, Params, alpha, beta, params, paramMask, NULL, NULL, NULL, 0.0);

    psFree(alpha);
    psFree(Alpha);
    psFree(beta);
    psFree(Params);
    if (yWt == NULL) {
        psFree(dy);
    }
    return (true);
}

// measure linear model prediction
psF64 p_psMinLM_dLinear(
    const psVector *Beta,
    const psVector *beta,
    psF64 lambda)
{

    /* get linear model prediction */
    psF64 dLinear = 0;
    psF64 *B = Beta->data.F64;
    psF64 *b = beta->data.F64;
    for (int i = 0; i < beta->n; i++) {
        dLinear += lambda*PS_SQR(B[i]) + B[i]*b[i];
    }
    return(0.5*dLinear);
}

// XXX EAM: this needs to respect the mask on params
// alpha, beta, params are already allocated
psF64 p_psMinLM_SetABX(
    psImage  *alpha,
    psVector *beta,
    const psVector *params,
    const psVector *paramMask,
    const psArray  *x,
    const psVector *y,
    const psVector *dy,
    psMinimizeLMChi2Func func)
{
    PS_ASSERT_IMAGE_NON_NULL(alpha, NAN);
    PS_ASSERT_VECTOR_NON_NULL(beta, NAN);
    PS_ASSERT_VECTOR_NON_NULL(params, NAN);
    PS_ASSERT_PTR_NON_NULL(x, NAN);
    PS_ASSERT_VECTOR_NON_NULL(y, NAN);
    PS_ASSERT_VECTOR_NON_NULL(dy, NAN);

    psF64 chisq;
    psF64 delta;
    psF64 weight;
    psF64 ymodel;
    psVector *deriv = psVectorAlloc(params->n, PS_TYPE_F32);

    // zero alpha and beta for summing below
    for (int j = 0; j < params->n; j++) {
        for (int k = 0; k < params->n; k++) {
            alpha->data.F64[j][k] = 0;
        }
        beta->data.F64[j] = 0;
    }
    chisq = 0.0;

    // calculate chisq, alpha, beta
    for (int i = 0; i < y->n; i++) {
        ymodel = func(deriv, params, (psVector *) x->data[i]);

        delta = ymodel - y->data.F32[i];
        chisq += PS_SQR(delta) * dy->data.F32[i];

        for (int j = 0; j < params->n; j++) {
            if ((paramMask != NULL) && (paramMask->data.U8[j]))
                continue;
            weight = deriv->data.F32[j] * dy->data.F32[i];
            for (int k = 0; k <= j; k++) {
                if ((paramMask != NULL) && (paramMask->data.U8[k]))
                    continue;
                alpha->data.F64[j][k] += weight * deriv->data.F32[k];
            }
            beta->data.F64[j] += weight * delta;
        }
    }

    // calculate lower-left half of alpha
    for (int j = 1; j < params->n; j++) {
        for (int k = 0; k < j; k++) {
            alpha->data.F64[k][j] = alpha->data.F64[j][k];
        }
    }

    // fill in pivots if we apply a mask
    if (paramMask != NULL) {
        for (int j = 0; j < params->n; j++) {
            if (paramMask->data.U8[j]) {
                alpha->data.F64[j][j] = 1;
                beta->data.F64[j] = 1;
            }
        }
    }

    psFree(deriv);
    return(chisq);
}


/******************************************************************************
psMinimizeLMChi2():  This routine will take an procedure which calculates an
arbitrary function and it's derivative and minimize the chi-squared match
between that function at the specified coords and the specified value at those
coords.
 
XXX: Put the ASSERTS in.
 
XXX EAM this is my re-implementation of MinLM
 
XXX: This must work for both F32 and F64.  F32 is currently implemented.
     Note: since the LUD routines are only implemented in F64, then we
     will have to convert all F32 input vectors to F64 regardless.  So,
     the F64 port might be.
 
XXX: Change the whole thing to F64, if input data is F32, convert it.
 *****************************************************************************/
psBool psMinimizeLMChi2(
    psMinimization *min,
    psImage *covar,
    psVector *params,
    const psVector *paramMask,
    const psArray *x,
    const psVector *y,
    const psVector *yWt,
    psMinimizeLMChi2Func func)
{
    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    PS_ASSERT_PTR_NON_NULL(min, false);
    // XXX: If covar not NULL, do asserts...
    PS_ASSERT_VECTOR_NON_NULL(params, false);
    PS_ASSERT_VECTOR_NON_EMPTY(params, false);
    PS_ASSERT_VECTOR_TYPE(params, PS_TYPE_F32, false);
    if (paramMask != NULL) {
        PS_ASSERT_VECTOR_TYPE(paramMask, PS_TYPE_U8, false);
        PS_ASSERT_VECTORS_SIZE_EQUAL(params, paramMask, false);
    }
    PS_ASSERT_PTR_NON_NULL(x, false);
    for (psS32 i = 0 ; i < x->n ; i++) {
        psVector *coord = (psVector *) (x->data[i]);
        PS_ASSERT_VECTOR_NON_NULL(coord, false);
        PS_ASSERT_VECTOR_TYPE(coord, PS_TYPE_F32, false);
    }
    PS_ASSERT_VECTOR_NON_NULL(y, false);
    PS_ASSERT_VECTOR_NON_EMPTY(y, false);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, false);
    if (yWt != NULL) {
        PS_ASSERT_VECTOR_TYPE(yWt, PS_TYPE_F32, false);
        PS_ASSERT_VECTORS_SIZE_EQUAL(y, yWt, false);
    }
    PS_ASSERT_PTR_NON_NULL(func, false);

    // this function has test and current values for several things
    // the current best value is in lower case
    // the next guess value is in upper case

    // allocate internal arrays (current vs Guess)
    psImage *alpha   = psImageAlloc(params->n, params->n, PS_TYPE_F64);
    psImage *Alpha   = psImageAlloc(params->n, params->n, PS_TYPE_F64);
    psVector *beta   = psVectorAlloc(params->n, PS_TYPE_F64);
    psVector *Beta   = psVectorAlloc(params->n, PS_TYPE_F64);
    psVector *Params = psVectorAlloc(params->n, PS_TYPE_F32);
    psVector *dy     = NULL;
    psF64 Chisq = 0.0;
    psF64 lambda = 0.001;
    // XXX: Code this properly.  Don't use mustFree00.
    psBool mustFree00 = false;
    psVector *beta_lim = NULL;
    psVector *param_min = NULL;
    psVector *param_max = NULL;

    // if we are provided a covar image, we expect to find these three vectors in first three rows
    if (covar != NULL) {
        mustFree00 = true;
        beta_lim  = psVectorAlloc(params->n, PS_TYPE_F32);
        param_min = psVectorAlloc(params->n, PS_TYPE_F32);
        param_max = psVectorAlloc(params->n, PS_TYPE_F32);
        for (int i = 0; i < params->n; i++) {
            beta_lim->data.F32[i] = covar->data.F64[0][i];
            param_min->data.F32[i] = covar->data.F64[1][i];
            param_max->data.F32[i] = covar->data.F64[2][i];
        }
        psImageRecycle(covar, params->n, params->n, PS_TYPE_F64);
    }

    // why is this needed here??? the initial guess on params is provided by the user
    Params = psVectorCopy(Params, params, PS_TYPE_F32);

    // the user provides the error or NULL.  we need to convert
    // to appropriate weights
    if (yWt != NULL) {
        dy = (psVector *) yWt;
    } else {
        dy = psVectorAlloc(y->n, PS_TYPE_F32);
        psVectorInit(dy, 1.0);
    }

    // calculate initial alpha and beta, set chisq (min->value)
    min->value = p_psMinLM_SetABX(alpha, beta, params, paramMask, x, y, dy, func);
    if (isnan(min->value)) {
        min->iter = min->maxIter;
        return(false);
    }
    // dump some useful info if trace is defined
    if (psTraceGetLevel(__func__) >= 6) {
        p_psImagePrint(psTraceGetDestination(), alpha, "alpha guess (0)");
        p_psVectorPrint(psTraceGetDestination(), beta, "beta guess (0)");
        p_psVectorPrint(psTraceGetDestination(), params, "params guess (0)");
    }
    if (psTraceGetLevel (__func__) >= 6) {
        //XXX:  p_psVectorPrintRow(psTraceGetDestination(), Params, "params guess");
    }

    // iterate until the tolerance is reached, or give up
    while ((min->iter < min->maxIter) && ((min->lastDelta > min->tol) || !isfinite(min->lastDelta))) {
        psTrace(__func__, 5, "Iteration number %d.  (max iterations is %d).\n", min->iter, min->maxIter);
        psTrace(__func__, 5, "Last delta is %f.  Min->tol is %f.\n", min->lastDelta, min->tol);

        // set a new guess for Alpha, Beta, Params
        p_psMinLM_GuessABP(Alpha, Beta, Params, alpha, beta, params, paramMask,
                           beta_lim, param_min, param_max, lambda);

        // measure linear model prediction
        psF64 dLinear = p_psMinLM_dLinear(Beta, beta, lambda);

        // dump some useful info if trace is defined
        if (psTraceGetLevel(__func__) >= 6) {
            p_psImagePrint(psTraceGetDestination(), Alpha, "alpha guess (1)");
            p_psVectorPrint(psTraceGetDestination(), Beta, "beta guess (1)");
            p_psVectorPrint(psTraceGetDestination(), Params, "params guess (1)");
        }
        if (psTraceGetLevel(__func__) >= 6) {
            //XXX: p_psVectorPrintRow(psTraceGetDestination(), Params, "params guess");
        }

        // calculate Chisq for new guess, update Alpha & Beta
        Chisq = p_psMinLM_SetABX(Alpha, Beta, Params, paramMask, x, y, dy, func);

        // XXX EAM alternate convergence criterion:
        // compare the delta (min->value - Chisq) with the
        // expected delta from the linear model (dLinear)
        // accept new guess (if improvement), or increase lambda
        psF64 rho = (min->value - Chisq) / dLinear;

        psTrace(__func__, 5, "last chisq: %f, new chisq %f, delta: %f, rho: %f\n", min->value,
                Chisq, min->lastDelta, rho);

        // dump some useful info if trace is defined
        if (psTraceGetLevel(__func__) >= 6) {
            p_psImagePrint(psTraceGetDestination(), Alpha, "alpha guess (2)");
            p_psVectorPrint(psTraceGetDestination(), Beta, "beta guess (2)");
            p_psVectorPrint(psTraceGetDestination(), Params, "params guess (2)");
        }

        /* if (Chisq < min->value) {  */
        if (rho > 0.0) {
            min->lastDelta = (min->value - Chisq) / (dy->n - params->n);
            min->value = Chisq;
            alpha  = psImageCopy(alpha, Alpha, PS_TYPE_F64);
            beta   = psVectorCopy(beta, Beta, PS_TYPE_F64);
            params = psVectorCopy(params, Params, PS_TYPE_F32);
            lambda *= 0.1;
        } else {
            lambda *= 10.0;
        }
        min->iter++;
    }
    psTrace(__func__, 5, "chisq: %f, last delta: %f, Niter: %d\n", min->value, min->lastDelta, min->iter);

    // construct & return the covariance matrix (if requested)
    if (covar != NULL) {
        p_psMinLM_GuessABP(covar, Beta, Params, alpha, beta, params, paramMask,
                           beta_lim, param_min, param_max, 0.0);
    }

    // free the internal temporary data
    psFree(alpha);
    psFree(Alpha);
    psFree(beta);
    psFree(Beta);
    psFree(Params);
    if (yWt == NULL) {
        psFree(dy);
    }
    if (mustFree00 == true) {
        psFree(beta_lim);
        psFree(param_min);
        psFree(param_max);
    }
    if (min->iter == min->maxIter) {
        psTrace(__func__, 3, "---- %s(false) end ----\n", __func__);
        return(false);
    }
    psTrace(__func__, 3, "---- %s(true) end ----\n", __func__);
    return(true);
}

// XXX EAM : temporary gauss-jordan solver based on gene's
// version based on the Numerical Recipes version
bool psGaussJordan(
    psImage *a,
    psVector *b)
{

    int *indxc,*indxr,*ipiv;
    int Nx, icol, irow;
    int i, j, k, l, ll;
    float big, dum, pivinv;
    psF64 *vector;
    psF64 **matrix;

    Nx = a->numCols;
    matrix = a->data.F64;
    vector = b->data.F64;

    indxc = psAlloc(Nx*sizeof(int));
    indxr = psAlloc(Nx*sizeof(int));
    ipiv  = psAlloc(Nx*sizeof(int));
    for (j = 0; j < Nx; j++) {
        ipiv[j] = 0;
    }

    irow = icol = 0;
    big = fabs(matrix[0][0]);

    for (i = 0; i < Nx; i++) {
        big = 0.0;
        for (j = 0; j < Nx; j++) {
            if (!isfinite(matrix[i][j])) {
                psError(PS_ERR_UNKNOWN, false, "Input matrix contains NaNs.\n");
                goto fescape;
            }
            if (ipiv[j] != 1) {
                for (k = 0; k < Nx; k++) {
                    if (ipiv[k] == 0) {
                        if (fabs (matrix[j][k]) >= big) {
                            big  = fabs(matrix[j][k]);
                            irow = j;
                            icol = k;
                        }
                    } else {
                        if (ipiv[k] > 1) {
                            psError(PS_ERR_UNKNOWN, false, "Singular Matrix (1).\n");
                            goto fescape;
                        }
                    }
                }
            }
        }
        ipiv[icol]++;
        if (irow != icol) {
            for (l = 0; l < Nx; l++) {
                PS_SWAP(matrix[irow][l], matrix[icol][l]);
            }
            PS_SWAP(vector[irow], vector[icol]);
        }
        indxr[i] = irow;
        indxc[i] = icol;
        if (matrix[icol][icol] == 0.0) {
            psError(PS_ERR_UNKNOWN, false, "Singular Matrix (2).\n");
            goto fescape;
        }
        pivinv = 1.0 / matrix[icol][icol];
        matrix[icol][icol] = 1.0;
        for (l = 0; l < Nx; l++) {
            matrix[icol][l] *= pivinv;
        }
        vector[icol] *= pivinv;

        for (ll = 0; ll < Nx; ll++) {
            if (ll != icol) {
                dum = matrix[ll][icol];
                matrix[ll][icol] = 0.0;
                for (l = 0; l < Nx; l++) {
                    matrix[ll][l] -= matrix[icol][l]*dum;
                }
                vector[ll] -= vector[icol]*dum;
            }
        }
    }

    for (l = Nx - 1; l >= 0; l--) {
        if (indxr[l] != indxc[l]) {
            for (k = 0; k < Nx; k++) {
                PS_SWAP(matrix[k][indxr[l]], matrix[k][indxc[l]]);
            }
        }
    }
    psFree(ipiv);
    psFree(indxr);
    psFree(indxc);
    return(true);

fescape:
    psFree(ipiv);
    psFree(indxr);
    psFree(indxc);
    return(false);
}

static void minimizationFree(psMinimization *min)
{
    // There are no dynamically allocated items
}

/******************************************************************************
 *****************************************************************************/
psMinimization *psMinimizationAlloc(int maxIter,
                                    float tol)
{
    PS_ASSERT_INT_NONNEGATIVE(maxIter, NULL);

    psMinimization *min = psAlloc(sizeof(psMinimization));
    psMemSetDeallocator(min, (psFreeFunc)minimizationFree);
    *(int*)&min->maxIter = maxIter;
    *(float*)&min->tol = tol;
    min->value = 0.0;
    min->iter = 0;
    min->lastDelta = NAN;

    return(min);
}

bool psMemCheckMinimization(psPtr ptr)
{
    return( psMemGetDeallocator(ptr) == (psFreeFunc)minimizationFree );
}


// This macro takes as input the vector BASE and adds a multiple of the vector
// LINE to it.  We assume BASEMASK is non-null.
#define PS_VECTOR_ADD_MULTIPLE(BASE, BASEMASK, LINE, OUT, MUL) \
for (psS32 i=0;i<BASE->n;i++) { \
    if (BASEMASK->data.U8[i] == 0) { \
        OUT->data.F32[i] = BASE->data.F32[i] + (MUL * LINE->data.F32[i]); \
    } else { \
        OUT->data.F32[i] = BASE->data.F32[i]; \
    } \
} \

#define PS_VECTOR_F32_CHECK_ZERO_VECTOR(IN, BOOL_VAR) \
BOOL_VAR = true; \
for (psS32 i=0;i<IN->n;i++) { \
    if (fabs(IN->data.F32[i]) >= FLT_EPSILON) { \
        BOOL_VAR = false; \
        break; \
    } \
} \

#define PS_VECTOR_WITH_MASK_F32_CHECK_ZERO_VECTOR(IN, INMASK, BOOL_VAR) \
BOOL_VAR = true; \
for (psS32 i=0;i<IN->n;i++) { \
    if ((INMASK->data.U8[i] == 0) && (fabs(IN->data.F32[i]) >= FLT_EPSILON)) { \
        BOOL_VAR = false; \
        break; \
    } \
} \


/******************************************************************************
 ******************************************************************************
Powell routines.
******************************************************************************
*****************************************************************************/

/******************************************************************************
p_psDetermineBracket():  This routine takes as input an arbitrary function,
and the parameter to vary, and the line along which it must vary.  This
function produces as output a bracket [a, b, c] such that
f(param + b * line) < f(param + a * line)
f(param + b * line) < f(param + c * line)
a < b < c
 
Algorithm:
 
XXX completely ad hoc: start with the user-supplied starting parameter and
call that b.  Calculate a/c as a fractional amount smaller/larger than b.
Repeat this process until a local minimum is found.
 
XXX: new algorithm:  start at x=0, expand in one direction until the function
decreases.  Then you have two points in the bracket.  Keep going until it
increases, or x is too large.  If thst does not work, expand in the other
direction.
 
XXX: This is F32 only.  Must add F64 support (actually, make the defaults F64,
and convert F32 vectors to F64).
 
XXX: output bracket vector should be an input as well.
*****************************************************************************/
psVector *p_psDetermineBracket(psVector *params,
                               psVector *line,
                               const psVector *paramMask,
                               const psArray *coords,
                               psMinimizePowellFunc func)
{
    psF32 a = 0.0;
    psF32 b = 0.0;
    psF32 c = 0.0;
    psF32 fa = 0.0;
    psF32 fb = 0.0;
    psF32 fc = 0.0;
    psS32 iter = 100;
    psF32 aDir = 0.0;
    psF32 cDir = 0.0;
    psF32 new_aDir = 0.0;
    psF32 new_cDir = 0.0;
    psVector *bracket = psVectorAlloc(3, PS_TYPE_F32);
    psF32 stepSize = PS_DETERMINE_BRACKET_STEP_SIZE;
    psVector *tmp = NULL;
    psBool boolLineIsNull = true;

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

    // If the line vector is zero, then return NULL.
    PS_VECTOR_WITH_MASK_F32_CHECK_ZERO_VECTOR(params, paramMask, boolLineIsNull);
    if (boolLineIsNull == true) {
        psTrace(".psLib.dataManip.p_psDetermineBracket", 2,
                "p_psDetermineBracket() called with zero line vector.\n");
        psTrace(".psLib.dataManip.p_psDetermineBracket", 4,
                "---- p_psDetermineBracket() end (NULL) ----\n");
        psFree(bracket);
        return(NULL);
    }

    tmp = psVectorAlloc(params->n, PS_TYPE_F32);

    b = 0;
    a = -stepSize;
    c = stepSize;

    PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, a);
    fa = func(tmp, coords);

    PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, b);
    fb = func(tmp, coords);

    PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, c);
    fc = func(tmp, coords);

    if (fa < fb) {
        aDir = -1;
    } else {
        aDir = 1;
    }

    if (fc < fb) {
        cDir = -1;
    } else {
        cDir = 1;
    }

    psTrace(".psLib.dataManip.p_psDetermineBracket", 6,
            "(a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", a, b, c, fa, fb, fc);

    while (iter > 0) {
        psTrace(".psLib.dataManip.p_psDetermineBracket", 6,
                "psDetermineBracket(): iteration %d\n", iter);
        if ((fb < fa) && (fb < fc)) {
            bracket->data.F32[0] = a;
            bracket->data.F32[1] = b;
            bracket->data.F32[2] = c;
            psFree(tmp);
            psTrace(".psLib.dataManip.p_psDetermineBracket", 6,
                    "---- p_psDetermineBracket() end ----\n");
            return(bracket);
        }
        stepSize*= (1.0 + stepSize);
        a =- stepSize;
        c =+ stepSize;

        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, a);
        fa = func(tmp, coords);

        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, c);
        fc = func(tmp, coords);

        psTrace(".psLib.dataManip.p_psDetermineBracket", 6,
                "Iter(%d): (a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", iter, a, b, c, fa, fb, fc);

        if (fa < fb) {
            new_aDir = -1;
        } else {
            new_aDir = 1;
        }

        if (fc < fb) {
            new_cDir = -1;
        } else {
            new_cDir = 1;
        }
        if ((new_aDir == 1) && (aDir == -1)) {
            bracket->data.F32[0] = a;
            bracket->data.F32[1] = b;
            bracket->data.F32[2] = c;
            psFree(tmp);
            psTrace(".psLib.dataManip.p_psDetermineBracket", 4,
                    "---- p_psDetermineBracket() end ----\n");
            return(bracket);
        }

        if ((new_cDir == 1) && (cDir == -1)) {
            bracket->data.F32[0] = a;
            bracket->data.F32[1] = b;
            bracket->data.F32[2] = c;
            psFree(tmp);
            psTrace(".psLib.dataManip.p_psDetermineBracket", 4,
                    "---- p_psDetermineBracket() end ----\n");
            return(bracket);
        }
        aDir = new_aDir;
        cDir = new_cDir;
        iter--;
    }
    psFree(tmp);
    psFree(bracket);
    psTrace(".psLib.dataManip.p_psDetermineBracket", 4,
            "---- p_psDetermineBracket() end (NULL) ----\n");
    return(NULL);
}


#define RETURN_FINAL_BRACKET(d) \
if (a < c) { \
    bracket->data.F32[0] = a; \
    bracket->data.F32[1] = b; \
    bracket->data.F32[2] = c; \
} else { \
    bracket->data.F32[0] = c; \
    bracket->data.F32[1] = b; \
    bracket->data.F32[2] = a; \
} \
psTrace(".psLib.dataManip.p_psDetermineBracket", 4, \
        "---- p_psDetermineBracket() end ----\n"); \
psTrace(".psLib.dataManip.p_psDetermineBracket", 4, "Final bracket (a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", a, b, c, fa, fb, fc); \
return(bracket); \

#define PS_DETERMINE_BRACKET_MAX_ITERATIONS 100
psVector *p_psDetermineBracket2(psVector *params,
                                psVector *line,
                                const psVector *paramMask,
                                const psArray *coords,
                                psMinimizePowellFunc func)
{
    psF32 a = 0.0;
    psF32 b = 0.0;
    psF32 c = 0.0;
    psF32 fa = 0.0;
    psF32 fb = 0.0;
    psF32 fc = 0.0;
    psS32 iter = 0;
    PS_VECTOR_GEN_STATIC_RECYCLED(tmp, params->n, PS_TYPE_F32);
    psBool boolLineIsNull = true;
    psF32 prevMin = 0.0;
    psS32 countMin = 0;

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

    // If the line vector is zero, then return NULL.
    PS_VECTOR_WITH_MASK_F32_CHECK_ZERO_VECTOR(params, paramMask, boolLineIsNull);
    if (boolLineIsNull == true) {
        psTrace(".psLib.dataManip.p_psDetermineBracket", 2,
                "p_psDetermineBracket() called with zero line vector.\n");
        psTrace(".psLib.dataManip.p_psDetermineBracket", 4,
                "---- p_psDetermineBracket() end (NULL) ----\n");
        return(NULL);
    }

    // We determine in what x-direction does the function decrease.
    a = 0.0;
    fa = func(params, coords);
    b = 0.5;
    iter = 0;
    do {
        b*= (1.0 + PS_DETERMINE_BRACKET_STEP_SIZE);
        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, b);
        fb = func(tmp, coords);
    } while ((fabs(fb - fa) < FLT_EPSILON) && (iter++ < 100));

    if (fb > fa) {
        a = b;
        fa = fb;
        b = 0.0;
        fb = func(params, coords);
    }
    c = b;

    // At this point we have (a, b) and we know that (fa >= fb).  Initially, c=b;
    // We keep stretching b out further from "a" until (fc > previous fc).  If
    // that happens, then we have our bracket.
    psVector *bracket = psVectorAlloc(3, PS_TYPE_F32);
    iter = 0;
    while (iter < PS_DETERMINE_BRACKET_MAX_ITERATIONS) {
        psTrace(".psLib.dataManip.p_psDetermineBracket", 6,
                "psDetermineBracket(): iterationA %d\n", iter);
        c+= (1.0 + PS_DETERMINE_BRACKET_STEP_SIZE) * (c - a);

        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmp, c);
        fc = func(tmp, coords);

        psTrace(".psLib.dataManip.p_psDetermineBracket", 6,
                "Iteration(%d) (bracket): (a, b, c) is (%f %f %f) (fa, fb, fc) is (%f %f %f)\n", iter, a, b, c, fa, fb, fc);

        if ((fb < fa) && (fb < fc)) {
            RETURN_FINAL_BRACKET();
        } else {
            b = c;
            fb = fc;
        }

        // This code maintains a count of how many times the minimum fc has
        // stayed the same.  If it gets too high, we exit this loop.
        if (fc == prevMin) {
            countMin++;
        } else {
            countMin = 0;
        }
        prevMin = fc;
        if (countMin == 10) {
            RETURN_FINAL_BRACKET();
        }

        iter++;
    }

    psFree(bracket);
    psTrace(".psLib.dataManip.p_psDetermineBracket", 4,
            "---- p_psDetermineBracket() end (NULL) (BAD) ----\n");
    return(NULL);
}

/******************************************************************************
This routine takes as input a possibly multi-dimensional function, along
with an initial guess at the parameters of that function and vector "line"
of the same size as the parameter vector.  It will minimize the function
along that vector and returns the offset along that vector at which the
minimum is determined.
 
XXX: This routine is not very efficient in terms of total evaluations of the
function.
XXX: This is F32 only (make it F64).
XXX: Since this is an internal function, many of the parameter checks are
     redundant.
XXX: Don't modify the psMinimization argument.
 *****************************************************************************/
#define PS_LINEMIN_MAX_ITERATIONS 30
psF32 p_psLineMin(psMinimization *min,
                  psVector *params,
                  psVector *line,
                  const psVector *paramMask,
                  const psArray *coords,
                  psMinimizePowellFunc func)
{
    PS_ASSERT_PTR_NON_NULL(min, NAN);
    PS_ASSERT_VECTOR_NON_NULL(params, NAN);
    PS_ASSERT_VECTOR_NON_EMPTY(params, NAN);
    PS_ASSERT_VECTOR_TYPE(params, PS_TYPE_F32, NAN);
    PS_ASSERT_VECTOR_NON_NULL(line, NAN);
    PS_ASSERT_VECTOR_NON_EMPTY(line, NAN);
    PS_ASSERT_VECTOR_TYPE(line, PS_TYPE_F32, NAN);
    PS_ASSERT_VECTOR_NON_NULL(paramMask, NAN);
    PS_ASSERT_VECTOR_NON_EMPTY(paramMask, NAN);
    PS_ASSERT_VECTOR_TYPE(paramMask, PS_TYPE_U8, NAN);
    PS_ASSERT_PTR_NON_NULL(coords, NAN);
    PS_ASSERT_PTR_NON_NULL(func, NAN);
    psVector *bracket;
    psF32 a = 0.0;
    psF32 b = 0.0;
    psF32 c = 0.0;
    psF32 n = 0.0;
    psF32 fa = 0.0;
    psF32 fb = 0.0;
    psF32 fc = 0.0;
    psF32 fn = 0.0;
    psF32 mul = 0.0;
    PS_VECTOR_GEN_STATIC_RECYCLED(tmpa, params->n, PS_TYPE_F32);
    PS_VECTOR_GEN_STATIC_RECYCLED(tmpb, params->n, PS_TYPE_F32);
    PS_VECTOR_GEN_STATIC_RECYCLED(tmpc, params->n, PS_TYPE_F32);
    PS_VECTOR_GEN_STATIC_RECYCLED(tmpn, params->n, PS_TYPE_F32);
    psS32 i = 0;
    psS32 boolLineIsNull = true;
    psS32 numIterations = 0;

    psTrace(".psLib.dataManip.p_psLineMin", 4, "---- p_psLineMin() begin ----\n");
    PS_VECTOR_F32_CHECK_ZERO_VECTOR(line, boolLineIsNull);

    if (boolLineIsNull == true) {
        min->value = func(params, coords);
        psTrace(".psLib.dataManip.p_psLineMin", 2,
                "p_psLineMin() called with zero line vector.  Return 0.0.  Function value is %f\n", min->value);
        return(0.0);
    }

    for (i=0;i<params->n;i++) {
        psTrace(".psLib.dataManip.p_psLineMin", 6,
                "(params, paramMask, line)[%d] is (%f %d %f)\n", i,
                params->data.F32[i],
                paramMask->data.U8[i],
                line->data.F32[i]);
    }

    bracket = p_psDetermineBracket2(params, line, paramMask, coords, func);
    if (bracket == NULL) {
        psError(PS_ERR_UNKNOWN, false,
                "Could not bracket minimum.  Returning NAN.\n");
        return(NAN);
    }
    numIterations = 0;
    while (numIterations < PS_LINEMIN_MAX_ITERATIONS) {
        numIterations++;
        psTrace(".psLib.dataManip.p_psLineMin", 6,
                "p_psLineMin(): iteration %d\n", numIterations);

        a = bracket->data.F32[0];
        b = bracket->data.F32[1];
        c = bracket->data.F32[2];
        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmpa, a);
        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmpb, b);
        PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, tmpc, c);
        fa = func(tmpa, coords);
        fb = func(tmpb, coords);
        fc = func(tmpc, coords);
        psTrace(".psLib.dataManip.p_psLineMin", 6,
                "LineMin: f(%f %f %f) is (%f %f %f)\n", a, b, c, fa, fb, fc);

        // We determine which is the biggest segment in [a,b,c] then split
        // that with the point n.
        if ((b-a) > (c-b)) {
            // This is the golden section formula
            n = a + (0.69 * (b-a));
            for (i=0;i<params->n;i++) {
                tmpn->data.F32[i] = params->data.F32[i] + (n * line->data.F32[i]);
            }
            fn = func(tmpn, coords);

            if (fn > fb) {
                // a = n, b = b, c = c
                bracket->data.F32[0] = n;
            } else {
                // a = a, b = n, c = b
                bracket->data.F32[1] = n;
                bracket->data.F32[2] = b;
            }
        } else {
            n = b + (0.69 * (c-b));
            for (i=0;i<params->n;i++) {
                tmpn->data.F32[i] = params->data.F32[i] + (n * line->data.F32[i]);
            }
            fn = func(tmpn, coords);

            if (fn > fb) {
                // a = a, b = b, c = n
                bracket->data.F32[2] = n;
            } else {
                // a = b, b = n, c = c
                bracket->data.F32[0] = b;
                bracket->data.F32[1] = n;
            }
        }
        psTrace(".psLib.dataManip.p_psLineMin", 6,
                "LineMin: new bracket is (%f %f %f)\n", bracket->data.F32[0], bracket->data.F32[1], bracket->data.F32[2]);

        mul = bracket->data.F32[1];
        if ((fabs(a-b) < min->tol) && (fabs(b-c) < min->tol)) {
            PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, params, mul);
            min->value = func(params, coords);
            psFree(bracket);
            psTrace(".psLib.dataManip.p_psLineMin", 4,
                    "---- p_psLineMin() end.a (%f) (%f) ----\n", mul, min->value);
            return(mul);
        }
    }

    mul = bracket->data.F32[1];
    PS_VECTOR_ADD_MULTIPLE(params, paramMask, line, params, mul);
    min->value = func(params, coords);
    psTrace(".psLib.dataManip.p_psLineMin", 4,
            "---- p_psLineMin() end.b (%f) %f ----\n", mul, min->value);

    psFree(bracket);
    return(mul);
}


/******************************************************************************
This routine must minimize a possibly multi-dimensional function.  The
function to be minimized "func" is:
    psF32 func(psVector *params, psArray *coords)
The "params" are the parameters of the function which are varied.  The data
points at which the function is varied are in the argument "coords" which is
a psArray of psVectors: each vector represents a different coordinate.
 
XXX: We do not use Brent's method.
 
XXX: The SDR is silent about data types.  F32 is implemented here.
Reimplement in F64, convert F32 vectors to F64.
 *****************************************************************************/
#define PS_MINIMIZE_POWELL_LINEMIN_MAX_ITERATIONS 20
#define PS_MINIMIZE_POWELL_LINEMIN_ERROR_TOLERANCE 0.01

bool psMinimizePowell(psMinimization *min,
                      psVector *params,
                      const psVector *paramMask,
                      const psArray *coords,
                      psMinimizePowellFunc func)
{
    PS_ASSERT_PTR_NON_NULL(min, NULL);
    PS_ASSERT_VECTOR_NON_NULL(params, NULL);
    PS_ASSERT_VECTOR_NON_EMPTY(params, NULL);
    PS_ASSERT_VECTOR_TYPE(params, PS_TYPE_F32, NULL);
    PS_ASSERT_PTR_NON_NULL(coords, NULL);
    PS_ASSERT_PTR_NON_NULL(func, NULL);
    psS32 numDims = params->n;
    PS_VECTOR_GEN_STATIC_RECYCLED(pQP, numDims, PS_TYPE_F32);
    PS_VECTOR_GEN_STATIC_RECYCLED(u, numDims, PS_TYPE_F32);
    PS_VECTOR_GEN_STATIC_RECYCLED(Q, numDims, PS_TYPE_F32);
    psS32 i = 0;
    psS32 j = 0;
    psVector *myParamMask = NULL;
    psMinimization dummyMin;
    psF32 mul = 0.0;
    psF32 baseFuncVal = 0.0;
    psF32 currFuncVal = 0.0;
    psS32 biggestIter = 0;
    psF32 biggestDiff = 0.0;
    psS32 iterationNumber = 0;

    psTrace(".psLib.dataManip.psMinimizePowell", 4,
            "---- psMinimizePowell() begin ----\n");
    psTrace(".psLib.dataManip.psMinimizePowell", 6,
            "min->maxIter is %d\n", min->maxIter);
    psTrace(".psLib.dataManip.psMinimizePowell", 6,
            "min->tol is %f\n", min->tol);

    if (paramMask == NULL) {
        myParamMask = psVectorRecycle(myParamMask, params->n, PS_TYPE_U8);
        p_psMemSetPersistent(myParamMask, true);
        p_psMemSetPersistent(myParamMask->data.U8, true);
        for (i=0;i<myParamMask->n;i++) {
            myParamMask->data.U8[i] = 0;
        }
    } else {
        myParamMask = (psVector *) paramMask;
    }
    PS_ASSERT_VECTORS_SIZE_EQUAL(params, myParamMask, NULL);

    // 1: Set v[i] to be the unit vectors for each dimension in params
    psArray *v = psArrayAlloc(numDims);
    for (i=0;i<numDims;i++) {
        (v->data[i]) = (psVector *) psVectorAlloc(numDims, PS_TYPE_F32);
        for (j=0;j<numDims;j++) {
            if (i == j) {
                ((psVector *) (v->data[i]))->data.F32[j] = 1.0;
            } else {
                ((psVector *) (v->data[i]))->data.F32[j] = 0.0;
            }
        }
    }

    // 2: Set Q to be the initial params (P in the ADD)
    for (i=0;i<numDims;i++) {
        Q->data.F32[i] = params->data.F32[i];
    }

    while (iterationNumber < min->maxIter) {
        iterationNumber++;
        psTrace(".psLib.dataManip.psMinimizePowell", 6,
                "psMinimizePowell() iteration %d\n", iterationNumber);

        // 3: For each dimension in params, move Q only in the vector v[i] to
        //    minimize the function.

        baseFuncVal = func(Q, coords);
        currFuncVal = baseFuncVal;
        psTrace(".psLib.dataManip.psMinimizePowell", 6,
                "Current function value is %f\n", currFuncVal);

        biggestDiff = 0;
        biggestIter = 0;
        for (i=0;i<numDims;i++) {
            if (myParamMask->data.U8[i] == 0) {
                *(int*)&dummyMin.maxIter = PS_MINIMIZE_POWELL_LINEMIN_MAX_ITERATIONS;
                *(float*)&dummyMin.tol = PS_MINIMIZE_POWELL_LINEMIN_ERROR_TOLERANCE;
                mul = p_psLineMin(&dummyMin,
                                  Q,
                                  ((psVector *) v->data[i]),
                                  myParamMask,
                                  coords,
                                  func);
                if (isnan(mul)) {
                    psError(PS_ERR_UNKNOWN, false,
                            "Could not perform line minimization.  Returning FALSE.\n");
                    psFree(v);
                    return(false);
                }
                psTrace(".psLib.dataManip.psMinimizePowell", 6,
                        "LineMin along dimension %d has multiple %f\n", i, mul);

                if (fabs(dummyMin.value - currFuncVal) > biggestDiff) {
                    biggestDiff = fabs(dummyMin.value - currFuncVal);
                    biggestIter = i;
                }
                currFuncVal = dummyMin.value;
            }
        }
        psTrace(".psLib.dataManip.psMinimizePowell", 6,
                "New function value is %f\n", currFuncVal);

        // 4: Set the vector u = Q - P
        for (i=0;i<numDims;i++) {
            if (myParamMask->data.U8[i] == 0) {
                u->data.F32[i] = Q->data.F32[i] - params->data.F32[i];

                psTrace(".psLib.dataManip.psMinimizePowell", 6,
                        "u[i]=Q[i]-P[i] (%f = %f - %f)\n", u->data.F32[i],
                        Q->data.F32[i],
                        params->data.F32[i]);

            } else {
                u->data.F32[i] = 0.0;
            }
        }

        // 5: Move Q only in the direction u, and minimize the function.
        for (i=0;i<numDims;i++) {
            psTrace(".psLib.dataManip.psMinimizePowell", 6,
                    "u[i] is %f\n", u->data.F32[i]);
        }

        mul = p_psLineMin(&dummyMin, params, u, myParamMask, coords, func);
        if (isnan(mul)) {
            psError(PS_ERR_UNKNOWN, false,
                    "Could not perform line minimization.  Returning FALSE.\n");
            psFree(v);
            return(false);
        }

        // 6:
        if (dummyMin.value > currFuncVal) {
            psFree(v);
            min->iter = iterationNumber;
            // XXX: Ensure that currFuncVal is the correct value to use here.
            min->value = currFuncVal;
            // XXX: ensure that the lastDelta should be 0.0.
            min->lastDelta = 0.0;
            psTrace(".psLib.dataManip.psMinimizePowell", 4,
                    "---- psMinimizePowell() end (1)(true) ----\n");
            return(true);
        }

        for (i=0;i<numDims;i++) {
            if (myParamMask->data.U8[i] == 0) {
                pQP->data.F32[i] = (2 * Q->data.F32[i]) - params->data.F32[i];
            } else {
                pQP->data.F32[i] = params->data.F32[i];
            }
        }
        psF32 fqp = func(pQP, coords);
        psF32 term1 = (baseFuncVal - currFuncVal) - biggestDiff;
        term1*= term1;
        term1*= 2.0 * (baseFuncVal - (2.0 * currFuncVal) + fqp);
        psF32 term2 = baseFuncVal - fqp;
        term2*= term2 * biggestDiff;
        if (term1 < term2) {
            for (i=0;i<numDims;i++) {
                if (myParamMask->data.U8[i] == 0) {
                    ((psVector *) v->data[biggestIter])->data.F32[i] = u->data.F32[i];
                }
            }
        }

        // 7: Set P to Q
        for (i=0;i<numDims;i++) {
            if (myParamMask->data.U8[i] == 0) {
                params->data.F32[i] = Q->data.F32[i];
            }
        }

        // 8: Go to step 3 until the change is less than some tolerance.
        if (fabs(baseFuncVal - currFuncVal) <= min->tol) {
            psFree(v);
            // XXX: Ensure that currFuncVal is the correct value to use here.
            min->value = currFuncVal;
            min->iter = iterationNumber;
            min->lastDelta = currFuncVal - baseFuncVal;
            psTrace(".psLib.dataManip.psMinimizePowell", 4,
                    "---- psMinimizePowell() end (2) (true) ----\n");
            return(true);
        }
    }

    psFree(v);
    min->iter = iterationNumber;
    psTrace(".psLib.dataManip.psMinimizePowell", 4,
            "---- psMinimizePowell() end (0) (false) ----\n");
    return(false);
}


/******************************************************************************
This routine is to be used with the psMinimizeChi2Powell() function below.
and the psMinimizePowell() function above.
 
The basic idea is calculate chi-squared for a set of params/coords/errors.
This functions uses global variables to receive the function pointer, the
data values, and the data errors.
XXX: This is F32 only.  Must implement F64.
 *****************************************************************************/
static psF32 myPowellChi2Func(const psVector *params,
                              const psArray *coords)
{
    psTrace(".psLib.dataManip.myPowellChi2Func", 4,
            "---- myPowellChi2Func() begin ----\n");
    PS_ASSERT_VECTOR_NON_NULL(params, NAN);
    PS_ASSERT_VECTOR_NON_EMPTY(params, NAN);
    PS_ASSERT_VECTOR_NON_NULL(myValue, NAN);
    PS_ASSERT_VECTOR_NON_EMPTY(myValue, NAN);
    PS_ASSERT_PTR_NON_NULL(coords, NAN);

    psF32 chi2 = 0.0;
    psF32 d;
    psS32 i;
    psVector *tmp;

    tmp = Chi2PowellFunc(params, coords);
    if (myError == NULL) {
        for (i=0;i<coords->n;i++) {
            d = (tmp->data.F32[i] - myValue->data.F32[i]);
            chi2+= d * d;
        }
    } else {
        for (i=0;i<coords->n;i++) {
            d = (tmp->data.F32[i] - myValue->data.F32[i]) / myError->data.F32[i];
            chi2+= d * d;
        }
    }
    psFree(tmp);
    psTrace(".psLib.dataManip.myPowellChi2Func", 4,
            "---- myPowellChi2Func() end (chi2 is %f) ----\n", chi2);
    return(chi2);
}


/******************************************************************************
This routine must minimize the chi-squared match of a set of data points and
values for a possibly multi-dimensional function.
 
The basic idea is to use the psMinimizePowell() function defined above.  In
order to do so, we defined above a function myPowellChi2Func() which takes
the "func" function and returns chi-squared over the params/coords/values.
We then use that function myPowellChi2Func() in the call to
psMinimizePowell().
 *****************************************************************************/
bool psMinimizeChi2Powell(psMinimization *min,
                          psVector *params,
                          const psVector *paramMask,
                          const psArray *coords,
                          const psVector *value,
                          const psVector *error,
                          psMinimizeChi2PowellFunc model)
{
    myValue = (psVector *) value;
    myError = (psVector *) error;

    Chi2PowellFunc = model;

    return(psMinimizePowell(min, params, paramMask, coords, myPowellChi2Func));
}


/******************************************************************************
 ******************************************************************************
 Analytical 1-D fitting routines.
 ******************************************************************************
 *****************************************************************************/
// XXX: Make this a general type conversion macro, or function
#define PS_VECTOR_GEN_F64_FROM_F32(VECF64, VECF32) \
VECF64 = psVectorAlloc(VECF32->n, PS_TYPE_F64); \
for (psS32 i = 0 ; i < VECF32->n ; i++) { \
    VECF64->data.F64[i] = (psF64) VECF32->data.F32[i]; \
} \

#define PS_VECTOR_GEN_CHEBY_INDEX(VECF64, SIZE) \
VECF64 = psVectorAlloc(SIZE, PS_TYPE_F64); \
for (psS32 i = 0 ; i < SIZE ; i++) { \
    VECF64->data.F64[i] = ((2.0 / ((psF64) (SIZE - 1))) * ((psF64) i)) - 1.0; \
}\
/******************************************************************************
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 (int 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);
    }

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


/******************************************************************************
Polynomial2DEvalVectorD(myPoly, x, y): This routine takes as input two
psVectors x and y, and evaluates myPoly for each pair of (x, y), and stores it
in the output vector.  This routine works on single-precision polynomials with
double precision data.
 *****************************************************************************/
psVector *Polynomial2DEvalVectorD(
    const psPolynomial2D *myPoly,
    const psVector *x,
    const psVector *y)
{
    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);

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

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

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

    return(tmp);
}

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

/******************************************************************************
vectorFitPolynomial1DCheb():  This routine will fit a Chebyshev polynomial of
degree myPoly to the data points (x, y) and return the coefficients of that
polynomial.
 
XXX: mask, maskValue, yErr are currently ignored.
 
XXX: Change arg order to that of the psLib function.
*****************************************************************************/
static psPolynomial1D *vectorFitPolynomial1DCheby(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector* y,
    const psVector* yErr,
    const psVector* x)
{
    // XXX: these ASSERTS are redundant.
    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;

    // XXX: 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
        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.
        fScalar = p_psVectorInterpolate((psVector *) x,
                                        (psVector *) y,
                                        3,
                                        &tmpScalar);

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

        psTrace(".psLib.dataManip.vectorFitPolynomial1DCheby", 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);
    // XXX: is this loop bound correct?
    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.
 *****************************************************************************/
psPolynomial1D* VectorFitPolynomial1DOrd(
    psPolynomial1D* myPoly,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x)
{
    // XXX: these ASSERTS are redundant.
    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;

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

    if (psTraceGetLevel (".psLib.dataManip.VectorFitPolynomial1DOrd") >= 5) {
        psTrace(__func__, 6, "VectorFitPolynomial1D()\n");
        for (int 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.
    // XXX: Use psLib function.
    //
    PS_VECTOR_SET_F64(B, 0.0);
    PS_IMAGE_SET_F64(A, 0.0);

    // xSums look like: 1, x, x^2, ... x^(2n+1)
    // Build the B and A data structs.
    for (int k = 0; k < f->n; k++) {
        if ((mask != NULL) && mask->data.U8[k])
            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 should filter fErr == 0 values
            wt = 1.0 / PS_SQR(fErr->data.F64[k]);
        }
        for (int 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 (int i = 0; i < nTerm; i++) {
            for (int j = 0; j < nTerm; j++) {
                A->data.F64[i][j] += xSums->data.F64[i + j] * wt;
            }
        }
    }

    // GaussJordan version
    if (0) {
        // does the solution in place
        psGaussJordan (A, B);

        // the first nTerm entries in B correspond directly to the desired
        // polynomial coefficients.  this is only true for the 1D case
        for (int 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);
        coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
        for (int 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(".psLib.dataManip.VectorFitPolynomial1DOrd", 4,
            "---- VectorFitPolynomial1DOrd() End ----\n");
    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 (f->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(f64, f);
    } else {
        f64 = (psVector *) f;
    }

    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 (x->type.type != PS_TYPE_F64) {
            PS_VECTOR_GEN_F64_FROM_F32(x64, x);
        } else {
            x64 = (psVector *) x;
        }
    }

    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
        if (fErr->type.type != PS_TYPE_F64) {
            PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr);
        } 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 (x == NULL) {
            // If x==NULL, create an x64 vector with x values set to (-1:1).
            PS_VECTOR_GEN_CHEBY_INDEX(x64, f64->n);
        }
        // XXX: Change arg order.
        // XXX: Must modify this routine so that x64 or fErr64 can be NULL.
        poly = vectorFitPolynomial1DCheby(poly, NULL, 0, f64, fErr64, x64);
        if (x == NULL) {
            psFree(x64);
        }
    } else {
        psError(PS_ERR_UNKNOWN, true, "Incorrect polynomial type.  Returning NULL.\n");
        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);
}


psPolynomial1D *psVectorClipFitPolynomial1D(
    psPolynomial1D *poly,
    psStats *stats,
    const psVector *mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x)
{
    // Internal pointers for possibly NULL vectors.
    psVector *x32 = NULL;

    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(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, 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_F32, NULL);
    }
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL);
    }

    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");
    // Free psVectors that were created for NULL arguments.
    if (x == NULL) {
        psFree(x32);
    }
    return(NULL);
}


/******************************************************************************
 ******************************************************************************
 2-D Vector Fitting 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.
 
// XXX: What about the maskValue?
 *****************************************************************************/
psPolynomial2D* VectorFitPolynomial2DOrd(
    psPolynomial2D* myPoly,
    const psVector* mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y)
{
    // These ASSERTS are redundant.
    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(y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, 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);
    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;

    // XXX:Watch for changes to the psPolys: nTerm != nOrder.
    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.
    // XXX: Use psLib function.
    //
    PS_VECTOR_SET_F64(B, 0.0);
    PS_IMAGE_SET_F64(A, 0.0);

    // 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 (int k  = 0; k < x->n; k++) {
        if ((mask != NULL) && mask->data.U8[k])
            continue;
        Sums = BuildSums2D(Sums, x->data.F64[k], y->data.F64[k], nXterm, nYterm);

        if (fErr == NULL) {
            wt = 1.0;
        } else {
            // XXX: this should probably by fErr^2 !!
            // this should filter fErr == 0 values
            // XXX: Why isn't this fErr^2?
            wt = 1.0 / fErr->data.F64[k];
        }

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

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

    // does the solution in place
    // XXX: Check return codes!
    psGaussJordan (A, B);

    // XXX: Check return codes!
    // ALUD = psMatrixLUD(ALUD, &outPerm, A);
    // coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);

    for (int n = 0; n < nXterm; n++) {
        for (int m = 0; m < nYterm; m++) {
            myPoly->coeff[n][m] = B->data.F64[n+m*nXterm];
        }
    }

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

    psTrace(".psLib.dataManip.VectorFitPolynomial2DOrd", 4,
            "---- VectorFitPolynomial2DOrd() begin ----\n");
    return (myPoly);
}


psPolynomial2D* RobustFit2D_nomask(
    psPolynomial2D* poly,
    const psVector* x,
    const psVector* y,
    const psVector* f,
    const psVector* df)
{
    psVector *X;
    psVector *Y;
    psVector *Z;
    psVector *dZ;

    psVector *fFit   = NULL;
    psVector *fResid = NULL;
    psStats  *stats  = NULL;

    X  = psVectorCopy (NULL, x, PS_TYPE_F64);
    Y  = psVectorCopy (NULL, y, PS_TYPE_F64);
    Z  = psVectorCopy (NULL, f, PS_TYPE_F64);
    dZ = psVectorCopy (NULL, df, PS_TYPE_F64);

    for (int N = 0; N < 3; N++) {
        // XXX EAM : this would be better defined with an element mask
        poly   = VectorFitPolynomial2DOrd(poly, NULL, 0, X, Y, Z, dZ);
        fFit   = Polynomial2DEvalVectorD(poly, x, y);
        fResid = (psVector *) psBinaryOp(NULL, (void *) f, "-", (void *) fFit);

        stats = psStatsAlloc (PS_STAT_CLIPPED_MEAN | PS_STAT_CLIPPED_STDEV);
        stats  = psVectorStats (stats, fResid, NULL, NULL, 0);
        psTrace (".psphot.RobustFit", 4, "residual stats for robust fit:  %g +/- %g (%d pts)\n", stats->clippedMean, stats->clippedStdev, stats->clippedNvalues);

        // re-create X, Y, Z, dZ if pts are valid
        int n = 0;
        for (int i = 0; i < fResid->n; i++) {
            if (fabs(fResid->data.F64[i] - stats->clippedMean) > 3*stats->clippedStdev) {
                continue;
            }
            X->data.F64[n]  =  x->data.F64[i];
            Y->data.F64[n]  =  y->data.F64[i];
            Z->data.F64[n]  =  f->data.F64[i];
            dZ->data.F64[n] = df->data.F64[i];
            n++;
        }
        X->n = n;
        Y->n = n;
        Z->n = n;
        dZ->n = n;
    }
    return (poly);
}

// XXX EAM : be careful here with F32 vs F64 vectors
/*
 Basically, you repetitively fit a polynomial to a set of data points,
 reject the points that did not fit well, then refit the polynomial.
 
 Basically, simply fit the polynomial to the data.  They compare the
 fit (by evaluating the x data with that polynomial and subtracting
 from the original f data).  That's the residual.  Loop through all
 data and if the ((residual - mean) > 3*stDev), mask that data point,
 and fit the polynomial again.  Do this 3 times.
*/

psPolynomial2D* RobustFit2D(psPolynomial2D* poly,
                            const psVector* mask,
                            const psVector* x,
                            const psVector* y,
                            const psVector* f,
                            const psVector* df)
{
    PS_ASSERT_VECTOR_NON_NULL(mask, NULL);
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_NON_NULL(df, NULL);

    psVector *fFit   = NULL;
    psVector *fResid = psVectorAlloc (x->n, PS_TYPE_F64);
    psStats  *stats  = psStatsAlloc (PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_STDEV);

    for (int N = 0; N < 3; N++) {
        poly   = VectorFitPolynomial2DOrd (poly, mask, 0, x, y, f, df);
        fFit   = Polynomial2DEvalVectorD (poly, x, y);
        fResid = (psVector *) psBinaryOp(fResid, (void *) f, "-", (void *) fFit);

        stats  = psVectorStats (stats, fResid, NULL, mask, 1);
        psTrace (".psphot.RobustFit", 4, "residual stats for robust fit:  %g +/- %g\n",
                 stats->sampleMean, 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 (int i = 0; i < fResid->n; i++) {
            if (mask->data.U8[i])
                continue;
            if (fabs(fResid->data.F64[i] - stats->sampleMean) > 3*stats->sampleStdev) {
                mask->data.U8[i] = 1;
                continue;
            }
        }
        psFree (fFit);
    }
    psFree (fResid);
    psFree (stats);
    return (poly);
}


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

    //
    // f
    //
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (f->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(f64, f);
    } else {
        f64 = (psVector *) f;
    }
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    //
    // x
    //
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    if (x->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(x64, x);
    } else {
        x64 = (psVector *) x;
    }

    //
    // y
    //
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    if (y->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(y64, y);
    } else {
        y64 = (psVector *) y;
    }

    //
    // fErr
    //
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
        if (fErr->type.type != PS_TYPE_F64) {
            PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr);
        } 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)
{
    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(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, 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, y, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL);
    }

    if (mask == NULL) {
        // XXX: Change argument order.
        poly = RobustFit2D_nomask(poly, x, y, f, fErr);
    } else {
        // XXX: Use maskValue.
        // XXX: Change argument order.
        poly = RobustFit2D(poly, mask, x, y, f, fErr);
    }

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

/******************************************************************************
 ******************************************************************************
 3-D Vector Fitting 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.
 
XXX: This routine needs to be written.  Currently, this is simply a shell.  We
can assume that all vectors have been converted to F64, that (f, x, y, z) are
non-null and F64.  fErr may be NULL, but will be F64 is not.
 *****************************************************************************/
psPolynomial3D* VectorFitPolynomial3DOrd(
    psPolynomial3D* myPoly,
    const psVector* mask,
    psMaskType maskValue,
    const psVector *f,
    const psVector *fErr,
    const psVector *x,
    const psVector *y,
    const psVector *z)
{
    // These ASSERTS are redundant.
    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);
    }

    psError(PS_ERR_UNKNOWN, true, "3-D Polynomial Fitting is not Implemented.\n");
    return (NULL);
}

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

    //
    // f
    //
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (f->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(f64, f);
    } else {
        f64 = (psVector *) f;
    }
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    //
    // x
    //
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    if (x->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(x64, x);
    } else {
        x64 = (psVector *) x;
    }

    //
    // y
    //
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    if (y->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(y64, y);
    } else {
        y64 = (psVector *) y;
    }

    //
    // z
    //
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    if (z->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(y64, z);
    } else {
        z64 = (psVector *) z;
    }

    //
    // fErr
    //
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
        if (fErr->type.type != PS_TYPE_F64) {
            PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr);
        } 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)
{
    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(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, 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, y, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL);
    }

    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");
    return(NULL);
}


/******************************************************************************
 ******************************************************************************
 4-D Vector Fitting 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.
 
XXX: This routine needs to be written.  Currently, this is simply a shell.  We
can assume that all vectors have been converted to F64, that (f, x, y, z) are
non-null and F64.  fErr may be NULL, but will be F64 is not.
 *****************************************************************************/
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)
{
    // These ASSERTS are redundant.
    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);
    }

    psError(PS_ERR_UNKNOWN, true, "4-D Polynomial Fitting is not Implemented.\n");
    return (NULL);
}

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

    //
    // f
    //
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
    if (f->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(f64, f);
    } else {
        f64 = (psVector *) f;
    }
    if (mask != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_U8, NULL);
    }

    //
    // x
    //
    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, x, NULL);
    if (x->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(x64, x);
    } else {
        x64 = (psVector *) x;
    }

    //
    // y
    //
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    if (y->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(y64, y);
    } else {
        y64 = (psVector *) y;
    }

    //
    // z
    //
    PS_ASSERT_VECTOR_NON_NULL(z, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
    if (z->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(y64, z);
    } else {
        z64 = (psVector *) z;
    }

    //
    // t
    //
    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL);
    if (t->type.type != PS_TYPE_F64) {
        PS_VECTOR_GEN_F64_FROM_F32(y64, t);
    } else {
        t64 = (psVector *) t;
    }

    //
    // fErr
    //
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(f, fErr, NULL);
        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
        if (fErr->type.type != PS_TYPE_F64) {
            PS_VECTOR_GEN_F64_FROM_F32(fErr64, fErr);
        } 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)
{
    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(f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, 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, y, NULL);
    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(f, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL);
    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(t, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(f, t, NULL);
    PS_ASSERT_VECTOR_TYPE(t, PS_TYPE_F32, NULL);
    if (fErr != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, mask, NULL);
        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL);
    }

    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");

    return(NULL);
}


