Index: /branches/eam_rel9_b1/psLib/src/math/psMinimize.c
===================================================================
--- /branches/eam_rel9_b1/psLib/src/math/psMinimize.c	(revision 5800)
+++ /branches/eam_rel9_b1/psLib/src/math/psMinimize.c	(revision 5801)
@@ -10,6 +10,6 @@
  *  @author EAM, IfA
  *
- *  @version $Revision: 1.147.4.1 $ $Name: not supported by cvs2svn $
- *  @date $Date: 2005-12-07 20:49:43 $
+ *  @version $Revision: 1.147.4.1.2.1 $ $Name: not supported by cvs2svn $
+ *  @date $Date: 2005-12-17 20:26:29 $
  *
  *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -32,4 +32,5 @@
 #include "psBinaryOp.h"
 #include "psLogMsg.h"
+
 /*****************************************************************************/
 /* DEFINE STATEMENTS                                                         */
@@ -1441,21 +1442,69 @@
 }
 
-// here is the definition for BuildSums4D.  ifdef'ed away until it is used
-// by psPolynomial4DFit..
-# if (0)
-    /******************************************************************************
+/******************************************************************************
+BuildSums3D(sums, x, y, z, nXterm, nYterm, nZterm): this routine calculates the powers of
+input parameter "x", "y", and "z" between 0 and input parameter nXterms*2,
+nYterm*2, and nZterm*2.  The result is returned as a psImage sums.
+ *****************************************************************************/
+static psF64 ***BuildSums3D(
+    psF64 ***sums,
+    psF64 x,
+    psF64 y,
+    psF64 z,
+    psS32 nXterm,
+    psS32 nYterm,
+    psS32 nZterm)
+{
+    psS32 nXsum = 0;
+    psS32 nYsum = 0;
+    psS32 nZsum = 0;
+    psF64 xSum = 1.0;
+    psF64 ySum = 1.0;
+    psF64 zSum = 1.0;
+
+    nXsum = 2*nXterm;
+    nYsum = 2*nYterm;
+    nZsum = 2*nZterm;
+    if (sums == NULL) {
+        sums = (psF64 ***) psAlloc (nXsum*sizeof(psF64));
+        for (int i = 0; i < nXsum; i++) {
+            sums[i] = (psF64 **) psAlloc (nYsum*sizeof(psF64));
+            for (int j = 0; j < nYsum; j++) {
+                sums[i][j] = (psF64 *) psAlloc (nZsum*sizeof(psF64));
+            }
+        }
+    }
+    // careful with this function: there is no size checking and realloc for reuse
+
+    zSum = 1.0;
+    for (int k = 0; k < nZsum; k++) {
+        ySum = zSum;
+        for (int j = 0; j < nYsum; j++) {
+            xSum = ySum;
+            for (int i = 0; i < nXsum; i++) {
+                sums[i][j][k] = xSum;
+                xSum *= x;
+            }
+            ySum *= y;
+        }
+        zSum *= z;
+    }
+    return (sums);
+}
+
+/******************************************************************************
     BuildSums4D(sums, x, y, z, t, nXterm, nYterm, nZterm, nTterm). equiv to
     BuildSums2D(). The result is returned as a double ****
-     *****************************************************************************/
-    static double ****BuildSums4D(
-        psF64 ****sums,
-        psF64 x,
-        psF64 y,
-        psF64 z,
-        psF64 t,
-        psS32 nXterm,
-        psS32 nYterm,
-        psS32 nZterm,
-        psS32 nTterm)
+*****************************************************************************/
+static psF64 ****BuildSums4D(
+    psF64 ****sums,
+    psF64 x,
+    psF64 y,
+    psF64 z,
+    psF64 t,
+    psS32 nXterm,
+    psS32 nYterm,
+    psS32 nZterm,
+    psS32 nTterm)
 {
     psS32 nXsum = 0;
@@ -1505,5 +1554,4 @@
     return (sums);
 }
