/** @file  pmAstrometryObjects.c
*
*  @brief This file defines the basic types for matching objects
*  based on their astrometry.
*
*  @ingroup AstroImage
*
*  @author EAM, IfA
*
*  @version $Revision: 1.37 $ $Name: not supported by cvs2svn $
*  @date $Date: 2008-01-15 02:49:45 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*/

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

/******************************************************************************/
/*  INCLUDE FILES                                                             */
/******************************************************************************/
#include <stdio.h>
#include <strings.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <unistd.h>   // for unlink
#include <pslib.h>

#include "pmHDU.h"
#include "pmFPA.h"
#include "pmAstrometryObjects.h"

# define DEG_RAD 57.295779513082322
# define RAD_DEG  0.017453292519943

#define PM_ASTROMETRYOBJECTS_DEBUG 1
/******************************************************************************
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) {
        return (-1);
    }

    if (diff < FLT_EPSILON) {
        return (+1);
    }

    return (0);
}

/************************************************************************************************************/
/*
 * 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 = psArrayAllocEmpty(x1->n);

    const double RADIUS_SQR = PS_SQR(RADIUS);
    double dX, dY, dR;

    int jStart;
    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++;
            continue;
        }
        if (dX >= +RADIUS) {
            j++;
            continue;
        }

        jStart = j;
        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;

            if (dR > RADIUS_SQR) {
                j++;
                continue;
            }

            // got a match; add to output list
            pmAstromMatch *match = pmAstromMatchAlloc (sorted1->data.S32[i], sorted2->data.S32[j]);
            psArrayAdd (matches, 100, match);
            psFree (match);

            j++;
        }
        j = jStart;
        i++;
    }

    return (matches);
}

/************************************************************************************************************/
// macro to generate code for radius match function based on desired member
// radius is in units of matching member (eg, pixels for chip, microns for FP, etc)
#define MAKE_ASTROM_RADIUS(FUNC, MEMBER) \
psArray *FUNC( \
               const psArray *st1, \
               const psArray *st2, \
               double RADIUS) \
{ \
    PS_ASSERT_PTR_NON_NULL(st1, NULL); \
    PS_ASSERT_PTR_NON_NULL(st2, NULL); \
    \
    assert(st1->n == 0 || pmAstromObjTest(st1->data[0])); \
    assert(st2->n == 0 || pmAstromObjTest(st2->data[0])); \
    \
    /* sort both lists by X coord; st1 first */ \
    psVector *x1 = psVectorAlloc(st1->n, PS_TYPE_F64); \
    for (int i = 0; i < st1->n; i++) { \
        x1->data.F64[i] = ((pmAstromObj *)st1->data[i])->MEMBER->x; \
    } \
    const psVector *sorted1 = psVectorSortIndex(NULL, x1); \
    assert (sorted1->type.type == PS_TYPE_S32); \
    \
    psVector *y1 = psVectorAlloc(st1->n, PS_TYPE_F64); \
    for (int i = 0; i < st1->n; i++) { \
        x1->data.F64[i] = ((pmAstromObj *)st1->data[sorted1->data.S32[i]])->MEMBER->x; \
        y1->data.F64[i] = ((pmAstromObj *)st1->data[sorted1->data.S32[i]])->MEMBER->y; \
    } \
    \
    /* now st2 */ \
    psVector *x2 = psVectorAlloc(st2->n, PS_TYPE_F64); \
    for (int i = 0; i < st2->n; i++) { \
        x2->data.F64[i] = ((pmAstromObj *)st2->data[i])->MEMBER->x; \
    } \
    const psVector *sorted2 = psVectorSortIndex(NULL, x2); \
    \
    psVector *y2 = psVectorAlloc(st2->n, PS_TYPE_F64); \
    for (int i = 0; i < st2->n; i++) { \
        x2->data.F64[i] = ((pmAstromObj *)st2->data[sorted2->data.S32[i]])->MEMBER->x; \
        y2->data.F64[i] = ((pmAstromObj *)st2->data[sorted2->data.S32[i]])->MEMBER->y; \
    } \
    /* Do the work */ \
    psArray *matches = match_lists(x1, y1, x2, y2, sorted1, sorted2, RADIUS); \
    \
    psFree((void *)sorted1); \
    psFree((void *)sorted2); \
    psFree(x1); \
    psFree(y1); \
    psFree(x2); \
    psFree(y2); \
    \
    psLogMsg (__func__, 3, "radius match: %ld pairs (radius: %f)\n", matches->n, RADIUS); \
    return (matches); \
}

