/** @file pmSubtraction.c
 *
 *  @author Paul Price, IfA
 *  @author GLG, MHPCC
 *
 *  Copyright 2004-2007 Institute for Astronomy, University of Hawaii
 *
 */

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

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <pslib.h>

#include "pmHDU.h"                      // Required for pmFPA.h
#include "pmFPA.h"
#include "pmSubtractionStamps.h"
#include "pmSubtractionEquation.h"

#include "pmSubtraction.h"

//#define TESTING

#define PIXEL_LIST_BUFFER 100           // Number of entries to add to pixel list at a time
#define MIN_SAMPLE_STATS    7           // Minimum number to use sample statistics; otherwise use quartiles

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Private (file-static) functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

// Generate the kernel to apply to the variance from the normal kernel
static psKernel *varianceKernel(psKernel *out, // Output kernel
                                psKernel *normalKernel // Normal kernel
                                )
{
    // Kernel range
    int xMin = normalKernel->xMin, xMax = normalKernel->xMax;
    int yMin = normalKernel->yMin, yMax = normalKernel->yMax;

    if (!out) {
        out = psKernelAlloc(xMin, xMax, yMin, yMax);
    }

    // Take the square of the normal kernel
    double sumNormal = 0.0, sumVariance = 0.0; // Sum of the normal and variance kernels
    for (int v = yMin; v <= yMax; v++) {
        for (int u = xMin; u <= xMax; u++) {
            float value = normalKernel->kernel[v][u]; // Value of interest
            float value2 = PS_SQR(value); // Value squared
            sumNormal += value;
            sumVariance += value2;
            out->kernel[v][u] = value2;
        }
    }

    // Normalise so that the sum of the variance kernel is the square of the sum of the normal kernel
    // This is required to keep the relative scaling between the image and the weight map
    psBinaryOp(out->image, out->image, "*", psScalarAlloc(PS_SQR(sumNormal) / sumVariance, PS_TYPE_F32));

    return out;
}

// Generate an image of the solved kernel
static psKernel *solvedKernel(psKernel *kernel, // Kernel, to return
                              const pmSubtractionKernels *kernels, // Kernel basis functions
                              const psImage *polyValues, // Spatial polynomial values
                              bool wantDual // Want the dual (second) kernel?
                              )
{
    assert(kernels);
    assert(polyValues);

    int numKernels = kernels->num;      // Number of kernel basis functions
    int size = kernels->size;           // Kernel half-size
    if (!kernel) {
        kernel = psKernelAlloc(-size, size, -size, size);
    }
    psImageInit(kernel->image, 0.0);

    for (int i = 0; i < numKernels; i++) {
        double value = p_pmSubtractionSolutionCoeff(kernels, polyValues, i, wantDual); // Polynomial value

        switch (kernels->type) {
          case PM_SUBTRACTION_KERNEL_POIS: {
              int u = kernels->u->data.S32[i]; // Offset in x
              int v = kernels->v->data.S32[i]; // Offset in y
              kernel->kernel[v][u] += value;
              kernel->kernel[0][0] -= value;
              break;
          }
          /* SPAM and FRIES use the same method */
          case PM_SUBTRACTION_KERNEL_SPAM:
          case PM_SUBTRACTION_KERNEL_FRIES: {
              int uStart = kernels->u->data.S32[i];
              int uStop = kernels->uStop->data.S32[i];
              int vStart = kernels->v->data.S32[i];
              int vStop = kernels->vStop->data.S32[i];

              // Normalising sum of kernel component to unity
              float norm = 1.0 / (float)((uStop - uStart + 1) * (vStop - vStart + 1));

              for (int v = vStart; v <= vStop; v++) {
                  for (int u = uStart; u <= uStop; u++) {
                      kernel->kernel[v][u] += norm * value;
                      kernel->kernel[0][0] -= value;
                  }
              }
              break;
          }
          case PM_SUBTRACTION_KERNEL_GUNK: {
              if (i < kernels->inner) {
                  // Using pre-calculated function
                  psKernel *preCalc = kernels->preCalc->data[i]; // Precalculated values
                  // Iterating over the kernel
                  for (int v = -size; v <= size; v++) {
                      for (int u = -size; u <= size; u++) {
                          kernel->kernel[v][u] += preCalc->kernel[v][u] * value;
                          // Photometric scaling is built into the preCalc kernel --- no subtraction!
                      }
                  }
              } else {
                  // Using delta function
                  int u = kernels->u->data.S32[i]; // Offset in x
                  int v = kernels->v->data.S32[i]; // Offset in y
                  kernel->kernel[v][u] += value;
                  kernel->kernel[0][0] -= value;
              }
              break;
          }
          case PM_SUBTRACTION_KERNEL_ISIS: {
              psKernel *preCalc = kernels->preCalc->data[i]; // Precalculated values
              // Iterating over the kernel
              for (int v = -size; v <= size; v++) {
                  for (int u = -size; u <= size; u++) {
                      kernel->kernel[v][u] += preCalc->kernel[v][u] * value;
                      // Photometric scaling is built into the preCalc kernel --- no subtraction!
                  }
              }
              break;
          }
          case PM_SUBTRACTION_KERNEL_RINGS: {
              psArray *preCalc = kernels->preCalc->data[i]; // Precalculated data
              psVector *uCoords = preCalc->data[0]; // u coordinates
              psVector *vCoords = preCalc->data[1]; // v coordinates
              psVector *poly = preCalc->data[2]; // Polynomial values
              int num = uCoords->n;     // Number of pixels

              for (int j = 0; j < num; j++) {
                  int u = uCoords->data.S32[j], v = vCoords->data.S32[j]; // Kernel coordinates
                  kernel->kernel[v][u] += poly->data.F32[j] * value;
              }
              // Photometric scaling is built into the kernel --- no subtraction!
              break;
          }
          default:
            psAbort("Should never get here.");
        }
    }

    // Put in the normalisation component
    kernel->kernel[0][0] += (wantDual ? 1.0 : p_pmSubtractionSolutionNorm(kernels));

    return kernel;
}

