Index: trunk/psLib/src/math/psMinimize.c
===================================================================
--- trunk/psLib/src/math/psMinimize.c	(revision 3578)
+++ trunk/psLib/src/math/psMinimize.c	(revision 3852)
@@ -9,6 +9,6 @@
  *  @author GLG, MHPCC
  *
- *  @version $Revision: 1.110 $ $Name: not supported by cvs2svn $
- *  @date $Date: 2005-03-31 01:02:15 $
+ *  @version $Revision: 1.111 $ $Name: not supported by cvs2svn $
+ *  @date $Date: 2005-05-05 22:10:12 $
  *
  *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -557,30 +557,37 @@
 }
 
-/******************************************************************************
-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.
- *****************************************************************************/
+psF64 p_psImageGetElementF64(psImage *a, int i, int j);
+
+// XXX EAM these two functions are useful for testing
+// XXX EAM this should move to psImage.c
+bool p_psImagePrint (FILE *f, psImage *a, char *name)
+{
+
+    fprintf (f, "matrix: %s\n", name);
+
+    for (int j = 0; j < a[0].numRows; j++) {
+        for (int i = 0; i < a[0].numCols; i++) {
+            fprintf (f, "%f  ", p_psImageGetElementF64(a, i, j));
+        }
+        fprintf (f, "\n");
+    }
+    fprintf (f, "\n");
+    return (true);
+}
+
+// XXX EAM this should move to psVector.c
+bool p_psVectorPrint (FILE *f, psVector *a, char *name)
+{
+
+    fprintf (f, "vector: %s\n", name);
+
+    for (int i = 0; i < a[0].n; i++) {
+        fprintf (f, "%f\n", p_psVectorGetElementF64(a, i));
+    }
+    fprintf (f, "\n");
+    return (true);
+}
+
+// XXX EAM this is my re-implementation of MinLM
 psBool psMinimizeLMChi2(psMinimization *min,
                         psImage *covar,
@@ -591,4 +598,350 @@
                         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;
+}
+
+# define 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 (!finite(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++) {
+                SWAP (matrix[irow][l], matrix[icol][l]);
+            }
+            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++)
+                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);
@@ -636,6 +989,6 @@
     psF32 currChi2 = 0.0;
     psF32 newChi2 = 0.0;
-    psF32 lamda = 0.00005;
-    lamda = 0.05;
+    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,
@@ -661,5 +1014,11 @@
         //
         currChi2 = 0.0;
-        currValueVec = func(deriv, params, x);
+        // 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]) *
@@ -670,4 +1029,5 @@
         }
 
+        // XXX EAM: this is just for tracing
         for (p=0;p<numParams;p++) {
             psTrace(".psLib.dataManip.psMinimize", 6,
@@ -679,5 +1039,5 @@
         //
         // 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)) {
@@ -690,6 +1050,9 @@
         //
         // 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++) {
@@ -707,5 +1070,5 @@
         //
         // Calculate the ALPHA matrix.
-        //
+        // XXX EAM: also wrong? (missing yErr)
         for (k=0;k<numParams;k++) {
             for (l=0;l<numParams;l++) {
@@ -773,5 +1136,5 @@
         //
         newChi2 = 0.0;
-        newValueVec = func(deriv, newParams, x);
+        // newValueVec = func(deriv, newParams, x);
         for (n=0;n<numData;n++) {
             newChi2+= (newValueVec->data.F32[n] - y->data.F32[n]) *
@@ -1098,5 +1461,5 @@
     min->value = 0.0;
     min->iter = 0;
-    min->lastDelta = 0.0;
+    min->lastDelta = tol + 1;
 
     return(min);