/** @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.1.14.1 $ $Name: not supported by cvs2svn $
*  @date $Date: 2006-02-17 17:13:41 $
*
*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
*/

/******************************************************************************/
/*  INCLUDE FILES                                                             */
/******************************************************************************/
#include <string.h>
#include <math.h>
#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?
 ******************************************************************************/

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



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

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

            j++;
        }
        j = jStart;
        i++;
    }
    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);

    // take the matched stars, first fit
    for (int i = 0; i < match->n; i++) {

        pair    = match->data[i];
        rawStar = raw->data[pair->i1];
        refStar = ref->data[pair->i2];

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

    // no masking, no errors
    psVectorClipFitPolynomial2D (map->x, stats, NULL, 0, X, wt, x, y);
    psVectorClipFitPolynomial2D (map->y, stats, NULL, 0, Y, wt, x, y);
    psFree (x);
    psFree (y);
    psFree (X);
    psFree (Y);
    psFree (wt);
    psFree (stats);

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

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

    return;
}


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

    obj->pix.x = 0;
    obj->pix.y = 0;

    obj->FP.x = 0;
    obj->FP.y = 0;

    obj->TP.x = 0;
    obj->TP.y = 0;

    obj->sky.r = 0;
    obj->sky.d = 0;

    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[0] = old[0];

    return(obj);
}


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


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

    match->i1 = i1;
    match->i2 = i2;

    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 emphasize 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(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];

        // XXX EAM : This routine, and pmAstromGridMatch, need to handle failure cases better
    }
    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);
        newStat.angle  = angle;
        newStat.center = center;
        if (newStat.minMetric < minStat.minMetric) {
            minStat = newStat;
        }
        psFree (rot);
    }
    fprintf (stderr, "best: %f %f (%d pts, %f var, %f met)\n", newStat.offset.x, newStat.offset.y, newStat.nMatch, newStat.minVar, newStat.minMetric);
    return (minStat);
}


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

