# include "psphotInternal.h"

// fit flux (and optionally sky model) to all reasonable sources
// with the linear fitting process.  sources must have an associated
// model with selected pixels, and the fit radius must be defined

// given the set of sources, each of which points to the pixels in the 
// science image, we construct a set of simulated sources with their own pixels.  
// these are used to determine the simultaneous linear fit of fluxes.  
// the analysis is performed wrt the simulated pixel values

static bool SetBorderMatrixElements (psSparseBorder *border, pmReadout *readout, psArray *sources, bool constant_weights, int SKY_FIT_ORDER);

bool psphotFitSourcesLinear (pmReadout *readout, psArray *sources, psMetadata *recipe, pmPSF *psf, bool final) {

    bool status;
    float x;
    float y;
    float f;
    // float r;

    psTimerStart ("psphot");

    // source analysis is done in spatial order
    sources = psArraySort (sources, pmSourceSortByY);

    // storage array for fitSources
    psArray *fitSources = psArrayAllocEmpty (sources->n);

    // option to limit analysis to a specific region
    char *region = psMetadataLookupStr (&status, recipe, "ANALYSIS_REGION");
    psRegion AnalysisRegion = psRegionFromString (region);
    AnalysisRegion = psRegionForImage (readout->image, AnalysisRegion);
    if (psRegionIsNaN (AnalysisRegion)) psAbort("analysis region mis-defined");

    bool CONSTANT_PHOTOMETRIC_WEIGHTS =
        psMetadataLookupBool(&status, recipe, "CONSTANT_PHOTOMETRIC_WEIGHTS");
    if (!status) {
        psAbort("You must provide a value for the BOOL recipe CONSTANT_PHOTOMETRIC_WEIGHTS");
    }
    int SKY_FIT_ORDER = psMetadataLookupS32(&status, recipe, "SKY_FIT_ORDER");
    if (!status) {
	SKY_FIT_ORDER = 0;
    }
    bool SKY_FIT_LINEAR = psMetadataLookupBool(&status, recipe, "SKY_FIT_LINEAR");
    if (!status) {
	SKY_FIT_LINEAR = false;
    }

    // select the sources which will be used for the fitting analysis
    for (int i = 0; i < sources->n; i++) {
        pmSource *source = sources->data[i];

        // skip non-astronomical objects (very likely defects)
        if (source->type == PM_SOURCE_TYPE_DEFECT) continue;
        if (source->type == PM_SOURCE_TYPE_SATURATED) continue;
	if (source->type == PM_SOURCE_TYPE_STAR &&
            source->mode & PM_SOURCE_MODE_SATSTAR) continue;
        if (final) {
            if (source->mode &  PM_SOURCE_MODE_SUBTRACTED) continue;
        } else {
            if (source->mode &  PM_SOURCE_MODE_BLEND) continue;
        }

        // save the original coords
        x = source->peak->xf;
        y = source->peak->yf;

	// is the source in the region of interest?
        if (x < AnalysisRegion.x0) continue;
        if (y < AnalysisRegion.y0) continue;
        if (x > AnalysisRegion.x1) continue;
        if (y > AnalysisRegion.y1) continue;

        psArrayAdd (fitSources, 100, source);
    }
    psLogMsg ("psphot.ensemble", PS_LOG_MINUTIA, "built fitSources: %f (%ld objects)\n", psTimerMark ("psphot"), sources->n);

    // vectors to store stats for each object
    // psVector *weight = psVectorAlloc (fitSources->n, PS_TYPE_F32);
    psVector *errors = psVectorAlloc (fitSources->n, PS_TYPE_F32);

    // create the border matrix (includes the sparse matrix)
    // for just sky: 1 row; for x,y terms: 3 rows
    psSparse *sparse = psSparseAlloc (fitSources->n, 100);
    int nBorder = (SKY_FIT_ORDER == 0) ? 1 : 3;
    psSparseBorder *border = psSparseBorderAlloc (sparse, nBorder);

    // fill out the sparse matrix elements and border elements (B)
    // SRCi is the current source of interest
    // SRCj is a possibly overlapping source
    for (int i = 0; i < fitSources->n; i++) {
        pmSource *SRCi = fitSources->data[i];

        // diagonal elements of the sparse matrix (auto-cross-product)
        f = pmSourceModelDotModel (SRCi, SRCi, CONSTANT_PHOTOMETRIC_WEIGHTS);
        psSparseMatrixElement (sparse, i, i, f);

	// the formal error depends on the weighting scheme
	if (CONSTANT_PHOTOMETRIC_WEIGHTS) {
	    float var = pmSourceModelDotModel (SRCi, SRCi, false);
	    errors->data.F32[i] = 1.0 / sqrt(var);
	} else {
	    errors->data.F32[i] = 1.0 / sqrt(f);
	}


        // find the image x model value
        f = pmSourceDataDotModel (SRCi, SRCi, CONSTANT_PHOTOMETRIC_WEIGHTS);
        psSparseVectorElement (sparse, i, f);

	// add the per-source weights (border region)
	switch (SKY_FIT_ORDER) {
	  case 1:
	    f = pmSourceModelWeight (SRCi, 1, CONSTANT_PHOTOMETRIC_WEIGHTS);
	    psSparseBorderElementB (border, i, 1, f);
	    f = pmSourceModelWeight (SRCi, 2, CONSTANT_PHOTOMETRIC_WEIGHTS);
	    psSparseBorderElementB (border, i, 2, f);

	  case 0:
	    f = pmSourceModelWeight (SRCi, 0, CONSTANT_PHOTOMETRIC_WEIGHTS);
	    psSparseBorderElementB (border, i, 0, f);
	    break;

	  default:
	    psAbort("Invalid SKY_FIT_ORDER %d\n", SKY_FIT_ORDER);
	    break;
	}

        // loop over all other stars following this one
        for (int j = i + 1; j < fitSources->n; j++) {
            pmSource *SRCj = fitSources->data[j];

            // skip over disjoint source images, break after last possible overlap
            if (SRCi->pixels->row0 + SRCi->pixels->numRows < SRCj->pixels->row0) break;
            if (SRCj->pixels->row0 + SRCj->pixels->numRows < SRCi->pixels->row0) continue;
            if (SRCi->pixels->col0 + SRCi->pixels->numCols < SRCj->pixels->col0) continue;
            if (SRCj->pixels->col0 + SRCj->pixels->numCols < SRCi->pixels->col0) continue;

            // got an overlap; calculate cross-product and add to output array
            f = pmSourceModelDotModel (SRCi, SRCj, CONSTANT_PHOTOMETRIC_WEIGHTS);
            psSparseMatrixElement (sparse, j, i, f);
        }
    }

    psSparseResort (sparse);
    psLogMsg ("psphot.ensemble", PS_LOG_MINUTIA, "built matrix: %f (%d elements)\n", psTimerMark ("psphot"), sparse->Nelem);

    // set the sky, sky_x, sky_y components of border matrix
    SetBorderMatrixElements (border, readout, fitSources, CONSTANT_PHOTOMETRIC_WEIGHTS, SKY_FIT_ORDER);

    psSparseConstraint constraint;
    constraint.paramMin   = 0.0;
    constraint.paramMax   = 1e8;
    constraint.paramDelta = 1e8;

    // solve for normalization terms (need include local sky?)
    psVector *norm = NULL;
    psVector *skyfit = NULL;
    if (SKY_FIT_LINEAR) {
	psSparseBorderSolve (&norm, &skyfit, constraint, border, 5);
	fprintf (stderr, "skyfit: %f\n", skyfit->data.F32[0]);
    } else {
	norm = psSparseSolve (NULL, constraint, sparse, 5);
	skyfit = NULL;
    }

    // adjust I0 for fitSources and subtract
    for (int i = 0; i < fitSources->n; i++) {
        pmSource *source = fitSources->data[i];
	pmModel *model = pmSourceGetModel (NULL, source);

        // assign linearly-fitted normalization
        if (isnan(norm->data.F32[i])) {
            psAbort("linear fitted source is nan");
        }
        model->params->data.F32[PM_PAR_I0] = norm->data.F32[i];
        model->dparams->data.F32[PM_PAR_I0] = errors->data.F32[i];
	// XXX is the value of 'errors' modified by the sky fit?

        // subtract object
	pmSourceSub (source, PM_MODEL_OP_FULL);
        source->mode |= PM_SOURCE_MODE_SUBTRACTED;
        if (!final) source->mode |= PM_SOURCE_MODE_TEMPSUB;
	// XXX not sure about the use of TEMPSUB
    }

    // measure chisq for each source
    for (int i = 0; final && (i < fitSources->n); i++) {
        pmSource *source = fitSources->data[i];
        pmModel *model = pmSourceGetModel (NULL, source);
        pmSourceChisq (model, source->pixels, source->maskObj, source->weight);
    }

    // psFree (index);
    psFree (sparse);
    psFree (fitSources);
    psFree (norm);
    psFree (skyfit);
    psFree (errors);
    psFree (border);

    psLogMsg ("psphot.ensemble", PS_LOG_INFO, "measure ensemble of PSFs: %f sec\n", psTimerMark ("psphot"));
    return true;
}