// Subtract the (0,0) element to preserve photometric scaling
static void convolveSub(psKernel *convolved, // Convolved image
                        const psKernel *image, // Image being convolved
                        int footprint  // Size of region of interest
                        )
{
    // Can't use psBinaryOp because the images are of different size
    for (int y = -footprint; y <= footprint; y++) {
        for (int x = -footprint; x <= footprint; x++) {
            convolved->kernel[y][x] -= image->kernel[y][x];
        }
    }
    return;
}

// Generate the convolution given some offset
static psKernel *convolveOffset(const psKernel *image, // Image to convolve (a kernel for convenience)
                                int u, int v, // Offset to apply
                                int footprint // Size of region of interest
                                )
{
    psKernel *convolved = psKernelAlloc(-footprint, footprint, -footprint, footprint); // Convolved image
    int numBytes = (2 * footprint + 1) * PSELEMTYPE_SIZEOF(PS_TYPE_F32); // Number of bytes to copy
    for (int y = -footprint; y <= footprint; y++) {
        // Convolution with a delta function is just the value specified by the offset
        memcpy(&convolved->kernel[y][-footprint], &image->kernel[y - v][-footprint - u], numBytes);
    }
    return convolved;
}


// Convolve an image using FFT
static void convolveFFT(psImage *target,// Place the result in here
                        const psImage *image, // Image to convolve
                        const psImage *mask, // Mask image
                        psMaskType maskVal, // Value to mask
                        const psKernel *kernel, // Kernel by which to convolve
                        psRegion region,// Region of interest
                        float background, // Background to add
                        int size        // Size of (square) kernel
                        )
{
    psRegion border = psRegionSet(region.x0 - size, region.x1 + size,
                                  region.y0 - size, region.y1 + size); // Add a border

    // Casting away const so psImageSubset can add the child
    psImage *subImage = psImageSubset((psImage*)image, border); // Subimage to convolve
    psImage *subMask = mask ? psImageSubset((psImage*)mask, border) : NULL; // Subimage mask
    psImage *convolved = psImageConvolveFFT(NULL, subImage, subMask, maskVal, kernel); // Convolution
    psFree(subImage);
    psFree(subMask);

    // Now, we have to chop off the borders, and stick it in where it belongs
    psImage *subConv = psImageSubset(convolved, psRegionSet(size, -size, size, -size)); // Cut off the edges
    psImage *subTarget = psImageSubset(target, region); // Target for this subregion
    if (background != 0.0) {
        psBinaryOp(subTarget, subConv, "+", psScalarAlloc(background, PS_TYPE_F32));
    } else {
        int numBytes = subTarget->numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F32); // Number of bytes to copy
        for (int y = 0; y < subTarget->numRows; y++) {
            memcpy(subTarget->data.F32[y], subConv->data.F32[y], numBytes);
        }
    }
    psFree(subConv);
    psFree(convolved);
    psFree(subTarget);
    return;
}

