/** @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 GLF, MHPCC
 *
 *  @version $Revision: 1.58 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2004-10-14 01:44:48 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 *
 *  XXX: must follow coding name standards on local functions.
 *
 *  XXX: Section 4.5.1.1 (predefined functions for Gauss minimization via
 *       LMM) is not addressed here.  We are waiting for subsequent SDRs
 *       which will redefine the LMM functions.
 *
 */
/*****************************************************************************/
/* INCLUDE FILES                                                             */
/*****************************************************************************/
#include "psMinimize.h"
/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/

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

/*****************************************************************************/
/* GLOBAL VARIABLES                                                          */
/*****************************************************************************/
static psMinimizeChi2PowellFunc Chi2PowellFunc = NULL;
static psVector *myValue;
static psVector *myError;

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

// None

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

/******************************************************************************
p_psBuildSums1D(x, polyOrder, sums): this routine calculates the powers of
input parameter "x" between 0 and input parameter polyOrder.  The result is
returned as a psVector sums.
 
XXX: change name
XXX: Use a static vector.
 *****************************************************************************/
void p_psBuildSums1D(double x,
                     int polyOrder,
                     psVector* sums)
{
    int i = 0;
    double xSum = 0.0;

    if (sums == NULL) {
        sums = psVectorAlloc(polyOrder, PS_TYPE_F64);
    }

    if (polyOrder > sums->n) {
        sums = psVectorRealloc(sums, polyOrder);
    }

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

/*****************************************************************************
CalculateSecondDerivs(): Given a set of x/y vectors corresponding to a
tabulated function at n points, this routine calculates the second
derivatives of the interpolating cubic splines at those n points.
 
The first and second derivatives at the endpoints, undefined in the SDR, are
here defined to be 0.0.  They can be modified via ypo and yp1.
 
This routine assumes that vectors x and y are of the appropriate types/sizes
(F32).
 
XXX: This algorithm is derived from the Numerical Recipes.
XXX: use recycled vectors for internal data.
XXX: do an F64 version?
 *****************************************************************************/
float *CalculateSecondDerivs(const psVector* restrict x,        ///< Ordinates (or NULL to just use the indices)
                             const psVector* restrict y)        ///< Coordinates
{
    psTrace(".psLib.dataManip.CalculateSecondDerivs", 4,
            "---- CalculateSecondDerivs() begin ----\n");

    int i;
    int k;
    bool mustFreeX = false;
    float sig;
    float p;
    int n = y->n;
    float *u = (float *) psAlloc(n * sizeof(float));
    float *derivs2 = (float *) psAlloc(n * sizeof(float));
    float *X = (float *) & (x->data.F32[0]);
    float *Y = (float *) & (y->data.F32[0]);
    float qn;

    // XXX: The second derivatives at the endpoints, undefined in the SDR,
    // are set in psConstants.h: LEFT_SPLINE_DERIV, RIGHT_SPLINE_DERIV.
    derivs2[0] = -0.5;
    u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - LEFT_SPLINE_DERIV);

    for (i=1;i<=(n-2);i++) {
        sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
        p = sig * derivs2[i-1] + 2.0;
        derivs2[i] = (sig - 1.0) / p;
        u[i] = ((Y[i+1] - Y[i])/(X[i+1]-X[i])) - ((Y[i]-Y[i-1])/(X[i]-X[i-1]));
        u[i] = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (sig * u[i-1])) / p;

        psTrace(".psLib.dataManip.CalculateSecondDerivs", 6,
                "X[%d] is %f\n", i, X[i]);
        psTrace(".psLib.dataManip.CalculateSecondDerivs", 6,
                "Y[%d] is %f\n", i, Y[i]);
        psTrace(".psLib.dataManip.CalculateSecondDerivs", 6,
                "u[%d] is %f\n", i, u[i]);
    }

    qn = 0.5;
    u[n-1] = (3.0/(X[n-1]-X[n-2])) * (RIGHT_SPLINE_DERIV - (Y[n-1]-Y[n-2])/(X[n-1]-X[n-2]));
    derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0);

    for (k=(n-2);k>=0;k--) {
        derivs2[k] = derivs2[k] * derivs2[k+1] + u[k];

        psTrace(".psLib.dataManip.CalculateSecondDerivs", 6,
                "derivs2[%d] is %f\n", k, derivs2[k]);
    }

    if (mustFreeX == true) {
        psFree(X);
    }
    psFree(u);

    psTrace(".psLib.dataManip.CalculateSecondDerivs", 4,
            "---- CalculateSecondDerivs() end ----\n");
    return(derivs2);
}

/******************************************************************************
p_psNRSpline1DEval(): This routine does NR-style evaluation of cubic splines.
It takes advantage of the 2nd derivatives of the cubic splines, which are
stored in the psSPline1D data structure, and computes the interpolated value
directly, without computing (or using) the interpolating cubic spline
polynomial.
 
This routine is here mostly for a sanity check on the psLib function
evalSpline() which computes the interpolated value based on the cubic spline
polynomials which are stored in psSpline1D.
 
XXX: check types/sizes
XXX: This is F32 only
 *****************************************************************************/
