Index: trunk/pswarp/src/pswarpMapGrid.c
===================================================================
--- trunk/pswarp/src/pswarpMapGrid.c	(revision 10954)
+++ trunk/pswarp/src/pswarpMapGrid.c	(revision 10957)
@@ -1,5 +1,153 @@
 # include "pswarp.h"
 
-static pswarpMapFree (pswarpMap *map) {
+// construct a grid with superpixel spacing of nXpix, nYpix
+// XXX for the moment, ignore readout->cell->chip offsets
+pswarpMapGrid *pswarpMapGridFromImage (pmReadout *dest, pmReadout *src, int nXpix, int nYpix) {
+
+    // split the difference of the remainder
+    int xMin = 0.5*(src->image->numCols % nXpix);
+    int yMin = 0.5*(src->image->numRows % nYpix);
+
+    pswarpMapGrid *grid = pswarpMapGridAlloc (src->image->numCols / nXpix, src->image->numRows / nYpix);
+
+    int ni = 0;
+    int nj = 0;
+    for (int i = xMin; i < src->image->numCols; i += nXpix, ni++) {
+	for (int j = yMin; j < src->image->numRows; j += nYpix, nj++) {
+	    pswarpMapSetLocalModel (grid->maps[ni][nj], dest, src, i, j);
+	}
+    }
+	    
+    grid->nXpix = nXpix;
+    grid->nYpix = nYpix;
+    grid->xMin = xMin;
+    grid->yMin = yMin;
+    return grid;
+}
+
+bool pswarpMapGridSetGrid (pswarpMapGrid *grid, int ix, int iy, int *gridX, int *gridY) {
+
+    *gridX = (ix - grid->xMin + 0.5*grid->nXpix) / grid->nXpix;
+    *gridY = (iy - grid->yMin + 0.5*grid->nYpix) / grid->nYpix;
+    return true;
+}
+
+bool pswarpMapGridNextGrid (pswarpMapGrid *grid, int gridX, int gridY, int *nextX, int *nextY) {
+
+    *nextX = gridX*grid->nXpix + grid->xMin + 0.5*grid->nXpix;
+    *nextY = gridY*grid->nYpix + grid->yMin + 0.5*grid->nYpix;
+    return true;
+}
+
+// measure the max error accumulated in appling one grid point to its neighbors
+double pswarpMapGridMaxError (pswarpMapGrid *grid) {
+
+    double xRaw, yRaw;
+    double xRef, yRef;
+    double maxError = 0;
+
+    for (int i = 0; i < grid->nXpts - 1; i++) {
+	for (int j = 0; j < grid->nYpts - 1; j++) {
+
+	    // measure the output coordinates for the next grid position using the current grid map
+	    // compare with the coordinates measured using the next grid map
+	    pswarpMapApply (&xRaw, &yRaw, grid->maps[i][j], grid->maps[i][j]->xo + grid->nXpix, grid->maps[i][j]->yo);
+	    pswarpMapApply (&xRef, &yRef, grid->maps[i+1][j], grid->maps[i][j]->xo + grid->nXpix, grid->maps[i][j]->yo);
+
+	    double posError = hypot (xRaw-xRef, yRaw-yRef);
+	    maxError = PS_MAX (maxError, posError);
+	}
+    }	
+    return maxError;
+}
+
+bool pswarpMapApply (double *outX, double *outY, pswarpMap *map, double inX, double inY) {
+
+    *outX = map->Xo + map->Xx*inX + map->Xy*inY;
+    *outY = map->Yo + map->Yx*inX + map->Yy*inY;
+
+    return true;
+}
+
+// determine the map for the given pixel from src to dest. pixel is in src coords
+bool pswarpMapSetLocalModel (pswarpMap *map, pmReadout *dest, pmReadout *src, int ix, int iy) {
+
+    pmCell *cell = NULL;
+
+    cell = src->parent;
+    pmChip *chipSrc = cell->parent;
+    pmFPA *fpaSrc = chipSrc->parent;
+
+    cell = dest->parent;
+    pmChip *chipDest = cell->parent;
+    pmFPA *fpaDest = chipDest->parent;
+
+    // XXX save these as static for speed?
+    psPlane *offset = psPlaneAlloc();
+
+    psPlane *FP = psPlaneAlloc();
+    psPlane *TP = psPlaneAlloc();
+    psSphere *sky = psSphereAlloc();
+
+    psPlane *V00 = psPlaneAlloc();
+    psPlane *V10 = psPlaneAlloc();
+    psPlane *V01 = psPlaneAlloc();
+
+    // XXX need to include readout->cell->chip offsets
+
+    // V(0,0) position
+    offset->x = ix;
+    offset->y = iy;
+    psPlaneTransformApply(FP, chipSrc->toFPA, offset);
+    psPlaneTransformApply (TP, fpaSrc->toTPA, FP);
+    psDeproject (sky, TP, fpaSrc->toSky);
+    psProject (TP, sky, fpaDest->toSky);
+    psPlaneTransformApply (FP, fpaDest->fromTPA, TP);
+    psPlaneTransformApply (V00, chipDest->fromFPA, FP);
+    
+    // V(1,0) position
+    offset->x = ix + 1;
+    offset->y = iy;
+    psPlaneTransformApply(FP, chipSrc->toFPA, offset);
+    psPlaneTransformApply (TP, fpaSrc->toTPA, FP);
+    psDeproject (sky, TP, fpaSrc->toSky);
+    psProject (TP, sky, fpaDest->toSky);
+    psPlaneTransformApply (FP, fpaDest->fromTPA, TP);
+    psPlaneTransformApply (V10, chipDest->fromFPA, FP);
+    
+    // V(0,1) position
+    offset->x = ix;
+    offset->y = iy + 1;
+    psPlaneTransformApply(FP, chipSrc->toFPA, offset);
+    psPlaneTransformApply (TP, fpaSrc->toTPA, FP);
+    psDeproject (sky, TP, fpaSrc->toSky);
+    psProject (TP, sky, fpaDest->toSky);
+    psPlaneTransformApply (FP, fpaDest->fromTPA, TP);
+    psPlaneTransformApply (V01, chipDest->fromFPA, FP);
+
+    map->Xx = V10->x - V00->x;
+    map->Xy = V01->x - V00->x;
+    map->Xo = V00->x + map->Xx*ix + map->Xy*iy;
+
+    map->Yx = V10->y - V00->y;
+    map->Yy = V01->y - V00->y;
+    map->Yo = V00->y + map->Yx*ix + map->Yy*iy;
+ 
+    map->xo = ix;
+    map->yo = iy;
+
+    psFree (offset);
+    psFree (FP);
+    psFree (TP);
+    psFree (sky);
+
+    psFree (V00);
+    psFree (V10);
+    psFree (V01);
+
+    return true;
+}
+
+static void pswarpMapFree (pswarpMap *map) {
   return;
 }
@@ -13,3 +161,38 @@
 }
 
