/** @file  pmSource.c
 *
 *  Functions to define and manipulate sources on images
 *
 *  @author GLG, MHPCC
 *  @author EAM, IfA: significant modifications.
 *
 *  @version $Revision: 1.8 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2009-02-06 02:31:25 $
 *
 *  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 "pmSpan.h"
#include "pmFootprint.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"

/******************************************************************************
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->variance (optional)
    pmSource->mask (optional)

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

this version optionally clips input pixels on S/N 
*****************************************************************************/
# define VALID_RADIUS(X,Y,RAD2) (((RAD2) >= (PS_SQR(X) + PS_SQR(Y))) ? 1 : 0)

bool pmSourceMoments(pmSource *source, psF32 radius, psF32 sigma, psF32 minSN, psImageMaskType maskVal)
{
    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);

    // use sky from moments if defined, 0.0 otherwise
    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);
    psF32 minSN2 = PS_SQR(minSN);
    psF32 rsigma2 = 0.5 / PS_SQR(sigma);

    // 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.  since these values are subtracted just to minimize
    // the dynamic range and are added back below, the exact value does not matter. these are
    // (int) so they can be used in the image index below.

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

    // we are guaranteed to have a valid pixel and variance at this location (right? right?)
    // psF32 weightNorm = source->pixels->data.F32[yPeak][xPeak] / sqrt (source->variance->data.F32[yPeak][xPeak]);
    // psAssert (isfinite(source->pixels->data.F32[yPeak][xPeak]), "peak must be on valid pixel");
    // psAssert (isfinite(source->variance->data.F32[yPeak][xPeak]), "peak must be on valid pixel");
    // psAssert (source->variance->data.F32[yPeak][xPeak] > 0, "peak must be on valid pixel");

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

        psF32 yDiff = row - yPeak;
	if (fabs(yDiff) > radius) continue;

        psF32 *vPix = source->pixels->data.F32[row];
        psF32 *vWgt = source->variance->data.F32[row];
        psImageMaskType *vMsk = (source->maskObj == NULL) ? NULL : source->maskObj->data.PS_TYPE_IMAGE_MASK_DATA[row];

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

            psF32 xDiff = col - xPeak;
	    if (fabs(xDiff) > radius) continue;

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

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

	    // skip pixels below specified significance level.  this is allowed, but should be
	    // avoided -- the over-weights the wings of bright stars compared to those of faint
	    // stars.
            if (PS_SQR(pDiff) < minSN2*wDiff) continue;
            // if (pDiff < 0) continue; // XXX : MWV says I should include < 0.0 valued points...

	    // Apply a Gaussian window function.  Be careful with the window function.  S/N
	    // weighting over weights the sky for faint sources
	    if (sigma > 0.0) {
		// XXX a lot of extra flops; can we pre-calculate?
		psF32 z  = (PS_SQR(xDiff) + PS_SQR(yDiff))*rsigma2;
		assert (z >= 0.0);
		psF32 weight  = exp(-z);

		wDiff *= weight;
		pDiff *= weight;
	    } 

	    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
    // also offset from pixel index to pixel coordinate
    // (the calculation above uses pixel index instead of coordinate)
    // 0.5 PIX: moments are calculated using the pixel index and converted here to pixel coords
    source->moments->Mx = Mx + xOff + 0.5;
    source->moments->My = My + yOff + 0.5;

    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.  Note: the calculation below uses pixel index, so we do not
    // correct xCM, yCM to pixel coordinate here.
    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 yDiff = row - yCM;
	if (fabs(yDiff) > radius) continue;

	psF32 *vPix = source->pixels->data.F32[row];
	psF32 *vWgt = source->variance->data.F32[row];
	psImageMaskType  *vMsk = (source->maskObj == NULL) ? NULL : source->maskObj->data.PS_TYPE_IMAGE_MASK_DATA[row];

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

	    psF32 xDiff = col - xCM;
	    if (fabs(xDiff) > radius) continue;

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

	    // skip pixels below specified significance level.  this is allowed, but should be
	    // avoided -- the over-weights the wings of bright stars compared to those of faint
	    // stars.
	    if (PS_SQR(pDiff) < minSN2*wDiff) continue;
	    // if (pDiff < 0) continue;

	    // Apply a Gaussian window function.  Be careful with the window function.  S/N
	    // weighting over weights the sky for faint sources
	    if (sigma > 0.0) {
		// XXX a lot of extra flops; can we do pre-calculate?
		psF32 z  = (PS_SQR(xDiff) + PS_SQR(yDiff))*rsigma2;
		assert (z >= 0.0);
		psF32 weight  = exp(-z);

		wDiff *= weight;
		pDiff *= weight;
	    } 

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

    return(true);
}
