Index: /trunk/psModules/src/astrom/pmAstrometryObjects.c
===================================================================
--- /trunk/psModules/src/astrom/pmAstrometryObjects.c	(revision 7926)
+++ /trunk/psModules/src/astrom/pmAstrometryObjects.c	(revision 7927)
@@ -8,6 +8,6 @@
 *  @author EAM, IfA
 *
-*  @version $Revision: 1.9 $ $Name: not supported by cvs2svn $
-*  @date $Date: 2006-06-21 03:21:16 $
+*  @version $Revision: 1.10 $ $Name: not supported by cvs2svn $
+*  @date $Date: 2006-07-18 15:36:32 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -20,4 +20,5 @@
 #include <string.h>
 #include <math.h>
+#include <assert.h>
 #include <unistd.h>   // for unlink
 #include <pslib.h>
@@ -25,61 +26,13 @@
 #include "pmAstrometryObjects.h"
 
+
 #define PM_ASTROMETRYOBJECTS_DEBUG 1
 /******************************************************************************
-pmAstromObjSortByFPX(**a, **b): sort by mag (descending)
+pmAstromObjSortByMag(**a, **b): sort by mag (descending)
  
 Is this a private routine?
 Should we do the early asserts?
  ******************************************************************************/
-
-// XXX an example (might be faster)
-void pmAstromObjSortByChipX2 (psArray *objects)
-{
-
-    int l,j,ir,i;
-    float v1, v2;
-    pmAstromObj *tmp;
-    pmAstromObj *obj;
-
-    int N = objects->n;
-
-    if (N < 2)
-        return;
-    l = N >> 1;
-    ir = N - 1;
-    for (;;) {
-        if (l > 0) {
-            tmp = objects->data[--l];
-        } else {
-            tmp = objects->data[ir];
-            objects->data[ir] = objects->data[0];
-            if (--ir == 0) {
-                objects->data[0] = tmp;
-                return;
-            }
-        }
-        i = l;
-        j = (l << 1) + 1;
-        while (j <= ir) {
-            obj = objects->data[j];
-            v1 = obj->chip->x;
-            obj = objects->data[j+1];
-            v2 = obj->chip->x;
-            if (j < ir && v1 < v2)
-                ++j;
-            obj = objects->data[j];
-            v1 = obj->chip->x;
-            v2 = tmp->chip->x;
-            if (v2 < v1) {
-                objects->data[i] = objects->data[j];
-                j += (i=j) + 1;
-            } else
-                j = ir + 1;
-        }
-        objects->data[i] = tmp;
-    }
-}
-
-int pmAstromObjSortByFPX(
+int pmAstromObjSortByMag(
     const void **a,
     const void **b)
@@ -95,60 +48,4 @@
     pmAstromObj *B = *(pmAstromObj **)b;
 
-    psF32 diff = A->FP->x - B->FP->x;
-    if (diff > FLT_EPSILON) {
-        return (+1);
-    }
-
-    if (diff < FLT_EPSILON) {
-        return (-1);
-    }
-    return (0);
-}
-
-int pmAstromObjSortByChipX(
-    const void **a,
-    const void **b)
-{
-    if (PM_ASTROMETRYOBJECTS_DEBUG) {
-        PS_ASSERT_PTR_NON_NULL(a, 0);
-        PS_ASSERT_PTR_NON_NULL(*a, 0);
-        PS_ASSERT_PTR_NON_NULL(b, 0);
-        PS_ASSERT_PTR_NON_NULL(*b, 0);
-    }
-
-    pmAstromObj *A = *(pmAstromObj **)a;
-    pmAstromObj *B = *(pmAstromObj **)b;
-
-    psF32 diff = A->chip->x - B->chip->x;
-    if (diff > FLT_EPSILON) {
-        return (+1);
-    }
-
-    if (diff < FLT_EPSILON) {
-        return (-1);
-    }
-    return (0);
-}
-
-/******************************************************************************
-pmAstromObjSortByMag(**a, **b): sort by mag (descending)
- 
-Is this a private routine?
-Should we do the early asserts?
- ******************************************************************************/
-int pmAstromObjSortByMag(
-    const void **a,
-    const void **b)
-{
-    if (PM_ASTROMETRYOBJECTS_DEBUG) {
-        PS_ASSERT_PTR_NON_NULL(a, 0);
-        PS_ASSERT_PTR_NON_NULL(*a, 0);
-        PS_ASSERT_PTR_NON_NULL(b, 0);
-        PS_ASSERT_PTR_NON_NULL(*b, 0);
-    }
-
-    pmAstromObj *A = *(pmAstromObj **)a;
-    pmAstromObj *B = *(pmAstromObj **)b;
-
     psF32 diff = A->Mag - B->Mag;
     if (diff > FLT_EPSILON) {
@@ -163,38 +60,24 @@
 }
 
-
-/******************************************************************************
-pmAstromRadiusMatch(st1, st2, config)
- ******************************************************************************/
-psArray *pmAstromRadiusMatch(
-    psArray *st1,
-    psArray *st2,
-    psMetadata *config)
-{
-    PS_ASSERT_PTR_NON_NULL(st1, NULL);
-    PS_ASSERT_PTR_NON_NULL(st2, NULL);
-    PS_ASSERT_PTR_NON_NULL(config, NULL);
-    bool status;
+/************************************************************************************************************/
+/*
+ * Working routine to match two lists (where x[12] are sorted), given psVectors of their coordinates and the
+ * permutation used to sort in x
+ */
+static psArray *match_lists(const psVector *x1, const psVector *y1, // x/y coordinates of first set of objects
+                            const psVector *x2, const psVector *y2, // x/y   "    "    "  second "   "  "   "
+                            const psVector *sorted1, const psVector *sorted2, // mapping to original order
+                            const double RADIUS) // matching radius
+{
+    psArray *matches = psArrayAlloc(x1->n);
+    matches->n = 0;
+
+    const double RADIUS_SQR = PS_SQR(RADIUS);
+    double dX, dY, dR;
+
     int jStart;
-    double dX, dY, dR;
-
-    double RADIUS = psMetadataLookupF32 (&status, config, "PSASTRO.MATCH.RADIUS");
-    double RADIUS_SQR = PS_SQR (RADIUS);
-
-    // sort both lists by Focal Plane X coord
-    st1 = psArraySort (st1, pmAstromObjSortByFPX);
-    st2 = psArraySort (st2, pmAstromObjSortByFPX);
-
-    // pmAstromObjSortByChipX2 (st1);
-    // pmAstromObjSortByChipX2 (st2);
-
-    psArray *matches = psArrayAlloc (100);
-    matches->n = 0;
-
-    int i = 0;
-    int j = 0;
-    while ((i < st1->n) && (j < st2->n)) {
-
-        dX = ((pmAstromObj *)st1->data[i])->FP->x - ((pmAstromObj *)st2->data[j])->FP->x;
+    int i = 0, j = 0;
+    while (i < x1->n && j < x2->n) {
+        dX = x1->data.F64[i] - x2->data.F64[j];
         if (dX <= -RADIUS) {
             i++;
@@ -207,8 +90,8 @@
 
         jStart = j;
-        while ((fabs(dX) < RADIUS) && (j < st2->n)) {
-
-            dX = ((pmAstromObj *)st1->data[i])->FP->x - ((pmAstromObj *)st2->data[j])->FP->x;
-            dY = ((pmAstromObj *)st1->data[i])->FP->y - ((pmAstromObj *)st2->data[j])->FP->y;
+        while (fabs(dX) < RADIUS && j < x2->n) {
+
+            dX = x1->data.F64[i] - x2->data.F64[j];
+            dY = y1->data.F64[i] - y2->data.F64[j];
             dR = dX*dX + dY*dY;
 
@@ -219,5 +102,5 @@
 
             // got a match; add to output list
-            pmAstromMatch *match = pmAstromMatchAlloc (i, j);
+            pmAstromMatch *match = pmAstromMatchAlloc (sorted1->data.S32[i], sorted2->data.S32[j]);
             psArrayAdd (matches, 100, match);
             psFree (match);
@@ -228,76 +111,144 @@
         i++;
     }
-    psLogMsg (__func__, 3, "radius match: %d pairs (radius: %f)\n", matches->n, RADIUS);
+
     return (matches);
 }
-
-
-
-/******************************************************************************
-pmAstromRadiusMatchChip(st1, st2, config) : match objects on the chip
- ******************************************************************************/
-psArray *pmAstromRadiusMatchChip(
-    psArray *st1,
-    psArray *st2,
-    psMetadata *config)
+/******************************************************************************
+pmAstromRadiusMatch(st1, st2, config)
+ ******************************************************************************/
+psArray *pmAstromRadiusMatch(
+    const psArray *st1,
+    const psArray *st2,
+    const psMetadata *config)
 {
     PS_ASSERT_PTR_NON_NULL(st1, NULL);
     PS_ASSERT_PTR_NON_NULL(st2, NULL);
     PS_ASSERT_PTR_NON_NULL(config, NULL);
-    bool status;
-    int jStart;
-    double dX, dY, dR;
-
+
+    assert(st1->n == 0 || pmIsAstromObj(st1->data[0]));
+    assert(st2->n == 0 || pmIsAstromObj(st2->data[0]));
+
+    bool status = false;
     double RADIUS = psMetadataLookupF32 (&status, config, "PSASTRO.MATCH.RADIUS");
-    double RADIUS_SQR = PS_SQR (RADIUS);
-
-    // sort both lists by Focal Plane X coord
-    st1 = psArraySort (st1, pmAstromObjSortByChipX);
-    st2 = psArraySort (st2, pmAstromObjSortByChipX);
-
-    psArray *matches = psArrayAlloc (100);
-    matches->n = 0;
-
-    int i = 0;
-    int j = 0;
-    while ((i < st1->n) && (j < st2->n)) {
-
-        dX = ((pmAstromObj *)st1->data[i])->chip->x - ((pmAstromObj *)st2->data[j])->chip->x;
-        if (dX <= -RADIUS) {
-            i++;
-            continue;
-        }
-        if (dX >= +RADIUS) {
-            j++;
-            continue;
-        }
-
-        jStart = j;
-        while ((dX > -RADIUS) && (j < st2->n)) {
-
-            dX = ((pmAstromObj *)st1->data[i])->chip->x - ((pmAstromObj *)st2->data[j])->chip->x;
-            dY = ((pmAstromObj *)st1->data[i])->chip->y - ((pmAstromObj *)st2->data[j])->chip->y;
-            dR = dX*dX + dY*dY;
-
-            if (dR > RADIUS_SQR) {
-                j++;
-                continue;
-            }
-
-            // got a match; add to output list
-            pmAstromMatch *match = pmAstromMatchAlloc (i, j);
-            psArrayAdd (matches, 100, match);
-            psFree (match);
-
-            j++;
-        }
-        j = jStart;
-        i++;
-    }
+    if (!status) {
+        psError(PS_ERR_IO, false, "Failed to lookup matching radius");
+        return NULL;
+    }
+    /*
+     * sort both lists by Focal Plane X coord; st1 first
+     */
+    psVector *x1 = psVectorAlloc(st1->n, PS_TYPE_F64);
+    x1->n = x1->nalloc;
+    for (int i = 0; i < st1->n; i++) {
+        x1->data.F64[i] = ((pmAstromObj *)st1->data[i])->FP->x;
+    }
+    const psVector *sorted1 = psVectorSortIndex(NULL, x1);
+    assert (sorted1->type.type == PS_TYPE_S32);
+
+    psVector *y1 = psVectorAlloc(st1->n, PS_TYPE_F64);
+    y1->n = y1->nalloc;
+    for (int i = 0; i < st1->n; i++) {
+        x1->data.F64[i] = ((pmAstromObj *)st1->data[sorted1->data.S32[i]])->FP->x;
+        y1->data.F64[i] = ((pmAstromObj *)st1->data[sorted1->data.S32[i]])->FP->y;
+    }
+
+    // now st2
+    psVector *x2 = psVectorAlloc(st2->n, PS_TYPE_F64);
+    x2->n = x2->nalloc;
+    for (int i = 0; i < st2->n; i++) {
+        x2->data.F64[i] = ((pmAstromObj *)st2->data[i])->FP->x;
+    }
+    const psVector *sorted2 = psVectorSortIndex(NULL, x2);
+
+    psVector *y2 = psVectorAlloc(st2->n, PS_TYPE_F64);
+    y2->n = y2->nalloc;
+    for (int i = 0; i < st2->n; i++) {
+        x2->data.F64[i] = ((pmAstromObj *)st2->data[sorted2->data.S32[i]])->FP->x;
+        y2->data.F64[i] = ((pmAstromObj *)st2->data[sorted2->data.S32[i]])->FP->y;
+    }
+    /*
+     * Do the work
+     */
+    psArray *matches = match_lists(x1, y1, x2, y2, sorted1, sorted2, RADIUS);
+
+    psFree(sorted1);
+    psFree(sorted2);
+    psFree(x1);
+    psFree(y1);
+    psFree(x2);
+    psFree(y2);
+
     psLogMsg (__func__, 3, "radius match: %d pairs (radius: %f)\n", matches->n, RADIUS);
     return (matches);
 }
 
-
+/******************************************************************************
+pmAstromRadiusMatchChip(st1, st2, config) : match objects on the chip
+ ******************************************************************************/
+psArray *pmAstromRadiusMatchChip(
+    const psArray *st1,
+    const psArray *st2,
+    const psMetadata *config)
+{
+    PS_ASSERT_PTR_NON_NULL(st1, NULL);
+    PS_ASSERT_PTR_NON_NULL(st2, NULL);
+    PS_ASSERT_PTR_NON_NULL(config, NULL);
+
+    assert(st1->n == 0 || pmIsAstromObj(st1->data[0]));
+    assert(st2->n == 0 || pmIsAstromObj(st2->data[0]));
+
+    bool status = false;
+    double RADIUS = psMetadataLookupF32 (&status, config, "PSASTRO.MATCH.RADIUS");
+    if (!status) {
+        psError(PS_ERR_IO, false, "Failed to lookup matching radius");
+        return NULL;
+    }
+    /*
+     * sort both lists by chip X coord; st1 first
+     */
+    psVector *x1 = psVectorAlloc(st1->n, PS_TYPE_F64);
+    x1->n = x1->nalloc;
+    for (int i = 0; i < st1->n; i++) {
+        x1->data.F64[i] = ((pmAstromObj *)st1->data[i])->FP->x;
+    }
+    const psVector *sorted1 = psVectorSortIndex(NULL, x1);
+    assert (sorted1->type.type == PS_TYPE_S32);
+
+    psVector *y1 = psVectorAlloc(st1->n, PS_TYPE_F64);
+    y1->n = y1->nalloc;
+    for (int i = 0; i < st1->n; i++) {
+        x1->data.F64[i] = ((pmAstromObj *)st1->data[sorted1->data.S32[i]])->chip->x;
+        y1->data.F64[i] = ((pmAstromObj *)st1->data[sorted1->data.S32[i]])->chip->y;
+    }
+
+    // now st2
+    psVector *x2 = psVectorAlloc(st2->n, PS_TYPE_F64);
+    x2->n = x2->nalloc;
+    for (int i = 0; i < st2->n; i++) {
+        x2->data.F64[i] = ((pmAstromObj *)st2->data[i])->FP->x;
+    }
+    const psVector *sorted2 = psVectorSortIndex(NULL, x2);
+
+    psVector *y2 = psVectorAlloc(st2->n, PS_TYPE_F64);
+    y2->n = y2->nalloc;
+    for (int i = 0; i < st2->n; i++) {
+        x2->data.F64[i] = ((pmAstromObj *)st2->data[sorted2->data.S32[i]])->chip->x;
+        y2->data.F64[i] = ((pmAstromObj *)st2->data[sorted2->data.S32[i]])->chip->y;
+    }
+    /*
+     * Do the work
+     */
+    psArray *matches = match_lists(x1, y1, x2, y2, sorted1, sorted2, RADIUS);
+
+    psFree(sorted1);
+    psFree(sorted2);
+    psFree(x1);
+    psFree(y1);
+    psFree(x2);
+    psFree(y2);
+
+    psLogMsg (__func__, 3, "radius match: %d pairs (radius: %f)\n", matches->n, RADIUS);
+    return (matches);
+}
 
 /******************************************************************************
@@ -398,5 +349,5 @@
  ******************************************************************************/
 psArray *pmAstromRotateObj(
-    psArray *old,
+    const psArray *old,
     psPlane center,
     double angle)
@@ -406,5 +357,5 @@
     double X, Y;
     pmAstromObj *newObj;
-    pmAstromObj *oldObj;
+    const pmAstromObj *oldObj;
 
     psArray *new = psArrayAlloc (old->n);
@@ -434,7 +385,7 @@
 
 /******************************************************************************
-pmAstromObjFree(obj)
- ******************************************************************************/
-static void pmAstromObjFree(pmAstromObj *obj)
+astromObjFree(obj)
+ ******************************************************************************/
+static void astromObjFree(pmAstromObj *obj)
 {
     if (obj == NULL) {
@@ -459,5 +410,5 @@
 {
     pmAstromObj *obj = psAlloc (sizeof(pmAstromObj));
-    psMemSetDeallocator (obj, (psFreeFunc) pmAstromObjFree);
+    psMemSetDeallocator (obj, (psFreeFunc)astromObjFree);
 
     obj->pix  = psPlaneAlloc();
@@ -473,9 +424,15 @@
 }
 
+bool pmIsAstromObj(const psPtr ptr)
+{
+    return (psMemGetDeallocator(ptr) == (psFreeFunc)astromObjFree);
+}
+
+
 
 /******************************************************************************
 pmAstromObjCopy(old)
  ******************************************************************************/
-pmAstromObj *pmAstromObjCopy(pmAstromObj *old)
+pmAstromObj *pmAstromObjCopy(const pmAstromObj *old)
 {
     PS_ASSERT_PTR_NON_NULL(old, NULL);
@@ -557,7 +514,7 @@
  ******************************************************************************/
 pmAstromStats pmAstromGridAngle(
-    psArray *raw,
-    psArray *ref,
-    psMetadata *config)
+    const psArray *raw,
+    const psArray *ref,
+    const psMetadata *config)
 {
     // XXX: What to do if input parameters are bad?
@@ -573,5 +530,5 @@
     int iX, iY;     // corresponding grid bin
 
-    pmAstromObj *ob1, *ob2; // short-cut pointers to the objects
+    const pmAstromObj *ob1, *ob2; // short-cut pointers to the objects
     pmAstromStats stats;    // output match statistics
 
@@ -643,5 +600,5 @@
         // only check bins with at least 1/2 of max bin
         int minNpts = 0.5*imStats->max;
-        fprintf (stderr, "minNpts: %d, max: %d\n", minNpts, (int)(imStats->max));
+        psTrace("psModule.astrom.grid.angle", 5, "minNpts: %d, max: %d", minNpts, (int)(imStats->max));
 
         // find the 'best' bin
@@ -691,8 +648,7 @@
  ******************************************************************************/
 
-pmAstromStats pmAstromGridMatch(
-    psArray *raw,
-    psArray *ref,
-    psMetadata *config)
+pmAstromStats pmAstromGridMatch(const psArray *raw,
+                                const psArray *ref,
+                                const psMetadata *config)
 {
     // XXX: What to do if input parameters are bad?
@@ -704,5 +660,5 @@
     bool status;
     double xMin, xMax, yMin, yMax;
-    pmAstromObj *obj;
+    const pmAstromObj *obj;
     psArray *rot;
 
@@ -732,5 +688,7 @@
         rot = pmAstromRotateObj (raw, center, angle);
         newStat = pmAstromGridAngle (rot, ref, config);
-        fprintf (stderr, "try %f : %f %f (%d pts, %f var, %f met (%f log))\n", angle, newStat.offset.x, newStat.offset.y, newStat.nMatch, newStat.minVar, newStat.minMetric, log10(newStat.minMetric));
+        psTrace("psModule.astrom.grid.match", 5, "try %f : %f %f (%d pts, %f var, %f met (%f log))",
+                angle, newStat.offset.x, newStat.offset.y, newStat.nMatch, newStat.minVar, newStat.minMetric,
+                log10(newStat.minMetric));
 
         newStat.angle  = angle;
@@ -738,9 +696,12 @@
         if (newStat.minMetric < minStat.minMetric) {
             minStat = newStat;
-            fprintf (stderr, "use %f : %f %f (%d pts, %f var, %f met (%f log))\n", angle, minStat.offset.x, minStat.offset.y, minStat.nMatch, minStat.minVar, minStat.minMetric, log10(minStat.minMetric));
+            psTrace("psModule.astrom.grid.match", 5, "use %f : %f %f (%d pts, %f var, %f met (%f log))",
+                    angle, minStat.offset.x, minStat.offset.y, minStat.nMatch, minStat.minVar, minStat.
+                    minMetric, log10(minStat.minMetric));
         }
         psFree (rot);
     }
-    fprintf (stderr, "best: %f %f (%d pts, %f var, %f met)\n", minStat.offset.x, minStat.offset.y, minStat.nMatch, minStat.minVar, log10(minStat.minMetric));
+    psTrace("psModule.astrom.grid.match", 4, "best: %f %f (%d pts, %f var, %f met)",
+            minStat.offset.x, minStat.offset.y, minStat.nMatch, minStat.minVar, log10(minStat.minMetric));
     return (minStat);
 }