#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <assert.h>

#include <pslib.h>
#include "pmHDU.h"
#include "pmFPA.h"
#include "pmHDUUtils.h"
#include "pmHDUGenerate.h"
#include "pmFPAMaskWeight.h"

#define PIXELS_BUFFER 100               // Size of buffer for allocating pixel lists
#define RENORM_NUM_SIGMA 3.0            // Number of standard deviations for Gaussian kernel
#define RENORM_PEAK 2.0                 // Number of standard deviations of noise for fake source peak flux


//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// File-static (private) functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

// Create the parent mask images that reside in the HDU
static void createParentMasks(pmHDU *hdu // The HDU for which to create
                             )
{
    assert(hdu);
    assert(hdu->images);

    // Generate the parent mask images
    psArray *images = hdu->images;      // Array of images
    psArray *masks = hdu->masks;        // Array of masks
    if (!masks) {
        masks = psArrayAlloc(images->n);
        hdu->masks = masks;
    }

    for (long i = 0; i < images->n; i++) {
        psImage *image = images->data[i]; // The image for this readout
        if (!image || masks->data[i]) {
            continue;
        }
        masks->data[i] = psImageAlloc(image->numCols, image->numRows, PS_TYPE_U8);
        psImageInit(masks->data[i], 0);
    }

    return;
}

// Create the parent weight images that reside in the HDU
static void createParentWeights(pmHDU *hdu // The HDU for which to create
                               )
{
    assert(hdu);
    assert(hdu->images);

    // Generate the parent mask images
    psArray *images = hdu->images;      // Array of images
    psArray *weights = hdu->weights;    // Array of weight images
    if (!weights) {
        weights = psArrayAlloc(images->n);
        hdu->weights = weights;
    }

    for (long i = 0; i < images->n; i++) {
        psImage *image = images->data[i]; // The image for this readout
        if (!image || weights->data[i]) {
            continue;
        }
        weights->data[i] = psImageAlloc(image->numCols, image->numRows, PS_TYPE_F32);
    }

    return;
}

