Index: trunk/psModules/src/astrom/pmAstrometryDistortion.c
===================================================================
--- trunk/psModules/src/astrom/pmAstrometryDistortion.c	(revision 7153)
+++ trunk/psModules/src/astrom/pmAstrometryDistortion.c	(revision 7282)
@@ -7,6 +7,6 @@
 *  @author EAM, IfA
 *
-*  @version $Revision: 1.2 $ $Name: not supported by cvs2svn $
-*  @date $Date: 2006-05-19 15:49:37 $
+*  @version $Revision: 1.3 $ $Name: not supported by cvs2svn $
+*  @date $Date: 2006-06-02 02:15:35 $
 *
 *  Copyright 2006 Institute for Astronomy, University of Hawaii
@@ -57,5 +57,5 @@
     // XXX we need to get the chip dimensions from the metadata somewhere
     int Nx = 2;
-    int Ny = 5;
+    int Ny = 2;
     int DX = 2080 / Nx;
     int DY = 4600 / Ny;
@@ -108,5 +108,8 @@
                 psVectorExtend (dP, 100, 1);
                 psVectorExtend (dQ, 100, 1);
-            }
+                Npts++;
+            }
+            if (Npts < 5)
+                continue;
 
             // fit the collection of positions and offsets with a local 1st order gradient
