Index: /trunk/Ohana/src/opihi/cmd.data/fit.c
===================================================================
--- /trunk/Ohana/src/opihi/cmd.data/fit.c	(revision 3686)
+++ /trunk/Ohana/src/opihi/cmd.data/fit.c	(revision 3687)
@@ -3,10 +3,11 @@
 int fit (int argc, char **argv) {
   
-  double **c, **b, *s, X, Y, dY;
-  int i, j, nterm, mterm, order;
-  int N, Weight, Quiet;
+  double **c, **b, *s, X, Y, dY, dY2;
+  double ClipNSigma, mean, sigma, maxsigma;
+  int i, j, nterm, mterm, order, Npt, Nmask;
+  int N, Weight, Quiet, ClipNiter;
   Vector *xvec, *yvec, *dyvec;
-  float *x, *y, *dy;
-  char name[64];
+  float *x, *y, *dy, *yf, *yfit;
+  char name[64], *mask;
 
   Quiet = FALSE;
@@ -20,4 +21,14 @@
   }
 
+  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"))) {
@@ -29,5 +40,5 @@
 
   if (argc != 4) {
-    fprintf (stderr, "USAGE: fit x y order [-dy wt]\n");
+    fprintf (stderr, "USAGE: fit x y order [-dy wt] [-quiet/-q] [-clip Nsigma Niter]\n");
     return (FALSE);
   }
@@ -36,60 +47,115 @@
   if ((yvec = SelectVector (argv[2], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
 
+  if (xvec[0].Nelements != yvec[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);
+  }
+  
   order = atof (argv[3]);
   nterm = order + 1;
   mterm = 2*order + 1;
 
-  if (xvec[0].Nelements != yvec[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;
-  if (Weight) dy = dyvec[0].elements;
+  ALLOCATE (yfit, float, xvec[0].Nelements);
+  ALLOCATE (mask, char, xvec[0].Nelements);
+  memset (mask, 0, xvec[0].Nelements);
 
   ALLOCATE (s, double, 2*mterm);
-  bzero (s, 2*mterm*sizeof(double));
   ALLOCATE (b, double *, nterm);
   ALLOCATE (c, double *, nterm);
   for (i = 0; i < nterm; i++) {
     ALLOCATE (c[i], double, nterm);
-    bzero (c[i], nterm*sizeof(double));
     ALLOCATE (b[i], double, 1);
-    bzero (b[i], sizeof(double));
-  }
-
-  for (i = 0; i < xvec[0].Nelements; i++, x++, y++) {
-    if (!(finite(*x) && finite(*y))) continue;
-    dY = 1.0;
-    if (Weight) { 
-      dY = 1.0 / SQ(*dy);
-      dy ++;
-    }
-    X = 1*dY;
-    Y = *y*dY;
-    for (j = 0; j < nterm; j++) {
-      s[j] += X;
-      b[j][0] += Y;
-      X = X * (*x);
-      Y = Y * (*x);
-    }
-    for (j = nterm; j < mterm; j++) {
-      s[j] += X;
-      X = X * (*x);
-    }
-  }
-
-  for (i = 0; i < nterm; i++) {
-    for (j = 0; j < nterm; j++) {
-      c[i][j] = s[i + j];
-    }
-  }
-  if (!gaussj (c, nterm, b, 1)) goto escape;
-
+  }
+
+  for (N = 0; N < ClipNiter; N++) {
+
+    /* init registers for current pass */
+    memset (s, 0, 2*mterm*sizeof(double));
+    for (i = 0; i < nterm; i++) {
+      memset (c[i], 0, nterm*sizeof(double));
+      memset (b[i], 0, sizeof(double));
+    }
+
+    /* perform linear fit */
+    x = xvec[0].elements;
+    y = yvec[0].elements;
+    if (Weight) dy = dyvec[0].elements;
+    for (i = 0; i < xvec[0].Nelements; i++, x++, y++) {
+      if (mask[i]) continue;
+      if (!(finite(*x) && finite(*y))) continue;
+      dY = 1.0;
+      if (Weight) { 
+	dY = 1.0 / SQ(*dy);
+	dy ++;
+      }
+      X = 1*dY;
+      Y = *y*dY;
+      for (j = 0; j < nterm; j++) {
+	s[j] += X;
+	b[j][0] += Y;
+	X = X * (*x);
+	Y = Y * (*x);
+      }
+      for (j = nterm; j < mterm; j++) {
+	s[j] += X;
+	X = X * (*x);
+      }
+    }
+    for (i = 0; i < nterm; i++) {
+      for (j = 0; j < nterm; j++) {
+	c[i][j] = s[i + j];
+      }
+    }
+    if (!gaussj (c, nterm, b, 1)) goto escape;
+
+    /* generate fitted values */
+    x = xvec[0].elements;
+    yf = yfit;
+    for (i = 0; i < xvec[0].Nelements; i++, x++, yf++) {
+      if (!finite(*x)) continue;
+      *yf = 0;
+      X = 1;
+      for (j = 0; j < order + 1; j++) {
+	*yf += b[j][0]*X;
+	X = X * (*x);
+      }
+    }
+
+    /* measure fit residual scatter */
+    x  = xvec[0].elements;
+    y  = yvec[0].elements;
+    yf = yfit;
+    dY = dY2 = 0;
+    for (i = Npt = 0; i < xvec[0].Nelements; i++, x++, y++, yf++) {
+      if (mask[i]) continue;
+      if (!finite(*x)) continue;
+      dY  += (*y - *yf);
+      dY2 += SQ(*y - *yf);
+      Npt ++;
+    }
+    mean  = dY / Npt;
+    sigma = sqrt (fabs(dY2/Npt - SQ(mean)));
+    maxsigma = ClipNSigma * sigma;
+
+    /* mask outlier points */
+    x  = xvec[0].elements;
+    y  = yvec[0].elements;
+    yf = yfit;
+    Nmask = 0;
+    for (i = 0; ClipNSigma && (i < xvec[0].Nelements); i++, x++, y++, yf++) {
+      dY = (*y - *yf);
+      if (fabs(dY) > maxsigma) {
+	mask[i] = TRUE;
+	Nmask ++;
+      } else {
+	mask[i] = FALSE;
+      }	
+    }
+  }
+      
   /* print & save basic fit parameters */
   if (!Quiet) fprintf (stderr, "y = ");
@@ -112,4 +178,26 @@
   if (!Quiet) fprintf (stderr, "\n");
 
+  set_variable ("dC", sigma);
+  set_variable ("Cnv", (xvec[0].Nelements - Nmask));
+
+  /* save mask and yfit for testing? */
+  if (1) { 
+    Vector *fvec, *mvec;
+    if ((fvec = SelectVector ("yfit", ANYVECTOR, TRUE)) == NULL) return (FALSE);    
+    if ((mvec = SelectVector ("mask", ANYVECTOR, TRUE)) == NULL) return (FALSE);    
+    free (fvec[0].elements);
+    fvec[0].elements = yfit;
+    fvec[0].Nelements = xvec[0].Nelements;
+    mvec[0].Nelements = xvec[0].Nelements;
+
+    REALLOCATE (mvec[0].elements, float, xvec[0].Nelements);
+    for (i = 0; i < xvec[0].Nelements; i++) {
+      mvec[0].elements[i] = mask[i];
+    }
+  } else {
+    free (yfit);
+  }
+  free (mask);
+
   for (i = 0; i < nterm; i++) {
     free (b[i]);