float p_psNRSpline1DEval(psSpline1D *spline,
                         const psVector* restrict x,
                         const psVector* restrict y,
                         float X)
{
    int n;
    int klo;
    int khi;
    float H;
    float A;
    float B;
    float C;
    float D;
    float Y;

    n = spline->n;
    klo = p_psVectorBinDisectF32(spline->domains, (spline->n)+1, X);
    khi = klo + 1;
    H = (spline->domains)[khi] - (spline->domains)[klo];
    A = ((spline->domains)[khi] - X) / H;
    B = (X - (spline->domains)[klo]) / H;
    C = ((A*A*A)-A) * (H*H/6.0);
    D = ((B*B*B)-B) * (H*H/6.0);

    Y = (A * y->data.F32[klo]) +
        (B * y->data.F32[khi]) +
        (C * (spline->p_psDeriv2)[klo]) +
        (D * (spline->p_psDeriv2)[khi]);

    return(Y);
}

/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - PUBLIC                                          */
/*****************************************************************************/

/*****************************************************************************
psVectorFitSpline1D(): given a psSpline1D data structure and a set of x/y
vectors, this routine generates the linear or cublic splines which satisfy
those data points.
 
The formula for calculating the spline polynomials is derived from Numerical
Recipes in C.  The basic idea is that the polynomial is
 (1)     y = (A * y[0]) +
 (2)         (B * y[1]) +
 (3)         ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 +
 (4)         ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0
Where:
 H = x[1]-x[0]
 A = (x[1]-x)/H
 B = (x-x[0])/H
The bulk of the code in this routine is the expansion of the above equation
into a polynomial in terms of x, and then saving the coefficients of the
powers of x in the spline polynomials.  This gets pretty complicated.
 
XXX: usage of yErr is not specified in IfA documentation.
XXX: Is the x argument redundant?  What do we do if the x argument is
supplied, but does not equal the domains specified in mySpline?
XXX: can psSpline be NULL?
 
XXX: reimplement this assuming that mySpline is NULL?
 
XXX: What happens if X is NULL, then an index vector is generated for X, but
that index vector lies outside the range vectors in mySpline?
 *****************************************************************************/
psSpline1D *psVectorFitSpline1D(psSpline1D *mySpline,              ///< The spline which will be generated.
                                const psVector* restrict x,        ///< Ordinates (or NULL to just use the indices)
                                const psVector* restrict y,        ///< Coordinates
                                const psVector* restrict yErr)     ///< Errors in coordinates, or NULL
{
    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
            "---- psVectorFitSpline1D() begin ----\n");
    int numSplines = (y->n)-1;
    float tmp;
    float H;
    int i;
    float slope;
    psVector *x32 = NULL;
    psVector *y32 = NULL;
    psVector *yErr32 = NULL;
    static psVector *x32Static = NULL;
    static psVector *y32Static = NULL;
    static psVector *yErr32Static = NULL;

    PS_CHECK_NULL_PTR_RETURN_NULL(mySpline);
    PS_CHECK_NULL_PTR_RETURN_NULL(y);
    VECTOR_CHECK_TYPE_F32_OR_F64_RETURN_NULL(y);
    CONVERT_VECTOR_F64_TO_F32(y, y32, y32Static);

    // If yErr==NULL, set all errors equal.
    if (yErr == NULL) {
        GEN_PERSIST_YERR_VEC_F32(yErr32Static, y->n);
        yErr32 = yErr32Static;
    } else {
        VECTOR_CHECK_TYPE_F32_OR_F64_RETURN_NULL(yErr);
        CONVERT_VECTOR_F64_TO_F32(yErr, yErr32, yErr32Static);
    }

    // If x==NULL, create an x32 vector with x values set to (0:n).
    if (x == NULL) {
        GEN_PERSIST_X_INDEX_VEC_F32(x32Static, y->n);
        x32 = x32Static;
    } else {
        VECTOR_CHECK_TYPE_F32_OR_F64_RETURN_NULL(x);
        CONVERT_VECTOR_F64_TO_F32(x, x32, x32Static);
    }
    PS_CHECK_VECTOR_SIZE_EQUAL_RETURN_NULL(x32, y32);
    PS_CHECK_VECTOR_SIZE_EQUAL_RETURN_NULL(yErr32, y32);

    if (y32->n != (1 + mySpline->n)) {
        psError(__func__, "data size / spline size mismatch (%d %d)\n",
                y32->n, mySpline->n);
        return(NULL);
    }

    // If these are linear splines, which means their polynomials will have
    // two coefficients, then we do the simple calculation.
    if (2 == (mySpline->spline[0])->n) {
        for (i=0;i<mySpline->n;i++) {
            slope = (y32->data.F32[i+1] - y32->data.F32[i]) /
                    (mySpline->domains[i+1] - mySpline->domains[i]);
            (mySpline->spline[i])->coeff[0] = y32->data.F32[i] -
                                              (slope * mySpline->domains[i]);

            (mySpline->spline[i])->coeff[1] = slope;
            psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
                    "---- mySpline %d coeffs are (%f, %f)\n", i,
                    (mySpline->spline[i])->coeff[0],
                    (mySpline->spline[i])->coeff[1]);
        }
        psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
                "---- Exiting psVectorFitSpline1D()()\n");
        return((psSpline1D *) mySpline);
    }

    // Check if these are cubic splines (n==4).  If not, psError.
    if (4 != (mySpline->spline[0])->n) {
        psError(__func__, "Don't know how to generate %d-order splines.",
                (mySpline->spline[0])->n-1);
        return(NULL);
    }

    // If we get here, then we know these are cubic splines.  We first
    // generate the second derivatives at each data point.
    mySpline->p_psDeriv2 = CalculateSecondDerivs(x32, y32);
    for (i=0;i<y32->n;i++)
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "Second deriv[%d] is %f\n", i, mySpline->p_psDeriv2[i]);

    // We generate the coefficients of the spline polynomials.  I can't
    // concisely explain how this code works.  See above function comments
    // and Numerical Recipes in C.
    for (i=0;i<numSplines;i++) {
        H = x32->data.F32[i+1] - x32->data.F32[i];
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
                "x data (%f - %f) (%f)\n", x->data.F32[i], x->data.F32[i+1], H);
        //
        // ******** Calculate 0-order term ********
        //
        // From (1)
        (mySpline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H);
        // From (2)
        ((mySpline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H);
        // From (3)
        tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
        tmp-= (x32->data.F32[i+1] / H);
        tmp*= (mySpline->p_psDeriv2)[i] * H * H / 6.0;
        ((mySpline->spline[i])->coeff[0])+= tmp;
        // From (4)
        tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
        tmp+= (x32->data.F32[i] / H);
        tmp*= (mySpline->p_psDeriv2)[i+1] * H * H / 6.0;
        ((mySpline->spline[i])->coeff[0])+= tmp;

        //
        // ******** Calculate 1-order term ********
        //
        // From (1)
        (mySpline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H;
        // From (2)
        ((mySpline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H);
        // From (3)
        tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
        tmp+= (1.0 / H);
        tmp*= ((mySpline->p_psDeriv2)[i]) * H * H / 6.0;
        ((mySpline->spline[i])->coeff[1])+= tmp;
        // From (4)
        tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
        tmp-= (1.0 / H);
        tmp*= ((mySpline->p_psDeriv2)[i+1]) * H * H / 6.0;
        ((mySpline->spline[i])->coeff[1])+= tmp;

        //
        // ******** Calculate 2-order term ********
        //
        // From (3)
        (mySpline->spline[i])->coeff[2] = ((mySpline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H);
        // From (4)
        ((mySpline->spline[i])->coeff[2])-= (((mySpline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H));

        //
        // ******** Calculate 3-order term ********
        //
        // From (3)
        (mySpline->spline[i])->coeff[3] = -((mySpline->p_psDeriv2)[i]) / (6.0 * H);
        // From (4)
        ((mySpline->spline[i])->coeff[3])+=  ((mySpline->p_psDeriv2)[i+1]) / (6.0 * H);

        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]);
        psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
                "(mySpline->spline[%d])->coeff[3] is %f\n", i, (mySpline->spline[i])->coeff[3]);

    }

    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
            "---- psVectorFitSpline1D() end ----\n");
    return(mySpline);
}

