/** @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
 *
 *  @version $Revision: 1.116 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-05-11 22:02:16 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 *
 *  XXX: must follow coding name standards on local functions.
 *
 */
/*****************************************************************************/
/* INCLUDE FILES                                                             */
/*****************************************************************************/
#include <stdio.h>
#include <float.h>
#include <math.h>

#include "psMinimize.h"
#include "psStats.h"
#include "psImageIO.h"
/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/
#define PS_SEG psLib
#define PS_PWD dataManip
#define PS_FILE psMinimize
/*****************************************************************************/
/* 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: Use a static vector.
 *****************************************************************************/
static void buildSums1D(psF64 x,
                        psS32 polyOrder,
                        psVector* sums)
{
    psS32 i = 0;
    psF64 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?
 *****************************************************************************/
static psF32 *calculateSecondDerivs(const psVector* x,        ///< Ordinates (or NULL to just use the indices)
                                    const psVector* y)        ///< Coordinates
{
    psTrace(".psLib.dataManip.calculateSecondDerivs", 4,
            "---- calculateSecondDerivs() begin ----\n");

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

    // XXX: The second derivatives at the endpoints, undefined in the SDR,
    // are set in psConstants.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV.
    derivs2[0] = -0.5;
    u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_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])) * (PS_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]);
    }

    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: This is F32 only
 
XXX: spline->knots must be psF32
 *****************************************************************************/
/*
psF32 p_psNRSpline1DEval(psSpline1D *spline,
                         const psVector* x,
                         const psVector* y,
                         psF32 X)
{
    PS_PTR_CHECK_NULL(spline, NAN);
    PS_INT_CHECK_NON_NEGATIVE(spline->n, NAN);
    PS_VECTOR_CHECK_NULL(spline->domains, NAN);
    PS_PTR_CHECK_NULL(spline->p_psDeriv2, NAN);
    PS_VECTOR_CHECK_NULL(x, NAN);
    PS_VECTOR_CHECK_TYPE(x, PS_TYPE_F32, NAN);
    PS_VECTOR_CHECK_NULL(y, NAN);
    PS_VECTOR_CHECK_TYPE(y, PS_TYPE_F32, NAN);
    PS_VECTOR_CHECK_TYPE(spline->knots, PS_TYPE_F32, NULL);
 
    psS32 n;
    psS32 klo;
    psS32 khi;
    psF32 H;
    psF32 A;
    psF32 B;
    psF32 C;
    psF32 D;
    psF32 Y;
 
    n = spline->n;
    klo = p_psVectorBinDisect32(spline->knots->data.F32, (spline->n)+1, X);
    if (klo < 0) {
        psLogMsg(__func__, PS_LOG_WARN,
                 "WARNING: psMinimize.c: p_psNRSpline1DEval(): p_psVectorBinDisect32 returned an error (%d).\n", klo);
        return(NAN);
    }
    khi = klo + 1;
    H = spline->knots->data.F32[khi] - spline->knots->data.F32[klo];
    A = (spline->knots->data.F32[khi] - X) / H;
    B = (X - spline->knots->data.F32[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 knots 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?
 
XXX: Assumes mySpline->knots is psF32.  Must add psU32 and psF64.
 *****************************************************************************/
