/** @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 George Gusciora, MHPCC
 *
 *  @version $Revision: 1.23 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2004-07-15 23:52:34 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 */
/*****************************************************************************/
/* INCLUDE FILES            */
/*****************************************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include <float.h>
#include <math.h>

#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>

#include "psMemory.h"
#include "psVector.h"
#include "psImage.h"
#include "psTrace.h"
#include "psError.h"
#include "psAbort.h"
#include "psFunctions.h"
#include "psMinimize.h"
#include "psMatrix.h"
/*****************************************************************************/
/* DEFINE STATEMENTS           */
/*****************************************************************************/
#define MAX_LMM_ITERATIONS 100
#define MAX_MINIMIZE_ITERATIONS 100

/** Preprocessor macro to generate error on a NULL 1DPolynomial */
#define PS_CHECK_NULL_1DPOLY(NAME)                                                          \
if (NAME == NULL || NAME->coeff == NULL) {                                                         \
    psError(__func__,"Invalid operation: %s or its data is NULL.", #NAME);                          \
}

/** Preprocessor macro to generate error on a NULL vector */
#define PS_CHECK_NULL_VECTOR(NAME)                                                          \
if (NAME == NULL || NAME->data.V == NULL) {                                                         \
    psError(__func__,"Invalid operation: %s or its data is NULL.", #NAME);                          \
}

/** Preprocessor macro to generate error for zero length vector */
#define PS_CHECK_EMPTY_VECTOR(NAME)                                                          \
if (NAME->n < 1) {                                                                                  \
    psError(__func__,"Invalid operation: %s has zero n value.", #NAME);                             \
}

/** Preprocessor macro to generate error on differing size vectors */
#define PS_CHECK_VECTOR_SIZE_EQUAL(VEC1, VEC2)                                                          \
if (VEC1->n != VEC2->n) {               \
    psError(__func__,"Vector %s has size %d, Vector %s has size %d.", #VEC1, VEC1->n, #VEC2, VEC2->n); \
}

/** Preprocessor macro to generate error on a NULL image */
#define PS_CHECK_NULL_IMAGE(NAME)                                                           \
if (NAME == NULL || NAME->data.V == NULL) {                                                         \
    psError(__func__,"Invalid operation: %s or its data is NULL.", #NAME);                          \
}

/** Preprocessor macro to generate error for zero length rows or columns */
#define PS_CHECK_EMPTY_IMAGE(NAME)                                                           \
if (NAME->numCols < 1 || NAME->numRows < 1) {                                                       \
    psError(__func__,"Invalid operation: %s has zero rows or columns (%dx%d).", #NAME,              \
            NAME->numCols, NAME->numRows);                                                          \
}


/*****************************************************************************/
/* TYPE DEFINITIONS           */
/*****************************************************************************/
typedef struct
{
    size_t n;   // Number of data points points in domain.
    int paramCount;   // Number of non-masked parameters.
    psVector *restrict       initialGuess;
    const psImage *restrict  domain;
    const psVector *restrict data;
    const psVector *restrict  errors;
    const psVector *restrict paramMask;
    float (*evalModel) (const psVector *, const psVector *);
    float (*d_evalModel) (const psVector *, const psVector *, int);
}
psMinChi2Data;

typedef struct
{
    int paramCount;   // Number of non-masked parameters.
    psVector *restrict       initialGuess;
    const psVector *restrict  coord;
    const psVector *restrict paramMask;
    float (*evalModel) (const psVector *, const psVector *);
    float (*d_evalModel) (const psVector *, const psVector *, int);
}
psMinimizeData;

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

// None

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

// None

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

/******************************************************************************
p_psMinFunc(*params, *funcData): We use the GSL procedure
gsl_multimin_fdfminimizer_iterate() to minimize an arbitary function supplied
by the user.  That GSL procedure requires the function to be minimized to be
in a different format than the psLib format.  The purpose of this procedure
is to serve as a GSL-format wrapper for the user-supplied procedure which is
to be minimized.
 
    params: The parameters of the function to be minimized.  These will be
 varied by GSL in order to minimize the function.
 
    funcData: a private psLib struct which contains the data point to be
 minimized, the function and derivative function pointers, an initial
 guess at the parameters, an option parameter mask, etc.
 *****************************************************************************/
double p_psMinFunc(const gsl_vector *params,
                   void *funcData)
{
    int i;    // Loop index variable.
    int j;    // Loop index variable.
    float tmpf;    // Temporary floating point variable.
    const psVector *restrict coord   = ((psMinimizeData *) funcData)->coord;
    const psVector *restrict mask     = ((psMinimizeData *) funcData)->paramMask;
    psVector *restrict initialGuess = ((psMinimizeData *)funcData)->initialGuess;
    float (*evalModel)(const psVector *, const psVector *) =
        ((psMinimizeData *) funcData)->evalModel;
    psVector *inputParameterList = NULL;

    // The GSL routines will call this function with the masked parameters
    // removed.  However, the user-supplied function (to be modified) does not
    // have those parameters removed.  Here will create a new parameter list
    // with the masked parameters added (we expand initialGuess).
    inputParameterList = psVectorAlloc(initialGuess->n, PS_TYPE_F32);
    if (mask != NULL) {
        j = 0;
        for (i=0;i<mask->n;i++) {
            if (mask->data.U8[i] != 0) {
                inputParameterList->data.F32[i] = initialGuess->data.F32[i];
            } else {
                inputParameterList->data.F32[i] = gsl_vector_get(params, j++);
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            inputParameterList->data.F32[i] = gsl_vector_get(params, i);
        }
    }

    // Call the user-supplied function.
    tmpf = evalModel(inputParameterList, coord);

    // Free allocated memory and return the value of the function.
    psFree(inputParameterList);
    return(tmpf);
}

/******************************************************************************
p_psMinFuncDeriv(*params, *funcData):  a GSL-like wrapper for the
user-supplied procedure which calculates the derviative of the function to be
minimized.
 
    params: The parameters of the function to be minimized.  These will be
 varied by GSL in order to minimize the function.
 
    funcData: a private psLib struct which contains the data point to be
 minimized, the function and derivative function pointers, an initial
 guess at the parameters, an option parameter mask, etc.
 
    df: we calculate the derivative of the function w.r.t. to each parameter
 in "params" and return those derivatives in this psVector.
 *****************************************************************************/
void p_psMinFuncDeriv(const gsl_vector *params,
                      void *funcData,
                      gsl_vector *df)
{
    int i;    // Loop index variable.
    int j;    // Loop index variable.
    float tmpf;    // Temporary floating point variable.
    const psVector *restrict coord   = ((psMinimizeData *) funcData)->coord;
    const psVector *restrict mask     = ((psMinimizeData *) funcData)->paramMask;
    psVector *restrict initialGuess = ((psMinimizeData *)funcData)->initialGuess;
    float (*d_evalModel)(const psVector *, const psVector *, int) =
        ((psMinimizeData *) funcData)->d_evalModel;
    psVector *inputParameterList = NULL;

    // The GSL routines will call this function with the masked parameters
    // removed.  However, the user-supplied function (to be modified) does not
    // have those parameters removed.  Here will create a new parameter list
    // with the masked parameters added (we expand initialGuess).
    inputParameterList = psVectorAlloc(initialGuess->n, PS_TYPE_F32);
    if (mask != NULL) {
        j = 0;
        for (i=0;i<mask->n;i++) {
            if (mask->data.U8[i] != 0) {
                inputParameterList->data.F32[i] = initialGuess->data.F32[i];
            } else {
                inputParameterList->data.F32[i] = gsl_vector_get(params, j++);
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            inputParameterList->data.F32[i] = gsl_vector_get(params, i);
        }
    }

    // Evaluate the derivative w.r.t. each parameter.
    // NOTE: we can probably remove the calls for masked parameters.
    for (i=0;i<initialGuess->n;i++) {
        tmpf = d_evalModel(inputParameterList, coord, i);
        gsl_vector_set(df, i, tmpf);
    }

    // Free allocated memory.
    psFree(inputParameterList);
}

/******************************************************************************
    Compute both p_psMinFunc and p_psMinFuncDeriv together.
 *****************************************************************************/
void p_psMinFuncFuncDeriv(const gsl_vector *params,
                          void *funcData,
                          double *f,
                          gsl_vector *df)
{
    *f = p_psMinFunc(params, funcData);
    p_psMinFuncDeriv(params, funcData, df);
}

// The first argument to evalModel() and d_evalModel() specifies the data
// point.  It must have the same size as the second dimension of *domain.
// The second argument must have the same size as *initialGuess and
// *paramMask.

/******************************************************************************
p_psMinChi2Func(*x, *funcData, *outdata): We use the GSL procedure
gsl_multifit_fdfsolver_iterate() to fit an arbitrary function, supplied by
the user, to a set of data points.  That GSL procedure requires the function
to be fit to be in a different format than the psLib format.  The purpose of
this procedure is to serve as a GSL-format wrapper for the user-supplied
procedure which is to be fit to the data.
 
    params: These are the parameters which are to be varied by GSL in order
  to minimize chi2 over the data set.
 
    funcData: this data structure contains the input values over which the
  function will be evaluated, the expected value of the function at
  those points, the amount of error tolerable at those points, a mask
  vector which specifies which parameters to the function are to be
  constant, and an initial guess at the parameters.
 
    outData: The function is evaluated at each point, then subtract the
  expected value and divide by the error.
 *****************************************************************************/
int p_psMinChi2Func(const gsl_vector *params,
                    void *funcData,
                    gsl_vector *outData)
{
    int i;    // Loop index variable.
    int j;    // Loop index variable.
    float tmpf;    // Temporary floating point variable.
    const psImage *restrict  domain   = ((psMinChi2Data *)funcData)->domain;
    const psVector *restrict data     = ((psMinChi2Data *)funcData)->data;
    const psVector *restrict errors   = ((psMinChi2Data *) funcData)->errors;
    const psVector *restrict mask     = ((psMinChi2Data *) funcData)->paramMask;
    psVector *restrict initialGuess = ((psMinChi2Data *)funcData)->initialGuess;
    float (*evalModel)(const psVector *, const psVector *) = ((psMinChi2Data *) funcData)->evalModel;
    psVector *inputParameterList = NULL;
    psVector *tmpVecPtr = NULL;

    tmpVecPtr = psVectorAlloc(domain->numCols, PS_TYPE_F32);

    // The GSL routines will call this function with the masked parameters
    // removed.  However, the user-supplied function (to be modified) does not
    // have those parameters removed.  Here will create a new parameter list
    // with the masked parameters added (we expand initialGuess).

    inputParameterList = psVectorAlloc(initialGuess->n, PS_TYPE_F32);
    if (mask != NULL) {
        j = 0;
        for (i=0;i<mask->n;i++) {
            if (mask->data.U8[i] != 0) {
                inputParameterList->data.F32[i] = initialGuess->data.F32[i];
            } else {
                inputParameterList->data.F32[i] = gsl_vector_get(params, j++);
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            inputParameterList->data.F32[i] = gsl_vector_get(params, i);
        }
    }

    // Evaluate the function at each data point.
    for (i=0;i<domain->numRows;i++) {
        for (j=0;j<domain->numCols;j++) {
            tmpVecPtr->data.F32[j] = domain->data.F32[i][j];
        }
        tmpf = evalModel(tmpVecPtr, inputParameterList);

        gsl_vector_set(outData, i, (tmpf - data->data.F32[i])/
                       errors->data.F32[i]);
    }

    // Free allocated memory.
    psFree(inputParameterList);
    psFree(tmpVecPtr);

    return GSL_SUCCESS;
}

/******************************************************************************
p_psMinChi2FuncDeriv(*x, *funcData, *outdata): a GSL-like wrapper for the
user-supplied procedure which calculates the derviative of the function to be
minimized.
    params: These are the parameters which are to be varied by GSL in order
  to minimize chi2 over the data set.
 
    funcData: this data structure contains the input values over which the
  function will be evaluated, the expected value of the function at
  those points, the amount of error tolerable at those points, a mask
  vector which specifies which parameters to the function are to be
  constant, and an initial guess at the parameters.
 
    J: The derivative is evaluated at each point and w.r.t. each parameter
 and returned in this data structure.
 *****************************************************************************/
int p_psMinChi2FuncDeriv(const gsl_vector *params,
                         void *funcData,
                         gsl_matrix *J)
{
    const psImage *restrict domain   = ((psMinChi2Data *)funcData)->domain;
    const psVector *restrict errors   = ((psMinChi2Data *) funcData)->errors;
    const psVector *restrict mask     = ((psMinChi2Data *) funcData)->paramMask;
    psVector *restrict initialGuess = ((psMinChi2Data *)funcData)->initialGuess;
    psVector *inputParameterList = NULL;
    psVector *tmpVecPtr = NULL;
    float (*d_evalModel)(const psVector *, const psVector *, int) = ((psMinChi2Data *) funcData)->d_evalModel;

    size_t i;
    int j;
    float tmpf;

    tmpVecPtr = psVectorAlloc(domain->numCols, PS_TYPE_F32);

    // The GSL routines will call this functions with the masked parameters
    // removed.  However, the user-supplied function (to be modified) does not
    // have those parameters removed.  Here will create a new parameter list
    // with the masked parameters added (we expand initialGuess).

    inputParameterList = psVectorAlloc(initialGuess->n, PS_TYPE_F32);
    if (mask != NULL) {
        j = 0;
        for (i=0;i<mask->n;i++) {
            if (mask->data.U8[i] != 0) {
                inputParameterList->data.F32[i] = initialGuess->data.F32[i];
            } else {
                inputParameterList->data.F32[i] = gsl_vector_get(params, j++);
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            inputParameterList->data.F32[i] = gsl_vector_get(params, i);
        }
    }

    // Evaluate the derivtaive at each data point, and w.r.t. each parameter.
    for (i=0;i<domain->numRows;i++) {
        for (j=0;j<tmpVecPtr->n;j++) {
            tmpVecPtr->data.F32[j] = domain->data.F32[i][j];
        }

        for (j=0;j<inputParameterList->n;j++) {
            tmpf = d_evalModel(tmpVecPtr, inputParameterList, j);
            gsl_matrix_set(J, i, j, (tmpf/errors->data.F32[i]));
        }
    }

    psFree(inputParameterList);
    psFree(tmpVecPtr);
    return GSL_SUCCESS;
}


int p_psMinChi2FuncFuncDeriv(const gsl_vector *params,
                             void *funcData,
                             gsl_vector *f,
                             gsl_matrix *J)
{
    p_psMinChi2Func(params, funcData, f);
    p_psMinChi2FuncDeriv(params, funcData, J);

    return GSL_SUCCESS;
}


/******************************************************************************
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.
 *****************************************************************************/
void p_psBuildSums1D(double x,
                     int polyOrder,
                     psVector *sums)
{
    int       i = 0;
    double    xSum = 0.0;

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


/******************************************************************************
p_psBuildSums1D(x): this routine returns a psVector with "x" elements.  The
values of the vector will be scaled uniformly between -1.0 and 1.0.
 *****************************************************************************/
psVector *psBuildImageScalingFactors(int x)

{
    int i = 0;                  // loop index variable.
    psVector *imageScalingFactors = NULL;


    imageScalingFactors = psVectorAlloc(x, PS_TYPE_F32);

    for (i=0;i<x;i++) {
        imageScalingFactors->data.F32[i] = (((float) 2*i) / ((float) x)) - 1.0;
    }

    return(imageScalingFactors);
}

/******************************************************************************
CURRENTLY NOT IN USE.
 
p_psPolyOrderCheck(A, N, *indx, *B, polyOrder,*flag) This routine checks if
all polyOrder-th terms in the polyOrder-th order sky background polynomial
defined by the coefficients in the array B[] are consistent with zero.  If
true, then *flag is set to 1.  Otherwise, *flag is set to 0.  The matrix
inversion code in the middle of this procedure draws from Numerical Recipes
in C page 48.
Input:
    A       This is the LUD decomposition of the original matrix A.
    N       The size of the matrix (plus 1, actually, since offset 1).
    indx    misc Numerical Recipes data structure.
    B       The coefficients of the sky polynomial.
    polyOrder The degree of the sky polynomial.
Output:
    *flag   Set this to 1 if we must recalculate the coefficients.
 *****************************************************************************/
void p_psPolyOrderCheck(float **A,
                        int N,
                        int *indx,
                        float *B,
                        int polyOrder,
                        int *flag)
{
    float     **y = NULL;  // This 2-D matrix will hold A^-1
    float      *col = NULL;             // misc NumerRecipes data structure
    float      *error=NULL;             // will hold the sqrt() of the
    // diagonal of y[][].
    int         i=0;                    // loop-index variable
    int         j=0;                    // loop-index variable
    int         numPolyTerms = 0;       // The number of terms in the
    // polynomial.
    int         lastTerm = 0;           // The index location of the first
    // n-th order term in array B[].
    int         firstTerm = 0;          // Index location of last such term.

    // Allocate the necessary data structures for this procedure...
    error = (float *) psAlloc((N + 1) * sizeof(float));
    col = (float *) psAlloc((N + 1) * sizeof(float));
    y = (float **) psAlloc((N + 1) * sizeof(float *));
    for(i=1;i<=N;i++) {
        y[i] = (float *) psAlloc((N + 1) * sizeof(float));
    }

    // Invert the matrix A and put the result in y[][].  This code is taken
    // from Numerical Recipes in C page 48.
    for(j=1;j<=N;j++) {
        for(i=1;i<=N;i++) {
            col[i] = 0.0;
        }
        col[j] = 1.0;
        // NOTE: substitue the LUD rotine
        //        lubksb(A, N, indx, col);
        for(i=1;i<=N;i++) {
            y[i][j] = col[i];
        }
    }

    // Determine where the first n-th order (in this comment, n equals
    // polyOrder) polynomial term is stored in the matrix B[], and also were
    // the last n-order term is stored.  Then we loop over all the n-order
    // terms and check if they are consistent with zero.

    numPolyTerms = (((polyOrder+1) * (polyOrder + 2)) / 2);
    lastTerm = numPolyTerms + 1;
    firstTerm = lastTerm - polyOrder;
    *flag = 1;
    for (i=firstTerm; i<=lastTerm; i++) {
        #ifdef DARWIN
        error[i] = (float)sqrt(y[i][i]);
        #else

        error[i] = sqrtf(y[i][i]);
        #endif

        if (!((B[i]  <= (2.0f * error[i])) &&
                ((-2.0f * error[i]) <= B[i]))) {
            *flag = 0;
        }
    }

    // Free all memory allocated in this routine.
    psFree(error);
    psFree(col);
    for(j=1;j<=N;j++) {
        psFree(y[j]);
    }
    psFree(y);
}




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



/******************************************************************************
psMinimize(initialGuess, myFunction, myFunctionDeriv, coord, paramMask):
 
This routine must minimize an arbitrary function; it determines the set of
parameters of that function such that the.
 *****************************************************************************/
psVector *
psMinimize(psVector *restrict initialGuess,
           float (*myFunction)(const psVector *restrict, const psVector *restrict),
           float (*myFunctionDeriv)(const psVector *restrict, const psVector *restrict, int),
           const psVector *restrict coord,
           const psVector *restrict paramMask)
{
    int status;
    int i = 0;
    int j = 0;
    int iter = 0;
    gsl_multimin_function_fdf f;
    const gsl_multimin_fdfminimizer_type *T;
    gsl_multimin_fdfminimizer *s;
    psMinimizeData inputData;
    gsl_vector *x;

    PS_CHECK_NULL_VECTOR(initialGuess);
    PS_CHECK_EMPTY_VECTOR(initialGuess);
    PS_CHECK_NULL_VECTOR(coord);
    PS_CHECK_EMPTY_VECTOR(coord);
    if (paramMask != NULL) {
        PS_CHECK_NULL_VECTOR(paramMask);
        PS_CHECK_EMPTY_VECTOR(paramMask);
        PS_CHECK_VECTOR_SIZE_EQUAL(initialGuess, paramMask);
    }

    inputData.initialGuess = initialGuess;
    inputData.coord = coord;
    inputData.paramMask = paramMask;
    inputData.evalModel = myFunction;
    inputData.d_evalModel = myFunctionDeriv;
    inputData.paramCount = 0;

    // If the user supplied a parameter mask, then count the number of
    // non-masked elements.  This will be used later in allocating a vector
    // for the parameters.
    if (paramMask != NULL) {
        for (i=0;i<paramMask->n;i++) {
            if (paramMask->data.U8[i] != 0) {
                inputData.paramCount++;
            }
        }
    } else {
        inputData.paramCount= initialGuess->n;
    }

    // The initial guess at the parameters for the function are written into
    // the vector inputParameterList.  If the paramMask is not NULL, then
    // masked parameters are masked out.
    x = gsl_vector_alloc(inputData.paramCount);
    if (paramMask != NULL) {
        j = 0;
        for (i=0;i<initialGuess->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                gsl_vector_set(x, j++, initialGuess->data.F32[i]);
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            gsl_vector_set(x, i, initialGuess->data.F32[i]);
        }
    }
    f.f = &p_psMinFunc;
    f.df = &p_psMinFuncDeriv;
    f.fdf = &p_psMinFuncFuncDeriv;
    f.n = inputData.paramCount;
    f.params = &inputData;

    T = gsl_multimin_fdfminimizer_conjugate_fr;
    s = gsl_multimin_fdfminimizer_alloc(T, inputData.paramCount);
    gsl_multimin_fdfminimizer_set(s, &f, x, 0.01, 1e-4);
    do {
        iter++;
        status = gsl_multimin_fdfminimizer_iterate(s);

        if (status)
            break;

        status = gsl_multimin_test_gradient(s->gradient, 1e-3);

        if (status == GSL_SUCCESS)
            printf ("Minimum found at:\n");

    } while (status == GSL_CONTINUE && iter < MAX_MINIMIZE_ITERATIONS);

    // In the above steps we had removed the masked elements from the
    // the solver.  This next code blocks puts those masked elements
    // into the solution.
    if (paramMask != NULL) {
        j = 0;
        for (i=0;i<initialGuess->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                initialGuess->data.F32[i] = gsl_vector_get(s->x, j++);
            } else {
                initialGuess->data.F32[i] = initialGuess->data.F32[i];
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            initialGuess->data.F32[i] = gsl_vector_get(s->x, i);
        }
    }
    return(initialGuess);
}






/******************************************************************************
    This routine must minimize an arbitrary function.
 *****************************************************************************/
psVector *
psMinimizeChi2(float (*evalModel)(const psVector *restrict, const psVector *restrict),
               float (*DevalModel)(const psVector *restrict, const psVector *restrict, int),
               const psImage *restrict domain,
               const psVector *restrict data,
               const psVector *restrict errors,
               psVector *restrict initialGuess,
               const psVector *restrict paramMask,
               float *chiSq)
{
    int numData = domain->numRows; // Number of data points
    int status;    // Return status for the GSL solver.
    int i = 0;    // Loop index variable.
    int j = 0;    // Loop index variable.
    int iter = 0;   // Iteration counter.
    gsl_multifit_function_fdf f; // GSL structure that contains the
    // functions/derivative to be solved.
    double *xInit = NULL;        // The initial guess at the parameters
    // with masked parameters removed.
    const gsl_multifit_fdfsolver_type *T;
    // This tells GSL to use the Levenberg-
    // Marquardt algorithm for chi2
    // minimization.
    gsl_multifit_fdfsolver *s; // GSL data structure.
    psMinChi2Data inputData;
    float chiSqOld = 0.0;

    PS_CHECK_NULL_IMAGE(domain);
    PS_CHECK_EMPTY_IMAGE(domain);
    PS_CHECK_NULL_VECTOR(data);
    PS_CHECK_EMPTY_VECTOR(data);
    PS_CHECK_NULL_VECTOR(errors);
    PS_CHECK_EMPTY_VECTOR(errors);
    PS_CHECK_NULL_VECTOR(initialGuess);
    PS_CHECK_EMPTY_VECTOR(initialGuess);
    PS_CHECK_VECTOR_SIZE_EQUAL(data, errors);
    if (domain->numRows != data->n) {
        psAbort(__func__,"Number of data points and data values not equal.");
    }
    if (paramMask != NULL) {
        PS_CHECK_NULL_VECTOR(paramMask);
        PS_CHECK_EMPTY_VECTOR(paramMask);
        PS_CHECK_VECTOR_SIZE_EQUAL(initialGuess, paramMask);
    }

    inputData.n = numData;
    inputData.paramCount = 0;
    inputData.initialGuess = initialGuess;
    inputData.domain = domain;
    inputData.data = data;
    inputData.errors = errors;
    inputData.paramMask = paramMask;
    inputData.evalModel = evalModel;
    inputData.d_evalModel = DevalModel;

    // If the user supplied a parameter mask, then count the number of
    // non-masked elements.  This will be used later in allocating a vector
    // for the parameters.
    if (paramMask != NULL) {
        for (i=0;i<paramMask->n;i++) {
            if (paramMask->data.U8[i] != 0) {
                inputData.paramCount++;
            }
        }
    } else {
        inputData.paramCount= initialGuess->n;
    }

    // The initial guess at the parameters for the function are written into
    // the vector inputParameterList.  If the paramMask is not NULL, then those
    // parameters are masked out.
    xInit = (double *) psAlloc(inputData.paramCount * sizeof(double));
    if (paramMask != NULL) {
        j = 0;
        for (i=0;i<initialGuess->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                xInit[j++] = initialGuess->data.F32[i];
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            xInit[i] = initialGuess->data.F32[i];
        }
    }

    const gsl_rng_type *type;
    gsl_rng *r;
    gsl_rng_env_setup();

    type = gsl_rng_default;
    r = gsl_rng_alloc(type);

    // Initialize the main data structure used by the GSL solver.  This will
    // contain pointers to the function to be minimized, it's derivative
    // function, the number of data points, the number of free parameters,
    // and the data structures those functions use.

    f.f = &p_psMinChi2Func;
    f.df = &p_psMinChi2FuncDeriv;
    f.fdf = &p_psMinChi2FuncFuncDeriv;
    f.n = numData;
    f.p = inputData.paramCount;
    f.params = &inputData;

    gsl_vector_view x = gsl_vector_view_array(xInit, inputData.paramCount);
    T = gsl_multifit_fdfsolver_lmsder;
    s = gsl_multifit_fdfsolver_alloc(T, numData, inputData.paramCount);
    gsl_multifit_fdfsolver_set(s, &f, &x.vector);
    *chiSq = 0.0;
    chiSqOld = 0.0;
    do {
        iter++;
        // Perform an iteration of the GSL solver.
        status = gsl_multifit_fdfsolver_iterate(s);
        //        printf ("psMinimize() status = %s\n", gsl_strerror(status));
        // If there was a problem, abort.
        if (status) {
            psAbort(__func__, "gsl_multifit_fdfsolver_iterate()\n");
        }

        // Test if the parameters changed by a small enough amount.
        // NOTE: This wasn't working right when the parameters fit exactly.
        // Figure out why.
        //        status = gsl_multifit_test_delta(s->dx, s->x, 1e-4, 1e-4);

        // We test for convergence if chiSquared changes by less than 1.0
        // as specified in the ADD.
        *chiSq = gsl_blas_dnrm2(s->f);
        if (fabs(*chiSq - chiSqOld) < 1.0) {
            status = GSL_SUCCESS;
        } else {
            status = GSL_CONTINUE;
        }
        chiSqOld = *chiSq;

    } while (status == GSL_CONTINUE && iter < MAX_LMM_ITERATIONS);


    // In the above steps we had removed the masked elements from the
    // the solver.  This next code blocks puts those masked elements
    // into the solution.
    if (paramMask != NULL) {
        j = 0;
        for (i=0;i<initialGuess->n;i++) {
            if (paramMask->data.U8[i] == 0) {
                initialGuess->data.F32[i] = gsl_vector_get(s->x, j++);
            } else {
                initialGuess->data.F32[i] = initialGuess->data.F32[i];
            }
        }
    } else {
        for (i=0;i<initialGuess->n;i++) {
            initialGuess->data.F32[i] = gsl_vector_get(s->x, i);
        }
    }

    // Calculate the chi-squared for the derived solution.
    *chiSq = gsl_blas_dnrm2(s->f);

    // Free all allocated memory
    // NOTE: Free x.
    gsl_multifit_fdfsolver_free(s);
    psFree(xInit);

    // Bye bye.
    return(initialGuess);
}


/******************************************************************************
    This routine must fit a polynomial of degree myPoly to the data points
    (x, y) and return the coefficients of that polynomial, as well as the
    error for each data poiny (yErr).
 
NOTE: yErr is currently ignored.
 *****************************************************************************/
psPolynomial1D *
psGetArrayPolynomial(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;

    PS_CHECK_NULL_1DPOLY(myPoly);
    PS_CHECK_NULL_VECTOR(x);
    PS_CHECK_EMPTY_VECTOR(x);
    PS_CHECK_NULL_VECTOR(y);
    PS_CHECK_EMPTY_VECTOR(y);
    PS_CHECK_NULL_VECTOR(yErr);
    PS_CHECK_EMPTY_VECTOR(yErr);
    PS_CHECK_VECTOR_SIZE_EQUAL(x, y);
    PS_CHECK_VECTOR_SIZE_EQUAL(y, yErr);

    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.
    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];
            }
        }
    }

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

    for(k=0;k<(polyOrder);k++) {
        myPoly->coeff[k] = coeffs->data.F64[k];
    }

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

    return(NULL);
}