// Identify a readout within the HDU, on the basis of the image pointer.
// This is a little dirty, but hopefully should work....
static long identifyReadout(pmHDU *hdu, // The HDU containing the readouts
                            pmReadout *readout // The readout to be identified
                           )
{
    assert(hdu);
    assert(readout);

    long index = -1;                    // Index of the readout
    for (long i = 0; i < hdu->images->n && index == -1; i++) {
        if (hdu->images->data[i] == readout->image->parent) {
            index = i;
        }
    }

    return index;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Public functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

bool pmReadoutSetMask(pmReadout *readout, psMaskType satMask, psMaskType badMask)
{
    PS_ASSERT_PTR_NON_NULL(readout, false);
    PS_ASSERT_IMAGE_NON_NULL(readout->image, false);

    pmCell *cell = readout->parent;     // The parent cell
    bool mdok = true;                   // Status of MD lookup

    // Get the "concepts" of interest
    float saturation = psMetadataLookupF32(&mdok, cell->concepts, "CELL.SATURATION"); // Saturation level
    if (!mdok || isnan(saturation)) {
        psError(PS_ERR_IO, true, "CELL.SATURATION is not set --- unable to set mask.\n");
        return false;
    }
    float bad = psMetadataLookupF32(&mdok, cell->concepts, "CELL.BAD"); // Bad level
    if (!mdok || isnan(bad)) {
        psError(PS_ERR_IO, true, "CELL.BAD is not set --- unable to set mask.\n");
        return false;
    }
    psTrace("psModules.camera", 5, "Saturation: %f, bad: %f\n", saturation, bad);


    // Set up the mask
    psImage *image = readout->image;    // The image pixels
    if (!readout->mask) {
        // Generate a (throwaway) mask image, if required
        readout->mask = psImageAlloc(image->numCols, image->numRows, PS_TYPE_U8);
    }
    psImage *mask = readout->mask;      // The mask pixels
    psImageInit(mask, 0);

    // Dereference pointers for speed
    psF32 **imageData = image->data.F32;// The image
    psU8  **maskData  = mask->data.U8;  // The mask

    for (int i = 0; i < image->numRows; i++) {
        for (int j = 0; j < image->numCols; j++) {
            if (imageData[i][j] >= saturation) {
                maskData[i][j] |= satMask;
            }
            if (imageData[i][j] <= bad) {
                maskData[i][j] |= badMask;
            }
            if (!isfinite(imageData[i][j])) {
                maskData[i][j] |= badMask;
            }
        }
    }

    return true;
}

// XXX this function creates the mask pixels, or uses the existing mask
// pixels.  currently, it will set mask bits if (value <= BAD) or (value >= SATURATION)
// should we optionally ignore these tests?
bool pmReadoutGenerateMask(pmReadout *readout, psMaskType satMask, psMaskType badMask)
{
    PS_ASSERT_PTR_NON_NULL(readout, false);

    pmCell *cell = readout->parent;     // The parent cell
    bool mdok = true;                   // Status of MD lookup

    // Create the mask image if required
    if (!readout->mask) {
        psRegion *trimsec = psMetadataLookupPtr(&mdok, cell->concepts, "CELL.TRIMSEC"); // Trim section
        if (!mdok || psRegionIsNaN(*trimsec)) {
            psError(PS_ERR_IO, true, "CELL.TRIMSEC is not set --- unable to set mask.\n");
            return false;
        }

        pmHDU *hdu = pmHDUFromCell(cell);   // The HDU containing the cell's pixels
        PS_ASSERT_PTR_NON_NULL(hdu, false);
        if (!hdu->images && !pmHDUGenerateForCell(cell)) {
            psError(PS_ERR_UNKNOWN, false, "Unable to generate HDU for cell.\n");
            return false;
        }

        createParentMasks(hdu);

        // Need to identify which readout we're working with....
        long index = identifyReadout(hdu, readout); // Index of the readout
        if (index == -1) {
            psError(PS_ERR_UNKNOWN, true, "Unable to identify readout image in HDU.\n");
            return false;
        }

        psImage *mask = psImageSubset(hdu->masks->data[index], *trimsec); // The mask pixels
        if (!mask) {
            psString trimsecString = psRegionToString(*trimsec);
            psError(PS_ERR_UNKNOWN, false, "Unable to set mask from HDU with trimsec: %s.\n", trimsecString);
            psFree(trimsecString);
            return false;
        }
        psImageInit(mask, 0);
        assert (readout->mask == NULL); // or else this is a memory leak.
        readout->mask = mask;
    }

    return pmReadoutSetMask(readout, satMask, badMask);
}

bool pmReadoutSetWeight(pmReadout *readout, bool poisson)
{
    PS_ASSERT_PTR_NON_NULL(readout, false);

    pmCell *cell = readout->parent;     // The parent cell
    bool mdok = true;                   // Status of MD lookup

    // Get the "concepts" of interest
    float gain = psMetadataLookupF32(&mdok, cell->concepts, "CELL.GAIN"); // Cell gain
    if (!mdok || isnan(gain)) {
        psError(PS_ERR_IO, true, "CELL.GAIN is not set --- unable to set weight.\n");
        return false;
    }
    float readnoise = psMetadataLookupF32(&mdok, cell->concepts, "CELL.READNOISE"); // Cell read noise
    if (!mdok || isnan(readnoise)) {
        psError(PS_ERR_IO, true, "CELL.READNOISE is not set --- unable to set weight.\n");
        return false;
    }
    if (psMetadataLookup(cell->concepts, "CELL.READNOISE.UPDATE")) {
        psError(PS_ERR_IO, true, "CELL.READNOISE has not yet been updated for the gain");
        return false;
    }

    if (poisson) {
        // Set weight image to the variance in ADU = f/g + rn^2
        psImage *image = readout->image;    // The image pixels
        readout->weight = (psImage*)psBinaryOp(readout->weight, image, "/", psScalarAlloc(gain, PS_TYPE_F32));

        // a negative weight is non-sensical. if the image value drops below 1, the weight must be 1.
        readout->weight = (psImage*)psUnaryOp(readout->weight, readout->weight, "abs");
        readout->weight = (psImage*)psBinaryOp(readout->weight, readout->weight, "max",
                                               psScalarAlloc(1, PS_TYPE_F32));
    } else {
        // Just use the read noise
        if (!readout->weight) {
            readout->weight = psImageAlloc(readout->image->numCols, readout->image->numRows, PS_TYPE_F32);
        }
        psImageInit(readout->weight, 0.0);
    }

    readout->weight = (psImage*)psBinaryOp(readout->weight, readout->weight, "+",
                                           psScalarAlloc(readnoise*readnoise/gain/gain, PS_TYPE_F32));

    return true;
}

// this function creates the weight pixels, or uses the existing weight pixels.  it will set
// the noise pixel values only if the weight image is not supplied
bool pmReadoutGenerateWeight(pmReadout *readout, bool poisson)
{
    PS_ASSERT_PTR_NON_NULL(readout, false);

    pmCell *cell = readout->parent;     // The parent cell
    bool mdok = true;                   // Status of MD lookup

    // Create the weight image if required
    if (readout->weight)
        return true;

    psRegion *trimsec = psMetadataLookupPtr(&mdok, cell->concepts, "CELL.TRIMSEC"); // Trim section
    if (!mdok || psRegionIsNaN(*trimsec)) {
        psError(PS_ERR_IO, true, "CELL.TRIMSEC is not set --- unable to set weight.\n");
        return false;
    }

    pmHDU *hdu = pmHDUFromCell(cell);   // The HDU containing the cell's pixels
    PS_ASSERT_PTR_NON_NULL(hdu, false);
    if (!hdu->images && !pmHDUGenerateForCell(cell)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to generate HDU for cell.\n");
        return false;
    }

    createParentWeights(hdu);

    // Need to identify which readout we're working with....
    long index = identifyReadout(hdu, readout); // Index of the readout
    if (index == -1) {
        psError(PS_ERR_UNKNOWN, true, "Unable to identify readout image in HDU.\n");
        return false;
    }

    psImage *weight = psImageSubset(hdu->weights->data[index], *trimsec); // The weight pixels
    if (!weight) {
        psString trimsecString = psRegionToString(*trimsec);
        psError(PS_ERR_UNKNOWN, false, "Unable to set weight from HDU with trimsec: %s.\n",
                trimsecString);
        psFree(trimsecString);
        return false;
    }
    psImageInit(weight, 0);
    readout->weight = weight;

    return pmReadoutSetWeight(readout, poisson);
}

bool pmReadoutGenerateMaskWeight(pmReadout *readout, psMaskType satMask, psMaskType badMask, bool poisson)
{
    PS_ASSERT_PTR_NON_NULL(readout, false);

    bool success = true;                // Was everything successful?

    success &= pmReadoutGenerateMask(readout, satMask, badMask);
    success &= pmReadoutGenerateWeight(readout, poisson);

    return success;
}

bool pmCellGenerateMaskWeight(pmCell *cell, psMaskType satMask, psMaskType badMask, bool poisson)
{
    PS_ASSERT_PTR_NON_NULL(cell, false);

    bool success = true;                // Was everything successful?
    psArray *readouts = cell->readouts; // Array of readouts
    for (int i = 0; i < readouts->n; i++) {
        pmReadout *readout = readouts->data[i]; // The readout
        success &= pmReadoutGenerateMaskWeight(readout, poisson, satMask, badMask);
    }

    return success;
}


bool pmReadoutWeightRenormPixels(const pmReadout *readout, psMaskType maskVal,
                                 psStatsOptions meanStat, psStatsOptions stdevStat, psRandom *rng)
{
    PM_ASSERT_READOUT_NON_NULL(readout, false);
    PM_ASSERT_READOUT_IMAGE(readout, false);
    PM_ASSERT_READOUT_WEIGHT(readout, false);

    psImage *image = readout->image, *mask = readout->mask, *weight = readout->weight; // Readout parts

    if (!psMemIncrRefCounter(rng)) {
        rng = psRandomAlloc(PS_RANDOM_TAUS, 0);
    }

    psStats *weightStats = psStatsAlloc(meanStat);// Statistics for mean
    if (!psImageBackground(weightStats, NULL, weight, mask, maskVal, rng)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to measure mean weight for image");
        psFree(weightStats);
        psFree(rng);
        return false;
    }
    float meanVariance = weightStats->robustMedian; // Mean variance
    psFree(weightStats);

    psStats *imageStats = psStatsAlloc(stdevStat);// Statistics for mean
    if (!psImageBackground(imageStats, NULL, image, mask, maskVal, rng)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to measure stdev of image");
        psFree(imageStats);
        psFree(rng);
        return false;
    }
    float stdevImage = imageStats->robustStdev; // Standard deviation of image
    psFree(imageStats);
    psFree(rng);

    float correction = PS_SQR(stdevImage) / meanVariance; // Correction to take variance to what it should be
    psLogMsg("psModules.camera", PS_LOG_INFO, "Renormalising weight map by %f", correction);
    psBinaryOp(weight, weight, "*", psScalarAlloc(correction, PS_TYPE_F32));

    return true;
}


bool pmReadoutWeightRenormPhot(const pmReadout *readout, psMaskType maskVal, int num, float width,
                               psStatsOptions meanStat, psStatsOptions stdevStat, psRandom *rng)
{
    PM_ASSERT_READOUT_NON_NULL(readout, false);
    PM_ASSERT_READOUT_IMAGE(readout, false);
    PM_ASSERT_READOUT_WEIGHT(readout, false);

    if (!psMemIncrRefCounter(rng)) {
        rng = psRandomAlloc(PS_RANDOM_TAUS, 0);
    }

    psImage *image = readout->image, *mask = readout->mask, *weight = readout->weight; // Readout images

    // Measure background
    psStats *bgStats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN | PS_STAT_ROBUST_STDEV);// Statistics for background
    if (!psImageBackground(bgStats, NULL, image, mask, maskVal, rng)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to measure background for image");
        psFree(bgStats);
        psFree(rng);
        return false;
    }
    float bgMean = bgStats->robustMedian; // Background level
    float bgNoise = bgStats->robustStdev; // Background standard deviation
    psFree(bgStats);
    psTrace("psModules.camera", 5, "Background is %f +/- %f\n", bgMean, bgNoise);


    // Construct kernels for flux measurement
    // We use N(0,width) and N(0,width/sqrt(2)) kernels, following psphotSignificanceImage.
    float sigFactor = 4.0 * M_PI * PS_SQR(width); // Factor for conversion from im/wt ratio to significance
    int size = RENORM_NUM_SIGMA, fullSize = 2 * size + 1; // Half-size and full size of Gaussian
    psVector *gauss = psVectorAlloc(fullSize, PS_TYPE_F32); // Gaussian for weighting
    psVector *gauss2 = psVectorAlloc(fullSize, PS_TYPE_F32); // Gaussian squared
    for (int i = 0, x = -size; i < fullSize; i++, x++) {
        gauss->data.F32[i] = expf(-0.5 * PS_SQR(x) / PS_SQR(width));
        gauss2->data.F32[i] = expf(-PS_SQR(x) / PS_SQR(width));
    }

    // Size of image
    int numCols = image->numCols, numRows = image->numRows; // Size of images
    int xSize = numCols - fullSize, ySize = numRows - fullSize; // Size of consideration
    int xOffset = size, yOffset = size;       // Offset to region of consideration

    // Measure fluxes
    float peakFlux = RENORM_PEAK * bgNoise;     // Peak flux for fake sources
    psVector *noise = psVectorAlloc(num, PS_TYPE_F32); // Measurements of the noise
    psVector *source = psVectorAlloc(num, PS_TYPE_F32); // Measurements of fake sources
    psVector *guess = psVectorAlloc(num, PS_TYPE_F32); // Guess at significance
    psVector *photMask = psVectorAlloc(num, PS_TYPE_MASK); // Mask for fluxes
    for (int i = 0; i < num; i++) {
        // Coordinates of interest
        int xPix = psRandomUniform(rng) * xSize + xOffset + 0.5;
        int yPix = psRandomUniform(rng) * ySize + yOffset + 0.5;
        psAssert(xPix - size >= 0 && xPix + size < numCols &&
                 yPix - size >= 0 && yPix + size < numRows,
                 "Bad pixel position: %d,%d", xPix, yPix);

        // Weighted aperture photometry
        // This has the same effect as smoothing the image by the window function
        float sumNoise = 0.0;       // Sum for noise measurement
        float sumSource = 0.0;      // Sum for source measurement
        float sumWeight = 0.0;      // Sum for weight measurement
        float sumGauss = 0.0, sumGauss2 = 0.0; // Sums of Gaussian kernels
        for (int v = 0, y = yPix - size; v < fullSize; v++, y++) {
            float xSumNoise = 0.0;  // Sum for noise measurement in x
            float xSumSource = 0.0; // Sum for source measurement in x
            float xSumWeight = 0.0; // Sum for weight measurement in x
            float xSumGauss = 0.0, xSumGauss2 = 0.0; // Sums of Gaussian kernels in x
            float yGauss = gauss->data.F32[v]; // Value of Gaussian in y
            for (int u = 0, x = xPix - size; u < fullSize; u++, x++) {
                if (mask && mask->data.PS_TYPE_MASK_DATA[y][x] & maskVal) {
                    continue;
                }
                float value = image->data.F32[y][x] - bgMean; // Value of image
                float xGauss = gauss->data.F32[u]; // Value of Gaussian in x
                float xGauss2 = gauss2->data.F32[u]; // Value of Gaussian^2 in x
                xSumNoise += value * xGauss;
                xSumSource += (value + peakFlux * xGauss * yGauss) * xGauss;
                xSumWeight += weight->data.F32[y][x] * xGauss2;
                xSumGauss += xGauss;
                xSumGauss2 += xGauss2;
            }
            float yGauss2 = gauss2->data.F32[v]; // Value of Gaussian^2 in y
            sumNoise += xSumNoise * yGauss;
            sumSource += xSumSource * yGauss;
            sumWeight += xSumWeight * yGauss2;
            sumGauss += xSumGauss * yGauss;
            sumGauss2 += xSumGauss2 * yGauss2;
        }

        photMask->data.PS_TYPE_MASK_DATA[i] = ((isfinite(sumNoise) && isfinite(sumSource) &&
                                                isfinite(sumWeight) && sumGauss > 0 && sumGauss2 > 0) ?
                                               0 : 0xFF);

        float smoothImageNoise = sumNoise / sumGauss; // Value of smoothed image pixel for noise
        float smoothImageSource = sumSource / sumGauss; // Value of smoothed image pixel for source
        float smoothWeight = sumWeight / sumGauss2; // Value of smoothed weight pixel

        noise->data.F32[i] = smoothImageNoise;
        source->data.F32[i] = smoothImageSource;
        guess->data.F32[i] = (sumSource > 0) ? sigFactor * PS_SQR(smoothImageSource) / smoothWeight : 0.0;
        psTrace("psModules.camera", 10, "Flux %d (%d,%d): %f, %f, %f\n",
                i, xPix, yPix, smoothImageNoise, smoothImageSource, smoothWeight);
    }
    psFree(gauss);
    psFree(gauss2);
    psFree(rng);

    // Standard deviation of fluxes gives us the real significance
    psStats *stdevStats = psStatsAlloc(stdevStat); // Statistics
    if (!psVectorStats(stdevStats, noise, NULL, photMask, 0xFF)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to measure standard deviation of fluxes");
        psFree(stdevStats);
        psFree(noise);
        psFree(source);
        psFree(guess);
        psFree(photMask);
        return false;
    }
    float stdev = psStatsGetValue(stdevStats, stdevStat); // Stadard deviation of fluxes
    psFree(stdevStats);
    psFree(noise);
    psTrace("psModules.camera", 5, "Standard deviation of fluxes is %f\n", stdev);

    // Ratio of measured significance to guessed significance
    psVector *ratio = psVectorAlloc(num, PS_TYPE_F32); // Ratio of measured to guess
    for (int i = 0; i < num; i++) {
        float measuredSig = PS_SQR(source->data.F32[i] / stdev); // Measured significance
        ratio->data.F32[i] = measuredSig / guess->data.F32[i];
        if (guess->data.F32[i] <= 0.0 || source->data.F32[i] <= 0.0 || !isfinite(ratio->data.F32[i])) {
            photMask->data.PS_TYPE_MASK_DATA[i] = 0xFF;
        }
        psTrace("psModules.camera", 9, "Ratio %d: %f, %f, %f\n",
                i, guess->data.F32[i], measuredSig, ratio->data.F32[i]);
    }
    psFree(source);
    psFree(guess);

    psStats *meanStats = psStatsAlloc(meanStat | stdevStat); // Statistics
    if (!psVectorStats(meanStats, ratio, NULL, photMask, 0xFF)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to measure mean ratio");
        psFree(meanStats);
        psFree(ratio);
        psFree(photMask);
        return false;
    }
    float meanRatio = psStatsGetValue(meanStats, meanStat); // Mean ratio
    psTrace("psModules.camera", 5, "Mean significance ratio is %f +/- %f\n",
            meanRatio, psStatsGetValue(meanStats, stdevStat));
    psFree(meanStats);
    psFree(ratio);
    psFree(photMask);

    psLogMsg("psModules.camera", PS_LOG_INFO, "Renormalising weight map by %f", meanRatio);
    psBinaryOp(weight, weight, "*", psScalarAlloc(meanRatio, PS_TYPE_F32));

    return true;
}


