# include "psphotInternal.h"
# include <gsl/gsl_sf_gamma.h>

float psphotModelContour (psImage *image, psImage *weight, psImage *mask, pmModel *model, float Ro);

// we need to call this function after sources have been fitted to the PSF model and
// subtracted.  To determine the CR-nature, this function examines the 9 pixels in the 3x3
// square containing the peak and compares the observed flux to the model.  To determine
// the EXT-nature, this function measures the amount of positive or negative total
// deviation from the psf model at the r = FWHM/2 position

bool psphotSourceSize (pmReadout *readout, psArray *sources, psMetadata *recipe, long first) {

    bool status;

    psTimerStart ("psphot");

    float CR_NSIGMA_LIMIT = psMetadataLookupF32 (&status, recipe, "PSPHOT.CR.NSIGMA.LIMIT");
    assert (status);

    float EXT_NSIGMA_LIMIT = psMetadataLookupF32 (&status, recipe, "PSPHOT.EXT.NSIGMA.LIMIT");
    assert (status);

    // loop over all source
    for (int i = first; i < sources->n; i++) {
	pmSource *source = sources->data[i];

	// skip source if it was already measured
	if (isfinite(source->crNsigma)) continue;

	// source must have been subtracted
	if (!(source->mode & PM_SOURCE_MODE_SUBTRACTED)) continue;

	psF32 **resid  = source->pixels->data.F32;
	psF32 **weight = source->weight->data.F32;
	psU8 **mask    = source->maskObj->data.U8;

	// check for extendedness: measure the delta flux significance at the 1 sigma contour
	source->extNsigma = psphotModelContour (source->pixels, source->weight, source->maskObj, source->modelPSF, 1.0);

	// XXX prevent a source from being both CR and EXT?
	if (source->extNsigma > EXT_NSIGMA_LIMIT) {
	  source->mode |= PM_SOURCE_MODE_EXT_LIMIT;
	}

	int xPeak = source->peak->xf - source->pixels->col0 + 0.5;
	int yPeak = source->peak->yf - source->pixels->row0 + 0.5;

	// XXX for now, skip sources which are too close to a boundary
	// XXX raise a flag?
	if (xPeak < 1) continue;
	if (xPeak > source->pixels->numCols - 2) continue;
	if (yPeak < 1) continue;
	if (yPeak > source->pixels->numRows - 2) continue;

	// XXX for now, just skip any sources with masked pixels
	// XXX raise a flag?
	bool keep = true;
	for (int iy = -1; (iy <= +1) && keep; iy++) {
	    for (int ix = -1; (ix <= +1) && keep; ix++) {
		if (mask[yPeak+iy][xPeak+ix]) { keep &= false; }
	    }
	}
	if (!keep) continue;

	// XXX need to deal with edge peaks... and mask
	float cX = 2*resid[yPeak][xPeak] - resid[yPeak+0][xPeak-1] - resid[yPeak+0][xPeak+1];
	float cY = 2*resid[yPeak][xPeak] - resid[yPeak+1][xPeak+0] - resid[yPeak+1][xPeak+0];
	float cL = 2*resid[yPeak][xPeak] - resid[yPeak-1][xPeak-1] - resid[yPeak+1][xPeak+1];
	float cR = 2*resid[yPeak][xPeak] - resid[yPeak+1][xPeak-1] - resid[yPeak-1][xPeak+1];

	float dcX = 4*weight[yPeak][xPeak] + weight[yPeak+0][xPeak-1] + weight[yPeak+0][xPeak+1];
	float dcY = 4*weight[yPeak][xPeak] + weight[yPeak+1][xPeak+0] + weight[yPeak+1][xPeak+0];
	float dcL = 4*weight[yPeak][xPeak] + weight[yPeak-1][xPeak-1] + weight[yPeak+1][xPeak+1];
	float dcR = 4*weight[yPeak][xPeak] + weight[yPeak+1][xPeak-1] + weight[yPeak-1][xPeak+1];

	float nX = cX / sqrt(dcX);
	float nY = cY / sqrt(dcY);
	float nL = cL / sqrt(dcL);
	float nR = cR / sqrt(dcR);

	// P(chisq > chisq_obs; Ndof) = gamma_Q (Ndof/2, chisq/2)
	// Ndof = 4 ? (four measurements, no free parameters)
	// XXX this value is going to be biased low because of systematic errors.
	// we need to calibrate it somehow
	// source->psfProb = gsl_sf_gamma_inc_Q (2, 0.5*chisq);

	// not strictly accurate: overcounts the chisq contribution from the center pixel (by factor of 4)
	source->psfChisq = PS_SQR (nX) + PS_SQR (nY) + PS_SQR (nX) + PS_SQR (nR);

	float fCR = 0.0;
	float fEXT = 0.0;
	int nCR = 0;
	int nEXT = 0;
	if (nX > 0.0) {
	    fCR += nX;
	    nCR ++;
	} else {
	    fEXT += nX;
	    nEXT ++;
	}
	if (nY > 0.0) {
	    fCR += nY;
	    nCR ++;
	} else {
	    fEXT += nY;
	    nEXT ++;
	}
	if (nL > 0.0) {
	    fCR += nL;
	    nCR ++;
	} else {
	    fEXT += nL;
	    nEXT ++;
	}
	if (nR > 0.0) {
	    fCR += nR;
	    nCR ++;
	} else {
	    fEXT += nR;
	    nEXT ++;
	}
	source->crNsigma  = (nCR > 0)  ? fCR / nCR : 0.0;
	// NOTE: abs needed to make the Nsigma value positive 

	if (!isfinite(source->crNsigma)) {
	  fprintf (stderr, ".");
	  source->crNsigma = -6.0;
	}

	if (source->crNsigma > CR_NSIGMA_LIMIT) {
	  source->mode |= PM_SOURCE_MODE_CR_LIMIT;
	}
    }

    psLogMsg ("psphot.size", PS_LOG_INFO, "measure source sizes for %ld sources: %f sec\n", sources->n - first, psTimerMark ("psphot"));

    return true;
}


