/** @file  pmSource.c
 *
 *  Functions to define and manipulate sources on images
 *
 *  @author GLG, MHPCC
 *  @author EAM, IfA: significant modifications.
 *
 *  @version $Revision: 1.5 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2008-10-07 20:06:42 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 *
 */

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

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <strings.h>
#include <pslib.h>
#include "pmHDU.h"
#include "pmFPA.h"
#include "pmFPAMaskWeight.h"
#include "pmPeaks.h"
#include "pmMoments.h"
#include "pmResiduals.h"
#include "pmGrowthCurve.h"
#include "pmTrend2D.h"
#include "pmPSF.h"
#include "pmModel.h"
#include "pmSource.h"

bool pmSourceMoments_Old(pmSource *source, psF32 radius);

/******************************************************************************
pmSourceMoments(source, radius): this function takes a subImage defined in the
pmSource data structure, along with the peak location, and determines the
various moments associated with that peak.

Uses the following elements:
    pmSource
    pmSource->peak
    pmSource->pixels
    pmSource->weight (optional)
    pmSource->mask (optional)

XXX: The peak calculations are done in image coords, not subImage coords.

this version clips input pixels on S/N
XXX EAM : this version returns false for several reasons
*****************************************************************************/
# define VALID_RADIUS(X,Y,RAD2) (((RAD2) >= (PS_SQR(X) + PS_SQR(Y))) ? 1 : 0)