// Convolve an image directly
static void convolveDirect(psImage *target, // Put the result here
                           const psImage *image, // Image to convolve
                           const psKernel *kernel, // Kernel by which to convolve
                           psRegion region,// Region of interest
                           float background, // Background to add
                           int size        // Size of (square) kernel
                           )
{
    for (int y = region.y0; y < region.y1; y++) {
        for (int x = region.x0; x < region.x1; x++) {
            target->data.F32[y][x] = background;
            for (int v = -size; v <= size; v++) {
                for (int u = -size; u <= size; u++) {
                    target->data.F32[y][x] += kernel->kernel[v][u] *
                        image->data.F32[y - v][x - u];
                }
            }
        }
    }
    return;
}

// Convolve a region of an image
static inline void convolveRegion(psImage *convImage, // Convolved image (output)
                                  psImage *convWeight, // Convolved weight map (output), or NULL
                                  psKernel **kernelImage, // Convolution kernel for the image
                                  psKernel **kernelWeight, // Convolution kernel for the weight map, or NULL
                                  const psImage *image, // Image to convolve
                                  const psImage *weight, // Weight map to convolve, or NULL
                                  const psImage *subMask, // Subtraction mask
                                  psMaskType maskVal, // Mask value to apply in convolution
                                  const pmSubtractionKernels *kernels, // Kernels
                                  psImage *polyValues, // Polynomial values
                                  float background, // Background value to apply
                                  psRegion region, // Region to convolve
                                  bool useFFT,  // Use FFT to convolve?
                                  bool wantDual // Want the dual convolution?
    )
{
    *kernelImage = solvedKernel(*kernelImage, kernels, polyValues, wantDual);
    if (weight) {
        *kernelWeight = varianceKernel(*kernelWeight, *kernelImage);
    }

    if (useFFT) {
        // Use Fast Fourier Transform to do the convolution
        // This provides a big speed-up for large kernels
        convolveFFT(convImage, image, subMask, maskVal, *kernelImage, region, background, kernels->size);
        if (weight) {
            convolveFFT(convWeight, weight, subMask, maskVal, *kernelWeight, region, 0.0, kernels->size);
        }
    } else {
        convolveDirect(convImage, image, *kernelImage, region, background, kernels->size);
        if (weight) {
            convolveDirect(convWeight, weight, *kernelWeight, region, 0.0, kernels->size);
        }
    }

    return;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Semi-public functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

psKernel *p_pmSubtractionConvolveStampPrecalc(const psKernel *image, const psKernel *kernel)
{
    PS_ASSERT_KERNEL_NON_NULL(image, NULL);
    PS_ASSERT_KERNEL_NON_NULL(kernel, NULL);

    psImage *conv = psImageConvolveFFT(NULL, image->image, NULL, 0, kernel); // Convolved image
    int x0 = - image->xMin, y0 = - image->yMin; // Position of centre of convolved image
    psKernel *convolved = psKernelAllocFromImage(conv, x0, y0); // Kernel version
    psFree(conv);
    return convolved;
}

psImage *p_pmSubtractionPolynomial(psImage *output, int spatialOrder, float x, float y)
{
    assert(spatialOrder >= 0);
    assert(x >= -1 && x <= 1);
    assert(y >= -1 && y <= 1);

    output = psImageRecycle(output, spatialOrder + 1, spatialOrder + 1, PS_TYPE_F64);
    output->data.F64[0][0] = 1.0;

    double value = 1.0;
    for (int i = 1; i <= spatialOrder; i++) {
        value *= x;
        output->data.F64[0][i] = value;
    }

    value = 1.0;
    for (int j = 1; j <= spatialOrder; j++) {
        value *= y;
        output->data.F64[j][0] = value;
    }

    for (int j = 1; j <= spatialOrder; j++) {
        for (int i = 1; i <= spatialOrder - j; i++) {
            output->data.F64[j][i] = output->data.F64[j][0] * output->data.F64[0][i];
        }
    }

    return output;
}

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


// Convolve a stamp by a single kernel basis function
static psKernel *convolveStampSingle(const pmSubtractionKernels *kernels, // Kernel basis functions
                                     int index, // Kernel basis function index
                                     const psKernel *image, // Image to convolve (a kernel for convenience)
                                     int footprint // Size of region of interest
    )
{
#if 0
    if (index == kernels->num) {
        // Normalisation component
        return convolveOffset(image, 0, 0, footprint);
    }
#endif

    switch (kernels->type) {
      case PM_SUBTRACTION_KERNEL_POIS: {
          int u = kernels->u->data.S32[index]; // Offset in x
          int v = kernels->v->data.S32[index]; // Offset in y
          psKernel *convolved = convolveOffset(image, u, v, footprint); // Convolved image
          convolveSub(convolved, image, footprint);
          return convolved;
      }
        // Method for SPAM and FRIES is the same
      case PM_SUBTRACTION_KERNEL_SPAM:
      case PM_SUBTRACTION_KERNEL_FRIES: {
          psKernel *convolved = psKernelAlloc(-footprint, footprint,
                                              -footprint, footprint); // Convolved image
          int uStart = kernels->u->data.S32[index];
          int uStop = kernels->uStop->data.S32[index];
          int vStart = kernels->v->data.S32[index];
          int vStop = kernels->vStop->data.S32[index];
          float norm = 1.0 / (uStop - uStart + 1) * (vStop - vStart + 1); // Normalisation
          for (int y = -footprint; y <= footprint; y++) {
              for (int x = -footprint; x <= footprint; x++) {
                  double sum = 0.0;
                  for (int v = vStart; v <= vStop; v++) {
                      for (int u = uStart; u <= uStop; u++) {
                          sum += image->kernel[y - v][x - u];
                      }
                  }
                  convolved->kernel[y][x] = norm * sum;
              }
          }
          convolveSub(convolved, image, footprint);
          return convolved;
      }
      case PM_SUBTRACTION_KERNEL_GUNK: {
          if (index < kernels->inner) {
              // Photometric scaling is already built in to the precalculated kernel
              return p_pmSubtractionConvolveStampPrecalc(image, kernels->preCalc->data[index]);
          }
          // Using delta function
          int u = kernels->u->data.S32[index]; // Offset in x
          int v = kernels->v->data.S32[index]; // Offset in y
          psKernel *convolved = convolveOffset(image, u, v, footprint); // Convolved image
          convolveSub(convolved, image, footprint);
          return convolved;
      }
      case PM_SUBTRACTION_KERNEL_ISIS: {
          // Photometric scaling is already built in to the precalculated kernel
          return p_pmSubtractionConvolveStampPrecalc(image, kernels->preCalc->data[index]);
      }
      case PM_SUBTRACTION_KERNEL_RINGS: {
          psKernel *convolved = psKernelAlloc(-footprint, footprint,
                                              -footprint, footprint); // Convolved image
          psArray *preCalc = kernels->preCalc->data[index]; // Precalculated data
          psVector *uCoords = preCalc->data[0]; // u coordinates
          psVector *vCoords = preCalc->data[1]; // v coordinates
          psVector *poly = preCalc->data[2]; // Polynomial values
          int num = uCoords->n;         // Number of pixels
          for (int y = -footprint; y <= footprint; y++) {
              for (int x = -footprint; x <= footprint; x++) {
                  double sum = 0.0;             // Accumulated sum from convolution
                  for (int j = 0; j < num; j++) {
                      int u = uCoords->data.S32[j], v = vCoords->data.S32[j]; // Kernel coordinates
                      sum += image->kernel[y - v][x - u] * poly->data.F32[j];
                  }
                  convolved->kernel[y][x] = sum;
                  // Photometric scaling is built into the kernel --- no subtraction!
              }
          }
          return convolved;
      }
      default:
        psAbort("Should never get here.");
    }
    return NULL;
}

// Convolve the stamp by each of the kernel basis functions
static psArray *convolveStamp(psArray *convolutions, // The convolutions
                              const psKernel *image, // Image to convolve
                              const pmSubtractionKernels *kernels, // Kernel basis functions
                              int footprint // Stamp half-size
    )
{
    assert(image);
    assert(kernels);
    assert(footprint >= 0);

    if (convolutions) {
        // Already done
        return convolutions;
    }

    int numKernels = kernels->num;      // Number of kernels
    convolutions = psArrayAlloc(numKernels);

    for (int i = 0; i < numKernels; i++) {
        convolutions->data[i] = convolveStampSingle(kernels, i, image, footprint);
    }

    return convolutions;
}


bool pmSubtractionConvolveStamp(pmSubtractionStamp *stamp, const pmSubtractionKernels *kernels, int footprint)
{
    PS_ASSERT_PTR_NON_NULL(stamp, false);
    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, false);
    PS_ASSERT_INT_NONNEGATIVE(footprint, false);

    if (stamp->status != PM_SUBTRACTION_STAMP_CALCULATE) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Stamp not marked for calculation.");
        return false;
    }

    switch (kernels->mode) {
      case PM_SUBTRACTION_MODE_1:
        stamp->convolutions1 = convolveStamp(stamp->convolutions1, stamp->image1, kernels, footprint);
        break;
      case PM_SUBTRACTION_MODE_2:
        stamp->convolutions2 = convolveStamp(stamp->convolutions2, stamp->image2, kernels, footprint);
        break;
      case PM_SUBTRACTION_MODE_UNSURE:
      case PM_SUBTRACTION_MODE_DUAL:
        stamp->convolutions1 = convolveStamp(stamp->convolutions1, stamp->image1, kernels, footprint);
        stamp->convolutions2 = convolveStamp(stamp->convolutions2, stamp->image2, kernels, footprint);
        break;
      default:
        psAbort("Unsupported subtraction mode: %x", kernels->mode);
    }

    return true;
}




