/** @file pmSubtraction.c
 *
 *  @author Paul Price, IfA
 *  @author GLG, MHPCC
 *
 *  @version $Revision: 1.32 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2007-07-21 04:30:57 $
 *
 *  Copyright 2004-2007 Institute for Astronomy, University of Hawaii
 *
 */

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

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

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

#include "pmSubtraction.h"

#define USE_FUNCTIONS_INSTEAD_OF_MACROS          // Can I pass the address of a static inline function?

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

// Given a stamp coordinates (x,y), generate a matrix where the elements (i,j) are x^i * y^j
static psImage *spatialPolyValues(int spatialOrder, // Maximum spatial polynomial order
                                  float x, float y // Normalised position of interest, [-1,1]
    )
{
    assert(spatialOrder >= 0);
    assert(x >= -1 && x <= 1);
    assert(y >= -1 && y <= 1);

    psImage *polyValues = psImageAlloc(spatialOrder + 1, spatialOrder + 1, PS_TYPE_F64); // Matrix to return
    polyValues->data.F64[0][0] = 1.0;

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

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

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

    return polyValues;
}

#ifndef USE_FUNCTIONS_INSTEAD_OF_MACROS
// Generate an image of the solved kernel
// Meant to replace the following function declaration:
// static inline psKernel *solvedKernel(psKernel *kernel, // Kernel, to return
//                                      const psVector *solution, // Solution to the least-squares problem
//                                      const pmSubtractionKernels *kernels, // Kernel basis functions
//                                      const psImage *polyValues, // Spatial polynomial values
//                                      double (*weightFunc)(double value) // Function for weighting
//     );
// because even though the weightFunc is "static inline", it still appears in the assembly.
// TARGET: Kernel, to return (psKernel*)
// SOLUTION: Solution to the least-squares problem (psVector*)
// KERNELS: Kernel basis functions (pmSubtractionKernels*)
// POLYVALUES: Spatial polynomial values (psImage*)
// FUNC: Function for weighting (double (*weightFunc)(double value))
#define SOLVED_KERNEL(TARGET, SOLUTION, KERNELS, POLYVALUES, FUNC) { \
    int numKernels = (SOLUTION)->n - 1;   /* Number of kernel basis functions */ \
    assert((KERNELS)->u->n == numKernels); \
    assert((KERNELS)->v->n == numKernels); \
    assert((KERNELS)->xOrder->n == numKernels); \
    assert((KERNELS)->yOrder->n == numKernels); \
    \
    /* Ensure the subIndex for POIS kernels is what is expected */ \
    assert(((KERNELS)->type != PM_SUBTRACTION_KERNEL_POIS && \
           (KERNELS)->type != PM_SUBTRACTION_KERNEL_SPAM && \
           (KERNELS)->type != PM_SUBTRACTION_KERNEL_FRIES) || \
           ((KERNELS)->u->data.S32[(KERNELS)->subIndex] == 0 && \
            (KERNELS)->v->data.S32[(KERNELS)->subIndex] == 0 && \
            (KERNELS)->xOrder->data.S32[(KERNELS)->subIndex] == 0 && \
            (KERNELS)->yOrder->data.S32[(KERNELS)->subIndex] == 0)); \
    \
    int size = (KERNELS)->size;           /* Kernel half-size */ \
    if (!(TARGET)) { \
        (TARGET) = psKernelAlloc(-size, size, -size, size); \
    } \
    psImageInit((TARGET)->image, 0.0); \
    \
    for (int i = 0; i < numKernels; i++) { \
        int xOrder = (KERNELS)->xOrder->data.S32[i]; /* Polynomial order in x */ \
        int yOrder = (KERNELS)->yOrder->data.S32[i]; /* Polynomial order in y */ \
        float value = FUNC((POLYVALUES)->data.F64[yOrder][xOrder] * (SOLUTION)->data.F64[i]); \
        float subValue = FUNC(-(SOLUTION)->data.F64[i]); /* Value to subtract (add because of minus) */ \
        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 */ \
              (TARGET)->kernel[v][u] += value; \
              if ((KERNELS)->spatialOrder > 0 && i != (KERNELS)->subIndex) { \
                  /* The (0,0) element is subtracted from most kernels to preserve photometric scaling */ \
                  (TARGET)->kernel[0][0] += subValue; \
              } \
              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]; \
            for (int v = vStart; v <= vStop; v++) { \
                for (int u = uStart; u <= uStop; u++) { \
                    (TARGET)->kernel[v][u] += value; \
                } \
            } \
            if ((KERNELS)->spatialOrder > 0 && i != (KERNELS)->subIndex) { \
                /* The (0,0) element is subtracted from most kernels to preserve photometric scaling */ \
                (TARGET)->kernel[0][0] += subValue; \
            } \
            break; \
          } \
          case PM_SUBTRACTION_KERNEL_GUNK: { \
              if (i < (KERNELS)->inner) { \
                  /* Using pre-calculated function */ \
                  psKernel *preCalc = (KERNELS)->preCalc->data[i]; /* Precalculated values */ \
                  for (int v = -size; v <= size; v++) { \
                      for (int u = -size; u <= size; u++) { \
                          (TARGET)->kernel[v][u] += value * FUNC(preCalc->kernel[v][u]); \
                      } \
                  } \
              } else { \
                  /* Using delta function */ \
                  int u = (KERNELS)->u->data.S32[i]; /* Offset in x */ \
                  int v = (KERNELS)->v->data.S32[i]; /* Offset in y */ \
                  (TARGET)->kernel[v][u] += value; \
              } \
              /* The (0,0) kernel is subtracted from other kernels to preserve photometric scaling */ \
              if ((KERNELS)->spatialOrder > 0 && i != (KERNELS)->subIndex) { \
                  (TARGET)->kernel[0][0] += subValue; \
              } \
              break; \
          } \
          case PM_SUBTRACTION_KERNEL_ISIS: { \
              psKernel *preCalc = (KERNELS)->preCalc->data[i]; /* Precalculated values */ \
              psKernel *subKernel = (KERNELS)->preCalc->data[(KERNELS)->subIndex]; /* Kernel to subtract */ \
              for (int v = -size; v <= size; v++) { \
                  for (int u = -size; u <= size; u++) { \
                      (TARGET)->kernel[v][u] += value * FUNC(preCalc->kernel[v][u]); \
                      /* Subtract (0,0) kernel from other kernels to preserve photometric scaling */ \
                      if ((KERNELS)->spatialOrder > 0 && i != (KERNELS)->subIndex) { \
                          (TARGET)->kernel[v][u] += subValue * FUNC(subKernel->kernel[v][u]); \
                      } \
                  } \
              } \
              break; \
          } \
          case PM_SUBTRACTION_KERNEL_RINGS: { \
              if (i == (KERNELS)->subIndex) { \
                  (TARGET)->kernel[0][0] += value; \
                  break; \
              } \
              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 */ \
                  (TARGET)->kernel[v][u] += value * FUNC(poly->data.F32[j]); \
              } \
              /* The (0,0) kernel is subtracted from other kernels to preserve photometric scaling */ \
              if (kernels->spatialOrder > 0) { \
                  kernel->kernel[0][0] += subValue; \
              } \
              break; \
          } \
          default: \
            psAbort("Should never get here."); \
        } \
    } \
}
#else
// Generate an image of the solved kernel
static inline psKernel *solvedKernel(psKernel *kernel, // Kernel, to return
                                     const psVector *solution, // Solution to the least-squares problem
                                     const pmSubtractionKernels *kernels, // Kernel basis functions
                                     const psImage *polyValues, // Spatial polynomial values
                                     double (*weightFunc)(double value) // Function for weighting
    )
{
    int numKernels = solution->n - 1;   // Number of kernel basis functions
    assert(kernels->u->n == numKernels);
    assert(kernels->v->n == numKernels);
    assert(kernels->xOrder->n == numKernels);
    assert(kernels->yOrder->n == numKernels);

    // Ensure the subIndex for POIS kernels is what is expected
    assert((kernels->type != PM_SUBTRACTION_KERNEL_POIS &&
            kernels->type != PM_SUBTRACTION_KERNEL_SPAM &&
            kernels->type != PM_SUBTRACTION_KERNEL_FRIES) ||
           (kernels->u->data.S32[kernels->subIndex] == 0 && kernels->v->data.S32[kernels->subIndex] == 0 &&
            kernels->xOrder->data.S32[kernels->subIndex] == 0 &&
            kernels->yOrder->data.S32[kernels->subIndex] == 0));

    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++) {
        int xOrder = kernels->xOrder->data.S32[i]; // Polynomial order in x
        int yOrder = kernels->yOrder->data.S32[i]; // Polynomial order in y
        float value = weightFunc(polyValues->data.F64[yOrder][xOrder] *
                                 solution->data.F64[i]); // Value to sum
        float subValue = weightFunc(-solution->data.F64[i]); // Value to subtract (actually add, because of -)
        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;
              if (kernels->spatialOrder > 0 && i != kernels->subIndex) {
                  // The (0,0) element is subtracted from most kernels to preserve photometric scaling
                  kernel->kernel[0][0] += subValue;
              }
              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];
              for (int v = vStart; v <= vStop; v++) {
                  for (int u = uStart; u <= uStop; u++) {
                      kernel->kernel[v][u] += value;
                  }
              }
              if (kernels->spatialOrder > 0 && i != kernels->subIndex) {
                  /* The (0,0) element is subtracted from most kernels to preserve photometric scaling */
                  kernel->kernel[0][0] += subValue;
              }
              break;
          }
          case PM_SUBTRACTION_KERNEL_GUNK: {
              if (i < kernels->inner) {
                  // Using pre-calculated function
                  psKernel *preCalc = kernels->preCalc->data[i]; // Precalculated values
                  for (int v = -size; v <= size; v++) {
                      for (int u = -size; u <= size; u++) {
                          kernel->kernel[v][u] += value * weightFunc(preCalc->kernel[v][u]);
                      }
                  }
              } 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;
              }
              // The (0,0) kernel is subtracted from other kernels to preserve photometric scaling
              if (kernels->spatialOrder > 0 && i != kernels->subIndex) {
                  kernel->kernel[0][0] += subValue;
              }
              break;
          }
          case PM_SUBTRACTION_KERNEL_ISIS: {
              psKernel *preCalc = kernels->preCalc->data[i];// Precalculated values
              psKernel *subKernel = kernels->preCalc->data[kernels->subIndex]; // Kernel to subtract
              for (int v = -size; v <= size; v++) {
                  for (int u = -size; u <= size; u++) {
                      kernel->kernel[v][u] += value * weightFunc(preCalc->kernel[v][u]);
                      // The (0,0) kernel is subtracted from other kernels to preserve photometric scaling
                      if (kernels->spatialOrder > 0 && i != kernels->subIndex) {
                          kernel->kernel[v][u] += subValue * weightFunc(subKernel->kernel[v][u]);
                      }
                  }
              }
              break;
          }
          case PM_SUBTRACTION_KERNEL_RINGS: {
              if (i == kernels->subIndex) {
                  kernel->kernel[0][0] += value;
                  break;
              }
              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] += value * weightFunc(poly->data.F32[j]);
              }

              // The (0,0) kernel is subtracted from other kernels to preserve photometric scaling
              if (kernels->spatialOrder > 0) {
                  kernel->kernel[0][0] += subValue;
              }
              break;
          }
          default:
            psAbort("Should never get here.");
        }
    }

    return kernel;
}
#endif