psSpline1D *psVectorFitSpline1D(psSpline1D *mySpline,     ///< The spline which will be generated.
                                const psVector* x,        ///< Ordinates (or NULL to just use the indices)
                                const psVector* y,        ///< Coordinates
                                const psVector* yErr)     ///< Errors in coordinates, or NULL
{
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_TYPE_F32_OR_F64(y, NULL);
    if (mySpline != NULL) {
        PS_VECTOR_CHECK_TYPE(mySpline->knots, PS_TYPE_F32, NULL);
    }

    psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
            "---- psVectorFitSpline1D() begin ----\n");
    psS32 numSplines = (y->n)-1;
    psF32 tmp;
    psF32 H;
    psS32 i;
    psF32 slope;
    psVector *x32 = NULL;
    psVector *y32 = NULL;
    psVector *yErr32 = NULL;
    static psVector *x32Static = NULL;
    static psVector *y32Static = NULL;
    static psVector *yErr32Static = NULL;

    PS_VECTOR_CONVERT_F64_TO_F32_STATIC(y, y32, y32Static);

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

    // If x==NULL, create an x32 vector with x values set to (0:n).
    if (x == NULL) {
        PS_VECTOR_GEN_X_INDEX_STATIC_F32(x32Static, y->n);
        x32 = x32Static;
    } else {
        PS_VECTOR_CHECK_TYPE_F32_OR_F64(x, NULL);
        PS_VECTOR_CONVERT_F64_TO_F32_STATIC(x, x32, x32Static);
    }
    PS_VECTOR_CHECK_SIZE_EQUAL(x32, y32, NULL);
    PS_VECTOR_CHECK_SIZE_EQUAL(yErr32, y32, NULL);

    /*
        XXX:
        This can not be implemented until SDR states what order spline should be
        created.
        Should we error if mySpline is not NULL?
        Should we error if mySPline is not NULL?
    */
    if (mySpline == NULL) {
        mySpline = psSpline1DAllocGeneric(x32, 3);
    }
    PS_PTR_CHECK_NULL(mySpline, NULL);
    PS_INT_CHECK_NON_NEGATIVE(mySpline->n, NULL);

    if (y32->n != (1 + mySpline->n)) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "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->knots->data.F32[i+1] - mySpline->knots->data.F32[i]);
            (mySpline->spline[i])->coeff[0] = y32->data.F32[i] -
                                              (slope * mySpline->knots->data.F32[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(PS_ERR_BAD_PARAMETER_SIZE, true,
                "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",
                x32->data.F32[i],
                x32->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);
}


/******************************************************************************
XXX: We assume unnormalized gaussians.
 *****************************************************************************/
psVector *psMinimizeLMChi2Gauss1D(psImage *deriv,
                                  const psVector *params,
                                  const psArray *coords)
{
    PS_PTR_CHECK_NULL(coords, NULL);
    PS_PTR_CHECK_NULL(params, NULL);

    psTrace(".psLib.dataManip.psMinimize", 4,
            "---- psMinimizeLMChi2Gauss1D() begin ----\n");
    psF32 x;
    psS32 i;
    psF32 mean = params->data.F32[0];
    psF32 stdev = params->data.F32[1];
    psVector *out = psVectorAlloc(coords->n, PS_TYPE_F32);

    psTrace(".psLib.dataManip.psMinimize", 6,
            "(mean, stdev) is (%f, %f)\n", mean, stdev);

    if (deriv == NULL) {
        deriv = psImageAlloc(params->n, coords->n, PS_TYPE_F32);
    } else {
        PS_IMAGE_CHECK_SIZE(deriv, params->n, coords->n, NULL);
        PS_IMAGE_CHECK_TYPE(deriv, PS_TYPE_F32, NULL);
    }

    for (i=0;i<coords->n;i++) {
        x = ((psVector *) (coords->data[i]))->data.F32[0];
        out->data.F32[i] = psGaussian(x, mean, stdev, false);
    }

    for (i=0;i<coords->n;i++) {
        x = ((psVector *) (coords->data[i]))->data.F32[0];
        psF32 tmp = (x - mean) * psGaussian(x, mean, stdev, false);
        deriv->data.F32[i][0] = tmp / (stdev * stdev);
        tmp = (x - mean) * (x - mean) *
              psGaussian(x, mean, stdev, 0);
        deriv->data.F32[i][1] = tmp / (stdev * stdev * stdev);
    }

    psTrace(".psLib.dataManip.psMinimize", 4,
            "---- psMinimizeLMChi2Gauss1D() end ----\n");
    return(out);
}

/*
XXX: from bug 230:
 
We first perform a rotation:
u = - (x-x0)*cos(theta) + (y-y0)*sin(theta)
v = (x-x0)*cos(theta) + (y-y0)*sin(theta)
 
Here u is the major axis, and v is the minor axis, x0,y0 is the centre, and
theta is the position angle.
 
Then the flux is
 
flux = norm * exp(-( u*u/2.0/sigmau/sigmau + v*v/2.0/sigmav/sigmav)
)/2.0/pi/sigmau/sigmav
 
Here sigmau and sigmav are the widths of the major and minor axes.
 
The "norm" parameter in the equation above corresponds to the normalisation.
 
Suggest order:
 
norm
x0
y0
sigma_u
sigma_v
theta
*/

psVector *psMinimizeLMChi2Gauss2D(psImage *deriv,
                                  const psVector *params,
                                  const psArray *coords)
{
    PS_PTR_CHECK_NULL(coords, NULL);
    PS_PTR_CHECK_NULL(params, NULL);

    psF64 normalization = params->data.F32[0];
    psF64 x0 = params->data.F32[1];
    psF64 y0 = params->data.F32[2];
    psF64 sigmaX = params->data.F32[3];
    psF64 sigmaY = params->data.F32[4];
    psF64 theta = params->data.F32[5];
    psVector *out = psVectorAlloc(coords->n, PS_TYPE_F32);

    if (deriv == NULL) {
        deriv = psImageAlloc(params->n, coords->n, PS_TYPE_F32);
    } else {
        PS_IMAGE_CHECK_SIZE(deriv, 6, coords->n, NULL);
        PS_IMAGE_CHECK_TYPE(deriv, PS_TYPE_F32, NULL);
    }

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

    for (psS32 i=0;i<coords->n;i++) {
        psF64 x = ((psVector *) coords->data[i])->data.F32[0];
        psF64 y = ((psVector *) coords->data[i])->data.F32[0];

        psF64 u = - (x-x0)*cos(theta) + (y-y0)*sin(theta);
        psF64 v = (x-x0)*cos(theta) + (y-y0)*sin(theta);

        psF64 flux = normalization * exp(-( u*u/(2.0 * sigmaX * sigmaX) +
                                            v*v/(2.0 * sigmaY * sigmaY)))/
                     (2.0 * M_PI * sigmaX * sigmaY);
        out->data.F32[i] = flux;

        // XXX: Calculate these correctly.
        deriv->data.F32[i][0] = 0.0;
        deriv->data.F32[i][1] = 0.0;
        deriv->data.F32[i][2] = 0.0;
        deriv->data.F32[i][3] = 0.0;
        deriv->data.F32[i][4] = 0.0;
        deriv->data.F32[i][5] = 0.0;
    }

    psTrace(".psLib.dataManip.psMinimize", 4,
            "---- psMinimizeLMChi2Gauss2D() end ----\n");
    return(out);
}

psF64 p_psImageGetElementF64(psImage *a, int i, int j);

// XXX EAM this is my re-implementation of MinLM
psBool psMinimizeLMChi2(psMinimization *min,
                        psImage *covar,
                        psVector *params,
                        const psVector *paramMask,
                        const psArray *x,
                        const psVector *y,
                        const psVector *yErr,
                        psMinimizeLMChi2Func func)
{
    PS_PTR_CHECK_NULL(min, NULL);
    PS_VECTOR_CHECK_NULL(params, NULL);
    PS_VECTOR_CHECK_EMPTY(params, NULL);
    PS_PTR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_EMPTY(y, NULL);
    PS_VECTOR_CHECK_SIZE_EQUAL(x, y, NULL);
    PS_PTR_CHECK_NULL(func, NULL);

    // 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_F64);
    psVector *dy     = NULL;
    psF64 chisq = 0.0;
    psF64 Chisq = 0.0;
    psF64 lambda = 0.001;

    // 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
    dy = psVectorAlloc (y->n, PS_TYPE_F32);
    if (yErr != NULL) {
        for (int i = 0; i < dy->n; i++) {
            dy->data.F32[i] = 1.0 / PS_SQR (yErr->data.F32[i]);
        }
    } else {
        for (int i = 0; i < dy->n; i++) {
            dy->data.F32[i] = 1.0;
        }
    }

    // calculate initial alpha and beta, set chisq (min->value)
    min->value = p_psMinLM_SetABX (alpha, beta, params, x, y, dy, func);
    # ifndef PS_NO_TRACE
    // dump some useful info if trace is defined
    if (psTraceGetLevel (".psLib.dataManip.psMinimizeLMChi2") > 4) {
        p_psImagePrint  (psTraceGetDestination(), alpha, "alpha guess");
        p_psVectorPrint (psTraceGetDestination(), beta, "beta guess");
        p_psVectorPrint (psTraceGetDestination(), params, "params guess");
    }
    # endif /* PS_NO_TRACE */


    // iterate until the tolerance is reached, or give up
    while ((min->lastDelta > min->tol) && (min->iter < min->maxIter)) {

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

        # ifndef PS_NO_TRACE
        // dump some useful info if trace is defined
        if (psTraceGetLevel (".psLib.dataManip.psMinimizeLMChi2") > 4) {
            p_psImagePrint  (psTraceGetDestination(), Alpha, "alpha guess");
            p_psVectorPrint (psTraceGetDestination(), Beta, "beta guess");
            p_psVectorPrint (psTraceGetDestination(), Params, "params guess");
        }
        # endif /* PS_NO_TRACE */

        // calculate Chisq for new guess, update Alpha & Beta
        Chisq = p_psMinLM_SetABX (Alpha, Beta, Params, x, y, dy, func);
        psTrace (".psLib.dataManip.psMinimizeLMChi2", 3, "chisq: %f, Chisq %f, delta: %f\n", chisq, Chisq, min->lastDelta);

        // accept new guess (if improvement), or increase lambda
        if (Chisq < min->value) {
            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 (".psLib.dataManip.psMinimizeLMChi2", 3, "chisq: %f, Chisq %f, delta: %f\n", chisq, Chisq, min->lastDelta);

    // free the internal temporary data
    psFree (alpha);
    psFree (Alpha);
    psFree (beta);
    psFree (Beta);
    psFree (Params);
    psFree (dy);
    return (true);
}

// XXX EAM: this needs to respect the mask on params
// XXX EAM: check not NULL on alpha, beta, params
// alpha, beta, params are already allocated
psF64 p_psMinLM_SetABX (psImage  *alpha,
                        psVector *beta,
                        psVector *params,
                        const psArray  *x,
                        const psVector *y,
                        const psVector *dy,
                        psMinimizeLMChi2Func func)
{

    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++) {
            weight = deriv->data.F32[j] * dy->data.F32[i];
            for (int k = 0; k <= j; k++) {
                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];
        }
    }
    psFree (deriv);
    return (chisq);
}

// XXX EAM : can we use static copies of LUv, LUm, A?
psBool p_psMinLM_GuessABP (psImage  *Alpha,
                           psVector *Beta,
                           psVector *Params,
                           psImage  *alpha,
                           psVector *beta,
                           psVector *params,
                           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 (".pslib.dataManip.psMinLM_GuessABP", 3, "using LUD version");

    // set new guess values (creates matrix A)
    A = psImageCopy (NULL, alpha, PS_TYPE_F64);
    for (int j = 0; j < params->n; j++) {
        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 (".pslib.dataManip.psMinLM_GuessABP", 3, "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++) {
        Alpha->data.F64[j][j] = alpha->data.F64[j][j] * (1.0 + lambda);
    }

    psGaussJordan (Alpha, Beta);
    # endif

    // apply beta to get new params values
    for (int j = 0; j < params->n; j++) {
        Params->data.F32[j] = params->data.F32[j] - Beta->data.F64[j];
    }

    # if (USE_LU_DECOMP)
        psFree (A);
    psFree (LUm);
    psFree (LUv);
    # endif

    return true;
}

// XXX" put this in constants.h
# define PS_SWAP(X,Y) {double tmp=(X); (X) = (Y); (Y) = tmp;}

// 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])) {
                // XXX EAM: this should use the psError stack
                fprintf (stderr, "GAUSSJ: NaN\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) {
                            // XXX EAM: this should use the psError stack
                            fprintf (stderr, "GAUSSJ: 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) {
            // XXX EAM: this should use the psError stack
            fprintf (stderr, "GAUSSJ: 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);
}

/******************************************************************************
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.
 
XXX: Must update the covar matrix.
 *****************************************************************************/
psBool psMinimizeLMChi2Old(psMinimization *min,
                           psImage *covar,
                           psVector *params,
                           const psVector *paramMask,
                           const psArray *x,
                           const psVector *y,
                           const psVector *yErr,
                           psMinimizeLMChi2Func func)
{
    PS_PTR_CHECK_NULL(min, NULL);
    PS_VECTOR_CHECK_NULL(params, NULL);
    PS_VECTOR_CHECK_EMPTY(params, NULL);
    PS_PTR_CHECK_NULL(x, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_EMPTY(y, NULL);
    PS_VECTOR_CHECK_SIZE_EQUAL(x, y, NULL);
    PS_PTR_CHECK_NULL(func, NULL);

    if (paramMask != NULL) {
        PS_VECTOR_CHECK_SIZE_EQUAL(params, paramMask, NULL);
    }
    if (yErr != NULL) {
        PS_VECTOR_CHECK_SIZE_EQUAL(y, yErr, NULL);
    }
    if (covar != NULL) {
        PS_IMAGE_CHECK_SIZE(covar, params->n, params->n, NULL);
    }

    psTrace(".psLib.dataManip.psMinimize", 4,
            "---- psMinimizeLMChi2() begin ----\n");
    psS32 numData = y->n;
    psS32 numParams = params->n;
    psS32 i;
    psS32 j;
    psS32 k;
    psS32 l;
    psS32 n;
    psS32 p;
    psVector *beta = psVectorAlloc(numParams, PS_TYPE_F64);
    psVector *perm = NULL;

    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);
    psImage *deriv = psImageAlloc(numParams, numData, PS_TYPE_F32);
    psVector *currValueVec = NULL;
    psVector *newValueVec = NULL;
    psF32 currChi2 = 0.0;
    psF32 newChi2 = 0.0;
    psF32 lamda = 0.00005;  // XXX EAM : this starting value is VERY small (lamda is mis-spelt)
    lamda = 0.05;  // XXX EAM : this starting value is quite large (lamda is mis-spelt)

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

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

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

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

        //
        // Calculate the current values and chi-squared of the function.
        //
        currChi2 = 0.0;
        // currValueVec = func(deriv, params, x);

        // XXX EAM: use BinaryOp ?
        // t1 = BinaryOp (NULL, currValueVec, "-", y);
        // t1 = BinaryOp (t1, t1, "*", t1);

        // XXX EAM: this ignores yErr
        for (n=0;n<numData;n++) {
            currChi2+= (currValueVec->data.F32[n] - y->data.F32[n]) *
                       (currValueVec->data.F32[n] - y->data.F32[n]);
            psTrace(".psLib.dataManip.psMinimize", 6,
                    "data[%d], chi2 calculation+= (%f - %f)^2\n", n,
                    currValueVec->data.F32[n], y->data.F32[n]);
        }

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

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

        //
        // Calculate the BETA vector.
        // XXX EAM: I think this is wrong
        for (p=0;p<numParams;p++) {
            if ((paramMask != NULL) && (paramMask->data.U8[p] != 0)) {
                continue;
            }
            beta->data.F64[p] = 0.0;
            for (n=0;n<numData;n++) {
                (beta->data.F64[p])+=
                    (y->data.F32[n] - currValueVec->data.F32[n]) *
                    deriv->data.F32[n][p];
            }
            // XXX: multiply by -1 here?
            (beta->data.F64[p])*= -1.0;
            psTrace(".psLib.dataManip.psMinimize", 6,
                    "beta->data.F64[%d] is %f.\n", p, beta->data.F64[p]);
        }
        psFree(currValueVec);

        //
        // Calculate the ALPHA matrix.
        // XXX EAM: also wrong? (missing yErr)
        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->data.F32[n][k] *
                                            deriv->data.F32[n][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] =
                        (psF64) ((1.0 + lamda) * alpha->data.F32[j][k]);
                } else {
                    A->data.F64[j][k] = (psF64) alpha->data.F32[j][k];
                }
            }
        }
        for (j=0;j<numParams;j++) {
            psTrace(".psLib.dataManip.psMinimize", 6, "Matrix A[][]:\n");
            for (k=0;k<numParams;k++) {
                psTrace(".psLib.dataManip.psMinimize", 6, "%f ", A->data.F64[j][k]);
            }
            psTrace(".psLib.dataManip.psMinimize", 6, "Matrix A[][]:\n");
        }

        //
        // Solve A * alpha = Beta
        //
        // XXX: How do we know if these functions were successful?
        //
        aOut = psMatrixLUD(aOut, &perm, A);
        paramDeltasF64 = psMatrixLUSolve(paramDeltasF64, aOut, beta, perm);

        //
        // Mask any masked parameters.
        //
        for (i=0;i<numParams;i++) {
            psTrace(".psLib.dataManip.psMinimize", 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] -
                                         (psF32) paramDeltasF64->data.F64[i];
            }
        }

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


        //
        // Calculate new function values.
        //
        newChi2 = 0.0;
        // newValueVec = func(deriv, newParams, x);
        for (n=0;n<numData;n++) {
            newChi2+= (newValueVec->data.F32[n] - y->data.F32[n]) *
                      (newValueVec->data.F32[n] - y->data.F32[n]);

        }
        psFree(newValueVec);

        psTrace(".psLib.dataManip.psMinimize", 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)/currChi2;
            min->value = newChi2;

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

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

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

/******************************************************************************
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: yErr is currently ignored.
*****************************************************************************/
static psPolynomial1D *vectorFitPolynomial1DCheby(psPolynomial1D* myPoly,
        const psVector* x,
        const psVector* y,
        const psVector* yErr)
{
    psS32 j;
    psS32 k;
    psS32 n = x->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 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.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->n;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():  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.
 *****************************************************************************/
static psPolynomial1D* vectorFitPolynomial1DOrd(psPolynomial1D* myPoly,
        const psVector* x,
        const psVector* y,
        const psVector* yErr)
{
    psS32 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;
    psS32 i = 0;
    psS32 j = 0;
    psS32 k = 0;
    psVector* xSums = NULL;

    psTrace(".psLib.dataManip.vectorFitPolynomial1DOrd", 4,
            "---- vectorFitPolynomial1DOrd() begin ----\n");
    // printf("VectorFitPolynomial1D()\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);
    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;
        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++) {
            buildSums1D(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++) {
            buildSums1D(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];
                }
            }
        }
    }

    // XXX: How do we know if these routines were successful?
    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]);
    }

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

    psTrace(".psLib.dataManip.vectorFitPolynomial1DOrd", 4,
            "---- vectorFitPolynomial1DOrd() 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* x,
                                        const psVector* y,
                                        const psVector* yErr)
{
    PS_POLY_CHECK_NULL(myPoly, NULL);
    PS_INT_CHECK_NON_NEGATIVE(myPoly->n, NULL);
    PS_VECTOR_CHECK_NULL(y, NULL);
    PS_VECTOR_CHECK_EMPTY(y, NULL);
    PS_VECTOR_CHECK_TYPE_F32_OR_F64(y, NULL);

    psS32 i;
    psVector *x64 = NULL;
    psVector *y64 = NULL;
    psVector *yErr64 = NULL;
    static psVector *x64Static = NULL;
    static psVector *y64Static = NULL;
    static psVector *yErr64Static = NULL;

    PS_VECTOR_CONVERT_F32_TO_F64_STATIC(y, y64, y64Static);
    // If yErr==NULL, set all errors equal.
    if (yErr == NULL) {
        PS_VECTOR_GEN_YERR_STATIC_F64(yErr64Static, y->n);
        yErr64 = yErr64Static;
    } else {
        PS_VECTOR_CHECK_TYPE_F32_OR_F64(yErr, NULL);
        PS_VECTOR_CONVERT_F32_TO_F64_STATIC(yErr, yErr64, yErr64Static);
    }

    // If x==NULL, create an x64 vector with x values set to (0:n).
    if (x == NULL) {
        PS_VECTOR_GEN_X_INDEX_STATIC_F64(x64Static, y->n);
        if (myPoly->type == PS_POLYNOMIAL_CHEB) {
            p_psNormalizeVectorRangeF64(x64Static, -1.0, 1.0);
        }
        x64 = x64Static;
    } else {
        PS_VECTOR_CHECK_TYPE_F32_OR_F64(x, NULL);
        PS_VECTOR_CONVERT_F32_TO_F64_STATIC(x, x64, x64Static);
        if (myPoly->type == PS_POLYNOMIAL_CHEB) {
            p_psNormalizeVectorRangeF64(x64, -1.0, 1.0);
        }
    }
    PS_VECTOR_CHECK_SIZE_EQUAL(x64, y64, NULL);
    PS_VECTOR_CHECK_SIZE_EQUAL(yErr64, y64, NULL);

    // Call the appropriate vector fitting routine.
    psPolynomial1D *rc = NULL;
    if (myPoly->type == PS_POLYNOMIAL_CHEB) {
        rc = vectorFitPolynomial1DCheby(myPoly, x64, y64, yErr64);
    } else if (myPoly->type == PS_POLYNOMIAL_ORD) {
        rc = vectorFitPolynomial1DOrd(myPoly, x64, y64, yErr64);
    } else {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "unknown polynomial type.\n");
        return(NULL);
    }
    if (rc == NULL) {
        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
        return(NULL);
    }

    return(myPoly);
}