bool pmReadoutWeightRenorm(const pmReadout *readout, psMaskType maskVal, psStatsOptions meanStat,
                           psStatsOptions stdevStat, int width, psRandom *rng)
{
    PM_ASSERT_READOUT_NON_NULL(readout, false);
    PM_ASSERT_READOUT_IMAGE(readout, false);
    PM_ASSERT_READOUT_WEIGHT(readout, false);
    PS_ASSERT_INT_POSITIVE(width, false);

    if (!psMemIncrRefCounter(rng)) {
        rng = psRandomAlloc(PS_RANDOM_TAUS, 0);
    }

    psImage *image = readout->image, *mask = readout->mask, *weight = readout->weight; // Readout images
    int numCols = image->numCols, numRows = image->numRows; // Size of images
    int xNum = numCols / width + 1, yNum = numRows / width + 1; // Number of renormalisation regions
    float xSize = numCols / (float)xNum, ySize = numRows / (float)yNum; // Size of renormalisation regions

    psStats *meanStats = psStatsAlloc(meanStat), *stdevStats = psStatsAlloc(stdevStat); // Statistics
    psVector *buffer = NULL;

    for (int j = 0; j < yNum; j++) {
        // Bounds in y
        int yMin = j * ySize;
        int yMax = (j + 1) * ySize;
        for (int i = 0; i < xNum; i++) {
            // Bounds in x
            int xMin = i * xSize;
            int xMax = (i + 1) * xSize;

            psRegion region = psRegionSet(xMin, xMax, yMin, yMax); // Region of interest
            psImage *subImage = psImageSubset(image, region); // Sub-image of the image pixels
            psImage *subWeight = psImageSubset(weight, region); // Sub image of the weight pixels
            psImage *subMask = mask ? psImageSubset(mask, region) : NULL; // Sub-image of the mask pixels

            if (!psImageBackground(stdevStats, &buffer, subImage, subMask, maskVal, rng) ||
                !psImageBackground(meanStats, &buffer, subWeight, subMask, maskVal, rng)) {
                // Nothing we can do about it, but don't want to keel over and die, so do our best to flag it.
                psString regionStr = psRegionToString(region); // String with region
                psWarning("Unable to measure statistics over %s", regionStr);
                psFree(regionStr);
                psErrorClear();
                psImageInit(subWeight, NAN);
                if (subMask) {
                    psImageInit(subMask, maskVal);
                }
            } else {
                double meanVar = psStatsGetValue(meanStats, meanStat); // Mean of variance map
                double stdev = psStatsGetValue(stdevStats, stdevStat); // Standard deviation of image
                psTrace("psModules.camera", 3,
                        "Region [%d:%d,%d:%d] has variance %lf, but mean of variance map is %lf\n",
                        xMin, xMax, yMin, yMax, PS_SQR(stdev), meanVar);
                psBinaryOp(subWeight, subWeight, "*", psScalarAlloc(PS_SQR(stdev) / meanVar, PS_TYPE_F32));
            }

            psFree(subImage);
            psFree(subWeight);
            psFree(subMask);
        }
    }
    psFree(meanStats);
    psFree(stdevStats);
    psFree(rng);
    psFree(buffer);

    return true;
}