// Generate the convolved pixel value
static inline double convolvePixel(const pmSubtractionKernels *kernels, // Kernel basis functions
                                   int index, // Kernel basis function index
                                   int x, int y, // Pixel around which to convolve
                                   const psImage *image, // Image to convolve
                                   const psImage *polyValues // Spatial polynomial values
                                   )
{
    int xOrder = kernels->xOrder->data.S32[index]; // Polynomial order in x
    int yOrder = kernels->yOrder->data.S32[index]; // Polynomial order in y
    double polyValue = polyValues->data.F64[yOrder][xOrder]; // Value of spatial polynomial

    switch (kernels->type) {
      case PM_SUBTRACTION_KERNEL_POIS: {
          // Convolution with a delta function is just the value specified by the offset
          int u = kernels->u->data.S32[index]; // Offset in x
          int v = kernels->v->data.S32[index]; // Offset in y
          double value = polyValue * image->data.F32[y + v][x + u]; // Value of convolution
          if (kernels->spatialOrder > 0 && index != kernels->subIndex) {
              // The (0,0) element is subtracted from most kernels to preserve photometric scaling
              value -= image->data.F32[y][x];
          }
          return value;
      }
        // Method for SPAM and FRIES is the same
      case PM_SUBTRACTION_KERNEL_SPAM:
      case PM_SUBTRACTION_KERNEL_FRIES: {
          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];
          double sum = 0.0;
          for (int v = vStart; v <= vStop; v++) {
              for (int u = uStart; u <= uStop; u++) {
                  sum += polyValue * image->data.F32[y + v][x + u];
              }
          }
          if (kernels->spatialOrder > 0 && index != kernels->subIndex) {
              // The (0,0) element is subtracted from most kernels to preserve photometric scaling
              sum -= image->data.F32[y][x];
          }
          return sum;
      }
      case PM_SUBTRACTION_KERNEL_GUNK: {
          double value;                 // The value to return
          if (index < kernels->inner) {
              // Using pre-calculated function
              psKernel *kernel = kernels->preCalc->data[index]; // The convolution kernel
              int size = kernels->size;     // Kernel half-size
              double sum = 0.0;             // Accumulated sum from convolution
              for (int v = -size; v <= size; v++) {
                  for (int u = -size; u <= size; u++) {
                      sum += kernel->kernel[v][u] * image->data.F32[y + v][x + u];
                  }
              }
              value = polyValue * sum;
          } else {
              // Using delta function
              int u = kernels->u->data.S32[index]; // Offset in x
              int v = kernels->v->data.S32[index]; // Offset in y
              value = polyValue * image->data.F32[y + v][x + u]; // Value of convolution
          }
          // The (0,0) delta function is subtracted from most kernels to preserve photometric scaling
          if (kernels->spatialOrder > 0 && index != kernels->subIndex) {
              value -= image->data.F32[y][x];
          }
          return value;
      }
      case PM_SUBTRACTION_KERNEL_ISIS: {
          psKernel *kernel = kernels->preCalc->data[index]; // The convolution kernel
          psKernel *subKernel = kernels->preCalc->data[kernels->subIndex]; // Kernel to subtract
          int size = kernels->size;     // Kernel half-size
          double sum = 0.0;             // Accumulated sum from convolution
          double sub = 0.0;             // Accumulated sum to subtract
          for (int v = -size; v <= size; v++) {
              for (int u = -size; u <= size; u++) {
                  sum += kernel->kernel[v][u] * image->data.F32[y + v][x + u];
                  // The (0,0) kernel is subtracted from other kernels to preserve photometric scaling
                  if (kernels->spatialOrder > 0 && index != kernels->subIndex) {
                      sub += subKernel->kernel[v][u] * image->data.F32[y + v][x + u];
                  }
              }
          }
          return polyValue * sum - sub;
      }
      case PM_SUBTRACTION_KERNEL_RINGS: {
          if (index == kernels->subIndex) {
              return image->data.F32[0][0];
          }
          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
          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->data.F32[y + v][x + u] * poly->data.F32[j];
          }
          // The (0,0) kernel is subtracted from other kernels to preserve photometric scaling
          return polyValue * sum - image->data.F32[0][0];
      }
      default:
        psAbort("Should never get here.");
    }
    return NAN;
}

