Index: trunk/psLib/src/math/psMinimize.c
===================================================================
--- trunk/psLib/src/math/psMinimize.c	(revision 1819)
+++ trunk/psLib/src/math/psMinimize.c	(revision 1831)
@@ -9,6 +9,6 @@
  *  @author George Gusciora, MHPCC
  *
- *  @version $Revision: 1.39 $ $Name: not supported by cvs2svn $
- *  @date $Date: 2004-09-16 19:30:24 $
+ *  @version $Revision: 1.40 $ $Name: not supported by cvs2svn $
+ *  @date $Date: 2004-09-18 01:50:45 $
  *
  *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
@@ -844,10 +844,9 @@
     return (initialGuess);
 }
+
 /******************************************************************************
 This routine will take an procedure which calculates an arbitrary function
 and it's derivative and minimize it.
  
-GUS
- 
 XXX: Do this:
  After checking that all entries in the paramMask are 1 or 0, when
@@ -855,6 +854,4 @@
  
      A[i][i] = (1 + lambda*paramask[i]) * alpha[i][i];
- 
- 
  *****************************************************************************/
 bool psMinimizeLM(psMinimization *min,
@@ -982,5 +979,5 @@
 
         oldValue = min->value;
-        newValue = func(newDeriv, NRparams, coords);
+        newValue = func(deriv, NRparams, coords);
         psTrace(".psLib.dataManip.psMinimizeLM", 4,
                 "old/new values are (%f, %f)\n", oldValue, newValue);
@@ -1029,4 +1026,238 @@
     psTrace(".psLib.dataManip.psMinimizeLM", 4,
             "---- psMinimizeLM() end (false) ----\n");
+    return(false);
+}
+
+/******************************************************************************
+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];
+ *****************************************************************************/
+bool psMinimizeLMChi2(psMinimization *min,
+                      psVector *params,
+                      const psVector *paramMask,
+                      psImage *covar,
+                      const psArray *coords,
+                      const psVector *value,
+                      psMinimizeLMChi2Func func)
+{
+    psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
+            "---- psMinimizeLMChi2() begin ----\n");
+    int numData = value->n;
+    int numParams = params->n;
+    int i;
+    int j;
+    int k;
+    int l;
+    int n;
+    int p;
+    psVector *beta = psVectorAlloc(numParams, PS_TYPE_F64);
+    psVector *perm = psVectorAlloc(numParams, PS_TYPE_F64);
+
+    psVector *paramDeltasF64 = psVectorAlloc(numParams, PS_TYPE_F64);
+    psVector *origParams = psVectorAlloc(numParams, PS_TYPE_F32);
+    psVector *newParams = psVectorAlloc(numParams, PS_TYPE_F32);
+
+    psImage *alpha = psImageAlloc(numParams, numParams, PS_TYPE_F32);
+    psImage *A = psImageAlloc(numParams, numParams, PS_TYPE_F64);
+    psImage *aOut = psImageAlloc(numParams, numParams, PS_TYPE_F64);
+
+    psVector **deriv = (psVector **) psAlloc(numData * sizeof(psVector *));
+    for (i=0;i<numData;i++) {
+        deriv[i] = psVectorAlloc(numParams, PS_TYPE_F32);
+    }
+
+    psVector *currValueVec = psVectorAlloc(value->n, PS_TYPE_F32);
+    psVector *newValueVec = psVectorAlloc(value->n, PS_TYPE_F32);
+
+    float currChi2 = 0.0;
+    float newChi2 = 0.0;
+    float lamda = 0.00005;
+
+    psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+            "min->maxIter is %d\n", min->maxIter);
+    psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+            "min->tol is %f\n", min->tol);
+
+    for (p=0;p<numParams;p++) {
+        origParams->data.F32[p] = params->data.F32[p];
+    }
+
+    min->lastDelta = HUGE;
+    min->lastDelta = 12345.0;
+    min->iter = 0;
+
+    while ((min->lastDelta > min->tol) && (min->iter < min->maxIter)) {
+        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
+                "------------------------------------------------------\n");
+        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
+                "Iteration %d.  Delta is %f\n", min->iter, min->lastDelta);
+
+        //
+        // Calculate the current values and chi-squared of the function.
+        //
+        currChi2 = 0.0;
+        for (n=0;n<numData;n++) {
+            currValueVec->data.F32[n] = func(deriv[n],
+                                             params,
+                                             (psVector *) coords->data[n]);
+            currChi2+= (currValueVec->data.F32[n] * currValueVec->data.F32[n]);
+        }
+
+        for (p=0;p<numParams;p++) {
+            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+                    "params->data.F32[%d] is %f.\n", p, params->data.F32[p]);
+        }
+        psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+                "Current chi-squared is (%f)\n", currChi2);
+
+        //
+        // Mask elements of the derivative for each data point.
+        //
+        for (p=0;p<numParams;p++) {
+            if ((paramMask != NULL) && (paramMask->data.U8[p] != 0)) {
+                for (n=0;n<numData;n++) {
+                    (deriv[n])->data.F32[p] = 0.0;
+                }
+            }
+        }
+
+        //
+        // Calculate the BETA vector.
+        //
+        for (p=0;p<numParams;p++) {
+            beta->data.F64[p] = 0.0;
+            for (n=0;n<numData;n++) {
+                (beta->data.F64[p])+=
+                    (value->data.F32[n] - currValueVec->data.F32[n]) *
+                    (deriv[n])->data.F32[p];
+            }
+            // XXX: multiple by -1 here?
+            (beta->data.F64[p])*= -1.0;
+            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+                    "beta->data.F64[%d] is %f.\n", p, beta->data.F64[p]);
+        }
+
+        //
+        // Calculate the ALPHA matrix.
+        //
+        for (k=0;k<numParams;k++) {
+            for (l=0;l<numParams;l++) {
+                alpha->data.F32[k][l] = 0.0;
+                for (n=0;n<numData;n++) {
+                    alpha->data.F32[k][l]+= (deriv[n])->data.F32[k] *
+                                            (deriv[n])->data.F32[l];
+                }
+            }
+        }
+
+        //
+        // Calculate the matrix A.
+        //
+        for (j=0;j<numParams;j++) {
+            for (k=0;k<numParams;k++) {
+                if (j == k) {
+                    A->data.F64[j][k] =
+                        (double) ((1.0 + lamda) * alpha->data.F32[j][k]);
+                } else {
+                    A->data.F64[j][k] = (double) alpha->data.F32[j][k];
+                }
+            }
+        }
+        for (j=0;j<numParams;j++) {
+            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6, "Matrix A[][]:\n");
+            for (k=0;k<numParams;k++) {
+                psTrace(".psLib.dataManip.psMinimizeLMChi2", 6, "%f ", A->data.F64[j][k]);
+            }
+            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6, "Matrix A[][]:\n");
+        }
+
+        //
+        // Solve A * alpha = Beta
+        //
+        aOut = psMatrixLUD(aOut, perm, A);
+        paramDeltasF64 = psMatrixLUSolve(paramDeltasF64, aOut, beta, perm);
+
+        //
+        // Mask any masked parameters.
+        //
+        for (i=0;i<numParams;i++) {
+            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+                    "paramDeltasF64->data.F64[%d] is %f.\n", i, paramDeltasF64->data.F64[i]);
+            if ((paramMask != NULL) && (paramMask->data.U8[i] != 0)) {
+                newParams->data.F32[i] = origParams->data.F32[i];
+            } else {
+                newParams->data.F32[i] = params->data.F32[i] -
+                                         (float) paramDeltasF64->data.F64[i];
+            }
+        }
+
+        psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+                "Calling func() with new parameters:\n");
+        for (i=0;i<numParams;i++) {
+            psTrace(".psLib.dataManip.psMinimizeLMChi2", 6,
+                    "newParams->data.F32[%d] is %f.\n", i, newParams->data.F32[i]);
+        }
+
+
+        //
+        // Calculate new function values.
+        //
+        newChi2 = 0.0;
+        for (n=0;n<numData;n++) {
+            newValueVec->data.F32[n] = func(deriv[n], newParams,
+                                            (psVector *) coords->data[n]);
+            newChi2+= (newValueVec->data.F32[n] * newValueVec->data.F32[n]);
+        }
+        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
+                "old/new chi-squareds are (%f, %f)\n", currChi2, newChi2);
+
+        //
+        // If the new chi-squared is lower, then keep it.
+        //
+        if (currChi2 > newChi2) {
+            min->lastDelta = currChi2 - newChi2;
+            min->value = newChi2;
+
+            // We already masked params.
+            for (i=0;i<numParams;i++) {
+                params->data.F32[i] = (float) newParams->data.F32[i];
+            }
+            lamda*= 0.1;
+        } else {
+            lamda*= 10.0;
+        }
+        psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
+                "lamda is %f\n", lamda);
+        min->iter++;
+    }
+    psFree(beta);
+    psFree(perm);
+    psFree(paramDeltasF64);
+    psFree(origParams);
+    psFree(newParams);
+    psFree(alpha);
+    psFree(A);
+    psFree(aOut);
+    for (i=0;i<numData;i++) {
+        psFree(deriv[i]);
+    }
+    psFree(deriv);
+    psFree(currValueVec);
+    psFree(newValueVec);
+
+    if ((min->iter < min->maxIter) ||
+            (min->lastDelta <= min->tol)) {
+        return(true);
+    }
+
+    psTrace(".psLib.dataManip.psMinimizeLMChi2", 4,
+            "---- psMinimizeLMChi2() end (false) ----\n");
     return(false);
 }