// Calculate the weight terms for the sky fit component of the matrix.  This function operates
// on the pixels which correspond to all of the sources of interest.  These elements fill in
// the border matrix components in the sparse matrix equation.
static bool SetBorderMatrixElements (psSparseBorder *border, pmReadout *readout, psArray *sources, bool constant_weights, int SKY_FIT_ORDER) {

    // generate the image-wide weight terms
    // turn on MARK for all image pixels
    psRegion fullArray = psRegionSet (0, 0, 0, 0);
    fullArray = psRegionForImage (readout->mask, fullArray);
    psImageMaskRegion (readout->mask, fullArray, "OR", PM_MASK_MARK);

    // turn off MARK for all object pixels
    for (int i = 0; i < sources->n; i++) {
        pmSource *source = sources->data[i];
	pmModel *model = pmSourceGetModel (NULL, source);
	if (model == NULL) continue;
        float x = model->params->data.F32[PM_PAR_XPOS];
        float y = model->params->data.F32[PM_PAR_YPOS];
	psImageMaskCircle (source->maskView, x, y, model->radiusFit, "AND", PS_NOT_U8(PM_MASK_MARK));
    }	

    // accumulate the image statistics from the masked regions
    psF32 **image  = readout->image->data.F32;
    psF32 **weight = readout->weight->data.F32;
    psU8  **mask   = readout->mask->data.U8;

    double w, x, y, x2, xy, y2, xc, yc, wt, f, fo, fx, fy;
    w = x = y = x2 = xy = y2 = fo = fx = fy = 0;
    
    int col0 = readout->image->col0;
    int row0 = readout->image->row0;

    for (int j = 0; j < readout->image->numRows; j++) {
	for (int i = 0; i < readout->image->numCols; i++) {
	    if (mask[j][i]) continue;
	    if (constant_weights) {
		wt = 1.0;
	    } else {
		wt = weight[j][i];
	    }
	    f = image[j][i];
	    w   += 1/wt;
	    fo  += f/wt;
	    if (SKY_FIT_ORDER == 0) continue;

	    xc  = i + col0;
	    yc  = j + row0;
	    x  +=    xc/wt;
	    y  +=    yc/wt;
	    x2 += xc*xc/wt;
	    xy += xc*yc/wt;
	    y2 += yc*yc/wt;
	    fx +=  f*xc/wt;
	    fy +=  f*yc/wt;
	}
    }

    // turn off MARK for all image pixels
    psImageMaskRegion (readout->mask, fullArray, "AND", PS_NOT_U8(PM_MASK_MARK));

    // set the Border T elements
    psSparseBorderElementG (border, 0, fo);
    psSparseBorderElementT (border, 0, 0, w);
    if (SKY_FIT_ORDER > 0) {
	psSparseBorderElementG (border, 0, fx);
	psSparseBorderElementG (border, 0, fy);
	psSparseBorderElementT (border, 1, 0, x);
	psSparseBorderElementT (border, 2, 0, y);
	psSparseBorderElementT (border, 0, 1, x);
	psSparseBorderElementT (border, 1, 1, x2);
	psSparseBorderElementT (border, 2, 1, xy);
	psSparseBorderElementT (border, 0, 2, y);
	psSparseBorderElementT (border, 1, 2, xy);
	psSparseBorderElementT (border, 2, 2, y2);
    }    
    return true;
}