// Weighting function for use with solvedKernel: no weighting applied, suitable for combining image pixels
static inline double imageWeighting(double value)
{
    return value;
}

// Weighting function for use with solvedKernel: weighting suitable for combining variances
static inline double varianceWeighting(double value)
{
    return PS_SQR(value);
}


// Mark a pixel as blank in the image, mask and weight
static inline void markBlank(psImage *image, // Image to mark as blank
                             psImage *mask, // Mask to mark as blank (or NULL)
                             psImage *weight, // Weight map to mark as blank (or NULL)
                             int x, int y, // Coordinates to mark blank
                             psMaskType blank // Blank mask value
    )
{
    image->data.F32[y][x] = NAN;
    if (mask) {
        mask->data.PS_TYPE_MASK_DATA[y][x] |= blank;
    }
    if (weight) {
        weight->data.F32[y][x] = NAN;
    }
    return;
}

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

psImage *pmSubtractionMask(const psImage *inMask, const psImage *refMask, psMaskType maskVal,
                           int size, int footprint)
{
    PS_ASSERT_IMAGE_NON_NULL(inMask, NULL);
    PS_ASSERT_IMAGE_TYPE(inMask, PS_TYPE_MASK, NULL);
    PS_ASSERT_IMAGE_NON_NULL(refMask, NULL);
    PS_ASSERT_IMAGE_TYPE(refMask, PS_TYPE_MASK, NULL);
    PS_ASSERT_IMAGES_SIZE_EQUAL(inMask, refMask, NULL);
    PS_ASSERT_INT_NONNEGATIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(footprint, NULL);

    // Size of the images
    int numCols = inMask->numCols;
    int numRows = inMask->numRows;

    // Worried about the masks for bad pixels and bad stamps colliding, so make our own mask
    psImage *mask = psImageAlloc(numCols, numRows, PS_TYPE_MASK); // The global mask
    psImageInit(mask, 0);

    // Dereference for convenience
    psMaskType **maskData = mask->data.PS_TYPE_MASK_DATA;
    psMaskType **inData = inMask->data.PS_TYPE_MASK_DATA;
    psMaskType **refData = refMask->data.PS_TYPE_MASK_DATA;

    // Block out a border around the edge of the image

    // Bottom stripe
    for (int y = 0; y < PS_MIN(size + footprint, numRows); y++) {
        for (int x = 0; x < numCols; x++) {
            maskData[y][x] |= PM_SUBTRACTION_MASK_BORDER;
        }
    }
    // Either side
    for (int y = PS_MIN(size + footprint, numRows); y < numRows - size - footprint; y++) {
        for (int x = 0; x < PS_MIN(size + footprint, numCols); x++) {
            maskData[y][x] |= PM_SUBTRACTION_MASK_BORDER;
        }
        for (int x = PS_MAX(numCols - size - footprint, 0); x < numCols; x++) {
            maskData[y][x] |= PM_SUBTRACTION_MASK_BORDER;
        }
    }
    // Top stripe
    for (int y = PS_MAX(numRows - size - footprint, 0); y < numRows; y++) {
        for (int x = 0; x < numCols; x++) {
            maskData[y][x] |= PM_SUBTRACTION_MASK_BORDER;
        }
    }

    // Mask the bad pixels, and around them
    for (int y = 0; y < numRows; y++) {
        for (int x = 0; x < numCols; x++) {
            if (inData[y][x] & maskVal) {
                maskData[y][x] |= PM_SUBTRACTION_MASK_INPUT;
                // Block out the entire stamp footprint around this pixel
                for (int v = PS_MAX(y - footprint, 0); v <= PS_MIN(y + footprint, numRows - 1); v++) {
                    for (int u = PS_MAX(x - footprint, 0); u <= PS_MIN(x + footprint, numCols - 1); u++) {
                        maskData[v][u] |= PM_SUBTRACTION_MASK_FOOTPRINT;
                    }
                }
            }
            if (refData[y][x] & maskVal) {
                maskData[y][x] |= PM_SUBTRACTION_MASK_REF;

                // We want to block out with the CONVOLVE mask anything that would be bad if we convolved with
                // this bad pixel (within 'size').  Then we want to block out with the FOOTPRINT mask
                // everything within a footprint's distance of those (within 'footprint').

                // Bottom stripe
                for (int v = PS_MAX(y - footprint - size, 0); v < y - size; v++) {
                    for (int u = PS_MAX(x - footprint - size, 0);
                         u <= PS_MIN(x + footprint + size, numCols - 1); u++) {
                        maskData[v][u] |= PM_SUBTRACTION_MASK_FOOTPRINT;
                    }
                }
                // Middle
                for (int v = PS_MAX(y - size, 0); v <= PS_MIN(y + size, numRows - 1); v++) {
                    // Left side
                    for (int u = PS_MAX(x - footprint - size, 0); u < x - size; u++) {
                        maskData[v][u] |= PM_SUBTRACTION_MASK_FOOTPRINT;
                    }
                    // Centre
                    for (int u = PS_MAX(x - size, 0); u <= PS_MIN(x + size, numCols - 1); u++) {
                        maskData[v][u] |= PM_SUBTRACTION_MASK_FOOTPRINT | PM_SUBTRACTION_MASK_CONVOLVE;
                    }
                    // Right side
                    for (int u = x + size + 1; u <= PS_MIN(x + footprint + size, numCols - 1); u++) {
                        maskData[v][u] |= PM_SUBTRACTION_MASK_FOOTPRINT;
                    }
                }
                // Top stripe
                for (int v = y + size + 1; v <= PS_MIN(y + footprint + size, numRows - 1); v++) {
                    for (int u = PS_MAX(x - footprint - size, 0);
                         u <= PS_MIN(x + footprint + size, numCols - 1); u++) {
                        maskData[v][u] |= PM_SUBTRACTION_MASK_FOOTPRINT;
                    }
                }
            }
        }
    }

    return mask;
}