/******************************************************************************
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: Do this:
 After checking that all entries in the paramMask are 1 or 0, when
 forming the A matrix from alpha, try this:
 
     A[i][i] = (1 + lambda*paramask[i]) * alpha[i][i];
 
XXX: This is very different from what is specified in the SDR.  Must
coordinate with IfA on new SDR.
 
XXX: Do vector/image recycles.
 
XXX: probably yErr will be part of the SDR.
 
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.
 *****************************************************************************/
bool psMinimizeLMChi2(psMinimization *min,
                      psVector *params,
                      const psVector *paramMask,
                      psImage *covar,
                      const psArray *coords,
                      const psVector *value,
                      psMinimizeLMChi2Func func)
{
    PS_CHECK_NULL_PTR_RETURN_NULL(min);
    PS_CHECK_NULL_VECTOR_RETURN_NULL(params);
    PS_CHECK_EMPTY_VECTOR_RETURN_NULL(params);
    PS_CHECK_NULL_PTR_RETURN_NULL(coords);
    PS_CHECK_NULL_VECTOR_RETURN_NULL(value);
    PS_CHECK_EMPTY_VECTOR_RETURN_NULL(value);
    if (paramMask != NULL) {
        PS_CHECK_VECTOR_SIZE_EQUAL_RETURN_NULL(params, paramMask);
    }
    psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
            "---- psMinimizeLMChi2() begin ----\n");
    int numData = value->n;
    int numParams = params->n;
    int i;
    int j;
    int k;
    int l;
    int n;
    int p;
    psVector *beta = psVectorAlloc(numParams, PS_TYPE_F64);
    psVector *perm = psVectorAlloc(numParams, PS_TYPE_F64);

    psVector *paramDeltasF64 = psVectorAlloc(numParams, PS_TYPE_F64);
    psVector *origParams = psVectorAlloc(numParams, PS_TYPE_F32);
    psVector *newParams = psVectorAlloc(numParams, PS_TYPE_F32);

    psImage *alpha = psImageAlloc(numParams, numParams, PS_TYPE_F32);
    psImage *A = psImageAlloc(numParams, numParams, PS_TYPE_F64);
    psImage *aOut = psImageAlloc(numParams, numParams, PS_TYPE_F64);

    psVector **deriv = (psVector **) psAlloc(numData * sizeof(psVector *));
    for (i=0;i<numData;i++) {
        deriv[i] = psVectorAlloc(numParams, PS_TYPE_F32);
    }

    psVector *currValueVec = psVectorAlloc(value->n, PS_TYPE_F32);
    psVector *newValueVec = psVectorAlloc(value->n, PS_TYPE_F32);

    float currChi2 = 0.0;
    float newChi2 = 0.0;
    float lamda = 0.00005;

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

    for (p=0;p<numParams;p++) {
        origParams->data.F32[p] = params->data.F32[p];
    }

    min->lastDelta = HUGE;
    min->iter = 0;

    while ((min->lastDelta > min->tol) && (min->iter < min->maxIter)) {
        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
                "------------------------------------------------------\n");
        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
                "Iteration %d.  Delta is %f\n", min->iter, min->lastDelta);

        //
        // Calculate the current values and chi-squared of the function.
        //
        currChi2 = 0.0;
        for (n=0;n<numData;n++) {
            currValueVec->data.F32[n] = func(deriv[n],
                                             params,
                                             (psVector *) coords->data[n]);
            currChi2+= (currValueVec->data.F32[n] * currValueVec->data.F32[n]);
        }

        for (p=0;p<numParams;p++) {
            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
                    "params->data.F32[%d] is %f.\n", p, params->data.F32[p]);
        }
        psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
                "Current chi-squared is (%f)\n", currChi2);

        //
        // Mask elements of the derivative for each data point.
        //
        for (p=0;p<numParams;p++) {
            if ((paramMask != NULL) && (paramMask->data.U8[p] != 0)) {
                for (n=0;n<numData;n++) {
                    (deriv[n])->data.F32[p] = 0.0;
                }
            }
        }

        //
        // Calculate the BETA vector.
        //
        for (p=0;p<numParams;p++) {
            beta->data.F64[p] = 0.0;
            for (n=0;n<numData;n++) {
                (beta->data.F64[p])+=
                    (value->data.F32[n] - currValueVec->data.F32[n]) *
                    (deriv[n])->data.F32[p];
            }
            // XXX: multiple by -1 here?
            (beta->data.F64[p])*= -1.0;
            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
                    "beta->data.F64[%d] is %f.\n", p, beta->data.F64[p]);
        }

        //
        // Calculate the ALPHA matrix.
        //
        for (k=0;k<numParams;k++) {
            for (l=0;l<numParams;l++) {
                alpha->data.F32[k][l] = 0.0;
                for (n=0;n<numData;n++) {
                    alpha->data.F32[k][l]+= (deriv[n])->data.F32[k] *
                                            (deriv[n])->data.F32[l];
                }
            }
        }

        //
        // Calculate the matrix A.
        //
        for (j=0;j<numParams;j++) {
            for (k=0;k<numParams;k++) {
                if (j == k) {
                    A->data.F64[j][k] =
                        (double) ((1.0 + lamda) * alpha->data.F32[j][k]);
                } else {
                    A->data.F64[j][k] = (double) alpha->data.F32[j][k];
                }
            }
        }
        for (j=0;j<numParams;j++) {
            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6, "Matrix A[][]:\n");
            for (k=0;k<numParams;k++) {
                psTrace(".psLib.dataManip.psMinimizeLMChi2", 6, "%f ", A->data.F64[j][k]);
            }
            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6, "Matrix A[][]:\n");
        }

        //
        // Solve A * alpha = Beta
        //
        aOut = psMatrixLUD(aOut, perm, A);
        paramDeltasF64 = psMatrixLUSolve(paramDeltasF64, aOut, beta, perm);

        //
        // Mask any masked parameters.
        //
        for (i=0;i<numParams;i++) {
            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
                    "paramDeltasF64->data.F64[%d] is %f.\n", i, paramDeltasF64->data.F64[i]);
            if ((paramMask != NULL) && (paramMask->data.U8[i] != 0)) {
                newParams->data.F32[i] = origParams->data.F32[i];
            } else {
                newParams->data.F32[i] = params->data.F32[i] -
                                         (float) paramDeltasF64->data.F64[i];
            }
        }

        psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
                "Calling func() with new parameters:\n");
        for (i=0;i<numParams;i++) {
            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
                    "newParams->data.F32[%d] is %f.\n", i, newParams->data.F32[i]);
        }


        //
        // Calculate new function values.
        //
        newChi2 = 0.0;
        for (n=0;n<numData;n++) {
            newValueVec->data.F32[n] = func(deriv[n], newParams,
                                            (psVector *) coords->data[n]);
            newChi2+= (newValueVec->data.F32[n] * newValueVec->data.F32[n]);
        }
        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
                "old/new chi-squareds are (%f, %f)\n", currChi2, newChi2);

        //
        // If the new chi-squared is lower, then keep it.
        //
        if (currChi2 > newChi2) {
            min->lastDelta = currChi2 - newChi2;
            min->value = newChi2;

            // We already masked params.
            for (i=0;i<numParams;i++) {
                params->data.F32[i] = (float) newParams->data.F32[i];
            }
            lamda*= 0.1;
        } else {
            lamda*= 10.0;
        }
        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
                "lamda is %f\n", lamda);
        min->iter++;
    }
    psFree(beta);
    psFree(perm);
    psFree(paramDeltasF64);
    psFree(origParams);
    psFree(newParams);
    psFree(alpha);
    psFree(A);
    psFree(aOut);
    for (i=0;i<numData;i++) {
        psFree(deriv[i]);
    }
    psFree(deriv);
    psFree(currValueVec);
    psFree(newValueVec);

    if ((min->iter < min->maxIter) ||
            (min->lastDelta <= min->tol)) {
        return(true);
    }

    psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
            "---- psMinimizeLMChi2() end (false) ----\n");
    return(false);
}