int pmSubtractionRejectStamps(float *rmsPtr, int *numPtr, pmSubtractionStampList *stamps,
                              const psVector *deviations, psImage *subMask, float sigmaRej,
                              int footprint)
{
    PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, -1);
    PS_ASSERT_VECTOR_NON_NULL(deviations, -1);
    PS_ASSERT_VECTOR_TYPE(deviations, PS_TYPE_F32, -1);
    PS_ASSERT_IMAGE_NON_EMPTY(subMask, -1);
    PS_ASSERT_IMAGE_TYPE(subMask, PS_TYPE_MASK, -1);

    // I used to measure the rms deviation about zero, and use that as the sigma against which to clip, but
    // the distribution is actually something like a chi^2 or Student's t, both of which become Gaussian-like
    // with large N.  Therefore, let's just treat this as a Gaussian distribution.

    int numStamps = 0;                  // Number of used stamps
    psVector *mask = psVectorAlloc(stamps->num, PS_TYPE_MASK); // Mask, for statistics
    psVectorInit(mask, 0);
    for (int i = 0; i < stamps->num; i++) {
        pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
        if (stamp->status != PM_SUBTRACTION_STAMP_USED) {
            mask->data.PS_TYPE_MASK_DATA[i] = 0xff;
            continue;
        }
        numStamps++;
    }
    psTrace("psModules.imcombine", 1, "Number of good stamps: %d\n", numStamps);

    if (numStamps == 0) {
        psError(PS_ERR_UNKNOWN, true, "No good stamps found.");
        psFree(mask);
        return -1;
    }

    psStats *stats = psStatsAlloc(PS_STAT_SAMPLE_MEAN | PS_STAT_SAMPLE_STDEV |
                                  PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_QUARTILE); // Statistics for deviatns
    if (!psVectorStats(stats, deviations, NULL, mask, 0xff)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to measure statistics for deviations.");
        psFree(stats);
        psFree(mask);
        return -1;
    }
    psFree(mask);

    double mean, rms;                 // Mean and RMS of deviations
    if (numStamps < MIN_SAMPLE_STATS) {
        mean = stats->sampleMean;
        rms = stats->sampleStdev;
    } else {
        mean = stats->sampleMedian;
        rms = 0.74 * (stats->sampleUQ - stats->sampleLQ);
    }

    psTrace("psModules.imcombine", 1, "Mean: %f\n", mean);
    psTrace("psModules.imcombine", 1, "RMS deviation: %f\n", rms);

    if (rmsPtr) {
        *rmsPtr = rms;
    }
    if (numPtr) {
        *numPtr = numStamps;
    }

    if (!isfinite(sigmaRej) || sigmaRej <= 0.0) {
        // User just wanted to calculate and record the RMS for posterity
        psLogMsg("psModules.imcombine", PS_LOG_INFO, "RMS deviation from %d stamps: %lf",
                 numStamps, rms);
        return 0;
    }

    float limit = sigmaRej * rms; // Limit on maximum deviation
    psTrace("psModules.imcombine", 1, "Deviation limit: %f\n", limit);

    int numRejected = 0;                // Number of stamps rejected
    int numGood = 0;                    // Number of good stamps
    double newMean = 0.0;               // New mean
    for (int i = 0; i < stamps->num; i++) {
        pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
        if (stamp->status == PM_SUBTRACTION_STAMP_USED) {
            // Should we reject stars with low deviation?  Well, if this is really a Gaussian-like
            // distribution and they're low, then we have the right to ask why.  Isn't it suspicious that
            // they're anomalously low, compared to the rest of the population which (we hope) is indicative
            // of normality?  Besides, the standard deviation is going to be blown up by stars that didn't
            // subtract well, in which case very few (if any) stars will be legitimately rejected for being
            // low.
            if (deviations->data.F32[i] - mean > limit) {
                // Mask out the stamp in the image so you it's not found again
                psTrace("psModules.imcombine", 3, "Rejecting stamp %d (%d,%d)\n", i,
                        (int)(stamp->x + 0.5), (int)(stamp->y + 0.5));
                numRejected++;
                for (int y = stamp->y - footprint; y <= stamp->y + footprint; y++) {
                    for (int x = stamp->x - footprint; x <= stamp->x + footprint; x++) {
                        subMask->data.PS_TYPE_MASK_DATA[y][x] |= PM_SUBTRACTION_MASK_REJ;
                    }
                }

                // Set stamp for replacement
                stamp->x = 0;
                stamp->y = 0;
                stamp->xNorm = NAN;
                stamp->yNorm = NAN;
                stamp->status = PM_SUBTRACTION_STAMP_REJECTED;
                // Recalculate convolutions
                psFree(stamp->convolutions1);
                psFree(stamp->convolutions2);
                stamp->convolutions1 = stamp->convolutions2 = NULL;
                psFree(stamp->image1);
                psFree(stamp->image2);
                psFree(stamp->weight);
                stamp->image1 = stamp->image2 = stamp->weight = NULL;
                psFree(stamp->matrix1);
                psFree(stamp->matrix2);
                psFree(stamp->matrixX);
                stamp->matrix1 = stamp->matrix2 = stamp->matrixX = NULL;
                psFree(stamp->vector1);
                psFree(stamp->vector2);
                stamp->vector1 = stamp->vector2 = NULL;
            } else {
                numGood++;
                newMean += deviations->data.F32[i];
            }
        }
    }
    newMean /= numGood;

    if (numRejected > 0) {
        psLogMsg("psModules.imcombine", PS_LOG_INFO,
                 "%d good stamps; %d rejected.\nMean deviation: %lf --> %lf\n",
                 numGood, numRejected, mean, newMean);
    } else {
        psLogMsg("psModules.imcombine", PS_LOG_INFO,
                 "%d good stamps; 0 rejected.\nMean deviation: %lf\n",
                 numGood, mean);
    }

    return numRejected;
}