bool pmSubtractionCalculateEquation(psArray *stamps, const psImage *reference, const psImage *input,
                                    const psImage *weight, const pmSubtractionKernels *kernels, int footprint)
{
    PS_ASSERT_ARRAY_NON_NULL(stamps, false);
    PS_ASSERT_IMAGE_NON_NULL(reference, false);
    PS_ASSERT_IMAGE_TYPE(reference, PS_TYPE_F32, false);
    PS_ASSERT_IMAGE_NON_NULL(input, false);
    PS_ASSERT_IMAGE_TYPE(input, PS_TYPE_F32, false);
    PS_ASSERT_IMAGES_SIZE_EQUAL(reference, input, false);
    PS_ASSERT_PTR_NON_NULL(kernels, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->v, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->xOrder, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->yOrder, false);
    if (weight) {
        PS_ASSERT_IMAGE_NON_NULL(weight, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(weight, input, false);
        PS_ASSERT_IMAGE_TYPE(weight, PS_TYPE_F32, false);
    }
    if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->preCalc, false);
    }
    PS_ASSERT_INT_NONNEGATIVE(footprint, false);

    // Image size
    int numCols = reference->numCols;
    int numRows = reference->numRows;

    int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
    int numKernels = kernels->u->n;     // Number of kernel basis functions
    int numParams = numKernels + 1;     // Total number of parameters to solve for: coefficient of each kernel
                                        // basis function, and a constant background offset.
    int bgIndex = numKernels;           // Index in matrix for the background
    psVector *convolutions = psVectorAlloc(numKernels, PS_TYPE_F64); // Convolutions for each kernel

    // We iterate over each stamp, allocate the matrix and vectors if
    // necessary, and then calculate those matrix/vectors.
    for (int i = 0; i < stamps->n; i++) {
        pmSubtractionStamp *stamp = stamps->data[i]; // Stamp of interest
        if (stamp->status == PM_SUBTRACTION_STAMP_CALCULATE) {
            psImage *stampMatrix = stamp->matrix; // Least-squares matrix for this stamp
            psVector *stampVector = stamp->vector; // Least-squares vector for this stamp

            if (!stampMatrix) {
                stamp->matrix = stampMatrix = psImageAlloc(numParams, numParams, PS_TYPE_F64);
            }
            assert(stampMatrix->type.type == PS_TYPE_F64);
            assert(stampMatrix->numCols == numParams && stampMatrix->numRows == numParams);
            psImageInit(stampMatrix, 0.0);

            if (!stampVector) {
                stamp->vector = stampVector = psVectorAlloc(numParams, PS_TYPE_F64);
            }
            assert(stampVector->type.type == PS_TYPE_F64);
            assert(stampVector->n == numParams);
            psVectorInit(stampVector, 0.0);

            // Pre-evaluated spatial polynomial values
            psImage *polyValues = spatialPolyValues(spatialOrder,
                                                    2.0 * (float)(stamp->x - numCols/2.0) / (float)numCols,
                                                    2.0 * (float)(stamp->y - numRows/2.0) / (float)numRows);

            for (int y = stamp->y - footprint; y <= stamp->y + footprint; y++) {
                for (int x = stamp->x - footprint; x <= stamp->x + footprint; x++) {
                    float invNoise2 = 1.0 / (weight ? weight->data.F32[y][x] :
                                             input->data.F32[y][x]); // Inverse square noise

                    // Generate the convolutions
                    for (int i = 0; i < numKernels; i++) {
                        convolutions->data.F64[i] = convolvePixel(kernels, i, x, y, reference, polyValues);
                    }

                    // Generate the least-squares matrix and vector
                    // Upper diagonal only
                    for (int i = 0; i < numKernels; i++) {
                        for (int j = i; j < numKernels; j++) {
                            stampMatrix->data.F64[i][j] += convolutions->data.F64[i] *
                                convolutions->data.F64[j] * invNoise2;
                        }
                        stampVector->data.F64[i] += input->data.F32[y][x] * convolutions->data.F64[i] *
                            invNoise2;

                        // Background term
                        stampMatrix->data.F64[i][bgIndex] += convolutions->data.F64[i] * invNoise2;
                    }
                    // Background only terms
                    stampMatrix->data.F64[bgIndex][bgIndex] += invNoise2;
                    stampVector->data.F64[bgIndex] += input->data.F32[y][x] * invNoise2;
                }
            }
            psFree(polyValues);

            // Fill in lower diagonal of symmetric matrix, while checking for bad values
            bool bad = false;           // Are there bad values?
            for (int i = 0; i < numKernels; i++) {
                for (int j = 0; j < i; j++) {
                    stampMatrix->data.F64[i][j] = stampMatrix->data.F64[j][i];
                    if (!isfinite(stampMatrix->data.F64[j][i])) {
                        bad = true;
                    }
                }
                stampMatrix->data.F64[bgIndex][i] = stampMatrix->data.F64[i][bgIndex];
                if (!isfinite(stampMatrix->data.F64[i][bgIndex]) ||
                    !isfinite(stampMatrix->data.F64[i][i]) ||
                    !isfinite(stampVector->data.F64[i])) {
                    bad = true;
                }
            }
            if (!isfinite(stampVector->data.F64[bgIndex])) {
                bad = true;
            }

            if (bad) {
                stamp->status = PM_SUBTRACTION_STAMP_REJECTED;
                psTrace("psModules.imcombine", 3, "Rejecting stamp %d because of bad equation\n", i);
            } else {
                stamp->status = PM_SUBTRACTION_STAMP_USED;
            }

            if (psTraceGetLevel("psModules.imcombine.equation") >= 10) {
                psString matrixName = NULL;
                psStringAppend(&matrixName, "matrix%d.fits", i);
                psFits *matrixFile = psFitsOpen(matrixName, "w");
                psFree(matrixName);
                psFitsWriteImage(matrixFile, NULL, stampMatrix, 0, NULL);
                psFitsClose(matrixFile);
            }

        }
    }

    psFree(convolutions);

    return true;
}