/******************************************************************************/
/*
 * Match two lists of pmAstromObjs, based on the FP, TP, or chip coordinates
 */
MAKE_ASTROM_RADIUS(pmAstromRadiusMatch, FP)
MAKE_ASTROM_RADIUS(pmAstromRadiusMatchFP, FP)
MAKE_ASTROM_RADIUS(pmAstromRadiusMatchTP, TP)
MAKE_ASTROM_RADIUS(pmAstromRadiusMatchChip, chip)

/******************************************************************************
pmAstromMatchFit(map, raw, ref, match, stats): take two matched star lists
and fit a psPlaneTransform between them
 ******************************************************************************/
pmAstromFitResults *pmAstromMatchFit(
    psPlaneTransform *map,
    psArray *raw,
    psArray *ref,
    psArray *match,
    psStats *stats)
{
    PS_ASSERT_PTR_NON_NULL(map, NULL);
    PS_ASSERT_PTR_NON_NULL(raw, NULL);
    PS_ASSERT_PTR_NON_NULL(ref, NULL);
    PS_ASSERT_PTR_NON_NULL(match, NULL);
    PS_ASSERT_PTR_NON_NULL(stats, NULL);

    // reassign values for clip fit
    // XXX set wt based on mag error?
    psVector *X = psVectorAlloc (match->n, PS_TYPE_F32);
    psVector *Y = psVectorAlloc (match->n, PS_TYPE_F32);
    psVector *x = psVectorAlloc (match->n, PS_TYPE_F32);
    psVector *y = psVectorAlloc (match->n, PS_TYPE_F32);
    psVector *wt = psVectorAlloc (match->n, PS_TYPE_F32);
    // take the matched stars, first fit
    for (int i = 0; i < match->n; i++) {
        pmAstromMatch *pair = match->data[i];
        pmAstromObj *rawStar = raw->data[pair->raw];
        pmAstromObj *refStar = ref->data[pair->ref];

        X->data.F32[i] = rawStar->chip->x;
        Y->data.F32[i] = rawStar->chip->y;

        x->data.F32[i] = refStar->FP->x;
        y->data.F32[i] = refStar->FP->y;

        wt->data.F32[i] = 1.0;
    }

    // constant errors
    psVector *mask = psVectorAlloc (match->n, PS_TYPE_U8);
    psVectorInit (mask, 0);

    // the stats options supplied are used to perform the clip fitting
    pmAstromFitResults *results = pmAstromFitResultsAlloc();
    results->xStats = psStatsAlloc (PS_STAT_NONE);
    results->yStats = psStatsAlloc (PS_STAT_NONE);
    *results->xStats = *stats;
    *results->yStats = *stats;

    int nIter = stats->clipIter;

    results->xStats->clipIter = 1;
    results->yStats->clipIter = 1;

    // fit chip-to-FPA transformation
    // we run 'clipIter' cycles clipping in each of x and y, with only one iteration each
    // need to use the stats lookups functions to get the width and center
    for (int i = 0; i < nIter; i++) {
	if (!psVectorClipFitPolynomial2D (map->x, results->xStats, mask, 0xff, x, wt, X, Y)) {
            psError(PS_ERR_UNKNOWN, false, "failure in clip-fitting for x\n");
	    psFree (x);
	    psFree (y);
	    psFree (X);
	    psFree (Y);
	    psFree (wt);
	    psFree (mask);

	    return results;
	}
        psTrace ("psModules.astrom", 3, "x resid: %f +/- %f (%ld of %ld)\n", results->xStats->clippedMean, results->xStats->clippedStdev, results->xStats->clippedNvalues, x->n);

        if (!psVectorClipFitPolynomial2D (map->y, results->yStats, mask, 0xff, y, wt, X, Y)) {
            psError(PS_ERR_UNKNOWN, false, "failure in clip-fitting for y\n");
	    psFree (x);
	    psFree (y);
	    psFree (X);
	    psFree (Y);
	    psFree (wt);
	    psFree (mask);

	    return results;
	}
        psTrace ("psModules.astrom", 3, "y resid: %f +/- %f (%ld of %ld)\n", results->yStats->clippedMean, results->yStats->clippedStdev, results->yStats->clippedNvalues, y->n);
    }
    results->xStats->clipIter = stats->clipIter;
    results->yStats->clipIter = stats->clipIter;

    psFree (x);
    psFree (y);
    psFree (X);
    psFree (Y);
    psFree (wt);
    psFree (mask);

    return (results);
}


