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

static float psphotModelContour(const psImage *image, const psImage *weight, const psImage *mask,
                                psMaskType maskVal, const 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(pmConfig *config, pmReadout *readout, psArray *sources, psMetadata *recipe, long first)
{
    bool status;

    psTimerStart ("psphot.size");

    // user-defined masks to test for good/bad pixels (build from recipe list if not yet set)
    psMaskType maskVal = psMetadataLookupU8(&status, recipe, "MASK.PSPHOT"); // Mask value for bad pixels
    assert (maskVal);

    // bit to mask the cosmic-ray pixels
    psMaskType crMask  = pmConfigMaskGet("CR", config); // Mask value for cosmic rays

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

    int grow = psMetadataLookupS32(&status, recipe, "PSPHOT.CR.GROW"); // Growth size for CRs
    if (!status || grow < 0) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "PSPHOT.CR.GROW is not positive.");
        return false;
    }

    float soft = psMetadataLookupF32(&status, recipe, "PSPHOT.CR.NSIGMA.SOFTEN"); // Softening parameter
    if (!status || !isfinite(soft) || soft < 0.0) {
        psWarning("PSPHOT.CR.NSIGMA.SOFTEN not set; defaulting to zero.");
        soft = 0.0;
    }

    // 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)) {
            psTrace("psphot", 7, "Not calculating extNsigma,crNsigma since already measured\n");
            continue;
        }

        // source must have been subtracted
        if (!(source->mode & PM_SOURCE_MODE_SUBTRACTED)) {
            psTrace("psphot", 7, "Not calculating extNsigma,crNsigma since source is not subtracted\n");
            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, maskVal,
                                               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;
        }

        // Integer position of peak
        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 || xPeak > source->pixels->numCols - 2 ||
            yPeak < 1 || yPeak > source->pixels->numRows - 2) {
            psTrace("psphot", 7, "Not calculating crNsigma due to edge\n");
            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] & maskVal) {
                    keep = false;
                }
            }
        }
        if (!keep) {
            psTrace("psphot", 7, "Not calculating crNsigma due to masked pixels\n");
            continue;
        }

        // Compare the central pixel with those on either side, for the four possible lines through it.

        // Soften weights (add systematic error)
        float softening = soft * PS_SQR(source->peak->flux); // Softening for weights

        // Across the middle: y = 0
        float cX = 2*resid[yPeak][xPeak]   - resid[yPeak+0][xPeak-1]  - resid[yPeak+0][xPeak+1];
        float dcX = 4*weight[yPeak][xPeak] + weight[yPeak+0][xPeak-1] + weight[yPeak+0][xPeak+1];
        float nX = cX / sqrtf(dcX + softening);

        // Up the centre: x = 0
        float cY = 2*resid[yPeak][xPeak]   - resid[yPeak-1][xPeak+0]  - resid[yPeak+1][xPeak+0];
        float dcY = 4*weight[yPeak][xPeak] + weight[yPeak-1][xPeak+0] + weight[yPeak+1][xPeak+0];
        float nY = cY / sqrtf(dcY + softening);

        // Diagonal: x = y
        float cL = 2*resid[yPeak][xPeak]   - resid[yPeak-1][xPeak-1]  - resid[yPeak+1][xPeak+1];
        float dcL = 4*weight[yPeak][xPeak] + weight[yPeak-1][xPeak-1] + weight[yPeak+1][xPeak+1];
        float nL = cL / sqrtf(dcL + softening);

        // Diagonal: x = - y
        float cR = 2*resid[yPeak][xPeak]   - resid[yPeak+1][xPeak-1]  - resid[yPeak-1][xPeak+1];
        float dcR = 4*weight[yPeak][xPeak] + weight[yPeak+1][xPeak-1] + weight[yPeak-1][xPeak+1];
        float nR = cR / sqrtf(dcR + softening);

        // 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); also biases a bit low if any pixels are masked
        // XXX I am not sure I want to keep this value...
        source->psfChisq = PS_SQR(nX) + PS_SQR(nY) + PS_SQR(nL) + PS_SQR(nR);

        float fCR = 0.0;
        int nCR = 0;
        if (nX > 0.0) {
            fCR += nX;
            nCR ++;
        }
        if (nY > 0.0) {
            fCR += nY;
            nCR ++;
        }
        if (nL > 0.0) {
            fCR += nL;
            nCR ++;
        }
        if (nR > 0.0) {
            fCR += nR;
            nCR ++;
        }
        source->crNsigma  = (nCR > 0)  ? fCR / nCR : 0.0;
        if (!isfinite(source->crNsigma)) continue;

        // this source is thought to be a cosmic ray.  flag the detection and mask the pixels
        if (source->crNsigma > CR_NSIGMA_LIMIT) {
            source->mode |= PM_SOURCE_MODE_CR_LIMIT;
            pmPeak *peak = source->peak;
            psAssert (peak, "NULL peak");

            // replace the source flux
            pmSourceAdd (source, PM_MODEL_OP_FULL, maskVal);
            source->mode &= ~PM_SOURCE_MODE_SUBTRACTED;

            psImage *mask   = source->maskView;
            psImage *pixels = source->pixels;
            psImage *weight = source->weight;

            // XXX This should be a recipe variable
            # define SN_LIMIT 5.0

            int xo = peak->x - pixels->col0;
            int yo = peak->y - pixels->row0;

            // mark the pixels in this row to the left, then the right
            for (int ix = xo; ix >= 0; ix--) {
                float SN = pixels->data.F32[yo][ix] / sqrt(weight->data.F32[yo][ix]);
                if (SN > SN_LIMIT) {
                    mask->data.U8[yo][ix] |= crMask;
                }
            }
            for (int ix = xo + 1; ix < pixels->numCols; ix++) {
                float SN = pixels->data.F32[yo][ix] / sqrt(weight->data.F32[yo][ix]);
                if (SN > SN_LIMIT) {
                    mask->data.U8[yo][ix] |= crMask;
                }
            }

            // for each of the neighboring rows, mark the high pixels if they have a marked neighbor
            // first go up:
            for (int iy = PS_MIN(yo, mask->numRows-2); iy >= 0; iy--) {
                // mark the pixels in this row to the left, then the right
                for (int ix = 0; ix < pixels->numCols; ix++) {
                    float SN = pixels->data.F32[iy][ix] / sqrt(weight->data.F32[iy][ix]);
                    if (SN < SN_LIMIT) continue;

                    bool valid = false;
                    valid |= (mask->data.U8[iy+1][ix] & crMask);
                    valid |= (ix > 0) ? (mask->data.U8[iy+1][ix-1] & crMask) : 0;
                    valid |= (ix <= mask->numCols) ? (mask->data.U8[iy+1][ix+1] & crMask) : 0;

                    if (!valid) continue;
                    mask->data.U8[iy][ix] |= crMask;
                }
            }
            // next go down:
            for (int iy = PS_MIN(yo+1, mask->numRows-1); iy < pixels->numRows; iy++) {
                // mark the pixels in this row to the left, then the right
                for (int ix = 0; ix < pixels->numCols; ix++) {
                    float SN = pixels->data.F32[iy][ix] / sqrt(weight->data.F32[iy][ix]);
                    if (SN < SN_LIMIT) continue;

                    bool valid = false;
                    valid |= (mask->data.U8[iy-1][ix] & crMask);
                    valid |= (ix > 0) ? (mask->data.U8[iy-1][ix-1] & crMask) : 0;
                    valid |= (ix <= mask->numCols) ? (mask->data.U8[iy-1][ix+1] & crMask) : 0;

                    if (!valid) continue;
                    mask->data.U8[iy][ix] |= crMask;
                }
            }
        }
    }

    // now that we have masked pixels associated with CRs, we can grow the mask
    if (grow > 0) {
        bool oldThreads = psImageConvolveSetThreads(true); // Old value of threading for psImageConvolveMask
        psImage *newMask = psImageConvolveMask(NULL, readout->mask, crMask, crMask, -grow, grow, -grow, grow);
        psImageConvolveSetThreads(oldThreads);
        if (!newMask) {
            psError(PS_ERR_UNKNOWN, false, "Unable to grow CR mask");
            return false;
        }
        psFree(readout->mask);
        readout->mask = newMask;
    }

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

    psphotVisualPlotSourceSize (sources);
    psphotVisualShowSourceSize (sources);

    return true;
}

