# include "psphotInternal.h"

// for now, let's store the detections on the readout->analysis for each readout
bool psphotDeblendSatstars (pmConfig *config, const pmFPAview *view)
{
    bool status = true;

    int num = psMetadataLookupS32 (&status, config->arguments, "PSPHOT.INPUT.NUM");
    psAssert (status, "programming error: must define PSPHOT.INPUT.NUM");

    // loop over the available readouts
    for (int i = 0; i < num; i++) {
	if (!psphotDeblendSatstarsReadout (config, view, "PSPHOT.INPUT", i)) {
            psError (PSPHOT_ERR_CONFIG, false, "failed on saturated star deblend analysis for PSPHOT.INPUT entry %d", i);
	    return false;
	}
    }
    return true;
}

bool psphotDeblendSatstarsReadout (pmConfig *config, const pmFPAview *view, const char *filename, int fileIndex) {

    int N;
    pmSource *source;
    bool status;

    psTimerStart ("psphot.deblend.sat");

    int Nblend = 0;
    float SAT_MIN_RADIUS = 5.0;

    // find the currently selected readout
    pmFPAfile *file = pmFPAfileSelectSingle(config->files, filename, fileIndex); // File of interest
    psAssert (file, "missing file?");

    pmReadout *readout = pmFPAviewThisReadout(view, file->fpa);
    psAssert (readout, "missing readout?");

    pmDetections *detections = psMetadataLookupPtr (&status, readout->analysis, "PSPHOT.DETECTIONS");
    psAssert (detections, "missing detections?");

    psArray *sources = detections->newSources;
    psAssert (sources, "missing sources?");

    if (!sources->n) {
	psLogMsg ("psphot", PS_LOG_INFO, "no sources, skipping satstar blend");
	return true;
    }

    // select the appropriate recipe information
    psMetadata *recipe  = psMetadataLookupPtr (&status, config->recipes, PSPHOT_RECIPE);
    psAssert (recipe, "missing recipe?");

    pmCell *cell = readout->parent;
    float SATURATION = 0.75*psMetadataLookupF32 (&status, cell->concepts, "CELL.SATURATION");
    float SAT_TEST_LEVEL = 0.5*SATURATION;

    // we need sources spatially-sorted to find overlaps
    sources = psArraySort (sources, pmSourceSortByY);

    // source analysis is done in peak order (brightest first)
    // we use an index for this so the spatial sorting is kept
    psVector *SN = psVectorAlloc (sources->n, PS_DATA_F32);
    for (int i = 0; i < SN->n; i++) {
        source = sources->data[i];
        SN->data.F32[i] = source->peak->SN;
    }
    psVector *index = psVectorSortIndex (NULL, SN);
    // this results in an index of increasing SN

    // examine sources in decreasing SN order
    for (int i = sources->n - 1; i >= 0; i--) {
        N = index->data.U32[i];
        source = sources->data[N];

        // XXX filter? if (source->mode & PM_SOURCE_MODE_SATSTAR) continue;
        if (source->mode & PM_SOURCE_MODE_BLEND) continue;
        if (source->peak->flux < SATURATION) continue;

	// save these for reference below
	int xPeak = source->peak->x;
	int yPeak = source->peak->y;

        // generate a basic contour (set of x,y coordinates at-or-below flux level)
        psArray *contour = pmSourceContour (source->pixels, xPeak, yPeak, SAT_TEST_LEVEL);
        if (contour == NULL) continue;

	// contour consists of a set of X,Y coords giving the boundary
	psVector *xVec = contour->data[0];
	psVector *yVec = contour->data[1];
	if (xVec->n < 5) {
	    psFree(contour);
	    continue;
	}

	// find the center of the contour (let's just use mid[x,y])
	int xMin = xVec->data.F32[0];
	int xMax = xVec->data.F32[0];
	int yMin = yVec->data.F32[0];
	int yMax = yVec->data.F32[0];
	for (int j = 0; j < xVec->n; j++) {
	    xMin = PS_MIN (xMin, xVec->data.F32[j]);
	    xMax = PS_MAX (xMax, xVec->data.F32[j]);
	    yMin = PS_MIN (yMin, yVec->data.F32[j]);
	    yMax = PS_MAX (yMax, yVec->data.F32[j]);
	}	
	int xCenter = 0.5*(xMin + xMax);
	int yCenter = 0.5*(yMin + yMax);
	psFree (contour);

	psAssert (xCenter >= source->pixels->col0, "invalid shift in object center");
	psAssert (xCenter <  source->pixels->col0 + source->pixels->numCols, "invalid shift in object center");
	psAssert (yCenter >= source->pixels->row0, "invalid shift in object center");
	psAssert (yCenter <  source->pixels->row0 + source->pixels->numRows, "invalid shift in object center");

	// reset the peak for this source to the value of the center pixel
	source->peak->x = xCenter;
	source->peak->y = yCenter;
	source->peak->xf = xCenter;
	source->peak->yf = yCenter;
	
	// temporary array for overlapping objects we find
        psArray *overlap = psArrayAllocEmpty (100);

        // search backwards for overlapping sources
        for (int j = N - 1; j >= 0; j--) {
            pmSource *testSource = sources->data[j];
            if (testSource->peak->x <  source->pixels->col0) continue;
            if (testSource->peak->x >= source->pixels->col0 + source->pixels->numCols) continue;
            if (testSource->peak->y <  source->pixels->row0) break;
            if (testSource->peak->y >= source->pixels->row0 + source->pixels->numRows) {
                fprintf (stderr, "warning: invalid condition\n");
                continue;
            }
            psArrayAdd (overlap, 100, testSource);
        }

        // search forwards for overlapping sources
        for (int j = N + 1; j < sources->n; j++) {
            pmSource *testSource = sources->data[j];
            if (testSource->peak->x <  source->pixels->col0) continue;
            if (testSource->peak->x >= source->pixels->col0 + source->pixels->numCols) continue;
            if (testSource->peak->y <  source->pixels->row0) {
                fprintf (stderr, "warning: invalid condition\n");
                continue;
            }
            if (testSource->peak->y >= source->pixels->row0 + source->pixels->numRows) break;
            psArrayAdd (overlap, 100, testSource);
        }

        if (overlap->n == 0) {
            psFree (overlap);
            continue;
        }

	// now find the contour which is at 0.5*SAT_TEST_LEVEL (xPeak, yPeak is valid high pixel)
        contour = pmSourceContour (source->pixels, xPeak, yPeak, 0.5*SAT_TEST_LEVEL);
        if (contour == NULL) {
	    psFree (overlap);
	    continue; 
	}
	
	// any peaks within this contour should be dropped (mark as blend, drop after loop is done)
	// also drop any peaks which are too close to this peak
        psVector *xv = contour->data[0];
        psVector *yv = contour->data[1];
        for (int k = 0; k < overlap->n; k++) {
            pmSource *testSource = overlap->data[k];
	    float radius = hypot((testSource->peak->x - xCenter), (testSource->peak->y - yCenter));
	    if (radius < SAT_MIN_RADIUS) {
                testSource->mode |= PM_SOURCE_MODE_BLEND;
                Nblend ++;
		continue;
	    }
            for (int j = 0; j < xv->n; j+=2) {
                if (fabs(yv->data.F32[j] - testSource->peak->y) > 0.5) continue;
                if (xv->data.F32[j+0] > testSource->peak->x) break;
                if (xv->data.F32[j+1] < testSource->peak->x) break;
                testSource->mode |= PM_SOURCE_MODE_BLEND;
                Nblend ++;
                j = xv->n; // skip rest of contour
            }
        }
        psFree (overlap);
        psFree (contour);
    }

    // drop the sources marked as BLEND
    for (int i = 0; i < sources->n;) {
	pmSource *source = sources->data[i];

        if (!(source->mode & PM_SOURCE_MODE_BLEND)) {
	    i++;
	    continue;
	}

	// shuffle the remaining sources forward
	for (int j = i; j < sources->n - 1; j++) {
	    sources->data[j] = sources->data[j+1];
	}
	psFree (source);
	sources->n --;
    }

    psFree (SN);
    psFree (index);

    psLogMsg ("psphot", PS_LOG_INFO, "found %d satstar blend peaks, leaving %ld sources: %f sec\n", Nblend, sources->n, psTimerMark ("psphot.deblend.sat"));
    return true;
}