/******************************************************************************
pmAstromRotateObj(old, center, angle, angle): rotate & scale the focal-plane coordinates
about the center coordinate angle specified in radians
 ******************************************************************************/
psArray *pmAstromRotateObj(
    const psArray *old,
    psPlane center,
    double angle,
    double scale)
{
    PS_ASSERT_PTR_NON_NULL(old, NULL);

    double X, Y;
    pmAstromObj *newObj;
    const pmAstromObj *oldObj;

    psArray *new = psArrayAlloc (old->n);
    double cs = scale*cos(angle);
    double sn = scale*sin(angle);
    double xCenter = center.x;
    double yCenter = center.y;

    for (int i = 0; i < old->n; i++) {

        oldObj = (pmAstromObj *)old->data[i];
        newObj = pmAstromObjCopy (oldObj);

        X = oldObj->FP->x - xCenter;
        Y = oldObj->FP->y - yCenter;

        newObj->FP->x = X*cs + Y*sn + xCenter;
        newObj->FP->y = Y*cs - X*sn + yCenter;

        new->data[i] = newObj;
    }
    return (new);
}

/******************************************************************************
pmAstromStatsFree(stats)
 ******************************************************************************/
static void pmAstromStatsFree(pmAstromStats *stats)
{
    if (stats == NULL)
        return;
    return;
}

/******************************************************************************
pmAstromStatsAlloc()
 ******************************************************************************/
pmAstromStats *pmAstromStatsAlloc(void)
{
    pmAstromStats *stats = psAlloc (sizeof(pmAstromStats));
    psMemSetDeallocator (stats, (psFreeFunc)pmAstromStatsFree);

    //    stats->center = {0, 0, 0, 0};
    //    stats->offset = {0, 0, 0, 0};
    stats->angle     = 0.0;
    stats->scale     = 1.0;
    stats->minMetric = 0.0;
    stats->minVar    = 0.0;
    stats->nMatch    = 0;
    stats->nTest     = 0;
    stats->nSigma    = 0;

    return (stats);
}

/******************************************************************************
pmAstromFitResultsFree(stats)
 ******************************************************************************/
static void pmAstromFitResultsFree(pmAstromFitResults *results)
{
    if (results == NULL)
        return;
    psFree (results->xStats);
    psFree (results->yStats);
    return;
}

/******************************************************************************
pmAstromFitResultsAlloc()
 ******************************************************************************/
pmAstromFitResults *pmAstromFitResultsAlloc(void)
{
    pmAstromFitResults *results = psAlloc (sizeof(pmAstromFitResults));
    psMemSetDeallocator (results, (psFreeFunc)pmAstromFitResultsFree);

    results->xStats    = NULL;
    results->yStats    = NULL;
    results->nMatch    = 0;
    results->nSigma    = 0;

    return (results);
}

/******************************************************************************
astromObjFree(obj)
 ******************************************************************************/
static void astromObjFree(pmAstromObj *obj)
{
    if (obj == NULL) {
        return;
    }

    psFree(obj->pix);
    psFree(obj->cell);
    psFree(obj->chip);
    psFree(obj->FP);
    psFree(obj->TP);
    psFree(obj->sky);

    return;
}


/******************************************************************************
pmAstromObjAlloc()
 ******************************************************************************/