// given the PSF ellipse parameters, navigate around the 1sigma contour, return the total
// deviation in sigmas.  This is measured on the residual image - should we ignore negative
// deviations?
static float psphotModelContour(const psImage *image, const psImage *weight, const psImage *mask,
                                psMaskType maskVal, const pmModel *model, float Ro)
{
    psF32 *PAR = model->params->data.F32; // Model parameters
    float sxx = PAR[PM_PAR_SXX], sxy = PAR[PM_PAR_SXY], syy = PAR[PM_PAR_SYY]; // Ellipse parameters

    // We treat the contour as an ellipse:
    // Ro = (x / SXX)^2 + (y / SYY)^2 + x y SXY
    // y^2 (1/SYY^2) + y (x SXY) + (x / SXX)^2 - Ro = 0;
    // This is a quadratic, Ay^2 + By + C with A = 1/SYY^2, B = x*SXY, C = (x / SXX)^2 - Ro
    // The solution is y = [-B +/- sqrt (B^2 - 4 A C)] / [2 A], so:
    // y = [-(x SXY) +/- sqrt ((x SXY)^2 - 4 (1/SYY^2) ((x/SXX)^2 - Ro))] * [SYY^2 / 2]

    // min/max value of x is where B^2 - 4AC = 0; solve this for x
    float Q = Ro * PS_SQR(sxx) / (1.0 - PS_SQR(sxx * syy * sxy) / 4.0);
    if (Q < 0.0) {
        // ellipse is imaginary
        return NAN;
    }

    int radius = sqrtf(Q) + 0.5;        // Radius of ellipse
    int nPts = 0;                       // Number of points in ellipse
    float nSigma = 0.0;                 //

    for (int x = -radius; x <= radius; x++) {
        // Polynomial coefficients
        // XXX Should we be using the centre of the pixel as x or x+0.5?
        float A = PS_SQR (1.0 / syy);
        float B = x * sxy;
        float C = PS_SQR (x / sxx) - Ro;
        float T = PS_SQR(B) - 4*A*C;
        if (T < 0.0) {
            continue;
        }

        // y position in source frame
        float yP = (-B + sqrt (T)) / (2.0 * A);
        float yM = (-B - sqrt (T)) / (2.0 * A);

        // Get the closest pixel positions (image frame)
        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 || xPix >= image->numCols) {
            continue;
        }

        if (yPixM >= 0 && yPixM < image->numRows &&
            !(mask && (mask->data.PS_TYPE_MASK_DATA[yPixM][xPix] & maskVal))) {
            float dSigma = image->data.F32[yPixM][xPix] / sqrtf(weight->data.F32[yPixM][xPix]);
            nSigma += dSigma;
            nPts++;
        }

        if (yPixM == yPixP) {
            continue;
        }

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