/** @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.9 $ $Name: not supported by cvs2svn $
*  @date $Date: 2006-06-21 03:21:16 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*/

/******************************************************************************/
/*  INCLUDE FILES                                                             */
/******************************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <unistd.h>   // for unlink
#include <pslib.h>
#include "pmFPA.h"
#include "pmAstrometryObjects.h"

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

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

    return (0);
}


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

        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;
            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++;
    }
    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)
{
    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;

    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++;
    }
    psLogMsg (__func__, 3, "radius match: %d pairs (radius: %f)\n", matches->n, RADIUS);
    return (matches);
}



/******************************************************************************
pmAstromMatchFit(map, raw, ref, match, config): take two matched star lists
and fit a psPlaneTransform between them
 ******************************************************************************/
psPlaneTransform *pmAstromMatchFit(
    psPlaneTransform *map,
    psArray *raw,
    psArray *ref,
    psArray *match,
    psMetadata *config)
{
    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(config, NULL);

    bool status;
    pmAstromObj *rawStar, *refStar;
    pmAstromMatch *pair;

    if (map == NULL) {
        int nX = psMetadataLookupS32 (&status, config, "PSASTRO.CHIP.NX");
        if (!status) {
            nX = 1;
        }
        int nY = psMetadataLookupS32 (&status, config, "PSASTRO.CHIP.NY");
        if (!status) {
            nY = 1;
        }
        map = psPlaneTransformAlloc (nX, nY);
        // XXX EAM : not clear that we are allowed to use orders > 1
    }

    psStats *stats = psStatsAlloc (PS_STAT_CLIPPED_MEAN | PS_STAT_CLIPPED_STDEV);
    stats->clipSigma = psMetadataLookupF32 (&status, config, "PSASTRO.CHIP.NSIGMA");
    stats->clipIter  = psMetadataLookupS32 (&status, config, "PSASTRO.CHIP.NITER");

    // XXX EAM : clip fit seems to only work for F32!
    // XXX EAM : clip fit fails to handle NULL 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);
    X->n = X->nalloc;
    Y->n = Y->nalloc;
    x->n = x->nalloc;
    y->n = y->nalloc;
    wt->n = wt->nalloc;
    // take the matched stars, first fit
    for (int i = 0; i < match->n; i++) {

        pair    = match->data[i];
        rawStar = raw->data[pair->raw];
        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 *xMask = psVectorAlloc (match->n, PS_TYPE_U8);
    psVector *yMask = psVectorAlloc (match->n, PS_TYPE_U8);
    xMask->n = yMask->n = match->n;
    psVectorInit (xMask, 0);
    psVectorInit (yMask, 0);

    // fit chip-to-FPA transformation
    psVectorClipFitPolynomial2D (map->x, stats, xMask, 0, x, wt, X, Y);
    psLogMsg ("psModules.astrom", 3, "x resid: %f +/- %f (%d of %d)\n", stats->clippedMean, stats->clippedStdev, stats->clippedNvalues, x->n);

    psVectorClipFitPolynomial2D (map->y, stats, yMask, 0, y, wt, X, Y);
    psLogMsg ("psModules.astrom", 3, "y resid: %f +/- %f (%d of %d)\n", stats->clippedMean, stats->clippedStdev, stats->clippedNvalues, y->n);

    psFree (x);
    psFree (y);
    psFree (X);
    psFree (Y);
    psFree (wt);
    psFree (stats);
    psFree (xMask);
    psFree (yMask);

    return (map);
}


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

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

    psArray *new = psArrayAlloc (old->n);
    new->n = new->nalloc;
    double cs = cos(angle);
    double sn = 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);
}



/******************************************************************************
pmAstromObjFree(obj)
 ******************************************************************************/
static void pmAstromObjFree(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) pmAstromObjFree);

    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);
}


/******************************************************************************
pmAstromObjCopy(old)
 ******************************************************************************/
pmAstromObj *pmAstromObjCopy(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;

    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 (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(
    psArray *raw,
    psArray *ref,
    psMetadata *config)
{
    // XXX: What to do if input parameters are bad?
    pmAstromStats badStat;
    PS_ASSERT_PTR_NON_NULL(raw, badStat);
    PS_ASSERT_PTR_NON_NULL(ref, badStat);
    PS_ASSERT_PTR_NON_NULL(config, badStat);

    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

    pmAstromObj *ob1, *ob2; // short-cut pointers to the objects
    pmAstromStats stats;    // 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
    psS32 **NP = gridNP->data.S32;
    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 (stderr, "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);
        imStats = psImageStats (imStats, gridNP, NULL, 0);

        // 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));

        // 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
        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
    }
    psFree (gridNP);
    psFree (gridDX);
    psFree (gridDY);
    psFree (gridD2);
    return (stats);
}



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

pmAstromStats pmAstromGridMatch(
    psArray *raw,
    psArray *ref,
    psMetadata *config)
{
    // XXX: What to do if input parameters are bad?
    pmAstromStats badStat;
    PS_ASSERT_PTR_NON_NULL(raw, badStat);
    PS_ASSERT_PTR_NON_NULL(ref, badStat);
    PS_ASSERT_PTR_NON_NULL(config, badStat);

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

    pmAstromStats minStat;
    pmAstromStats newStat = {{0, 0, 0, 0}, {0, 0, 0, 0}, 0, 0, 0, 0};
    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 minAngle = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MIN.ANGLE");
    double maxAngle = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.MAX.ANGLE");
    double delAngle = psMetadataLookupF32 (&status, config, "PSASTRO.GRID.DEL.ANGLE");

    minStat.minMetric = 1e10;
    for (double angle = minAngle; angle <= maxAngle; angle += delAngle) {
        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));

        newStat.angle  = angle;
        newStat.center = center;
        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));
        }
        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));
    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);

    // 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);
    xHist->n = yHist->n = nBin;
    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
    psVectorSmooth(xHist, xHist, tweakSmooth, tweakNsigma);
    psVectorSmooth(yHist, yHist, tweakSmooth, tweakNsigma);

    // 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 = 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 = cos (stat.angle);
    double sn = 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;
*/