pmAstromObj *pmAstromObjAlloc(void)
{
    pmAstromObj *obj = psAlloc (sizeof(pmAstromObj));
    psMemSetDeallocator (obj, (psFreeFunc)astromObjFree);

    obj->pix  = psPlaneAlloc();
    obj->cell = psPlaneAlloc();
    obj->chip = psPlaneAlloc();
    obj->FP   = psPlaneAlloc();
    obj->TP   = psPlaneAlloc();
    obj->sky  = psSphereAlloc();
    obj->Mag  = 0;
    obj->dMag = 0;

    return (obj);
}

bool pmAstromObjTest(const psPtr ptr)
{
    return (psMemGetDeallocator(ptr) == (psFreeFunc)astromObjFree);
}



/******************************************************************************
pmAstromObjCopy(old)
 ******************************************************************************/
pmAstromObj *pmAstromObjCopy(const pmAstromObj *old)
{
    PS_ASSERT_PTR_NON_NULL(old, NULL);
    pmAstromObj *obj = pmAstromObjAlloc();

    *obj->pix  = *old->pix;
    *obj->cell = *old->cell;
    *obj->chip = *old->chip;
    *obj->FP   = *old->FP;
    *obj->TP   = *old->TP;
    *obj->sky  = *old->sky;
    obj->Mag   =  old->Mag;
    obj->dMag  =  old->dMag;

    return(obj);
}


/******************************************************************************
 ******************************************************************************/
static void pmAstromMatchFree (pmAstromMatch *match)
{
    if (match == NULL)
        return;
    return;
}


/******************************************************************************
 ******************************************************************************/
pmAstromMatch *pmAstromMatchAlloc(
    int raw,
    int ref)
{
    pmAstromMatch *match = psAlloc (sizeof(pmAstromMatch));
    psMemSetDeallocator(match, (psFreeFunc) pmAstromMatchFree);

    match->raw = raw;
    match->ref = ref;

    return (match);
}


static double maxOffpix;                // maximum allowed offset between lists, in raw pixels
static double Scale;                    // grid pixel scale static
double Offset;                          // deltas to pixels
/******************************************************************************
AstromGridBin(*dx, *dy, dX, dY): local function to convert x,y coords to grid
bins it requires the globals defined above.

 ******************************************************************************/
static bool AstromGridBin(
    int *dx,
    int *dy,
    double dX,
    double dY)
{
    if (PM_ASTROMETRYOBJECTS_DEBUG) {
        PS_ASSERT_PTR_NON_NULL(dx, false);
        PS_ASSERT_PTR_NON_NULL(dy, false);
    }

    if (!isfinite(dX)) return false;
    if (!isfinite(dY)) return false;

    if (fabs(dX) > maxOffpix) return false;
    if (fabs(dY) > maxOffpix) return false;

    *dx = dX / Scale + Offset;
    *dy = dY / Scale + Offset;
    return true;
}


/******************************************************************************
pmAstromGridAngle(raw, ref, config): match the two lists using the binned
delta-delta max.
 ******************************************************************************/