+static void pswarpMapGridFree (pswarpMapGrid *grid) {
+
+    if (grid == NULL) return;
+    if (grid->maps == NULL) return;
+
+    for (int i = 0; i < grid->nXpts; i++) {
+	for (int j = 0; j < grid->nYpts; j++) {
+	    psFree (grid->maps[i][j]);
+	}
+	psFree (grid->maps[i]);
+    }
+    psFree (grid->maps);
+    return;
+}
+
+pswarpMapGrid *pswarpMapGridAlloc (int nXpts, int nYpts) {
+
+  pswarpMapGrid *grid = (pswarpMapGrid *) psAlloc (sizeof(pswarpMapGrid));
+  psMemSetDeallocator(grid, (psFreeFunc) pswarpMapGridFree);
+
+  grid->maps = psAlloc (nXpts*sizeof(void **));
+  for (int i = 0; i < nXpts; i++) {
+      grid->maps[i] = psAlloc (nYpts*sizeof(void *));
+      for (int j = 0; j < nYpts; j++) {
+	  grid->maps[i][j] = pswarpMapAlloc();
+      }
+  }
+  grid->nXpts = nXpts;
+  grid->nYpts = nYpts;
+
+  grid->nXpix = 0;
+  grid->nYpix = 0;
   
+  return grid;
+}
+