psImage *pmSubtractionKernelImage(const pmSubtractionKernels *kernels, float x, float y, bool wantDual)
{
    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, NULL);
    PM_ASSERT_SUBTRACTION_KERNELS_SOLUTION(kernels, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(x, -1.0, 1.0, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(y, -1.0, 1.0, NULL);

    // Precalulate polynomial values
    psImage *polyValues = p_pmSubtractionPolynomial(NULL, kernels->spatialOrder, x, y);

    // The appropriate kernel
    psKernel *kernel = solvedKernel(NULL, kernels, polyValues, wantDual);

    psFree(polyValues);

    psImage *image = psMemIncrRefCounter(kernel->image); // Image of the kernel
    psFree(kernel);

    return image;
}


float pmSubtractionVarianceFactor(const pmSubtractionKernels *kernels, float x, float y, bool wantDual)
{
    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, NAN);
    PM_ASSERT_SUBTRACTION_KERNELS_SOLUTION(kernels, NAN);
    PS_ASSERT_FLOAT_WITHIN_RANGE(x, -1.0, 1.0, NAN);
    PS_ASSERT_FLOAT_WITHIN_RANGE(y, -1.0, 1.0, NAN);

    // Precalulate polynomial values
    psImage *polyValues = p_pmSubtractionPolynomial(NULL, kernels->spatialOrder, x, y);

    psKernel *kernel = solvedKernel(NULL, kernels, polyValues, wantDual); // The appropriate kernel
    psFree(polyValues);

    double sumKernel2 = 0.0;            // Sum of the kernel squared
    double sumKernel = 0.0;             // Sum of the kernel
    for (int y = kernel->yMin; y <= kernel->yMax; y++) {
        for (int x = kernel->xMin; x <= kernel->xMax; x++) {
            sumKernel += kernel->kernel[y][x];
            sumKernel2 += PS_SQR(kernel->kernel[y][x]);
        }
    }

    psFree(kernel);

    return sumKernel2 / PS_SQR(sumKernel);
}