/******************************************************************************
 *****************************************************************************/
psMinimization *psMinimizationAlloc(psS32 maxIter,
                                    psF32 tol)
{
    PS_INT_CHECK_NON_NEGATIVE(maxIter, NULL);

    psMinimization *min = psAlloc(sizeof(psMinimization));
    min->maxIter = maxIter;
    min->tol = tol;
    min->value = 0.0;
    min->iter = 0;
    min->lastDelta = tol + 1;

    return(min);
}

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


/******************************************************************************
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.
 
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
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_PTR_CHECK_NULL(min, NAN);
    PS_VECTOR_CHECK_NULL(params, NAN);
    PS_VECTOR_CHECK_EMPTY(params, NAN);
    PS_VECTOR_CHECK_TYPE(params, PS_TYPE_F32, NAN);
    PS_VECTOR_CHECK_NULL(line, NAN);
    PS_VECTOR_CHECK_EMPTY(line, NAN);
    PS_VECTOR_CHECK_TYPE(line, PS_TYPE_F32, NAN);
    PS_VECTOR_CHECK_NULL(paramMask, NAN);
    PS_VECTOR_CHECK_EMPTY(paramMask, NAN);
    PS_VECTOR_CHECK_TYPE(paramMask, PS_TYPE_U8, NAN);
    PS_PTR_CHECK_NULL(coords, NAN);
    PS_PTR_CHECK_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.
 
XXX: Check for F32 types?
 *****************************************************************************/