bool pmSourceMoments(pmSource *source, psF32 radius)
{
    PS_ASSERT_PTR_NON_NULL(source, false);
    PS_ASSERT_PTR_NON_NULL(source->peak, false);
    PS_ASSERT_PTR_NON_NULL(source->pixels, false);
    PS_ASSERT_FLOAT_LARGER_THAN(radius, 0.0, false);

    // XXX supply sky in a different way?
    psF32 sky = 0.0;
    if (source->moments == NULL) {
        source->moments = pmMomentsAlloc();
    } else {
        sky = source->moments->Sky;
    }

    // First Pass: calculate the first moments (these are subtracted from the coordinates below)
    // Sum = SUM (z - sky)
    // X1  = SUM (x - xc)*(z - sky)
    // .. etc

    psF32 peakPixel = -PS_MAX_F32;
    psS32 numPixels = 0;
    psF32 Sum = 0.0;
    psF32 Var = 0.0;
    psF32 X1 = 0.0;
    psF32 Y1 = 0.0;
    psF32 R2 = PS_SQR(radius);

    // a note about coordinates: coordinates of objects throughout psphot refer to the primary
    // image coordinates.  the source->pixels image has an offset relative to its parent of
    // col0,row0: a pixel (x,y) in the primary image has coordinates of (x-col0, y-row0) in
    // this subimage.  we subtract off the peak coordinates, adjusted to this subimage, to have
    // minimal round-off error in the sums

    psF32 xOff  = source->peak->x;
    psF32 yOff  = source->peak->y;
    psF32 xPeak = source->peak->x - source->pixels->col0; // coord of peak in subimage
    psF32 yPeak = source->peak->y - source->pixels->row0; // coord of peak in subimage

    for (psS32 row = 0; row < source->pixels->numRows ; row++) {

        psF32 *vPix = source->pixels->data.F32[row];
        psF32 *vWgt = source->weight->data.F32[row];
        psU8  *vMsk = (source->maskObj == NULL) ? NULL : source->maskObj->data.U8[row];

        for (psS32 col = 0; col < source->pixels->numCols ; col++, vPix++, vWgt++) {
            if (vMsk) {
                if (*vMsk) {
                    vMsk++;
                    continue;
                }
                vMsk++;
            }
	    if (isnan(*vPix)) continue;

            psF32 xDiff = col - xPeak;
            psF32 yDiff = row - yPeak;

            // radius is just a function of (xDiff, yDiff)
            if (!VALID_RADIUS(xDiff, yDiff, R2)) continue;

            psF32 pDiff = *vPix - sky;
            psF32 wDiff = *vWgt;

            // XXX EAM : should this limit be user-defined?
            if (PS_SQR(pDiff) < wDiff) continue;

            Var += wDiff;
            Sum += pDiff;

            psF32 xWght = xDiff * pDiff;
            psF32 yWght = yDiff * pDiff;

            X1  += xWght;
            Y1  += yWght;

            peakPixel = PS_MAX (*vPix, peakPixel);
            numPixels++;
        }
    }

    // if we have less than (1/2) of the possible pixels, force a retry
    // XXX EAM - the limit is a bit arbitrary.  make it user defined?
    if ((numPixels < 0.75*R2) || (Sum <= 0)) {
        psTrace ("psModules.objects", 3, "insufficient valid pixels (%d vs %d; %f) for source\n", numPixels, (int)(0.75*R2), Sum);
        return (false);
    }

    // calculate the first moment.
    float Mx = X1/Sum;
    float My = Y1/Sum;
    if ((fabs(Mx) > radius) || (fabs(My) > radius)) {
        psTrace ("psModules.objects", 3, "large centroid swing; invalid peak %d, %d\n", source->peak->x, source->peak->y);
        return (false);
    }

    psTrace ("psModules.objects", 5, "sky: %f  Mx: %f  My: %f  Sum: %f  X1: %f  Y1: %f  Npix: %d\n", sky, Mx, My, Sum, X1, Y1, numPixels);

    // add back offset of peak in primary image
    source->moments->Mx = Mx + xOff;
    source->moments->My = My + yOff;

    source->moments->Sum = Sum;
    source->moments->SN  = Sum / sqrt(Var);
    source->moments->Peak = peakPixel;
    source->moments->nPixels = numPixels;

    // Now calculate higher-order moments, using the above-calculated first moments to adjust coordinates
    // Xn  = SUM (x - xc)^n * (z - sky)

    psF32 XX = 0.0;
    psF32 XY = 0.0;
    psF32 YY = 0.0;
    psF32 XXX = 0.0;
    psF32 XXY = 0.0;
    psF32 XYY = 0.0;
    psF32 YYY = 0.0;
    psF32 XXXX = 0.0;
    psF32 XXXY = 0.0;
    psF32 XXYY = 0.0;
    psF32 XYYY = 0.0;
    psF32 YYYY = 0.0;

    Sum = 0.0;  // the second pass may include slightly different pixels, re-determine Sum

    // center of mass in subimage
    psF32 xCM = Mx + xPeak; // coord of peak in subimage
    psF32 yCM = My + yPeak; // coord of peak in subimage

    for (psS32 row = 0; row < source->pixels->numRows ; row++) {

        psF32 *vPix = source->pixels->data.F32[row];
        psF32 *vWgt = source->weight->data.F32[row];
        psU8  *vMsk = (source->maskObj == NULL) ? NULL : source->maskObj->data.U8[row];

        for (psS32 col = 0; col < source->pixels->numCols ; col++, vPix++, vWgt++) {
            if (vMsk) {
                if (*vMsk) {
                    vMsk++;
                    continue;
                }
                vMsk++;
            }
	    if (isnan(*vPix)) continue;

            psF32 xDiff = col - xCM;
            psF32 yDiff = row - yCM;

            // radius is just a function of (xDiff, yDiff)
	    psF32 r2  = PS_SQR(xDiff) + PS_SQR(yDiff);
	    psF32 r  = sqrt(r2);
            if (r > radius) continue;

            psF32 pDiff = *vPix - sky;
            psF32 wDiff = *vWgt;

            // XXX EAM : should this limit be user-defined?

            if (PS_SQR(pDiff) < wDiff) continue;
            if (pDiff < 0) continue;

            Sum += pDiff;

            psF32 x = xDiff * pDiff;
            psF32 y = yDiff * pDiff;

            psF32 xx = xDiff * x;
            psF32 xy = xDiff * y;
            psF32 yy = yDiff * y;

            psF32 xxx = xDiff * xx / r;
            psF32 xxy = xDiff * xy / r;
            psF32 xyy = xDiff * yy / r;
            psF32 yyy = yDiff * yy / r;

            psF32 xxxx = xDiff * xxx / r2;
            psF32 xxxy = xDiff * xxy / r2;
            psF32 xxyy = xDiff * xyy / r2;
            psF32 xyyy = xDiff * yyy / r2;
            psF32 yyyy = yDiff * yyy / r2;

            XX  += xx;
            XY  += xy;
            YY  += yy;

            XXX  += xxx;
            XXY  += xxy;
            XYY  += xyy;
            YYY  += yyy;

            XXXX  += xxxx;
            XXXY  += xxxy;
            XXYY  += xxyy;
            XYYY  += xyyy;
            YYYY  += yyyy;
        }
    }

    source->moments->Mxx = XX/Sum;
    source->moments->Mxy = XY/Sum;
    source->moments->Myy = YY/Sum;

    source->moments->Mxxx = XXX/Sum;
    source->moments->Mxxy = XXY/Sum;
    source->moments->Mxyy = XYY/Sum;
    source->moments->Myyy = YYY/Sum;

    source->moments->Mxxxx = XXXX/Sum;
    source->moments->Mxxxy = XXXY/Sum;
    source->moments->Mxxyy = XXYY/Sum;
    source->moments->Mxyyy = XYYY/Sum;
    source->moments->Myyyy = YYYY/Sum;

    if (source->moments->Mxx < 0) {
      fprintf (stderr, "error: neg second moment??\n");
    }
    if (source->moments->Myy < 0) {
      fprintf (stderr, "error: neg second moment??\n");
    }

    psTrace ("psModules.objects", 4, "Mxx: %f  Mxy: %f  Myy: %f  Mxxx: %f  Mxxy: %f  Mxyy: %f  Myyy: %f  Mxxxx: %f  Mxxxy: %f  Mxxyy: %f  Mxyyy: %f  Mxyyy: %f\n",
             source->moments->Mxx,   source->moments->Mxy,   source->moments->Myy, 
             source->moments->Mxxx,  source->moments->Mxxy,  source->moments->Mxyy,  source->moments->Myyy,
             source->moments->Mxxxx, source->moments->Mxxxy, source->moments->Mxxyy, source->moments->Mxyyy, source->moments->Myyyy);

    psTrace ("psModules.objects", 3, "peak %f %f (%f = %f) Mx: %f  My: %f  Sum: %f  Mxx: %f  Mxy: %f  Myy: %f  sky: %f  Npix: %d\n",
             source->peak->xf, source->peak->yf, source->peak->flux, source->peak->SN, source->moments->Mx,   source->moments->My, Sum, source->moments->Mxx,   source->moments->Mxy,   source->moments->Myy, sky, numPixels);

    if (source->moments->Mxx < 0) {
	fprintf (stderr, "error: neg second moment??\n");
    }
    if (source->moments->Myy < 0) {
	fprintf (stderr, "error: neg second moment??\n");
    }

    // XXX TEST:
    // pmSourceMoments_Old (source, radius);
    return(true);
}