/******************************************************************************
p_psVectorFitPolynomial1DCheb():  This routine will fit a Chebyshev
polynomial of degree myPoly to the data points (x, y) and return the
coefficients of that polynomial.
 
XXX: yErr is currently ignored.
 
XXX: Use private name?
*****************************************************************************/
psPolynomial1D *p_psVectorFitPolynomial1DCheby(psPolynomial1D* myPoly,
        const psVector* restrict x,
        const psVector* restrict y,
        const psVector* restrict yErr)
{
    int j;
    int k;
    int n = x->n;
    double fac;
    double sum;
    GEN_STATIC_RECYCLED_VECTOR(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.
    double min = -1.0;
    double max = 1.0;
    double bma = 0.5 * (max-min);  // 1
    double 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 (int i=0;i<n;i++) {
        // NR 5.8.4
        double Y = cos(M_PI * (0.5 + ((float) i)) / ((float) n));
        double X = (Y + bma + bpa) - 1.0;
        tmpScalar.data.F64 = X;

        // We interpolate against are 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.p_psVectorFitPolynomial1DCheby", 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/((float) n);
    // XXX: is this loop bound correct?
    for (j=0;j<myPoly->n;j++) {
        sum = 0.0;
        for (k=0;k<n;k++) {
            sum+= f->data.F64[k] *
                  cos(M_PI * ((float) j) * (0.5 + ((float) k)) / ((float) n));
        }

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

    return(myPoly);
}

/******************************************************************************
p_psVectorFitPolynomial1DOrd():  This routine will fit an ordinary
polynomial of degree myPoly to the data points (x, y) and return the
coefficients of that polynomial.
 
XXX: Use private name?
XXX: Use recycled vectors.
 *****************************************************************************/
psPolynomial1D* p_psVectorFitPolynomial1DOrd(psPolynomial1D* myPoly,
        const psVector* restrict x,
        const psVector* restrict y,
        const psVector* restrict yErr)
{
    int polyOrder = myPoly->n;
    psImage* A = NULL;
    psImage* ALUD = NULL;
    psVector* B = NULL;
    psVector* outPerm = NULL;
    psVector* X = NULL;         // NOTE: do we need this?
    psVector* coeffs = NULL;
    int i = 0;
    int j = 0;
    int k = 0;
    psVector* xSums = NULL;

    psTrace(".psLib.dataManip.psVectorFitPolynomial1D", 4,
            "---- psVectorFitPolynomial1D() begin ----\n");
    // printf("psVectorFitPolynomial1D()\n");
    // for (i=0;i<x->n;i++) {
    // printf("(x, y, yErr) is (%f, %f, %f)\n", x->data.F64[i], y->data.F64[i], yErr->data.F64[i]);
    // }

    A = psImageAlloc(polyOrder, polyOrder, PS_TYPE_F64);
    ALUD = psImageAlloc(polyOrder, polyOrder, PS_TYPE_F64);
    B = psVectorAlloc(polyOrder, PS_TYPE_F64);
    coeffs = psVectorAlloc(polyOrder, PS_TYPE_F64);
    X = psVectorAlloc(x->n, PS_TYPE_F64);
    outPerm = psVectorAlloc(polyOrder, PS_TYPE_F64);
    xSums = psVectorAlloc(1 + 2 * polyOrder, PS_TYPE_F64);

    // Initialize data structures.
    for (i = 0; i < polyOrder; i++) {
        B->data.F64[i] = 0.0;
        coeffs->data.F64[i] = 0.0;
        outPerm->data.F64[i] = 0.0;
        for (j = 0; j < polyOrder; j++) {
            A->data.F64[i][j] = 0.0;
            ALUD->data.F64[i][j] = 0.0;
        }
    }
    for (i = 0; i < X->n; i++) {
        X->data.F64[i] = x->data.F64[i];
    }

    // Build the B and A data structs.
    if (yErr == NULL) {
        for (i = 0; i < X->n; i++) {
            p_psBuildSums1D(X->data.F64[i], 2 * polyOrder, xSums);

            for (k = 0; k < polyOrder; k++) {
                B->data.F64[k] += y->data.F64[i] * xSums->data.F64[k];
            }

            for (k = 0; k < polyOrder; k++) {
                for (j = 0; j < polyOrder; j++) {
                    A->data.F64[k][j] += xSums->data.F64[k + j];
                }
            }
        }
    } else {
        for (i = 0; i < X->n; i++) {
            p_psBuildSums1D(X->data.F64[i], 2 * polyOrder, xSums);

            for (k = 0; k < polyOrder; k++) {
                B->data.F64[k] += y->data.F64[i] * xSums->data.F64[k] /
                                  yErr->data.F64[i];
            }

            for (k = 0; k < polyOrder; k++) {
                for (j = 0; j < polyOrder; j++) {
                    A->data.F64[k][j] += xSums->data.F64[k + j] /
                                         yErr->data.F64[i];
                }
            }
        }
    }

    ALUD = psMatrixLUD(ALUD, outPerm, A);
    coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);

    for (k = 0; k < polyOrder; k++) {
        myPoly->coeff[k] = coeffs->data.F64[k];
        // printf("myPoly->coeff[%d] is %f\n", k, myPoly->coeff[k]);
    }

    // for (i=0;i<x->n;i++) {
    // printf("HMMM: psEvalPolynomial1D(%f) is %f\n", x->data.F64[i], psEvalPolynomial1D(x->data.F64[i],
    // myPoly));
    // }

    psFree(A);
    psFree(ALUD);
    psFree(B);
    psFree(coeffs);
    psFree(X);
    psFree(outPerm);
    psFree(xSums);

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

/******************************************************************************
psVectorFitPolynomial1D():  This routine must fit a polynomial of degree
myPoly to the data points (x, y) and return the coefficients of that
polynomial.
 
XXX: type F32 is done via vector conversion only.
 *****************************************************************************/
psPolynomial1D* psVectorFitPolynomial1D(psPolynomial1D* myPoly,
                                        const psVector* restrict x,
                                        const psVector* restrict y,
                                        const psVector* restrict yErr)
{
    int i;
    psPolynomial1D *tmpPoly;
    PS_CHECK_NULL_1DPOLY_RETURN_NULL(myPoly);
    PS_CHECK_NULL_VECTOR_RETURN_NULL(y);
    PS_CHECK_EMPTY_VECTOR_RETURN_NULL(y);
    psVector *x64 = NULL;
    psVector *y64 = NULL;
    psVector *yErr64 = NULL;
    static psVector *x64Static = NULL;
    static psVector *y64Static = NULL;
    static psVector *yErr64Static = NULL;

    VECTOR_CHECK_TYPE_F32_OR_F64_RETURN_NULL(y);
    CONVERT_VECTOR_F32_TO_F64(y, y64, y64Static);

    // If yErr==NULL, set all errors equal.
    if (yErr == NULL) {
        GEN_PERSIST_YERR_VEC_F64(yErr64Static, y->n);
        yErr64 = yErr64Static;
    } else {
        VECTOR_CHECK_TYPE_F32_OR_F64_RETURN_NULL(yErr);
        CONVERT_VECTOR_F32_TO_F64(yErr, yErr64, yErr64Static);
    }

    // If x==NULL, create an x64 vector with x values set to (0:n).
    if (x == NULL) {
        GEN_PERSIST_X_INDEX_VEC_F64(x64Static, y->n);
        if (myPoly->type == PS_POLYNOMIAL_CHEB) {
            p_psNormalizeVector(x64Static);
        }
        x64 = x64Static;
    } else {
        VECTOR_CHECK_TYPE_F32_OR_F64_RETURN_NULL(x);
        CONVERT_VECTOR_F32_TO_F64(x, x64, x64Static);
    }
    PS_CHECK_VECTOR_SIZE_EQUAL_RETURN_NULL(x64, y64);
    PS_CHECK_VECTOR_SIZE_EQUAL_RETURN_NULL(yErr64, y64);

    // Call the appropriate vector fitting routine.
    if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        p_psVectorFitPolynomial1DCheby(myPoly, x64, y64, yErr64);
    } else if (myPoly->type == PS_POLYNOMIAL_ORD) {
        tmpPoly = p_psVectorFitPolynomial1DOrd(myPoly, x64, y64, yErr64);
    } else {
        psError(__func__, "unknown polynomial type.\n");
        return(NULL);
    }

    return(myPoly);
}



/******************************************************************************
 *****************************************************************************/
psMinimization *psMinimizationAlloc(int maxIter,
                                    float tol)
{
    psMinimization *min = psAlloc(sizeof(psMinimization));
    min->maxIter = maxIter;
    min->tol = tol;
    min->value = 0.0;
    min->iter = 0;
    min->lastDelta = 0.0;

    return(min);
}

/******************************************************************************
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) is less than f(param + a * line) and
                                         f(param + c * line).
 
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
 *****************************************************************************/
psVector *p_psDetermineBracket(psVector *params,
                               psVector *line,
                               const psVector *paramMask,
                               const psArray *coords,
                               psMinimizePowellFunc func)
{
    float a = 0.0;
    float b = 0.0;
    float c = 0.0;
    float fa = 0.0;
    float fb = 0.0;
    float fc = 0.0;
    int iter = 100;
    float aDir = 0.0;
    float cDir = 0.0;
    float new_aDir = 0.0;
    float new_cDir = 0.0;
    psVector *bracket = psVectorAlloc(3, PS_TYPE_F32);
    float stepSize = DETERMINE_BRACKET_STEP_SIZE;
    psVector *tmp = NULL;
    int i = 0;
    int null = 0;

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

    for (i=0;i<params->n;i++) {
        if (paramMask->data.U8[i] == 0) {
            if (line->data.F32[i] >= FLT_EPSILON) {
                null = 1;
            }
        }
    }

    if (null == 0) {
        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;

    for (i=0;i<params->n;i++) {
        if (paramMask->data.U8[i] == 0) {
            tmp->data.F32[i] = params->data.F32[i] + (a * line->data.F32[i]);
        } else {
            tmp->data.F32[i] = params->data.F32[i];
        }
    }
    fa = func(tmp, coords);

    for (i=0;i<params->n;i++) {
        if (paramMask->data.U8[i] == 0) {
            tmp->data.F32[i] = params->data.F32[i] + (b * line->data.F32[i]);
        } else {
            tmp->data.F32[i] = params->data.F32[i];
        }
    }
    fb = func(tmp, coords);

    for (i=0;i<params->n;i++) {
        if (paramMask->data.U8[i] == 0) {
            tmp->data.F32[i] = params->data.F32[i] + (c * line->data.F32[i]);
        } else {
            tmp->data.F32[i] = params->data.F32[i];
        }
    }
    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;

        for (i=0;i<params->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                tmp->data.F32[i] = params->data.F32[i] + (a * line->data.F32[i]);
            } else {
                tmp->data.F32[i] = params->data.F32[i];
            }
        }
        fa = func(tmp, coords);

        for (i=0;i<params->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                tmp->data.F32[i] = params->data.F32[i] + (c * line->data.F32[i]);
            } else {
                tmp->data.F32[i] = params->data.F32[i];
            }
        }
        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);
}