pmAstromStats *pmAstromGridAngle(
    const psArray *raw,
    const psArray *ref,
    const psMetadata *config)
{
    PS_ASSERT_PTR_NON_NULL(raw, NULL);
    PS_ASSERT_PTR_NON_NULL(ref, NULL);
    PS_ASSERT_PTR_NON_NULL(config, NULL);

    bool status;
    int nPix;       // size of matching grid
    int nPixHalf;   // half-size of matching grid
    double dX, dY;  // offset between a possible matched pair
    int iX, iY;     // corresponding grid bin

    const pmAstromObj *ob1, *ob2; // short-cut pointers to the objects

    pmAstromStats *stats = pmAstromStatsAlloc();    // output match statistics

    // max allowed offset in either X or Y directions
    double gridOffset = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.OFFSET");

    // sampling scale of the grid
    double gridScale  = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.SCALE");

    // set the static scaling factors
    nPixHalf = (int)(gridOffset / gridScale + 0.5);  // half-grid
    nPix = 2*nPixHalf + 1;                           // full grid width

    // these are globals used by p_pmAstromGridBin
    maxOffpix = gridScale * (nPixHalf + 0.5);            // max offset from true center
    Offset    = maxOffpix / gridScale;
    Scale     = gridScale;

    // images used as accumulators for the loop below
    psImage *gridNP = psImageAlloc (nPix, nPix, PS_TYPE_U32);
    psImage *gridDX = psImageAlloc (nPix, nPix, PS_TYPE_F32);
    psImage *gridDY = psImageAlloc (nPix, nPix, PS_TYPE_F32);
    psImage *gridD2 = psImageAlloc (nPix, nPix, PS_TYPE_F32);
    psImageInit (gridNP, 0);
    psImageInit (gridDX, 0);
    psImageInit (gridDY, 0);
    psImageInit (gridD2, 0);

    // short-cut names for grid images
    psU32 **NP = gridNP->data.U32;
    psF32 **DX = gridDX->data.F32;
    psF32 **DY = gridDY->data.F32;
    psF32 **D2 = gridD2->data.F32;

    // accumulate grids for focal plane (L,M) matches
    for (int i = 0; i < raw->n; i++) {
        ob1 = (pmAstromObj *)raw->data[i];
        for (int j = 0; j < ref->n; j++) {
            ob2 = (pmAstromObj *)ref->data[j];
            dX = ob1->FP->x - ob2->FP->x;
            dY = ob1->FP->y - ob2->FP->y;

            // fprintf (f, "dX,dY: %8.2f %8.2f : %8.2f %8.2f : %8.2f %8.2f\n", dX, dY, ob1->FP->x, ob2->FP->x, ob1->FP->y, ob2->FP->y);
            // find bin coordinates for this delta-delta
            if (!AstromGridBin (&iX, &iY, dX, dY)) {
                continue; // matched pair is too far offset
            }

            // accumulate bin stats
            NP[iY][iX] ++;
            DX[iY][iX] += dX;
            DY[iY][iX] += dY;
            D2[iY][iX] += PS_SQR(dX) + PS_SQR(dY);
        }
    }

    // now assess the grid images
    {
        double minMetric = 1e10;
        double minVar = 1e10;
        int minX = -1;
        int minY = -1;
        double metric, var;

        // find the max pixel
        psStats *imStats = psStatsAlloc (PS_STAT_MAX | PS_STAT_MAX | PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
        if (!psImageStats(imStats, gridNP, NULL, 0)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to get image statistics.\n");
            psFree(imStats);
            psFree(gridNP);
            psFree(gridDX);
            psFree(gridDY);
            psFree(gridD2);
            psFree(stats);
            return NULL;
        }

        # if 0
        char line[16];
        psFits *fits = psFitsOpen ("grid.image.fits", "w");
        psFitsWriteImage (fits, NULL, gridNP, 0, NULL);
        psFitsClose (fits);
        fprintf (stderr, "wrote grid image, press return to continue\n");
        fgets (line, 15, stdin);
        # endif

        // only check bins with at least 1/2 of max bin
        // XXX requiring at least 3 matches in bin
        int minNpts = PS_MAX (0.5*imStats->max, 5);
        psTrace("psModule.astrom", 5, "minNpts: %d, min: %d, max: %d, median: %f, stdev: %f", minNpts, (int)(imStats->min), (int)(imStats->max), imStats->sampleMedian, imStats->sampleStdev);

        // find the 'best' bin
        for (int j = 0; j < gridNP->numRows; j++) {
            for (int i = 0; i < gridNP->numCols; i++) {
                if (NP[j][i] < minNpts) continue;

                // this metric emphasizes a narrow peak with lots of sources over one with few.
                var = fabs((D2[j][i]/NP[j][i]) - PS_SQR(DX[j][i]/NP[j][i]) - PS_SQR(DY[j][i]/NP[j][i]));
                metric = var / PS_SQR(NP[j][i]) / PS_SQR(NP[j][i]);

                // fprintf (stderr, "try : %f %f (%d pts, %f var, %f met)\n", DX[j][i]/NP[j][i], DY[j][i]/NP[j][i], NP[j][i], var, metric);

                if (metric < minMetric) {
                    minMetric = metric;
                    minVar    = var;
                    minX      = i;
                    minY      = j;
                }
            }
        }

        // convert the bin to delta-delta
        if ((minX < 0) || (minY < 0))
        {
            // no valid matches found
            stats->offset.x   = 0;
            stats->offset.y   = 0;
            stats->minMetric  = minMetric;
            stats->minVar     = minVar;
            stats->nMatch     = 0;
        } else
        {
            stats->offset.x  = DX[minY][minX] / NP[minY][minX];
            stats->offset.y  = DY[minY][minX] / NP[minY][minX];
            stats->minMetric = minMetric;
            stats->minVar    = minVar;
            stats->nMatch    = NP[minY][minX];
        }

        psFree (imStats);
        // XXX EAM : This routine, and pmAstromGridMatch, need to handle failure cases better
    }

    // sort the NP values and choose
    psVector *listNP = psVectorAlloc (nPix*nPix, PS_TYPE_U32);
    int n = 0;
    for (int i = 0; i < nPix; i++) {
        for (int j = 0; j < nPix; j++) {
            listNP->data.U32[n] = gridNP->data.U32[j][i];
            n++;
        }
    }
    psVector *sort = psVectorSort (NULL, listNP);
    stats->nTest = sort->data.U32[sort->n - 5];
    stats->nSigma = (stats->nMatch - stats->nTest) / sqrt(stats->nTest);
    // XXX this needs a better analysis of the image histogram..
    // fprintf (stderr, "sigma: nMatch: %d, nTest: %d, nTen: %d\n", stats->nMatch, stats->nTest, sort->data.U32[sort->n - 10]);

    psFree (sort);
    psFree (listNP);
    psFree (gridNP);
    psFree (gridDX);
    psFree (gridDY);
    psFree (gridD2);
    return (stats);
}



/******************************************************************************
pmAstromGridMatch(*raw, *ref, *config): match two star lists.
 ******************************************************************************/

pmAstromStats *pmAstromGridMatch(const psArray *raw,
                                 const psArray *ref,
                                 const psMetadata *config)
{
    PS_ASSERT_PTR_NON_NULL(raw, NULL);
    PS_ASSERT_PTR_NON_NULL(ref, NULL);
    PS_ASSERT_PTR_NON_NULL(config, NULL);

    bool status;
    double xMin, xMax, yMin, yMax;
    const pmAstromObj *obj;
    psArray *rot;

    pmAstromStats *minStat = pmAstromStatsAlloc ();
    pmAstromStats *newStat = NULL;

    psPlane center;

    // find center of the raw field (focal-plane coords)
    xMin = yMin = +1e10;
    xMax = yMax = -1e10;
    for (int i = 0; i < raw->n; i++) {
        obj = (pmAstromObj *)raw->data[i];
        xMin = PS_MIN (obj->FP->x, xMin);
        xMax = PS_MAX (obj->FP->x, xMax);
        yMin = PS_MIN (obj->FP->y, yMin);
        yMax = PS_MAX (obj->FP->y, yMax);
    }
    center.x = 0.5*(xMin + xMax);
    center.y = 0.5*(yMin + yMax);

    double minScale = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MIN.SCALE");
    double maxScale = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MAX.SCALE");
    double delScale = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.DEL.SCALE");

    double minAngle = RAD_DEG*psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MIN.ANGLE");
    double maxAngle = RAD_DEG*psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MAX.ANGLE");
    double delAngle = RAD_DEG*psMetadataLookupF32 (&status, config, "PSASTRO.GRID.DEL.ANGLE");
    double minSigma = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MIN.SIGMA");

    minStat->minMetric = 1e10;
    for (double scale = minScale; scale <= maxScale; scale += delScale) {
	for (double angle = minAngle; angle <= maxAngle; angle += delAngle) {
	    rot = pmAstromRotateObj (raw, center, angle, scale);

# if 0
	    FILE *f1 = fopen ("raw.dat", "w");
	    for (int i = 0; i < rot->n; i++) {
		pmAstromObj *obj = rot->data[i];
		fprintf (f1, "%8.2f %8.2f   %6.2f\n", obj->FP->x, obj->FP->y, obj->Mag);
	    }
	    fclose (f1);
	    FILE *f2 = fopen ("ref.dat", "w");
	    for (int i = 0; i < ref->n; i++) {
		pmAstromObj *obj = ref->data[i];
		fprintf (f2, "%8.2f %8.2f   %6.2f\n", obj->FP->x, obj->FP->y, obj->Mag);
	    }
	    fclose (f2);
	    fprintf (stderr, "type return");
	    char c;
	    fscanf (stdin, "%c", &c);
# endif

	    newStat = pmAstromGridAngle (rot, ref, config);
	    newStat->angle  = angle;
	    newStat->scale  = scale;
	    newStat->center = center;

	    if (isfinite(newStat->minMetric) && (newStat->minMetric < minStat->minMetric)) {
		*minStat = *newStat;
		psLogMsg ("psModule.astrom", 4, "grid test - offset: %7.2f,%7.2f @ %6.1f deg x %7.3f (%4d pts, %5.1f sig, %5.1f var, %6.3f log metric) *",
			  minStat->offset.x, minStat->offset.y, DEG_RAD*minStat->angle, minStat->scale, minStat->nMatch, minStat->nSigma, minStat->minVar, log10(minStat->minMetric));
	    } else {
		psLogMsg ("psModule.astrom", 4, "grid test - offset: %7.2f,%7.2f @ %6.1f deg x %7.3f (%4d pts, %5.1f sig, %5.1f var, %6.3f log metric)",
			  newStat->offset.x, newStat->offset.y, DEG_RAD*newStat->angle, newStat->scale, newStat->nMatch, newStat->nSigma, newStat->minVar, log10(newStat->minMetric));

	    }
	    psFree (newStat);
	    psFree (rot);
	}
    }
    psLogMsg ("psModule.astrom.grid.match", 4, "grid best - offset: %7.2f,%7.2f @ %6.1f deg x %7.3f (%4d pts, %5.1f sig, %5.1f var, %6.3f log metric)",
              minStat->offset.x, minStat->offset.y, DEG_RAD*minStat->angle, minStat->scale, minStat->nMatch, minStat->nSigma, minStat->minVar, log10(minStat->minMetric));

    // I need to decide if a solution is likely to be a good solution or just a mis-match
    // one posibility: how significant is the peak relative to the 4th or 5th most significant pixel?

    if (minStat->nSigma < minSigma) {
        psError(PS_ERR_UNKNOWN, true, "Failed to find a valid match (%f sigma for best)", minStat->nSigma);
        psFree (minStat);
        return NULL;
    }
    return (minStat);
}

/******************************************************************************
pmAstromGridTweak(*raw, *ref, *recipe, stats): improve match for two star lists.
 ******************************************************************************/
pmAstromStats *pmAstromGridTweak(
    psArray *raw,
    psArray *ref,
    psMetadata *recipe,
    pmAstromStats *stats)
{
    bool status;
    pmAstromObj *ob1, *ob2;  // short-cut pointers to the objects
    double dX, dY;   // offset between a possible matched pair
    psArray *rot;
    int nBin, xBin, yBin;

    rot = pmAstromRotateObj (raw, stats->center, stats->angle, stats->scale);

    // sampling scale of the grid
    double tweakScale  = psMetadataLookupF32 (&status, recipe, "PSASTRO.TWEAK.SCALE");
    double tweakRange  = psMetadataLookupF32 (&status, recipe, "PSASTRO.TWEAK.RANGE");
    double tweakSmooth = psMetadataLookupF32 (&status, recipe, "PSASTRO.TWEAK.SMOOTH");
    double tweakNsigma = psMetadataLookupF32 (&status, recipe, "PSASTRO.TWEAK.NSIGMA");

    nBin = 2*tweakRange / tweakScale;
    psVector *xHist = psVectorAlloc (nBin, PS_TYPE_F32);
    psVector *yHist = psVectorAlloc (nBin, PS_TYPE_F32);
    psVectorInit (xHist, 0);
    psVectorInit (yHist, 0);

    // accumulate grids for focal plane (L,M) matches
    for (int i = 0; i < rot->n; i++) {
        ob1 = (pmAstromObj *)rot->data[i];
        for (int j = 0; j < ref->n; j++) {
            ob2 = (pmAstromObj *)ref->data[j];
            dX = ob1->FP->x - ob2->FP->x - stats->offset.x;
            dY = ob1->FP->y - ob2->FP->y - stats->offset.y;

            xBin = (dX + tweakRange) / tweakScale;
            yBin = (dY + tweakRange) / tweakScale;

            if (xBin < 0)
                continue;
            if (yBin < 0)
                continue;
            if (xBin >= nBin)
                continue;
            if (yBin >= nBin)
                continue;

            xHist->data.F32[xBin] += 1.0;
            yHist->data.F32[yBin] += 1.0;
        }
    }

    // smooth histgram vector with gaussian of 1sigma = radius
    psVector *xHistNew = psVectorSmooth(NULL, xHist, tweakSmooth, tweakNsigma);
    psVector *yHistNew = psVectorSmooth(NULL, yHist, tweakSmooth, tweakNsigma);
    psFree(xHist);
    psFree(yHist);
    xHist = xHistNew;
    yHist = yHistNew;

    // select peak in x and in y
    xBin = yBin = 0;
    double xMax = 0;
    double yMax = 0;
    for (int i = 0; i < nBin; i++) {
        if (xHist->data.F32[i] > xMax) {
            xBin = i;
            xMax = xHist->data.F32[i];
        }
        if (yHist->data.F32[i] > yMax) {
            yBin = i;
            yMax = yHist->data.F32[i];
        }
    }
    double xPeak = xBin*tweakScale - tweakRange;
    double yPeak = yBin*tweakScale - tweakRange;
    psLogMsg (__func__, 3, "tweak peak by %f,%f\n", xPeak, yPeak);

    // adjust offset by peak center
    pmAstromStats *tweak = pmAstromStatsAlloc();
    *tweak = *stats;
    tweak->offset.x += xPeak;
    tweak->offset.y += yPeak;

    psFree (rot);
    psFree (xHist);
    psFree (yHist);

    return tweak;
}

/******************************************************************************
pmAstromGridApply(*map, stat): apply the measured FPA offset and rotation
(stat) to the fpa astrom structures.
 ******************************************************************************/
psPlaneTransform *pmAstromGridApply(
    psPlaneTransform *map,
    pmAstromStats *stat)
{
    PS_ASSERT_PTR_NON_NULL(map, NULL);
    PS_ASSERT_POLY_NON_NULL(map->x, NULL);
    PS_ASSERT_POLY_NON_NULL(map->y, NULL);

    double cs = stat->scale * cos (stat->angle);
    double sn = stat->scale * sin (stat->angle);

    double dx = (map->x->coeff[0][0] - stat->center.x);
    double dy = (map->y->coeff[0][0] - stat->center.y);

    // new offset
    map->x->coeff[0][0] =  cs*dx + sn*dy - stat->offset.x + stat->center.x;
    map->y->coeff[0][0] = -sn*dx + cs*dy - stat->offset.y + stat->center.y;

    // original rotation matrix
    double pc1_1 = map->x->coeff[1][0];
    double pc1_2 = map->x->coeff[0][1];
    double pc2_1 = map->y->coeff[1][0];
    double pc2_2 = map->y->coeff[0][1];

    // new rotation matrix
    map->x->coeff[1][0] = +cs*pc1_1 + sn*pc2_1;
    map->x->coeff[0][1] = +cs*pc1_2 + sn*pc2_2;
    map->y->coeff[1][0] = -sn*pc1_1 + cs*pc2_1;
    map->y->coeff[0][1] = -sn*pc1_2 + cs*pc2_2;

    return (map);
}

/* Illustration of the grid bins
   dX        bin
   -35:-25 -> 0     bin = dX / Scale + Offset
   -25:-15 -> 1     Scale = 10
   -15:-05 -> 2     Offset = 3.5
   -05:+05 -> 3     nPix = 3 (maxOffset = 35 = (nPix + 0.5)*dXix
   +05:+15 -> 4     dPix = 10
   +15:+25 -> 5
   +25:+35 -> 6

   maxOffsetRequest = 30
   nPix = (int) (maxOffset / dPix + 0.5);
   maxOffset = (nPix + 0.5)*Scale;
*/