psVector *pmSubtractionSolveEquation(psVector *solution, const psArray *stamps)
{
    PS_ASSERT_ARRAY_NON_NULL(stamps, NULL);

    // Check inputs, while summing the stamp matrices and vectors
    long numParams = -1;                // Number of parameters
    for (int i = 0; i < stamps->n; i++) {
        pmSubtractionStamp *stamp = stamps->data[i]; // Stamp of interest
        PS_ASSERT_PTR_NON_NULL(stamp, NULL);
        if (stamp->status != PM_SUBTRACTION_STAMP_USED) {
            continue;
        }
        if (numParams == -1) {
            numParams = stamp->vector->n;
        } else {
            PS_ASSERT_VECTOR_SIZE(stamp->vector, numParams, NULL);
        }
        PS_ASSERT_VECTOR_TYPE(stamp->vector, PS_TYPE_F64, NULL);
        PS_ASSERT_IMAGE_TYPE(stamp->matrix, PS_TYPE_F64, NULL);
        PS_ASSERT_IMAGE_SIZE(stamp->matrix, numParams, numParams, NULL);
    }
    if (numParams == -1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "No suitable stamps found.");
        return NULL;
    }

    if (solution) {
        PS_ASSERT_VECTOR_TYPE(solution, PS_TYPE_F64, NULL);
        PS_ASSERT_VECTOR_SIZE(solution, numParams, NULL);
    }

    // Accumulate the least-squares matricies and vectors
    psImage *sumMatrix = psImageAlloc(numParams, numParams, PS_TYPE_F64); // Combined matrix for all stamps
    psVector *sumVector = psVectorAlloc(numParams, PS_TYPE_F64); // Combined vector for all stamps
    psVectorInit(sumVector, 0.0);
    psImageInit(sumMatrix, 0.0);
    for (int i = 0; i < stamps->n; i++) {
        pmSubtractionStamp *stamp = stamps->data[i]; // Stamp of interest
        if (stamp->status == PM_SUBTRACTION_STAMP_USED) {
            (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix);
            (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector);
        }
    }

    psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
    psImage *luMatrix = psMatrixLUD(NULL, &permutation, sumMatrix);
    psFree(sumMatrix);
    if (!luMatrix) {
        psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
        psFree(sumVector);
        psFree(luMatrix);
        psFree(permutation);
        return NULL;
    }

    solution = psMatrixLUSolve(solution, luMatrix, sumVector, permutation);
    psFree(sumVector);
    psFree(luMatrix);
    psFree(permutation);
    if (!solution) {
        psError(PS_ERR_UNKNOWN, true, "Failed to solve the least-squares system.\n");
        return NULL;
    }

    if (psTraceGetLevel("psModules.imcombine") >= 7) {
        for (int i = 0; i < solution->n; i++) {
            printf("       Solution %d: %f\n", i, solution->data.F64[i]);
        }
    }

    return solution;
}