#if 0
psArray *pmSubtractionKernelSolutions(const pmSubtractionKernels *kernels, float x, float y, bool wantDual)
{
    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, NULL);
    PM_ASSERT_SUBTRACTION_KERNELS_SOLUTION(kernels, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(x, -1.0, 1.0, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(y, -1.0, 1.0, NULL);

    psArray *images = psArrayAlloc(solution->n - 1); // Images of each kernel to return
    psVector *fakeSolution = psVectorAlloc(solution->n, PS_TYPE_F64); // Fake solution vector
    psVectorInit(fakeSolution, 0.0);

    for (int i = 0; i < solution->n - 1; i++) {
        fakeSolution->data.F64[i] = solution->data.F64[i];
        images->data[i] = pmSubtractionKernelImage(kernels, x, y, wantDual);
        fakeSolution->data.F64[i] = 0.0;
    }

    psFree(fakeSolution);

    return images;
}
#endif



bool pmSubtractionConvolve(pmReadout *out1, pmReadout *out2, const pmReadout *ro1, const pmReadout *ro2,
                           const psImage *subMask, psMaskType blank, const psRegion *region,
                           const pmSubtractionKernels *kernels, bool doBG, bool useFFT)
{
    int numCols = 0, numRows = 0;       // Image dimensions
    int x0 = 0, y0 = 0;                 // Image offset
    if (kernels->mode == PM_SUBTRACTION_MODE_1 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
        PM_ASSERT_READOUT_NON_NULL(out1, false);
        PM_ASSERT_READOUT_NON_NULL(ro1, false);
        PM_ASSERT_READOUT_IMAGE(ro1, false);
        numCols = ro1->image->numCols;
        numRows = ro1->image->numRows;
        x0 = ro1->col0;
        y0 = ro1->row0;
    }
    if (kernels->mode == PM_SUBTRACTION_MODE_2 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
        PM_ASSERT_READOUT_NON_NULL(out2, false);
        PM_ASSERT_READOUT_NON_NULL(ro2, false);
        PM_ASSERT_READOUT_IMAGE(ro2, false);
        if (numCols == 0 && numRows == 0) {
            numCols = ro2->image->numCols;
            numRows = ro2->image->numRows;
            x0 = ro2->col0;
            y0 = ro2->row0;
        }
    }
    if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
        PS_ASSERT_IMAGES_SIZE_EQUAL(ro1->image, ro2->image, false);
    }
    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, false);
    PM_ASSERT_SUBTRACTION_KERNELS_SOLUTION(kernels, false);
    if (subMask) {
        PS_ASSERT_IMAGE_NON_NULL(subMask, false);
        PS_ASSERT_IMAGE_TYPE(subMask, PS_TYPE_MASK, false);
        PS_ASSERT_IMAGE_SIZE(subMask, numCols, numRows, false);
    }
    if (region && psRegionIsNaN(*region)) {
        psString string = psRegionToString(*region);
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Input region (%s) contains NAN values", string);
        psFree(string);
        return false;
    }

    // Inputs
    psImage *image1 = NULL, *weight1 = NULL; // Image and weight map from input 1
    if (ro1) {
        image1 = ro1->image;
        weight1 = ro1->weight;
    }
    psImage *image2 = NULL, *weight2 = NULL; // Image and weight map from input 2
    if (ro2) {
        image2 = ro2->image;
        weight2 = ro2->weight;
    }

    // Outputs
    psImage *convImage1 = NULL, *convWeight1 = NULL; // Convolved image and weight from input 1
    if (kernels->mode == PM_SUBTRACTION_MODE_1 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
        convImage1 = out1->image;
        if (!convImage1) {
            convImage1 = out1->image = psImageAlloc(numCols, numRows, PS_TYPE_F32);
        }
        if (weight1) {
            if (!out1->weight) {
                out1->weight = psImageAlloc(numCols, numRows, PS_TYPE_F32);
            }
            convWeight1 = out1->weight;
            psImageInit(convWeight1, 0.0);
        }
    }
    psImage *convImage2 = NULL, *convWeight2 = NULL; // Convolved image and weight from input 2
    if (kernels->mode == PM_SUBTRACTION_MODE_2 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
        convImage2 = out2->image;
        if (!convImage2) {
            convImage2 = out2->image = psImageAlloc(numCols, numRows, PS_TYPE_F32);
        }
        if (weight2) {
            if (!out2->weight) {
                out2->weight = psImageAlloc(numCols, numRows, PS_TYPE_F32);
            }
            convWeight2 = out2->weight;
            psImageInit(convWeight2, 0.0);
        }
    }
    psImage *convMask = NULL;           // Convolved mask image (common to inputs 1 and 2)
    if (subMask) {
        if (kernels->mode == PM_SUBTRACTION_MODE_1 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
            if (!out1->mask) {
                out1->mask = psImageAlloc(numCols, numRows, PS_TYPE_MASK);
            }
            convMask = out1->mask;
        }
        if (kernels->mode == PM_SUBTRACTION_MODE_2 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
            if (convMask) {
                if (out2->mask) {
                    psFree(out2->mask);
                }
                out2->mask = psMemIncrRefCounter(convMask);
            } else {
                if (!out2->mask) {
                    out2->mask = psImageAlloc(numCols, numRows, PS_TYPE_MASK);
                }
                convMask = out2->mask;
            }
        }
        psImageInit(convMask, 0);
    }


    int size = kernels->size;           // Half-size of kernel
    int fullSize = 2 * size + 1;        // Full size of kernel

    // Get region for convolution: [xMin:xMax,yMin:yMax]
    int xMin = size, xMax = numCols - size;
    int yMin = size, yMax = numRows - size;
    if (region) {
        xMin = PS_MAX(region->x0, xMin);
        xMax = PS_MIN(region->x1, xMax);
        yMin = PS_MAX(region->y0, yMin);
        yMax = PS_MIN(region->y1, yMax);
    }

    // Size to use when calculating normalised coordinates (different from actual size when convolving
    // subimage)
    int xNormSize = (kernels->numCols > 0 ? kernels->numCols : numCols);
    int yNormSize = (kernels->numRows > 0 ? kernels->numRows : numRows);

    psMaskType maskSource;              // Mask these pixels when convolving
    psMaskType maskTarget;              // Mark these pixels as bad when propagating the subtractionMask
    switch (kernels->mode) {
      case PM_SUBTRACTION_MODE_1:
        maskSource = PM_SUBTRACTION_MASK_BAD_1;
        maskTarget = PM_SUBTRACTION_MASK_BAD_2 | PM_SUBTRACTION_MASK_CONVOLVE_1;
        break;
      case PM_SUBTRACTION_MODE_2:
        maskSource = PM_SUBTRACTION_MASK_BAD_2;
        maskTarget = PM_SUBTRACTION_MASK_BAD_1 | PM_SUBTRACTION_MASK_CONVOLVE_2;
        break;
      case PM_SUBTRACTION_MODE_DUAL:
        maskSource = PM_SUBTRACTION_MASK_BAD_1 | PM_SUBTRACTION_MASK_BAD_2;
        maskTarget = PM_SUBTRACTION_MASK_CONVOLVE_1 | PM_SUBTRACTION_MASK_CONVOLVE_2;
        break;
      default:
        psAbort("Unsupported subtraction mode: %x", kernels->mode);
    }

    psImage *polyValues = NULL;         // Pre-calculated polynomial values
    psKernel *kernelImage = NULL;       // Kernel for the images
    psKernel *kernelWeight = NULL;      // Kernel for the weight maps

    for (int j = yMin; j < yMax; j += fullSize) {
        int ySubMax = PS_MIN(j + fullSize, yMax); // Range for subregion of interest
        float yNorm = 2.0 * (float)(j + y0 + size + 1 - yNormSize/2.0) /
            (float)yNormSize; // Normalised coordinate
        for (int i = xMin; i < xMax; i += fullSize) {
            int xSubMax = PS_MIN(i + fullSize, xMax); // Range for subregion of interest
            float xNorm = 2.0 * (float)(i + x0 + size + 1 - xNormSize/2.0) /
                (float)xNormSize; // Normalised coordinate

            // Only generate polynomial values every kernel footprint, since we have already assumed
            // (with the stamps) that it does not vary rapidly on this scale.
            polyValues = p_pmSubtractionPolynomial(polyValues, kernels->spatialOrder, xNorm, yNorm);
            float background = doBG ? p_pmSubtractionSolutionBackground(kernels, polyValues) :
                0.0; // Background term
            psRegion subRegion = psRegionSet(i, xSubMax, j, ySubMax); // Sub-region to convolve

            if (kernels->mode == PM_SUBTRACTION_MODE_1 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
                convolveRegion(convImage1, convWeight1, &kernelImage, &kernelWeight, image1, weight1,
                               subMask, maskSource, kernels, polyValues, background, subRegion, useFFT,
                               false);
            }
            if (kernels->mode == PM_SUBTRACTION_MODE_2 || kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
                convolveRegion(convImage2, convWeight2, &kernelImage, &kernelWeight, image2, weight2,
                               subMask, maskSource, kernels, polyValues, background, subRegion, useFFT,
                               kernels->mode == PM_SUBTRACTION_MODE_DUAL);
            }

            // Propagate the mask
            if (subMask) {
                for (int y = j; y < ySubMax; y++) {
                    for (int x = i; x < xSubMax; x++) {
                        if (subMask->data.PS_TYPE_MASK_DATA[y][x] & maskTarget) {
                            convMask->data.PS_TYPE_MASK_DATA[y][x] |= blank;
                        }
                    }
                }
            }
        }
    }
    psFree(kernelImage);
    psFree(kernelWeight);
    psFree(polyValues);

    // Copy anything that wasn't convolved
    switch (kernels->mode) {
      case PM_SUBTRACTION_MODE_1:
        if (out2) {
            out2->image = psMemIncrRefCounter(ro2->image);
            out2->weight = psMemIncrRefCounter(ro2->weight);
            out2->mask = psMemIncrRefCounter(ro2->mask);
        }
        break;
      case PM_SUBTRACTION_MODE_2:
        if (out1) {
            out1->image = psMemIncrRefCounter(ro1->image);
            out1->weight = psMemIncrRefCounter(ro1->weight);
            out1->mask = psMemIncrRefCounter(ro1->mask);
        }
        break;
      case PM_SUBTRACTION_MODE_DUAL:
        break;
      default:
        psAbort("Should never get here.");
    }

    return true;
}