// some possible classes:
// all 4 curvatures are highly negative : CR
// all 4 curvatures are highly positive : EXT

// at least 2 are significantly negative, none are significantly positive : CR
// at least 2 are significantly positive, none are significantly negative : EXT

// any are significantly negative, some may be significantly positive : CR
// any are significantly positive, none may be significantly positive : EXT
	
/* Nn  Np  No
   4   0   0   CR_1
   3   1   0   CR_1
   3   0   1   CR_1
   2   2   0   CR_2
   2   1   1   CR_2
   2   0   2   CR_2
   1   3   0   CR_3
   1   2   1   CR_3
   1   1   2   CR_3
   1   0   3   CR_3
   0   4   0   EXT
   0   3   1   EXT
   0   2   2   EXT
   0   1   3   PSF
   0   0   4   PSF
*/

/* Alternatively, write a f(CR) = Sum(nX,etc if >0) */
	  

/* I can write the formal probability that the 4 measurements are consistent with a PSF
 * based on how large the error values are (Nsigma -> erf -> P(Nsigma)).
 * I should examine this value as a function of flux and also examine the distribution of
 * the probabilities for the PSF sources.
 */


// given the PSF ellipse parameters, navigate around the 1sigma contour, return the total
// deviation in sigmas.  This is measure on the residual image - should we ignore negative
// deviations?
float psphotModelContour (psImage *image, psImage *weight, psImage *mask, pmModel *model, float Ro) {

    psF32 *PAR = model->params->data.F32;

    // Ro = (x / SXX)^2 + (y / SYY)^2 + x y SXY;
    // y^2 (1/SYY^2) + y (x SXY) + (x / SXX)^2 - Ro = 0;
    // y = [-(x SXY) +/- sqrt ((x SXY)^2 - 4 (1/SYY^2) ((x/SXX)^2 - Ro))] * [SYY^2 / 2];
    // y = [-B +/- sqrt (B^2 - 4 A C)] / [2 A];

    // min/max value of x is where T -> 0
    // solve this for x2:
    float Q = Ro * PS_SQR(PAR[PM_PAR_SXX]) / (1.0 - PS_SQR(PAR[PM_PAR_SXX]*PAR[PM_PAR_SYY]*PAR[PM_PAR_SXY]) / 4.0);
    if (Q < 0.0) return NAN; // ellipse is imaginary

    int xMax = sqrt(Q);
    int xMin = -1.0*xMax;

    int nPts = 0;
    float nSigma = 0.0;

    for (int x = xMin; x <= xMax; x++) {
	float A = PS_SQR (1.0 / PAR[PM_PAR_SYY]);
	float B = x * PAR[PM_PAR_SXY];
	float C = PS_SQR (x / PAR[PM_PAR_SXX]) - Ro;

	float T = PS_SQR(B) - 4*A*C;
	if (T < 0.0) continue;
    
	float yP = (-B + sqrt (T)) / (2.0 * A);
	float yM = (-B - sqrt (T)) / (2.0 * A);

	int xPix  = x  + PAR[PM_PAR_XPOS] - image->col0 + 0.5;
	int yPixM = yM + PAR[PM_PAR_YPOS] - image->row0 + 0.5;
	int yPixP = yP + PAR[PM_PAR_YPOS] - image->row0 + 0.5;

	if (xPix < 0) continue;
	if (xPix >= image->numCols) continue;

	if ((yPixM >= 0) && (yPixM < image->numRows)) {
	    if (!mask || !mask->data.U8[yPixM][xPix]) {
		float dSigma = image->data.F32[yPixM][xPix] / sqrt (weight->data.F32[yPixM][xPix]);
		nSigma += dSigma;
		nPts ++;
	    }
	}
	
	if (yPixM == yPixP) continue;

	if ((yPixP >= 0) && (yPixP < image->numRows)) {
	    if (!mask || !mask->data.U8[yPixP][xPix]) {
		float dSigma = image->data.F32[yPixP][xPix] / sqrt (weight->data.F32[yPixP][xPix]);
		nSigma += dSigma;
		nPts ++;
	    }
	}
    }	
    nSigma /= nPts;
    return nSigma;
}