@@ -114,4 +117,6 @@
             psStats *stats = psStatsAlloc (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
             psVector *mask = psVectorAlloc (Npts, PS_TYPE_MASK);
+            mask->n = Npts;
+            psVectorInit (mask, 0);
 
             pmAstromGradient *grad = pmAstromGradientAlloc ();
@@ -141,4 +146,7 @@
 bool pmAstromFitDistortion(pmFPA *fpa, psArray *grads, psMetadata *config)
 {
+
+    psPolynomial2D *localX = NULL;
+    psPolynomial2D *localY = NULL;
 
     // assign the gradient elements to psVectors for fitting
@@ -168,21 +176,117 @@
     psStats *stats = psStatsAlloc (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
     psVector *mask = psVectorAlloc (grads->n, PS_TYPE_MASK);
-
-    // XXX the order of the gradient fits need to be 1 less than the distortion term
-    // XXX determine the distortion order(s) from the fpa->toTP structure
-
-    psPolynomial2D *localX = psPolynomial2DAlloc (PS_POLYNOMIAL_ORD, 1, 1);
-    psPolynomial2D *localY = psPolynomial2DAlloc (PS_POLYNOMIAL_ORD, 1, 1);
-    // XXX set masks based on fpa->toTP
-
+    mask->n = grads->n;
+    psVectorInit (mask, 0);
+
+    // the order of the gradient fits need to be 1 less than the distortion term
+    // determine the gradient order(s) from the fpa->toTP structure
+    localX = psPolynomial2DAlloc (PS_POLYNOMIAL_ORD, fpa->toTangentPlane->x->nX-1, fpa->toTangentPlane->x->nY);
+    localY = psPolynomial2DAlloc (PS_POLYNOMIAL_ORD, fpa->toTangentPlane->x->nX,   fpa->toTangentPlane->x->nY-1);
+
+    // set masks based on fpa->toTP
+    for (int i = 0; i < fpa->toTangentPlane->x->nX; i++) {
+        for (int j = 0; j < fpa->toTangentPlane->x->nY; j++) {
+            if (fpa->toTangentPlane->x->mask[i][j][0][0]) {
+                localX->mask[i-1][j] = 1;
+                localY->mask[i][j-1] = 1;
+            }
+        }
+    }
+
+    // fit the local gradients in each direction
     psVectorClipFitPolynomial2D (localX, stats, mask, 0xff, dPdL, NULL, L, M);
     psVectorClipFitPolynomial2D (localY, stats, mask, 0xff, dPdM, NULL, L, M);
 
-    // XXX update fpa->toTP distortion terms
-
+    // XXX test: generate fit
+    psVector *dPdLf = psPolynomial2DEvalVector (localX, L, M);
+    psVector *dPdMf = psPolynomial2DEvalVector (localY, L, M);
+
+    // update fpa->toTP distortion terms
+    fpa->toTangentPlane->x->coeff[0][0][0][0] = 0;
+    for (int i = 1; i < fpa->toTangentPlane->x->nX; i++) {
+        if (!fpa->toTangentPlane->x->mask[i][0][0][0]) {
+            fpa->toTangentPlane->x->coeff[i][0][0][0] = localX->coeff[i-1][0] / i;
+        }
+    }
+    for (int j = 1; j < fpa->toTangentPlane->x->nY; j++) {
+        if (!fpa->toTangentPlane->x->mask[0][j][0][0]) {
+            fpa->toTangentPlane->x->coeff[0][j][0][0] = localY->coeff[0][j-1] / j;
+        }
+    }
+    for (int i = 1; i < fpa->toTangentPlane->x->nX; i++) {
+        for (int j = 1; j < fpa->toTangentPlane->x->nY; j++) {
+            if (!fpa->toTangentPlane->x->mask[i][j][0][0]) {
+                fpa->toTangentPlane->x->coeff[i][j][0][0] = 0.5*(localX->coeff[i-1][j] / i + localY->coeff[i][j-1] / j);
+            }
+        }
+    }
+    psFree (localX);
+    psFree (localY);
+
+    // the order of the gradient fits need to be 1 less than the distortion term
+    // determine the gradient order(s) from the fpa->toTP structure
+    localX = psPolynomial2DAlloc (PS_POLYNOMIAL_ORD, fpa->toTangentPlane->y->nX-1, fpa->toTangentPlane->y->nY);
+    localY = psPolynomial2DAlloc (PS_POLYNOMIAL_ORD, fpa->toTangentPlane->y->nX,   fpa->toTangentPlane->y->nY-1);
+
+    // set masks based on fpa->toTP
+    for (int i = 0; i < fpa->toTangentPlane->y->nX; i++) {
+        for (int j = 0; j < fpa->toTangentPlane->y->nY; j++) {
+            if (fpa->toTangentPlane->y->mask[i][j][0][0]) {
+                localX->mask[i-1][j] = 1;
+                localY->mask[i][j-1] = 1;
+            }
+        }
+    }
+
+    // fit the local gradients in each direction
     psVectorClipFitPolynomial2D (localX, stats, mask, 0xff, dQdL, NULL, L, M);
     psVectorClipFitPolynomial2D (localY, stats, mask, 0xff, dQdM, NULL, L, M);
 
+    // XXX test: generate fit
+    psVector *dQdLf = psPolynomial2DEvalVector (localX, L, M);
+    psVector *dQdMf = psPolynomial2DEvalVector (localY, L, M);
+
+    FILE *f = fopen ("grads.dat", "w");
+    for (int i = 0; i < dPdLf->n; i++) {
+        fprintf (f, "%f %f  | %f %f  %f %f | %f %f  %f %f\n",
+                 L->data.F32[i], M->data.F32[i],
+                 dPdL->data.F32[i], dPdM->data.F32[i],
+                 dQdL->data.F32[i], dQdM->data.F32[i],
+                 dPdLf->data.F32[i], dPdMf->data.F32[i],
+                 dQdLf->data.F32[i], dQdMf->data.F32[i]);
+    }
+    fclose (f);
+
+    // update fpa->toTP distortion terms
+    fpa->toTangentPlane->y->coeff[0][0][0][0] = 0;
+    for (int i = 1; i < fpa->toTangentPlane->y->nX; i++) {
+        if (!fpa->toTangentPlane->y->mask[i][0][0][0]) {
+            fpa->toTangentPlane->y->coeff[i][0][0][0] = localX->coeff[i-1][0] / i;
+        }
+    }
+    for (int j = 1; j < fpa->toTangentPlane->y->nY; j++) {
+        if (!fpa->toTangentPlane->y->mask[0][j][0][0]) {
+            fpa->toTangentPlane->y->coeff[0][j][0][0] = localY->coeff[0][j-1] / j;
+        }
+    }
+    for (int i = 1; i < fpa->toTangentPlane->y->nX; i++) {
+        for (int j = 1; j < fpa->toTangentPlane->y->nY; j++) {
+            if (!fpa->toTangentPlane->y->mask[i][j][0][0]) {
+                fpa->toTangentPlane->y->coeff[i][j][0][0] = 0.5*(localX->coeff[i-1][j] / i + localY->coeff[i][j-1] / j);
+            }
+        }
+    }
+    psFree (localX);
+    psFree (localY);
+
     // XXX free unneeded structures
+    psFree (dPdL);
+    psFree (dPdM);
+    psFree (dQdL);
+    psFree (dQdM);
+    psFree (L);
+    psFree (M);
+    psFree (stats);
+    psFree (mask);
 
     return true;