bool pmReadoutMaskNonfinite(pmReadout *readout, psMaskType maskVal)
{
    PM_ASSERT_READOUT_NON_NULL(readout, false);
    PM_ASSERT_READOUT_IMAGE(readout, false);

    psImage *image = readout->image;    // Readout's image
    psImage *weight = readout->weight;  // Readout's weight
    int numCols = image->numCols, numRows = image->numRows; // Size of image

    if (!readout->mask) {
        readout->mask = psImageAlloc(numCols, numRows, PS_TYPE_MASK);
    }
    psImage *mask = readout->mask;      // Readout's mask

    for (int y = 0; y < numRows; y++) {
        for (int x = 0; x < numCols; x++) {
            if (!isfinite(image->data.F32[y][x]) || (weight && !isfinite(weight->data.F32[y][x]))) {
                mask->data.PS_TYPE_MASK_DATA[y][x] |= maskVal;
            }
        }
    }

    return true;
}



bool pmReadoutMaskApply(pmReadout *readout, psMaskType maskVal)
{
    PM_ASSERT_READOUT_NON_NULL(readout, false);
    PM_ASSERT_READOUT_IMAGE(readout, false);
    PM_ASSERT_READOUT_MASK(readout, false);

    int numCols = readout->image->numCols, numRows = readout->image->numRows; // Size of image
    psMaskType **maskData = readout->mask->data.PS_TYPE_MASK_DATA; // Dereference mask
    psF32 **imageData = readout->image->data.F32;// Dereference image
    psF32 **weightData = readout->weight ? readout->weight->data.F32 : NULL; // Dereference weight map

    for (int y = 0; y < numRows; y++) {
        for (int x = 0; x < numCols; x++) {
            if (maskData[y][x] & maskVal) {
                imageData[y][x] = NAN;
                if (weightData) {
                    weightData[y][x] = NAN;
                }
            }
        }
    }

    return true;
}


