Index: /trunk/Ohana/src/opihi/cmd.data/fit2d.c
===================================================================
--- /trunk/Ohana/src/opihi/cmd.data/fit2d.c	(revision 3896)
+++ /trunk/Ohana/src/opihi/cmd.data/fit2d.c	(revision 3897)
@@ -3,22 +3,48 @@
 int fit2d (int argc, char **argv) {
   
-  double **c, **b, **s, X, Y;
-  int k, K, i, j, nx, ny, nterm, mterm, wterm, order;
-  int N, Weight;
-  float *x, *y, *z; 
-  char name[64];
-  Vector *xvec, *yvec, *zvec, *dyvec;
+  double **c, **b, **s, X, Y, dZ, dZ2;
+  double ClipNSigma, mean, sigma, maxsigma;
+  int k, K, i, j, n, Npt, Nmask, nx, ny, nterm, mterm, wterm, order;
+  int N, Weight, Quiet, ClipNiter, VERBOSE;
+  float *x, *y, *z, *dz, *zfit, *zf; 
+  char name[64], *mask;
+  Vector *xvec, *yvec, *zvec, *dzvec;
+
+  VERBOSE = FALSE;
+  if ((N = get_argument (argc, argv, "-v"))) {
+    VERBOSE = TRUE;
+    remove_argument (N, &argc, argv);
+  }
+
+  Quiet = FALSE;
+  if ((N = get_argument (argc, argv, "-q"))) {
+    Quiet = TRUE;
+    remove_argument (N, &argc, argv);
+  }
+  if ((N = get_argument (argc, argv, "-quiet"))) {
+    Quiet = TRUE;
+    remove_argument (N, &argc, argv);
+  }
+
+  ClipNSigma = 0;
+  ClipNiter  = 1;
+  if ((N = get_argument (argc, argv, "-clip"))) {
+    remove_argument (N, &argc, argv);
+    ClipNSigma = atof(argv[N]);
+    remove_argument (N, &argc, argv);
+    ClipNiter  = atof(argv[N]);
+    remove_argument (N, &argc, argv);
+  }
 
   Weight = FALSE;
-  if ((N = get_argument (argc, argv, "-dy"))) {
-    remove_argument (N, &argc, argv);
-    if ((dyvec = SelectVector (argv[N], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
+  if ((N = get_argument (argc, argv, "-dz"))) {
+    remove_argument (N, &argc, argv);
+    if ((dzvec = SelectVector (argv[N], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
     remove_argument (N, &argc, argv);
     Weight = TRUE;
-    fprintf (stderr, "warning: weighting currently ignored!\n");
   }
 
   if (argc != 5) {
-    fprintf (stderr, "USAGE: fit x y z order [-dy`z wt]\n");
+    fprintf (stderr, "USAGE: fit x y z order [-dz wt]\n");
     return (FALSE);
   }
@@ -28,4 +54,17 @@
   if ((zvec = SelectVector (argv[3], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
 
+  if (xvec[0].Nelements != yvec[0].Nelements) {
+    fprintf (stderr, "vectors must have same length\n");
+    return (FALSE);
+  }
+  if (xvec[0].Nelements != zvec[0].Nelements) {
+    fprintf (stderr, "vectors must have same length\n");
+    return (FALSE);
+  }
+  if (Weight && xvec[0].Nelements != dzvec[0].Nelements) {
+    fprintf (stderr, "vectors must have same length\n");
+    return (FALSE);
+  }
+  
   order = atof (argv[4]);
   nterm = order + 1;
@@ -33,85 +72,168 @@
   mterm = 2*order + 1;
 
-  if (xvec[0].Nelements != yvec[0].Nelements) {
-    fprintf (stderr, "vectors must have same length\n");
-    return (FALSE);
-  }
-  if (xvec[0].Nelements != zvec[0].Nelements) {
-    fprintf (stderr, "vectors must have same length\n");
-    return (FALSE);
-  }
-  if (Weight && xvec[0].Nelements != dyvec[0].Nelements) {
-    fprintf (stderr, "vectors must have same length\n");
-    return (FALSE);
-  }
-  
-  x = xvec[0].elements;
-  y = yvec[0].elements;
-  z = zvec[0].elements;
-  /* dy = dyvec[0].elements; */
+  ALLOCATE (zfit, float, xvec[0].Nelements);
+  ALLOCATE (mask, char, xvec[0].Nelements);
+  memset (mask, 0, xvec[0].Nelements);
 
   /* allocate the summation matrices */
+  ALLOCATE (s, double *, mterm);
   ALLOCATE (b, double *, wterm);
   ALLOCATE (c, double *, wterm);
   for (i = 0; i < wterm; i++) {
     ALLOCATE (c[i], double, wterm);
-    bzero (c[i], wterm*sizeof(double));
     ALLOCATE (b[i], double, 1);
-    bzero (b[i], sizeof(double));
-  }
-  ALLOCATE (s, double *, mterm);
+  }
   for (i = 0; i < mterm; i++) {
     ALLOCATE (s[i], double, mterm);
-    bzero (s[i], mterm*sizeof(double));
-  }
-
-  /* add up the x,y values */
-  for (i = 0; i < xvec[0].Nelements; i++, x++, y++) {
-    Y = X = 1;
-    for (ny = 0; ny < mterm; ny++) {
-      X = Y;
-      for (nx = 0; nx < mterm - ny; nx++) {
-	s[nx][ny] += X;
+  }
+
+  for (N = 0; N < ClipNiter; N++) {
+
+    /* init registers for current pass */
+    for (i = 0; i < nterm; i++) {
+      memset (c[i], 0, wterm*sizeof(double));
+      memset (b[i], 0, sizeof(double));
+    }
+    for (i = 0; i < mterm; i++) {
+      memset (s[i], 0, mterm*sizeof(double));
+    }
+
+    x = xvec[0].elements;
+    y = yvec[0].elements;
+    z = zvec[0].elements;
+    if (Weight) dz = dzvec[0].elements;
+
+    /* add up the x,y values */
+    for (i = 0; i < xvec[0].Nelements; i++, x++, y++) {
+      if (mask[i]) continue;
+      if (!finite(*x) || !finite(*y)) continue;
+      dZ = 1.0;
+      if (Weight) { 
+	dZ = 1.0 / SQ(*dz);
+	dz ++;
+      }
+      Y = X = 1*dZ;
+      for (ny = 0; ny < mterm; ny++) {
+	X = Y;
+	for (nx = 0; nx < mterm - ny; nx++) {
+	  s[nx][ny] += X;
+	  X = X * (*x);
+	}
+	Y = Y * (*y);
+      }
+    }
+
+    /* add up the z values */
+    x = xvec[0].elements;
+    y = yvec[0].elements;
+    z = zvec[0].elements;
+    for (i = 0; i < xvec[0].Nelements; i++, x++, y++, z++) {
+      if (mask[i]) continue;
+      if (!finite(*x) || !finite(*y)) continue;
+      dZ = 1.0;
+      if (Weight) { 
+	dZ = 1.0 / SQ(*dz);
+	dz ++;
+      }
+      Y = X = *z*dZ;
+      for (j = 0, ny = 0; ny < nterm; ny++) {
+	X = Y;
+	for (nx = 0; nx < nterm - ny; nx++, j++) {
+	  b[j][0] += X;
+	  X = X * (*x);
+	}
+	Y = Y * (*y);
+      }
+    }
+
+    /* re-sort mterm x mterm matrix to wterm matrix */
+    for (k = j = 0; j < nterm; j++) {
+      for (i = 0; i < nterm - j; i++, k++) {
+	for (K = ny = 0; ny < nterm; ny++) {
+	  for (nx = 0; nx < nterm - ny; nx++, K++) {
+	    c[K][k] = s[nx+i][ny+j];
+	  }
+	}
+      }
+    }
+
+    gaussj (c, wterm, b, 1);
+
+    /** test print **/
+    if (VERBOSE) {
+      for (i = ny = 0; ny < nterm; ny++) {
+	for (nx = 0; nx < nterm - ny; nx++, i++) {
+	  fprintf (stderr, "x^%d y^%d: %g\n", nx, ny, b[i][0]);
+	}
+      }
+    }
+
+    /* the b[][0] terms are in the following order:
+       y^0 x^0, y^0 x^1, ... y^0 x^N
+       y^1 x^0, y^1 x^1, ... y^1 x^N
+       ...
+       y^N x^0, y^N x^1, ... y^N x^N
+    */
+    /* generate fitted values */
+    x = xvec[0].elements;
+    y = yvec[0].elements;
+    zf = zfit;
+    for (n = 0; n < xvec[0].Nelements; n++, x++, y++, zf++) {
+      if (!finite(*x) || !finite(*y)) continue;
+      *zf = 0;
+      Y = X = 1;
+      for (i = ny = 0; ny < nterm; ny++) {
+	Y = X;
+	for (nx = 0; nx < nterm - ny; nx++, i++) {
+	  *zf += b[i][0]*Y;
+	  Y = Y * (*y);
+	}
 	X = X * (*x);
       }
-      Y = Y * (*y);
-    }
-  }
-
-  /* add up the z values */
-  x = xvec[0].elements;
-  y = yvec[0].elements;
-  z = zvec[0].elements;
-  for (i = 0; i < xvec[0].Nelements; i++, x++, y++, z++) {
-    Y = X = *z;
-    for (N = 0, ny = 0; ny < nterm; ny++) {
-      X = Y;
-      for (nx = 0; nx < nterm - ny; nx++, N++) {
-	b[N][0] += X;
-	X = X * (*x);
-      }
-      Y = Y * (*y);
-    }
-  }
-
-  /* re-sort mterm x mterm matrix to wterm matrix */
-  for (k = j = 0; j < nterm; j++) {
-    for (i = 0; i < nterm - j; i++, k++) {
-      for (K = ny = 0; ny < nterm; ny++) {
-	for (nx = 0; nx < nterm - ny; nx++, K++) {
-	  c[K][k] = s[nx+i][ny+j];
-	}
-      }
-    }
-  }
-
-  gaussj (c, wterm, b, 1);
-
-  fprintf (stderr, "z = ");
+    }
+
+    /* measure fit residual scatter */
+    x  = xvec[0].elements;
+    y  = yvec[0].elements;
+    z  = zvec[0].elements;
+    zf = zfit;
+    dZ = dZ2 = 0;
+    for (i = Npt = 0; i < xvec[0].Nelements; i++, x++, y++, z++, zf++) {
+      if (mask[i]) continue;
+      if (!finite(*x) || !finite(*y)) continue;
+      dZ  += (*z - *zf);
+      dZ2 += SQ(*z - *zf);
+      Npt ++;
+    }
+    mean  = dZ / Npt;
+    sigma = sqrt (fabs(dZ2/Npt - SQ(mean)));
+    maxsigma = ClipNSigma * sigma;
+
+    if (VERBOSE) fprintf (stderr, "mean: %g, sigma: %g, maxsigma: %g\n", mean, sigma, maxsigma);
+
+    /* mask outlier points */
+    x  = xvec[0].elements;
+    y  = yvec[0].elements;
+    z  = zvec[0].elements;
+    zf = zfit;
+    Nmask = 0;
+    for (i = 0; ClipNSigma && (i < xvec[0].Nelements); i++, x++, y++, z++, zf++) {
+      dZ = (*z - *zf);
+      if (fabs(dZ) > maxsigma) {
+	mask[i] = TRUE;
+	Nmask ++;
+      } else {
+	mask[i] = FALSE;
+      }	
+    }
+    if (VERBOSE) fprintf (stderr, "pass: %d, Nmask: %d\n", N, Nmask, ClipNSigma);
+  }
+
+  if (!Quiet) fprintf (stderr, "z = ");
   for (N = ny = 0; ny < nterm; ny++) {
     for (nx = 0; nx < nterm - ny; nx++, N++) {
       sprintf (name, "CX%dY%d", nx, ny);
       set_variable (name, b[N][0]);
-      fprintf (stderr, "%f x^%d y^%d  ", b[N][0], nx, ny);
+      if (!Quiet) fprintf (stderr, "%f x^%d y^%d  ", b[N][0], nx, ny);
     }
   }
@@ -119,10 +241,22 @@
   set_variable (name, (double) order);
   
-  fprintf (stderr, "\n");
+  if (!Quiet) fprintf (stderr, "\n");
+
+  /* free internal data */
+  free (zfit);
+  free (mask);
+
+  for (i = 0; i < wterm; i++) {
+    free (c[i]);
+    free (b[i]);
+  }
+  free (b);
+  free (c);
+
+  for (i = 0; i < mterm; i++) {
+    free (s[i]);
+  }
+  free (s);
+
   return (TRUE);
-
 }
-
-
-
-