int pmSubtractionRejectStamps(psArray *stamps, const psImage *refImage, psImage *inImage,
                              psImage *subMask, const psVector *solution, int footprint, float sigmaRej,
                              const pmSubtractionKernels *kernels)
{
    PS_ASSERT_ARRAY_NON_NULL(stamps, -1);
    PS_ASSERT_IMAGE_NON_EMPTY(refImage, -1);
    PS_ASSERT_IMAGE_TYPE(refImage, PS_TYPE_F32, -1);
    PS_ASSERT_IMAGE_NON_EMPTY(inImage, -1);
    PS_ASSERT_IMAGE_TYPE(inImage, PS_TYPE_F32, -1);
    PS_ASSERT_IMAGES_SIZE_EQUAL(refImage, inImage, -1);
    PS_ASSERT_IMAGE_NON_EMPTY(subMask, -1);
    PS_ASSERT_IMAGE_TYPE(subMask, PS_TYPE_MASK, -1);
    PS_ASSERT_IMAGES_SIZE_EQUAL(refImage, subMask, -1);
    PS_ASSERT_VECTOR_NON_NULL(solution, -1);
    PS_ASSERT_VECTOR_TYPE(solution, PS_TYPE_F64, -1);
    PS_ASSERT_PTR_NON_NULL(kernels, -1);
    PS_ASSERT_VECTOR_SIZE(kernels->u, solution->n - 1, -1);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->v, -1);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->xOrder, -1);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->yOrder, -1);
    if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        PS_ASSERT_ARRAY_NON_EMPTY(kernels->preCalc, -1);
        PS_ASSERT_ARRAY_SIZE(kernels->preCalc, solution->n - 1, -1);
    }

    // Measure deviations
    psVector *deviations = psVectorAlloc(stamps->n, PS_TYPE_F32); // Mean deviation for stamps
    double totalSquareDev = 0.0;        // Total square deviation from zero
    int numStamps = 0;                  // Number of used stamps
    {
        int numCols = refImage->numCols, numRows = refImage->numRows; // Image dimensions
        psStats *stats = psStatsAlloc(PS_STAT_SAMPLE_MEAN); // Statistics
        psImage *convolvedStamp = psImageAlloc(2 * footprint + 1, 2 * footprint + 1,
                                               PS_TYPE_F32); // Convolved image of stamp
        psKernel *kernelImage = NULL;       // The kernel, with which to convolve the stamps
        float background = solution->data.F64[solution->n-1]; // The difference in background
        int size = kernels->size;       // Half-size of the kernel

        for (int i = 0; i < stamps->n; i++) {
            pmSubtractionStamp *stamp = stamps->data[i]; // The stamp of interest
            if (stamp->status != PM_SUBTRACTION_STAMP_USED) {
                continue;
            }
            int xStamp = stamp->x, yStamp = stamp->y; // Coordinates of stamp

            // Precalulate polynomial values
            psImage *polyValues = spatialPolyValues(kernels->spatialOrder,
                                                    2.0 * (float)(stamp->x - numCols/2.0) / (float)numCols,
                                                    2.0 * (float)(stamp->y - numRows/2.0) / (float)numRows);

#ifdef USE_FUNCTIONS_INSTEAD_OF_MACROS
            kernelImage = solvedKernel(kernelImage, solution, kernels, polyValues, imageWeighting);
#else
            SOLVED_KERNEL(kernelImage, solution, kernels, polyValues, imageWeighting);
#endif
            for (int y = yStamp - footprint, j = 0; y <= yStamp + footprint; y++, j++) {
                for (int x = xStamp - footprint, i = 0; x <= xStamp + footprint; x++, i++) {
                    convolvedStamp->data.F32[j][i] = background;
                    for (int v = -size; v <= size; v++) {
                        for (int u = -size; u <= size; u++) {
                            convolvedStamp->data.F32[j][i] += kernelImage->kernel[v][u] *
                                refImage->data.F32[y + v][x + u];
                        }
                    }
                }
            }

            psFree(polyValues);

            psRegion region = psRegionSet(xStamp - footprint, xStamp + footprint + 1,
                                          yStamp - footprint, yStamp + footprint + 1); // Region of interest
            psImage *inStamp = psImageSubset(inImage, region); // Image of stamp
            assert(inStamp->numCols == convolvedStamp->numCols &&
                   inStamp->numRows == convolvedStamp->numRows);
            (void)psBinaryOp(convolvedStamp, inStamp, "-", convolvedStamp);
            (void)psBinaryOp(convolvedStamp, convolvedStamp, "*", convolvedStamp);
            (void)psBinaryOp(convolvedStamp, convolvedStamp, "/", inStamp);
            psFree(inStamp);
            if (!psImageStats(stats, convolvedStamp, NULL, 0)) {
                psError(PS_ERR_UNKNOWN, false,
                        "Unable to calculate statistics on normalised residual image of stamp.");
                psFree(deviations);
                psFree(convolvedStamp);
                psFree(stats);
                return -1;
            }
            deviations->data.F32[i] = sqrt(stats->sampleMean / 2.0);
            psTrace("psModules.imcombine", 1, "Deviation for stamp %d: %f\n", i, deviations->data.F32[i]);
            totalSquareDev += PS_SQR(deviations->data.F32[i]);
            numStamps++;
        }

        psFree(kernelImage);
        psFree(convolvedStamp);
        psFree(stats);
    }

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

    int numRejected = 0;
    for (int i = 0; i < stamps->n; i++) {
        pmSubtractionStamp *stamp = stamps->data[i]; // Stamp of interest
        if (stamp->status == PM_SUBTRACTION_STAMP_USED && deviations->data.F32[i] > limit) {
            // Mask out the stamp in the image so you it's not found again
            psTrace("psModules.imcombine", 3, "Rejecting stamp %d\n", i);
            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->status = PM_SUBTRACTION_STAMP_REJECTED;
        }
    }

    psFree(deviations);

    return numRejected;
}