bool pmSourceMoments_Old(pmSource *source, psF32 radius)
{
    PS_ASSERT_PTR_NON_NULL(source, false);
    PS_ASSERT_PTR_NON_NULL(source->peak, false);
    PS_ASSERT_PTR_NON_NULL(source->pixels, false);
    PS_ASSERT_FLOAT_LARGER_THAN(radius, 0.0, false);

    psF32 sky = 0.0;
    if (source->moments == NULL) {
        source->moments = pmMomentsAlloc();
    } else {
        sky = source->moments->Sky;
    }

    // Sum = SUM (z - sky)
    // X1  = SUM (x - xc)*(z - sky)
    // X2  = SUM (x - xc)^2 * (z - sky)
    // XY  = SUM (x - xc)*(y - yc)*(z - sky)
    psF32 peakPixel = -PS_MAX_F32;
    psS32 numPixels = 0;
    psF32 Sum = 0.0;
    psF32 Var = 0.0;
    psF32 X1 = 0.0;
    psF32 Y1 = 0.0;
    psF32 X2 = 0.0;
    psF32 Y2 = 0.0;
    psF32 XY = 0.0;
    psF32 x  = 0;
    psF32 y  = 0;
    psF32 R2 = PS_SQR(radius);

    // a note about coordinates: coordinates of objects throughout psphot refer to the primary
    // image coordinates.  the source->pixels image has an offset relative to its parent of
    // col0,row0: a pixel (x,y) in the primary image has coordinates of (x-col0, y-row0) in
    // this subimage.  we subtract off the peak coordinates, adjusted to this subimage, to have
    // minimal round-off error in the sums

    psF32 xPeak = source->peak->x - source->pixels->col0; // coord of peak in subimage
    psF32 yPeak = source->peak->y - source->pixels->row0; // coord of peak in subimage

    for (psS32 row = 0; row < source->pixels->numRows ; row++) {

        psF32 *vPix = source->pixels->data.F32[row];
        psF32 *vWgt = source->weight->data.F32[row];
        psU8  *vMsk = (source->maskObj == NULL) ? NULL : source->maskObj->data.U8[row];

        for (psS32 col = 0; col < source->pixels->numCols ; col++, vPix++, vWgt++) {
            if (vMsk) {
                if (*vMsk) {
                    vMsk++;
                    continue;
                }
                vMsk++;
            }
	    if (isnan(*vPix)) continue;

            psF32 xDiff = col - xPeak;
            psF32 yDiff = row - yPeak;

            // radius is just a function of (xDiff, yDiff)
            if (!VALID_RADIUS(xDiff, yDiff, R2)) continue;

            psF32 pDiff = *vPix - sky;
            psF32 wDiff = *vWgt;

            // XXX EAM : should this limit be user-defined?
            if (PS_SQR(pDiff) < wDiff) continue;

            Var += wDiff;
            Sum += pDiff;

            psF32 xWght = xDiff * pDiff;
            psF32 yWght = yDiff * pDiff;

            X1  += xWght;
            Y1  += yWght;

            X2  += xDiff * xWght;
            XY  += xDiff * yWght;
            Y2  += yDiff * yWght;

            peakPixel = PS_MAX (*vPix, peakPixel);
            numPixels++;
        }
    }

    // if we have less than (1/4) of the possible pixels, force a retry
    // XXX EAM - the limit is a bit arbitrary.  make it user defined?
    if ((numPixels < 0.75*R2) || (Sum <= 0)) {
        psTrace ("psModules.objects", 3, "insufficient valid pixels (%d vs %d; %f) for source\n", numPixels, (int)(0.75*R2), Sum);
        return (false);
    }

    psTrace ("psModules.objects", 4, "sky: %f  Sum: %f  X1: %f  Y1: %f  X2: %f  Y2: %f  XY: %f  Npix: %d\n",
             sky, Sum, X1, Y1, X2, Y2, XY, numPixels);

    //
    // first moment X  = X1/Sum + xc
    // second moment X = sqrt (X2/Sum - (X1/Sum)^2)
    // Sxy             = XY / Sum
    //
    x = X1/Sum;
    y = Y1/Sum;
    if ((fabs(x) > radius) || (fabs(y) > radius)) {
        psTrace ("psModules.objects", 3, "large centroid swing; invalid peak %d, %d\n",
                 source->peak->x, source->peak->y);
        return (false);
    }

# if (PS_TRACE_ON) 
    float Sxx = PS_MAX(X2/Sum - PS_SQR(x), 0);
    float Sxy = XY / Sum;
    float Syy = PS_MAX(Y2/Sum - PS_SQR(y), 0);

    psTrace ("psModules.objects", 3,
             "sky: %f  Sum: %f  x: %f  y: %f  Sx: %f  Sy: %f  Sxy: %f\n",
             sky, Sum, x, y, Sxx, Sxy, Syy);
# endif

    return(true);
}