/******************************************************************************
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 anr 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
 *****************************************************************************/
float p_psLineMin(psMinimization *min,
                  psVector *params,
                  psVector *line,
                  const psVector *paramMask,
                  const psArray *coords,
                  psMinimizePowellFunc func)
{
    psVector *bracket;
    float a = 0.0;
    float b = 0.0;
    float c = 0.0;
    float n = 0.0;
    float fa = 0.0;
    float fb = 0.0;
    float fc = 0.0;
    float fn = 0.0;
    float mul = 0.0;
    psVector *tmpa = NULL;
    psVector *tmpb = NULL;
    psVector *tmpc = NULL;
    psVector *tmpn = NULL;
    int i = 0;
    int null = 0;

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

    if (paramMask != NULL) {
        for (i=0;i<params->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                if (line->data.F32[i] >= FLT_EPSILON) {
                    null = 1;
                    psTrace(".psLib.dataManip.p_psLineMin", 4,
                            "line->data.F32[%d] is %f\n", i, line->data.F32[i]);
                }
            }
        }
    } else {
        for (i=0;i<params->n;i++) {
            if (line->data.F32[i] >= FLT_EPSILON) {
                null = 1;
                psTrace(".psLib.dataManip.p_psLineMin", 4,
                        "line->data.F32[%d] is %f\n", i, line->data.F32[i]);
            }
        }
    }

    if (null == 0) {
        psTrace(".psLib.dataManip.p_psLineMin", 2,
                "p_psLineMin() called with zero line vector.\n");
        return(0.0);
    }

    tmpa = psVectorAlloc(params->n, PS_TYPE_F32);
    tmpb = psVectorAlloc(params->n, PS_TYPE_F32);
    tmpc = psVectorAlloc(params->n, PS_TYPE_F32);
    tmpn = psVectorAlloc(params->n, PS_TYPE_F32);

    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_psDetermineBracket(params, line, paramMask, coords, func);
    if (bracket == NULL) {
        psFree(tmpa);
        psFree(tmpb);
        psFree(tmpc);
        psFree(tmpn);
        psAbort(__func__, "(1) Could not bracket minimum.");
    }

    min->iter = 0;
    while (min->iter < min->maxIter) {
        min->iter++;
        psTrace(".psLib.dataManip.p_psLineMin", 6,
                "p_psLineMin(): iteration %d\n", min->iter);

        a = bracket->data.F32[0];
        b = bracket->data.F32[1];
        c = bracket->data.F32[2];

        for (i=0;i<params->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                tmpa->data.F32[i] = params->data.F32[i] + (a * line->data.F32[i]);
                tmpb->data.F32[i] = params->data.F32[i] + (b * line->data.F32[i]);
                tmpc->data.F32[i] = params->data.F32[i] + (c * line->data.F32[i]);
            } else {
                tmpa->data.F32[i] = params->data.F32[i];
                tmpb->data.F32[i] = params->data.F32[i];
                tmpc->data.F32[i] = params->data.F32[i];
            }
        }
        fa = func(tmpa, coords);
        fb = func(tmpb, coords);
        fc = func(tmpc, coords);
        psTrace(".psLib.dataManip.p_psLineMin", 6,
                "Iteration %d: f(%f %f %f) is (%f %f %f)\n", min->iter,
                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,
                "Iteration %d: new bracket is (%f %f %f)\n", min->iter, 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)) {
            for (i=0;i<params->n;i++) {
                if (paramMask->data.U8[i] == 0) {
                    params->data.F32[i]+= mul * line->data.F32[i];
                }
            }
            min->value = func(params, coords);
            psFree(bracket);
            psFree(tmpa);
            psFree(tmpb);
            psFree(tmpc);
            psFree(tmpn);
            psTrace(".psLib.dataManip.p_psLineMin", 4,
                    "---- p_psLineMin() end.a (%f) ----\n", mul);
            return(mul);
        }
    }

    psFree(bracket);
    psFree(tmpa);
    psFree(tmpb);
    psFree(tmpc);
    psFree(tmpn);

    psTrace(".psLib.dataManip.p_psLineMin", 4,
            "---- p_psLineMin() end.b (0.0) ----\n");
    return(0.0);
}