psImage *pmSubtractionKernelImage(const psVector *solution, const pmSubtractionKernels *kernels,
                                  float x, float y
                                 )
{
    PS_ASSERT_VECTOR_NON_NULL(solution, NULL);
    PS_ASSERT_PTR_NON_NULL(kernels, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(x, -1.0, 1.0, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(y, -1.0, 1.0, NULL);
    PS_ASSERT_VECTOR_SIZE(kernels->u, solution->n - 1, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->v, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->xOrder, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->yOrder, NULL);
    if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        PS_ASSERT_ARRAY_NON_NULL(kernels->preCalc, NULL);
        PS_ASSERT_ARRAY_SIZE(kernels->preCalc, solution->n - 1, NULL);
    }

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

    // The appropriate kernel
#ifdef USE_FUNCTIONS_INSTEAD_OF_MACROS
    psKernel *kernel = solvedKernel(NULL, solution, kernels, polyValues, imageWeighting);
#else
    psKernel *kernel = NULL;
    SOLVED_KERNEL(kernel, solution, kernels, polyValues, imageWeighting);
#endif

    psFree(polyValues);

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

    return image;
}

psArray *pmSubtractionKernelSolutions(const psVector *solution, const pmSubtractionKernels *kernels,
                                      float x, float y
                                      )
{
    PS_ASSERT_VECTOR_NON_NULL(solution, NULL);
    PS_ASSERT_PTR_NON_NULL(kernels, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(x, -1.0, 1.0, NULL);
    PS_ASSERT_FLOAT_WITHIN_RANGE(y, -1.0, 1.0, NULL);
    PS_ASSERT_VECTOR_SIZE(kernels->u, solution->n - 1, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->v, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->xOrder, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->yOrder, NULL);
    if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        PS_ASSERT_ARRAY_NON_NULL(kernels->preCalc, NULL);
        PS_ASSERT_ARRAY_SIZE(kernels->preCalc, solution->n - 1, 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(fakeSolution, kernels, x, y);
        fakeSolution->data.F64[i] = 0.0;
    }

    psFree(fakeSolution);

    return images;
}


bool pmSubtractionConvolve(psImage **outImage, psImage **outWeight, psImage **outMask,
                           const psImage *inImage, const psImage *inWeight, const psImage *subMask,
                           psMaskType blank, const psVector *solution, const pmSubtractionKernels *kernels)
{
    PS_ASSERT_IMAGE_NON_NULL(inImage, false);
    PS_ASSERT_IMAGE_TYPE(inImage, PS_TYPE_F32, false);
    if (subMask) {
        PS_ASSERT_IMAGE_NON_NULL(subMask, false);
        PS_ASSERT_IMAGE_TYPE(subMask, PS_TYPE_MASK, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(subMask, inImage, false);
    }
    if (inWeight) {
        PS_ASSERT_IMAGE_NON_NULL(inWeight, false);
        PS_ASSERT_IMAGE_TYPE(inWeight, PS_TYPE_F32, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(inWeight, inImage, false);
    }
    PS_ASSERT_PTR_NON_NULL(outImage, false);
    if (*outImage) {
        PS_ASSERT_IMAGE_NON_NULL(*outImage, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(*outImage, inImage, false);
        PS_ASSERT_IMAGE_TYPE(*outImage, PS_TYPE_F32, false);
    }
    if (outMask && *outMask) {
        PS_ASSERT_IMAGE_NON_NULL(*outMask, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(*outMask, inImage, false);
        PS_ASSERT_IMAGE_TYPE(*outMask, PS_TYPE_MASK, false);
        PS_ASSERT_IMAGE_NON_NULL(subMask, false);
    }
    if (outWeight && *outWeight) {
        PS_ASSERT_IMAGE_NON_NULL(*outWeight, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(*outWeight, inImage, false);
        PS_ASSERT_IMAGE_TYPE(*outWeight, PS_TYPE_F32, false);
        PS_ASSERT_IMAGE_NON_NULL(inWeight, false);
    }
    PS_ASSERT_VECTOR_NON_NULL(solution, false);
    PS_ASSERT_VECTOR_TYPE(solution, PS_TYPE_F64, false);
    PS_ASSERT_PTR_NON_NULL(kernels, false);
    PS_ASSERT_VECTOR_SIZE(kernels->u, solution->n - 1, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->v, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->xOrder, false);
    PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->yOrder, false);
    if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        PS_ASSERT_ARRAY_NON_NULL(kernels->preCalc, false);
        PS_ASSERT_ARRAY_SIZE(kernels->preCalc, solution->n - 1, false);
    }

    int size = kernels->size;           // Half-size of kernel
    float background = solution->data.F64[solution->n-1]; // The difference in background
    int numCols = inImage->numCols, numRows = inImage->numRows; // Image dimensions

    psImage *convImage = *outImage;    // Convolved image
    if (!convImage) {
        *outImage = psImageAlloc(numCols, numRows, PS_TYPE_F32);
        convImage = *outImage;
    }
    psImage *convMask = NULL;           // Convolved mask image
    if (outMask && subMask) {
        if (!*outMask) {
            *outMask = psImageAlloc(numCols, numRows, PS_TYPE_MASK);
        }
        convMask = *outMask;
        psImageInit(convMask, 0);
    }
    psImage *convWeight = NULL;         // Convolved weight (variance) image
    if (outWeight && inWeight) {
        if (!*outWeight) {
            *outWeight = psImageAlloc(numCols, numRows, PS_TYPE_F32);
        }
        convWeight = *outWeight;
        psImageInit(convWeight, 0.0);
    }

    psImage *polyValues = NULL;         // Pre-calculated polynomial values
    int fullSize = 2 * size + 1;        // Full size of kernel

    psKernel *kernelImage = NULL;       // Kernel for the image
    psKernel *kernelWeight = NULL;      // Kernel for the weight map

    for (int j = size; j < inImage->numRows - size; j += fullSize) {
        for (int i = size; i < inImage->numCols - size; i += fullSize) {

            // Only generate polynomial values every kernel footprint, since we have already assumed
            // (with the stamps) that it does not vary rapidly on this scale.
            psFree(polyValues);
            polyValues = spatialPolyValues(kernels->spatialOrder,
                                           2.0 * (float)(i + size + 1 - numCols/2.0) / (float)numCols,
                                           2.0 * (float)(j + size + 1 - numRows/2.0) / (float)numRows);
#ifdef USE_FUNCTIONS_INSTEAD_OF_MACROS
            kernelImage = solvedKernel(kernelImage, solution, kernels, polyValues, imageWeighting);
            if (inWeight) {
                kernelWeight = solvedKernel(kernelWeight, solution, kernels, polyValues,
                                            varianceWeighting);
            }
#else
            SOLVED_KERNEL(kernelImage, solution, kernels, polyValues, imageWeighting);
            if (inWeight) {
                SOLVED_KERNEL(kernelWeight, solution, kernels, polyValues, varianceWeighting);
            }
#endif

            for (int y = j; y < PS_MIN(j + fullSize, numRows - size); y++) {
                for (int x = i; x < PS_MIN(i + fullSize, numCols - size); x++) {
                    // Check and propagate the kernel footprint, if required
                    if (subMask && (subMask->data.PS_TYPE_MASK_DATA[y][x] &
                                    (PM_SUBTRACTION_MASK_INPUT | PM_SUBTRACTION_MASK_CONVOLVE))) {
                        convMask->data.PS_TYPE_MASK_DATA[y][x] |= blank;
                        convImage->data.F32[y][x] = NAN;
                        if (inWeight) {
                            convWeight->data.F32[y][x] = NAN;
                        }
                    } else {
                        // Convolve the image
                        convImage->data.F32[y][x] = background;
                        for (int v = -size; v <= size; v++) {
                            for (int u = -size; u <= size; u++) {
                                convImage->data.F32[y][x] += kernelImage->kernel[v][u] *
                                    inImage->data.F32[y + v][x + u];
                            }
                        }

                        // Convolve the weight (variance) map
                        if (inWeight) {
                            for (int v = -size; v <= size; v++) {
                                for (int u = -size; u <= size; u++) {
                                    convWeight->data.F32[y][x] += kernelWeight->kernel[v][u] *
                                        inWeight->data.F32[y + v][x + u];
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    psFree(kernelImage);
    psFree(kernelWeight);
    psFree(polyValues);

    // Mark the non-convolved part as blank
    for (int y = size; y < inImage->numRows - size; y++) {
        for (int x = 0; x < size; x++) {
            markBlank(convImage, convMask, convWeight, x, y, blank);
        }
        for (int x = numCols - size; x < numCols; x++) {
            markBlank(convImage, convMask, convWeight, x, y, blank);
        }
    }
    for (int y = 0; y < size; y++) {
        for (int x = 0; x < numCols; x++) {
            markBlank(convImage, convMask, convWeight, x, y, blank);
        }
    }
    for (int y = numRows - size; y < numRows; y++) {
        for (int x = 0; x < numCols; x++) {
            markBlank(convImage, convMask, convWeight, x, y, blank);
        }
    }

    return true;
}