#define PS_MINIMIZE_POWELL_LINEMIN_MAX_ITERATIONS 20
#define PS_MINIMIZE_POWELL_LINEMIN_ERROR_TOLERANCE 0.01

psBool psMinimizePowell(psMinimization *min,
                        psVector *params,
                        const psVector *paramMask,
                        const psArray *coords,
                        psMinimizePowellFunc func)
{
    PS_PTR_CHECK_NULL(min, NULL);
    PS_VECTOR_CHECK_NULL(params, NULL);
    PS_VECTOR_CHECK_EMPTY(params, NULL);
    PS_VECTOR_CHECK_TYPE(params, PS_TYPE_F32, NULL);
    PS_PTR_CHECK_NULL(coords, NULL);
    PS_PTR_CHECK_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_VECTOR_CHECK_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) {
                dummyMin.maxIter = PS_MINIMIZE_POWELL_LINEMIN_MAX_ITERATIONS;
                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;
            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;
            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);
}


/******************************************************************************
XXX: We assume unnormalized gaussians.
XXX: Currently, yErr is ignored.
 *****************************************************************************/
psVector *psMinimizePowellChi2Gauss1D(const psVector *params,
                                      const psArray *coords)
{
    PS_PTR_CHECK_NULL(coords, NULL);
    PS_PTR_CHECK_NULL(params, NULL);

    psF32 x;
    psS32 i;
    psF32 mean = params->data.F32[0];
    psF32 stdev = params->data.F32[1];
    psVector *out = psVectorAlloc(coords->n, PS_TYPE_F32);

    for (i=0;i<coords->n;i++) {
        x = ((psVector *) (coords->data[i]))->data.F32[0];
        out->data.F32[i] = psGaussian(x, mean, stdev, false);
    }

    return(out);
}

/******************************************************************************
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
 *****************************************************************************/
static psF32 myPowellChi2Func(const psVector *params,
                              const psArray *coords)
{
    psTrace(".psLib.dataManip.myPowellChi2Func", 4,
            "---- myPowellChi2Func() begin ----\n");
    PS_VECTOR_CHECK_NULL(params, NAN);
    PS_VECTOR_CHECK_EMPTY(params, NAN);
    PS_VECTOR_CHECK_NULL(myValue, NAN);
    PS_VECTOR_CHECK_EMPTY(myValue, NAN);
    PS_PTR_CHECK_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().
 *****************************************************************************/
psBool 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));
}