bool pmReadoutInterpolateBadPixels(pmReadout *readout, psMaskType maskVal, psImageInterpolateMode mode,
                                   float poorFrac, psMaskType maskPoor, psMaskType maskBad)
{
    PM_ASSERT_READOUT_NON_NULL(readout, false);
    PM_ASSERT_READOUT_IMAGE(readout, false);
    PM_ASSERT_READOUT_MASK(readout, false);
    if (!maskVal) {
        return true;
    }

    psImage *image = readout->image;    // Image
    psImage *mask = readout->mask;      // Mask
    psImage *weight = readout->weight;  // Weight map

    psImageInterpolation *interp = psImageInterpolationAlloc(mode, image, weight, mask, maskVal,
                                                             NAN, NAN, maskBad, maskPoor, poorFrac, 0);
    interp->shifting = false;           // Turn off "exact shifts" so we get proper interpolation

    int numCols = mask->numCols, numRows = mask->numRows; // Size of image

    psPixels *pixels = psPixelsAllocEmpty(PIXELS_BUFFER); // Pixels that have been interpolated
    psVector *imagePix = psVectorAllocEmpty(PIXELS_BUFFER, PS_TYPE_F32); // Corresponding values for image
    psVector *weightPix = psVectorAllocEmpty(PIXELS_BUFFER, PS_TYPE_F32); // Corresponding values for weight
    psVector *maskPix = psVectorAllocEmpty(PIXELS_BUFFER, PS_TYPE_MASK); // Corresponding values for mask

    long numBad = 0;                    // Number of bad pixels interpolated
    for (int y = 0; y < numRows; y++) {
        for (int x = 0; x < numCols; x++) {
            if (mask->data.PS_TYPE_MASK_DATA[y][x] & maskVal) {
                double imageValue, weightValue; // Image and weight value from interpolation
                psMaskType maskValue = 0; // Mask value from interpolation
                psImageInterpolateStatus status = psImageInterpolate(&imageValue, &weightValue, &maskValue,
                                                                     x, y, interp);
                if (status == PS_INTERPOLATE_STATUS_ERROR || status == PS_INTERPOLATE_STATUS_OFF) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to interpolate readout at %d,%d", x, y);
                    psFree(interp);
                    psFree(pixels);
                    psFree(imagePix);
                    psFree(weightPix);
                    psFree(maskPix);
                    return false;
                }
                if (status == PS_INTERPOLATE_STATUS_BAD) {
                    // It's still bad: couldn't interpolate enough
                    continue;
                }

                pixels = psPixelsAdd(pixels, PIXELS_BUFFER, x, y);
                imagePix = psVectorExtend(imagePix, PIXELS_BUFFER, 1);
                weightPix = psVectorExtend(weightPix, PIXELS_BUFFER, 1);
                maskPix = psVectorExtend(maskPix, PIXELS_BUFFER, 1);
                imagePix->data.F32[numBad] = imageValue;
                weightPix->data.F32[numBad] = weightValue;
                maskPix->data.PS_TYPE_MASK_DATA[numBad] = maskValue;
                numBad++;
            }
        }
    }

    psFree(interp);

    for (long i = 0; i < numBad; i++) {
        int x = pixels->data[i].x, y = pixels->data[i].y; // Coordinates of pixel
        image->data.F32[y][x] = imagePix->data.F32[i];
        weight->data.F32[y][x] = weightPix->data.F32[i];
        mask->data.PS_TYPE_MASK_DATA[y][x] = maskPix->data.PS_TYPE_MASK_DATA[i];
    }

    psFree(pixels);
    psFree(imagePix);
    psFree(weightPix);
    psFree(maskPix);

    psLogMsg("psModules.camera", PS_LOG_INFO, "Interpolated over %ld pixels", numBad);

    return true;
}