/******************************************************************************
This routine must minimize a possibly multi-dimensional function.  The
function to be minimized "func" is:
    float 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.
 
XXX: Define constants for default dummy number of iterations and tolerance.
 
XXX: Must define behavior on various NULL inputs.
 
XXX: Check for F32 types?
 *****************************************************************************/
bool psMinimizePowell(psMinimization *min,
                      psVector *params,
                      const psVector *paramMask,
                      const psArray *coords,
                      psMinimizePowellFunc func)
{
    PS_CHECK_NULL_VECTOR_RETURN_NULL(params);
    PS_CHECK_EMPTY_VECTOR_RETURN_NULL(params);
    PS_CHECK_NULL_PTR_RETURN_NULL(min);
    PS_CHECK_NULL_PTR_RETURN_NULL(coords);
    PS_CHECK_NULL_PTR_RETURN_NULL(func);

    int numDims = params->n;
    int i = 0;
    int j = 0;
    psVector **v = NULL;
    psVector *pQP = NULL;
    psVector *Q = NULL;
    psVector *u = NULL;
    psVector *myParamMask = NULL;  // XXX: Use a recylced vector here.
    psMinimization dummyMin;
    float mul = 0.0;
    float baseFuncVal = 0.0;
    float currFuncVal = 0.0;
    int biggestIter = 0;
    float biggestDiff = 0.0;
    float term1 = 0.0;
    float term2 = 0.0;
    float fqp = 0.0;
    float diff = 0.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);

    pQP = psVectorAlloc(numDims, PS_TYPE_F32);
    u = psVectorAlloc(numDims, PS_TYPE_F32);
    Q = psVectorAlloc(numDims, PS_TYPE_F32);

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

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

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

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

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

        baseFuncVal = func(Q, coords);
        currFuncVal = baseFuncVal;
        diff = 0.0;
        biggestDiff = 0;
        biggestIter = 0;
        for (i=0;i<numDims;i++) {
            if (myParamMask->data.U8[i] == 0) {
                dummyMin.maxIter = 100;
                dummyMin.tol = 0.01;
                mul = p_psLineMin(&dummyMin, Q, v[i], myParamMask, coords, func);
                if (fabs(dummyMin.value - currFuncVal) > biggestDiff) {
                    biggestDiff = fabs(dummyMin.value - currFuncVal);
                    biggestIter = i;
                }
                currFuncVal = dummyMin.value;
            }
        }

        // 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=Q-P (%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.
        psTrace(".psLib.dataManip.psMinimizePowell", 6, "HERE 01\n");
        for (i=0;i<numDims;i++) {
            psTrace(".psLib.dataManip.psMinimizePowell", 6,
                    "u[i] is %f\n", u->data.F32[i]);
        }

        mul = p_psLineMin(min, params, u, myParamMask, coords, func);

        // 6:
        if (min->value > currFuncVal) {
            for (i=0;i<numDims;i++) {
                psFree(v[i]);
            }
            psFree(v);
            psFree(u);
            psFree(pQP);
            psFree(Q);
            if (paramMask == NULL) {
                psFree(myParamMask);
            }
            psTrace(".psLib.dataManip.psMinimizePowell", 4,
                    "---- psMinimizePowell() end (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];
            }
        }
        fqp = func(pQP, coords);

        term1 = (baseFuncVal - currFuncVal) - biggestDiff;
        term1*= term1;
        term1*= 2.0 * (baseFuncVal - (2.0 * currFuncVal) + fqp);
        term2 = baseFuncVal - fqp;
        term2*= term2 * biggestDiff;
        if (term1 < term2) {
            for (i=0;i<numDims;i++) {
                if (myParamMask->data.U8[i] == 0) {
                    v[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) {
            for (i=0;i<numDims;i++) {
                psFree(v[i]);
            }
            psFree(v);
            psFree(u);
            psFree(pQP);
            psFree(Q);
            psTrace(".psLib.dataManip.psMinimizePowell", 4,
                    "---- psMinimizePowell() end (true) ----\n");
            if (paramMask == NULL) {
                psFree(myParamMask);
            }
            return(true);
        }
    }

    if (paramMask == NULL) {
        psFree(myParamMask);
    }
    for (i=0;i<numDims;i++) {
        psFree(v[i]);
    }
    psFree(v);
    psFree(u);
    psFree(pQP);
    psFree(Q);
    psTrace(".psLib.dataManip.psMinimizePowell", 4,
            "---- psMinimizePowell() end (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
 *****************************************************************************/
float myPowellChi2Func(const psVector *params,
                       const psArray *coords)
{
    float chi2 = 0.0;
    float d;
    int i;
    psVector *tmp;

    tmp = Chi2PowellFunc(params, coords);
    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);
    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 func)
{
    myValue = (psVector *) value;
    myError = (psVector *) error;
    Chi2PowellFunc = func;

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