-# endif /* BuildSums4D */
 
 /******************************************************************************
@@ -1961,4 +2009,7 @@
         float maxClipValue = +maxClipSigma*stats->sampleStdev;
 
+        psTrace (".psphot.VectorClipFit", 4, "residual stats for robust fit:  %g +/- %g\n", stats->sampleMedian, stats->sampleStdev);
+        psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue);
+
         // set mask if pts are not valid
         // we are masking out any point which is out of range
@@ -2120,5 +2171,4 @@
 }
 
-
 // XXX EAM : I have implemented a single function to handle the mask/nomask cases
 //           this function can be deprecated
@@ -2437,4 +2487,6 @@
         float minClipValue = -minClipSigma*stats->sampleStdev;
         float maxClipValue = +maxClipSigma*stats->sampleStdev;
+        psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev);
+        psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue);
 
         // set mask if pts are not valid
@@ -2526,6 +2578,91 @@
     }
 
-    psError(PS_ERR_UNKNOWN, true, "3-D Polynomial Fitting is not Implemented.\n");
-    return (NULL);
+    psImage    *A = NULL;
+    psVector   *B = NULL;
+    psF64 ***Sums = NULL;
+    psF64 wt;
+    psS32 nTerm;
+
+    // XXX:Watch for changes to the psPolys: nTerm != nOrder.
+    psS32 nXterm = 1 + myPoly->nX;
+    psS32 nYterm = 1 + myPoly->nY;
+    psS32 nZterm = 1 + myPoly->nZ;
+    nTerm = nXterm * nYterm * nZterm;
+
+    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    // Initialize data structures.
+    psVectorInit (B, 0.0);
+    psImageInit (A, 0.0);
+
+    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+
+    // Build the B and A data structs.
+    for (int k  = 0; k < x->n; k++) {
+        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
+            continue;
+        }
+
+        Sums = BuildSums3D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], nXterm, nYterm, nZterm);
+
+        if (fErr == NULL) {
+            wt = 1.0;
+        } else {
+            // this filters fErr == 0 values
+            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
+        }
+
+        // we could skip half of the array and assign at the end
+        // we must handle masked orders
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    B->data.F64[ix+iy*nXterm+iz*nXterm*nYterm] += f->data.F64[k] * Sums[ix][iy][iz] * wt;
+                }
+            }
+        }
+
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    for (int jx = 0; jx < nXterm; jx++) {
+                        for (int jy = 0; jy < nYterm; jy++) {
+                            for (int jz = 0; jz < nZterm; jz++) {
+                                A->data.F64[ix+iy*nXterm+iz*nXterm*nYterm][ix+iy*nXterm+iz*nXterm*nYterm] += Sums[ix+jx][iy+jy][iz+jz] * wt;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    // does the solution in place
+    psGaussJordan (A, B);
+
+    // select the appropriate solution entries
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                myPoly->coeff[ix][iy][iz] = B->data.F64[ix+iy*nXterm+iz*nXterm*nYterm];
+            }
+        }
+    }
+
+    psFree(A);
+    psFree(B);
+
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            psFree(Sums[ix][iy]);
+        }
+        psFree(Sums[ix]);
+    }
+    psFree(Sums);
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial3DOrd", 4,
+            "---- VectorFitPolynomial3DOrd() begin ----\n");
+    return (myPoly);
 }
 
@@ -2714,5 +2851,5 @@
     PS_ASSERT_PTR_NON_NULL(stats, NULL);
     PS_ASSERT_VECTOR_NON_NULL(f, NULL);
-    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
     if (mask != NULL) {
         PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
@@ -2721,20 +2858,87 @@
     PS_ASSERT_VECTOR_NON_NULL(x, NULL);
     PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
-    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F32, NULL);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
     PS_ASSERT_VECTOR_NON_NULL(y, NULL);
     PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
-    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, NULL);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
     PS_ASSERT_VECTOR_NON_NULL(f, NULL);
-    PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL);
-    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL);
+    // PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL);
+    // PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
     if (fErr != NULL) {
         PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, mask, NULL);
-        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL);
-    }
-
-    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");
-    return(NULL);
+        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
+    }
+
+    // clipping range defined by min and max and/or clipSigma
+    float minClipSigma;
+    float maxClipSigma;
+    if (isfinite(stats->max)) {
+        maxClipSigma = fabs(stats->max);
+    } else {
+        maxClipSigma = fabs(stats->clipSigma);
+    }
+    if (isfinite(stats->min)) {
+        minClipSigma = fabs(stats->min);
+    } else {
+        minClipSigma = fabs(stats->clipSigma);
+    }
+    psVector *fit   = NULL;
+    psVector *resid = psVectorAlloc (x->n, PS_TYPE_F64);
+
+    // eventual expansion: user supplies one of various stats option pairs,
+    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
+    // evaluate the clipping sigma
+    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
+    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
+
+    for (int N = 0; N < stats->clipIter; N++) {
+        int Nkeep = 0;
+
+        poly = psVectorFitPolynomial3D (poly, mask, maskValue, f, fErr, x, y, z);
+        fit = psPolynomial3DEvalVector (poly, x, y, z);
+        resid = (psVector *) psBinaryOp (resid, (void *) f, "-", (void *) fit);
+
+        stats  = psVectorStats (stats, resid, NULL, mask, maskValue);
+        float minClipValue = -minClipSigma*stats->sampleStdev;
+        float maxClipValue = +maxClipSigma*stats->sampleStdev;
+        psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev);
+        psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue);
+
+        // set mask if pts are not valid
+        // we are masking out any point which is out of range
+        // recovery is not allowed with this scheme
+        for (int i = 0; i < resid->n; i++) {
+            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian > maxClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian < minClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            Nkeep ++;
+        }
+
+        psTrace (".psphot.VectorClipFit", 4, "keeping %d of %d pts for fit\n",
+                 Nkeep, x->n);
+
+        psFree (fit);
+    }
+    // Free local temporary variables
+    psFree (resid);
+
+    if (poly == NULL) {
+        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
+        return(NULL);
+    }
+    return(poly);
 }
-
 
 /******************************************************************************
@@ -2792,6 +2996,105 @@
     }
 
-    psError(PS_ERR_UNKNOWN, true, "4-D Polynomial Fitting is not Implemented.\n");
-    return (NULL);
+    // I think this is 1 dimension down
+    psImage     *A = NULL;
+    psVector    *B = NULL;
+    psF64 ****Sums = NULL;
+    psF64 wt;
+    psS32 nTerm;
+
+    // XXX:Watch for changes to the psPolys: nTerm != nOrder.
+    psS32 nXterm = 1 + myPoly->nX;
+    psS32 nYterm = 1 + myPoly->nY;
+    psS32 nZterm = 1 + myPoly->nZ;
+    psS32 nTterm = 1 + myPoly->nZ;
+    nTerm = nXterm * nYterm * nZterm * nTterm;
+
+    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    // Initialize data structures.
+    psVectorInit (B, 0.0);
+    psImageInit (A, 0.0);
+
+    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+
+    // Build the B and A data structs.
+    for (int k  = 0; k < x->n; k++) {
+        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
+            continue;
+        }
+
+        Sums = BuildSums4D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], t->data.F64[k], nXterm, nYterm, nZterm, nTterm);
+
+        if (fErr == NULL) {
+            wt = 1.0;
+        } else {
+            // this filters fErr == 0 values
+            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
+        }
+
+        // we could skip half of the array and assign at the end
+        // we must handle masked orders
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    for (int it = 0; it < nTterm; it++) {
+                        B->data.F64[ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm] += f->data.F64[k] * Sums[ix][iy][iz][it] * wt;
+                    }
+                }
+            }
+        }
+
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    for (int it = 0; it < nTterm; it++) {
+                        for (int jx = 0; jx < nXterm; jx++) {
+                            for (int jy = 0; jy < nYterm; jy++) {
+                                for (int jz = 0; jz < nZterm; jz++) {
+                                    for (int jt = 0; jt < nTterm; jt++) {
+                                        A->data.F64[ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm][ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm]
+                                        += Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    // does the solution in place
+    psGaussJordan (A, B);
+
+    // select the appropriate solution entries
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                for (int it = 0; it < nTterm; it++) {
+                    myPoly->coeff[ix][iy][iz][it] = B->data.F64[ix+iy*nXterm+iz*nXterm*nYterm+iz*nXterm*nYterm*nZterm];
+                }
+            }
+        }
+    }
+
+    psFree(A);
+    psFree(B);
+
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                psFree(Sums[ix][iy][iz]);
+            }
+            psFree(Sums[ix][iy]);
+        }
+        psFree(Sums[ix]);
+    }
+    psFree(Sums);
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial3DOrd", 4,
+            "---- VectorFitPolynomial3DOrd() begin ----\n");
+    return (myPoly);
 }
 
@@ -3029,8 +3332,73 @@
     }
 
-    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");
-
-    return(NULL);
+    // clipping range defined by min and max and/or clipSigma
+    float minClipSigma;
+    float maxClipSigma;
+    if (isfinite(stats->max)) {
+        maxClipSigma = fabs(stats->max);
+    } else {
+        maxClipSigma = fabs(stats->clipSigma);
+    }
+    if (isfinite(stats->min)) {
+        minClipSigma = fabs(stats->min);
+    } else {
+        minClipSigma = fabs(stats->clipSigma);
+    }
+    psVector *fit   = NULL;
+    psVector *resid = psVectorAlloc (x->n, PS_TYPE_F64);
+
+    // eventual expansion: user supplies one of various stats option pairs,
+    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
+    // evaluate the clipping sigma
+    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
+    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
+
+    for (int N = 0; N < stats->clipIter; N++) {
+        int Nkeep = 0;
+
+        poly = psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t);
+        fit = psPolynomial4DEvalVector (poly, x, y, z, t);
+        resid = (psVector *) psBinaryOp (resid, (void *) f, "-", (void *) fit);
+
+        stats  = psVectorStats (stats, resid, NULL, mask, maskValue);
+        float minClipValue = -minClipSigma*stats->sampleStdev;
+        float maxClipValue = +maxClipSigma*stats->sampleStdev;
+        psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev);
+        psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue);
+
+        // set mask if pts are not valid
+        // we are masking out any point which is out of range
+        // recovery is not allowed with this scheme
+        for (int i = 0; i < resid->n; i++) {
+            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian > maxClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian < minClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            Nkeep ++;
+        }
+
+        psTrace (".psphot.VectorClipFit", 4, "keeping %d of %d pts for fit\n",
+                 Nkeep, x->n);
+
+        psFree (fit);
+    }
+    // Free local temporary variables
+    psFree (resid);
+
+    if (poly == NULL) {
+        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
+        return(NULL);
+    }
+    return(poly);
 }
-
-