Index: /trunk/psLib/src/math/psPolynomialMD.c
===================================================================
--- /trunk/psLib/src/math/psPolynomialMD.c	(revision 19084)
+++ /trunk/psLib/src/math/psPolynomialMD.c	(revision 19085)
@@ -27,6 +27,21 @@
     )
 {
+    if (!poly) return;
     psFree(poly->orders);
     psFree(poly->coeff);
+
+    psFree(poly->fitMatrix);
+    psFree(poly->fitVector);
+    psFree(poly->tmpMatrix);
+    psFree(poly->tmpVector);
+
+    psFree(poly->fitBuffer);
+
+    psFree(poly->ownMask);
+    psFree(poly->deviations);
+
+    psFree(poly->LUperm);
+    psFree(poly->LU);
+    
     return;
 }
@@ -111,46 +126,4 @@
 }
 
-// Accumulate and solve the least-squares equation
-static bool polynomialMDClipFit(psPolynomialMD *poly, // Polynomial
-                                psImage *matrix, // Least-squares matrix
-                                psVector *vector, // Least-squares vector
-                                psImage **lu, // LU-decomposed matrix
-                                psVector **perm, // Permutations vector
-                                const psArray *matrices, // Individual least-squares matrices
-                                const psArray *vectors, // Individual least-squares vectors
-                                const psVector *mask // Mask for values
-    )
-{
-    int numValues = vectors->n;         // Number of values
-
-    // Accumulate the least-squares matrix and vector
-    psImageInit(matrix, 0.0);
-    psVectorInit(vector, 0.0);
-    for (int i = 0; i < numValues; i++) {
-        if (mask->data.U8[i]) {
-            continue;
-        }
-
-        psImage *newMatrix = matrices->data[i]; // Matrix to accumulate
-        psVector *newVector = vectors->data[i]; // Vector to accumulate
-
-        polynomialMDAccumulate(matrix, vector, newMatrix, newVector);
-    }
-    polynomialMDFill(matrix);
-
-    // Solve least-squares equation
-    *lu = psMatrixLUD(*lu, perm, matrix);
-    if (!*lu) {
-        psError(PS_ERR_UNKNOWN, false, "Unable to LU-Decompose least-squares matrix.");
-        return false;
-    }
-    poly->coeff = psMatrixLUSolve(poly->coeff, *lu, vector, *perm);
-    if (!poly->coeff) {
-        psError(PS_ERR_UNKNOWN, false, "Unable to solve least-squares equation.");
-        return false;
-    }
-    return true;
-}
-
 // Calculate the standard deviation of the fit
 static void polynomialMDStdev(psPolynomialMD *poly, // Polynomial for which to measure stdev
@@ -158,5 +131,6 @@
                               const psArray *coords, // Array of coordinates
                               const psVector *values, // Measured values
-                              const psVector *mask // Mask for values
+                              const psVector *mask, // Mask for values
+			      psMaskType maskVal 
                               )
 {
@@ -169,5 +143,5 @@
     int numGood = numValues;            // Number of good values
     for (int i = 0; i < numValues; i++) {
-        if (mask && mask->data.U8[i]) {
+        if (mask && (mask->data.U8[i] & maskVal)) {
             numGood--;
             continue;
@@ -234,4 +208,18 @@
     poly->coeff = psVectorAlloc(numTerms, PS_TYPE_F64);
 
+    // internal temporary variables so we can use this in a tight, threaded loop
+    poly->fitMatrix = NULL;
+    poly->fitVector = NULL;
+    poly->tmpMatrix = NULL;
+    poly->tmpVector = NULL;
+
+    poly->fitBuffer = NULL;
+
+    poly->ownMask = NULL;
+    poly->deviations = NULL;
+
+    poly->LUperm = NULL;
+    poly->LU = NULL;
+
     return poly;
 }
@@ -288,12 +276,15 @@
     int numValues = values->n;          // Number of values
     int numTerms = poly->coeff->n;      // Number of terms
-    psImage *matrix = psImageAlloc(numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix
-    psImageInit(matrix, 0.0);
-    psVector *vector = psVectorAlloc(numTerms, PS_TYPE_F64); // Least-squares vector
-    psVectorInit(vector, 0.0);
-
-    psImage *newMatrix = psImageAlloc(numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix for each term
-    psVector *newVector = psVectorAlloc(numTerms, PS_TYPE_F64); // Least-squares vector for each term
-    psVector *buffer = psVectorAlloc(numTerms, PS_TYPE_F64); // Buffer for evaluations of polynomial stages
+
+    // we recycle matrices and vectors carried by poly to avoid alloc / threading hits
+    poly->fitMatrix = psImageRecycle(poly->fitMatrix, numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix
+    poly->fitVector = psVectorRecycle(poly->fitVector, numTerms, PS_TYPE_F64); // Least-squares vector
+
+    psImageInit(poly->fitMatrix, 0.0);
+    psVectorInit(poly->fitVector, 0.0);
+
+    poly->tmpMatrix = psImageRecycle (poly->tmpMatrix, numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix for each term
+    poly->tmpVector = psVectorRecycle(poly->tmpVector, numTerms, PS_TYPE_F64); // Least-squares vector for each term
+    poly->fitBuffer = psVectorRecycle(poly->fitBuffer, numTerms, PS_TYPE_F64); // Buffer for evaluations of polynomial stages
 
     for (int i = 0; i < numValues; i++) {
@@ -307,26 +298,17 @@
         }
 
-        polynomialMDLeastSquares(poly, newMatrix, newVector, coords, values->data.F32[i],
-                                 errors ? errors->data.F32[i] : 0.0, buffer);
-        polynomialMDAccumulate(matrix, vector, newMatrix, newVector);
-    }
-    polynomialMDFill(matrix);
-
-    psFree(newMatrix);
-    psFree(newVector);
-    psFree(buffer);
-
-    psVector *perm = NULL;              // Permutation vector
-    psImage *lu = psMatrixLUD(NULL, &perm, matrix); // LU-decomposed matrix
-    psFree(matrix);
-    if (!lu) {
+	float err = errors ? errors->data.F32[i] : 0.0;
+        polynomialMDLeastSquares(poly, poly->tmpMatrix, poly->tmpVector, coords, values->data.F32[i], err, poly->fitBuffer);
+        polynomialMDAccumulate(poly->fitMatrix, poly->fitVector, poly->tmpMatrix, poly->tmpVector);
+    }
+    polynomialMDFill(poly->fitMatrix);
+
+    poly->LU = psMatrixLUD(poly->LU, &poly->LUperm, poly->fitMatrix); // LU-decomposed matrix
+    if (!poly->LU) {
         psError(PS_ERR_UNKNOWN, false, "Unable to LU-Decompose least-squares matrix.");
-        psFree(vector);
         return false;
     }
-    poly->coeff = psMatrixLUSolve(poly->coeff, lu, vector, perm);
-    psFree(vector);
-    psFree(lu);
-    psFree(perm);
+
+    poly->coeff = psMatrixLUSolve(poly->coeff, poly->LU, poly->fitVector, poly->LUperm);
     if (!poly->coeff) {
         psError(PS_ERR_UNKNOWN, false, "Unable to solve least-squares equation.");
@@ -334,8 +316,148 @@
     }
 
-    polynomialMDStdev(poly, NULL, coordsArray, values, NULL);
+    polynomialMDStdev(poly, poly->deviations, coordsArray, values, mask, maskVal);
 
     return true;
 }
+
+// XXX this function should take a (psMaskType markVal) argument
+bool psPolynomialMDClipFit(psPolynomialMD *poly, const psVector *values, const psVector *errors,
+                           const psVector *mask, psMaskType maskVal, const psArray *coordsArray,
+                           int numIter, float rej) 
+{
+
+    PS_ASSERT_POLYNOMIALMD_NON_NULL(poly, false);
+    PS_ASSERT_VECTOR_NON_NULL(values, false);
+    PS_ASSERT_VECTOR_TYPE(values, PS_TYPE_F32, false);
+    if (errors) {
+        PS_ASSERT_VECTOR_NON_NULL(errors, false);
+        PS_ASSERT_VECTOR_TYPE(errors, PS_TYPE_F32, false);
+        PS_ASSERT_VECTORS_SIZE_EQUAL(values, errors, false);
+    }
+    if (maskVal == 0) {
+        mask = NULL;
+    }
+    if (mask) {
+        PS_ASSERT_VECTOR_NON_NULL(mask, false);
+        PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_MASK, false);
+        PS_ASSERT_VECTORS_SIZE_EQUAL(values, mask, false);
+    }
+    PS_ASSERT_ARRAY_NON_NULL(coordsArray, false);
+    PS_ASSERT_ARRAY_SIZE(coordsArray, (long)values->n, false);
+    PS_ASSERT_INT_NONNEGATIVE(numIter, false);
+    PS_ASSERT_FLOAT_LARGER_THAN(rej, 0.0, false);
+
+    int numValues = values->n;          // Number of values
+    int numTerms = poly->coeff->n;      // Number of terms
+
+    int numClipped = INT_MAX;           // Number of values clipped int an interation
+    int numGood = numValues;            // Number of good values
+
+    // copy the input mask to a local temporary mask 
+    poly->ownMask = psVectorRecycle(poly->ownMask, numValues, PS_TYPE_U8); // Our own mask for input values
+    psVectorInit(poly->ownMask, 0);
+    for (int i = 0; mask && (i < numValues); i++) {
+	if (mask->data.PS_TYPE_MASK_DATA[i] & maskVal) {
+	    poly->ownMask->data.U8[i] = 0xff;
+	    numGood--;
+	}
+    }
+
+    // make sure we have storage for the residuals
+    poly->deviations = psVectorRecycle (poly->deviations, numValues, PS_TYPE_F32);
+
+    for (int iter = 0; (iter < numIter) && (numClipped > 0) && (numGood > numTerms); iter++) {
+        numClipped = 0;
+
+	// XXX raise error?
+        if (!psPolynomialMDFit(poly, values, errors, poly->ownMask, maskVal, coordsArray)) {
+            return false;
+        }
+
+        psTrace("psLib.math", 7, "RMS from %d points is %lf\n", numGood, poly->stdevFit);
+
+        // Reject
+        float limit = rej * poly->stdevFit; // Rejection limit
+        for (int i = 0; i < numValues; i++) {
+
+# if (0)
+	    fprintf (stderr, "coords: ");
+	    psVector *coords = coordsArray->data[i];
+	    for (int j = 0; j < coords->n; j++) {
+		fprintf (stderr, "%f ", coords->data.F32[j]);
+	    }
+# endif
+	    
+	    psTrace("psLib.math", 10, "point %d (%f,%f: %f > %f)\n",
+		    i, values->data.F32[i], values->data.F32[i] - poly->deviations->data.F32[i], poly->deviations->data.F32[i], limit);
+
+            if (poly->ownMask->data.U8[i]) continue;
+
+            if (fabs(poly->deviations->data.F32[i]) > limit) {
+                psTrace("psLib.math", 9, "Rejected point %d (%f,%f: %f > %f)\n",
+                        i, values->data.F32[i], values->data.F32[i] + poly->deviations->data.F32[i], 
+                        poly->deviations->data.F32[i], limit);
+                poly->ownMask->data.U8[i] = 0xff;
+                numClipped++;
+                numGood--;
+            }
+        }
+        psTrace("psLib.math", 7, "Rejected %d points (%d remaining)\n", numClipped, numGood);
+    }
+
+    if (numClipped > 0) {
+        // Need to do a final re-evaluation of the fit
+        if (!psPolynomialMDFit(poly, values, errors, poly->ownMask, maskVal, coordsArray)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// XXX EAM : This function was trying to save calculation time at the expense of a LOT of allocs.
+# if (0)
+// Accumulate and solve the least-squares equation
+static bool polynomialMDClipFit(psPolynomialMD *poly, // Polynomial
+                                psImage *matrix, // Least-squares matrix
+                                psVector *vector, // Least-squares vector
+                                psImage **lu, // LU-decomposed matrix
+                                psVector **perm, // Permutations vector
+                                const psArray *matrices, // Individual least-squares matrices
+                                const psArray *vectors, // Individual least-squares vectors
+                                const psVector *mask, // Mask for values
+    )
+{
+    int numValues = vectors->n;         // Number of values
+
+    // Accumulate the least-squares matrix and vector
+    psImageInit(matrix, 0.0);
+    psVectorInit(vector, 0.0);
+    for (int i = 0; i < numValues; i++) {
+        if (mask->data.U8[i]) {
+            continue;
+        }
+
+        psImage *newMatrix = matrices->data[i]; // Matrix to accumulate
+        psVector *newVector = vectors->data[i]; // Vector to accumulate
+
+        polynomialMDAccumulate(matrix, vector, newMatrix, newVector);
+    }
+    polynomialMDFill(matrix);
+
+    // Solve least-squares equation
+    *lu = psMatrixLUD(*lu, perm, matrix);
+    if (!*lu) {
+        psError(PS_ERR_UNKNOWN, false, "Unable to LU-Decompose least-squares matrix.");
+        return false;
+    }
+    poly->coeff = psMatrixLUSolve(poly->coeff, *lu, vector, *perm);
+    if (!poly->coeff) {
+        psError(PS_ERR_UNKNOWN, false, "Unable to solve least-squares equation.");
+        return false;
+    }
+    return true;
+}
+
 
 bool psPolynomialMDClipFit(psPolynomialMD *poly, const psVector *values, const psVector *errors,
@@ -471,3 +593,3 @@
     return true;
 }
-
+# endif
Index: /trunk/psLib/src/math/psPolynomialMD.h
===================================================================
--- /trunk/psLib/src/math/psPolynomialMD.h	(revision 19084)
+++ /trunk/psLib/src/math/psPolynomialMD.h	(revision 19085)
@@ -13,4 +13,19 @@
     int numFit;                         ///< Number of values in fit
     float stdevFit;                     ///< Standard deviation of fit
+
+    psImage  *fitMatrix;
+    psVector *fitVector;
+
+    psImage  *tmpMatrix;
+    psVector *tmpVector;
+
+    psVector *fitBuffer;
+
+    psVector *ownMask;
+    psVector *deviations;
+
+    psVector *LUperm;
+    psImage  *LU;
+
 } psPolynomialMD;