/** @file  ImageSubtract.c
 *
 *  This file will contain code which creates a set of kernel basis
 *  functions, solves for their solution, and applies them to an image.
 *
 *  @author Paul Price, IfA (original prototype)
 *  @author GLG, MHPCC
 *
 *  @version $Revision: 1.1 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2005-09-28 20:43:52 $
 *
 *  Copyright 2004 Maui High Performance Computing Center, University of Hawaii
 *
 *   XXX: sync with IfA on this:
 *   The (x, y) (row, col) issue is becoming a problem.  In this file, and I
 *   think, the rest of psLib and psModules, the following conventions are used:
 *
 * 1) x will correspond to the column, and y will correspond to the row.
 * 2) When used in function prototypes, the column (and hence x) appears
 *    first.
 * 3) When used to index 2-D arrays, obviously, the row (and hence, y)
 *    appears first. (2 and 3 are the source of confusion).
 * 4) When (u, v) are used in certain structures.
 *  u corresponds to x
 *  v corresponds to y
 * 5) When element (a, b) is casually referred to in comments, or
 *    documentation it is unclear where a is the row, or the column.
 * 6) A convention on loop index variables (i, j) would be convenient.
 *    Currently, sometimes i corresponds to the column (x),
 *    usually it corresponds to the row (y).
 *
 *  XXX: The following variables are used an interpreted this way:
 * kernelSize: Means that the actual kernel is a square (1 + 2 * kernelSize) per side.
 * border: When accessing an image, a swath of pixels this wide is ignored.
 * footprint: When accessing a stample, a square of pixels, footprint pixels per side,
 *  are looked at.  We must ensure that (footprint+kernelSize) pixels exist
 *  around the center.
 */

#include<stdio.h>
#include<math.h>
#include "pslib.h"
#include "psConstants.h"
#include "pmImageSubtract.h"

/*******************************************************************************
    Private alloc/free functions.
XXX: It's not clear if the SubtractionKernels alloc/free functions are feasable.
 ******************************************************************************/
void p_pmStampFree(pmStamp *stamp)
{
    psFree(stamp->matrix);
    psFree(stamp->vector);
}

pmStamp *p_pmStampAlloc(pmStampStatus status)
{
    pmStamp *stamp = (pmStamp*)psAlloc(sizeof(pmStamp));
    stamp->x = 0;
    stamp->p_xSize = 0;
    stamp->y = 0;
    stamp->p_ySize = 0;
    stamp->matrix = NULL;
    stamp->vector = NULL;
    stamp->status = status;

    psMemSetDeallocator(stamp, (psFreeFunc)p_pmStampFree);

    return(stamp);
}

void p_pmSubtractionKernelsFree(psSubtractionKernels *kernels)
{
    psFree(kernels->u);
    psFree(kernels->v);
    psFree(kernels->sigma);
    psFree(kernels->xOrder);
    psFree(kernels->yOrder);
    psFree(kernels->preCalc);

    psFree(kernels);
}

psSubtractionKernels *p_pmSubtractionKernelsAlloc(int numBasisFunctions,
        pmSubtractionKernelsType type)
{
    psSubtractionKernels *tmp = (psSubtractionKernels *) psAlloc(sizeof(psSubtractionKernels));

    tmp->type = type;
    psMemSetDeallocator(tmp, (psFreeFunc) p_pmSubtractionKernelsFree);
    return(tmp);
}

/*******************************************************************************
psSubtractionKernels struct.
 ******************************************************************************/
psSubtractionKernels *pmSubtractionKernelsAllocPOIS(int size,
        int spatialOrder)
{
    psTrace("ImageSubtract.pmSubtractionKernelsAllocPOIS", 3,
            "Calling pmSubtractionKernelsAllocPOIS(%d, %d)\n", size, spatialOrder);
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_POSITIVE(spatialOrder, NULL);
    //
    // Calculate the number of basis functions (nBF)
    //
    psS32 xKernelHalfSize = size;
    psS32 yKernelHalfSize = size;
    psS32 nBF = (2 * xKernelHalfSize + 1) *
                (2 * yKernelHalfSize + 1) *
                (spatialOrder + 1) *
                (spatialOrder + 2) / 2;

    //
    // Generate the new psSubtractionKernels data structure:
    //
    psSubtractionKernels *tmp = (psSubtractionKernels *) psAlloc(sizeof(psSubtractionKernels));
    tmp->type = PM_SUBTRACTION_KERNEL_POIS;
    tmp->u = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->v = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->sigma = NULL;
    tmp->xOrder = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->yOrder = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->subIndex = 0;
    tmp->preCalc = NULL;
    tmp->size = size;
    tmp->spatialOrder = spatialOrder;

    //
    // This corresponds to the Kernel Basis Function with (u, v) = (0, 0)
    //
    // Put the (u,v) = (0,0) component right at the start of the list
    // for convenience.
    //
    psS32 ptr = 0;
    for (psS32 order = 0; order <= spatialOrder; order++) {
        for (psS32 xOrder = 0; xOrder <= order; xOrder++) {
            psS32 yOrder = order - xOrder;
            tmp->u->data.F32[ptr] = 0;
            tmp->v->data.F32[ptr] = 0;
            tmp->xOrder->data.F32[ptr] = xOrder;
            tmp->yOrder->data.F32[ptr] = yOrder;
            ptr++;
        }
    }

    //
    // Iterate over (u,v).  Generate a set of kernels for each (u, v).
    //
    for (psS32 v = -yKernelHalfSize; v <= yKernelHalfSize; v++) {
        for (psS32 u = -xKernelHalfSize; u <= xKernelHalfSize; u++) {
            // Already did (u,v) = (0,0): it's at the start, so skip it now.
            if ((u != 0) || (v != 0)) {
                //
                // Iterate over spatial order.  This loop creates the terms for
                // x^xOrder * y^yOrder  such that (xOrder+yOrder) <= spatialOrder.
                //
                for (psS32 order = 0; order <= spatialOrder; order++) {
                    for (psS32 xOrder = 0; xOrder <= order; xOrder++) {
                        psS32 yOrder = order - xOrder;
                        tmp->u->data.F32[ptr] = u;
                        tmp->v->data.F32[ptr] = v;
                        tmp->xOrder->data.F32[ptr] = xOrder;
                        tmp->yOrder->data.F32[ptr] = yOrder;
                        ptr++;
                    }
                }
            }
        }
    }

    psTrace("ImageSubtract.pmSubtractionKernelsAllocPOIS", 3,
            "Exiting pmSubtractionKernelsAllocPOIS(%d, %d)\n", size, spatialOrder);
    return(tmp);
}

/*******************************************************************************
XXX: Get the types correct (u, v, xOrder, yOrder).
 
XXX: Code review this.
 ******************************************************************************/
psSubtractionKernels *pmSubtractionKernelsAllocISIS(const psVector *sigmas,
        const psVector *orders,
        int size,
        int spatialOrder)
{
    PS_ASSERT_VECTOR_NON_NULL(sigmas, NULL);
    PS_ASSERT_VECTOR_NON_NULL(orders, NULL);
    psTrace("ImageSubtract.pmSubtractionKernelsAllocISIS", 3,
            "Calling pmSubtractionKernelsAllocISIS(%d, %d, %d, %d)\n",
            sigmas->n, orders->n, size, spatialOrder);
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_POSITIVE(spatialOrder, NULL);
    PS_ASSERT_VECTOR_TYPE(sigmas, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_TYPE(orders, PS_TYPE_S32, NULL);
    //
    // Calculate the number of basis functions (nBF).
    //
    psS32 numSigmas = sigmas->n;

    // XXX: Get rid of the sigma loop?  We merely multiple nBF by numSigmas?
    // XXX: Verify that all this is correct.
    psS32 nBF = 0;
    for (psS32 s = 0 ; s < numSigmas ; s++) {
        for (psS32 o = 0 ; o < orders->n ; o++) {
            nBF+=((orders->data.S32[o] + 1) * (orders->data.S32[o] + 2) / 2);
        }
    }
    nBF*= ((spatialOrder + 1) * (spatialOrder + 2) / 2);

    //
    // Generate the new psSubtractionKernels data structure:
    //
    psSubtractionKernels *tmp = (psSubtractionKernels *) psAlloc(sizeof(psSubtractionKernels));
    tmp->type = PM_SUBTRACTION_KERNEL_ISIS;
    tmp->u = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->v = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->sigma = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->xOrder = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->yOrder = psVectorAlloc(nBF, PS_TYPE_F32);
    tmp->subIndex = 0;
    tmp->size = size;
    tmp->spatialOrder = spatialOrder;
    tmp->preCalc = psArrayAlloc(nBF);

    //
    // We loop through all combinations of sigmas and polynomial orders
    // creating the kernel basis functions.
    //
    psS32 ptr = 0;
    for (psS32 sigPtr = 0 ; sigPtr < numSigmas ; sigPtr++) {
        tmp->sigma->data.F32[sigPtr] = sigmas->data.F32[sigPtr];
        //
        // (xOrderP, yOrderP) are the order of the polynomial that modify the
        // gaussian in the kernel.  They correspond to (j, k) in equation (5)
        // from the psModules SDRS.
        //
        for (psS32 o = 0 ; o < orders->n ; o++) {
            for (psS32 orderP = 0 ; orderP <= orders->data.S32[o] ; orderP++) {
                for (psS32 xOrderP = 0 ; xOrderP <= orderP ; xOrderP++) {
                    psS32 yOrderP = orderP - xOrderP;

                    psImage *currPreCalc = psImageAlloc(1 + (2 * size), 1 + (2 * size), PS_TYPE_F32);
                    PS_IMAGE_SET_F32(currPreCalc, 0.0);
                    psBool setPreCalc = true;
                    //
                    // We loop through all spatial orders.  Since they have no effect on
                    // the preCalc images, we only calculate them once, and store pointers
                    // in tmp->preCalc->data[ptr] for other spatial orders.
                    //
                    for (psS32 order = 0; order <= spatialOrder; order++) {
                        for (psS32 orderXTerm = 0; orderXTerm <= order; orderXTerm++) {
                            PS_ASSERT_INT_LESS_THAN(ptr, nBF, NULL);

                            psS32 orderYTerm = order - orderXTerm;

                            tmp->u->data.F32[ptr] = xOrderP;
                            tmp->v->data.F32[ptr] = yOrderP;
                            tmp->xOrder->data.F32[ptr] = orderXTerm;
                            tmp->yOrder->data.F32[ptr] = orderYTerm;
                            tmp->sigma->data.F32[ptr] = sigmas->data.F32[sigPtr];
                            tmp->preCalc->data[ptr] = (psPtr *) currPreCalc;

                            //
                            // We calculate the preCalc image only the first time through
                            // this loop.  Otherwise, we increment the memory reference
                            // counter.
                            //
                            if (setPreCalc == true) {
                                for (psS32 v = -size; v <= size; v++) {
                                    for (psS32 u = -size; u <= size; u++) {
                                        // Scale the (u,v) coordinates in kernel space to [-1.0:1.0].
                                        psF32 uScaled = ((psF32) u) / ((psF32) size);
                                        psF32 vScaled = ((psF32) v) / ((psF32) size);

                                        // Compute the value of the kernel at location (u, v):
                                        psF32 exponent = (PS_SQR(uScaled) + PS_SQR(vScaled)) /
                                                         (2.0 * PS_SQR(sigmas->data.F32[sigPtr]));
                                        currPreCalc->data.F32[v+size][u+size] =
                                            exp(-exponent) *
                                            pow(uScaled, orderXTerm) *
                                            pow(vScaled, orderYTerm);
                                    }
                                }
                                setPreCalc = false;
                            } else {
                                psMemIncrRefCounter(currPreCalc);
                            }
                            ptr++;
                        }
                    }
                }
            }
        }
    }

    psTrace("ImageSubtract.pmSubtractionKernelsAllocISIS", 3,
            "Exiting pmSubtractionKernelsAllocISIS(%d, %d, %d, %d)\n",
            sigmas->n, orders->n, size, spatialOrder);
    return(tmp);
}

/*******************************************************************************
pmSubtractionFindStamps(stamps, image, mask, maskVal, threshold, xNum, yNum, border)
 
XXX: The SDRS and the proptotype code differ significantly.
 Prototype: When a maximum pixel is found within a stamp, an area of size
     2*footprint is searched around that pixel looking for masked pixels.
 SDRS: none of that is required.
 
XXX: Do we need to care about case where yNum/xNum does not evenly divide the
nnumber of rows/columns in the image?
 ******************************************************************************/
psArray *pmSubtractionFindStamps(psArray *stamps,        ///< Output stamps, or NULL
                                 const psImage *image,   ///< Image for which to find stamps
                                 const psImage *mask,    ///< Mask
                                 psU32 maskVal,          ///< Value for mask
                                 psF32 threshold,        ///< Threshold for stamps in the image
                                 psS32 xNum,             ///< Number of stamps in x
                                 psS32 yNum,             ///< Number of stamps in y
                                 psS32 border            ///< Border around image to ignore (should be size of kernel or larger)
                                )
{
    psTrace("ImageSubtract.pmSubtractionFindStamps", 3,
            "Calling pmSubtractionFindStamps(%d, %f, %d, %d, %d)\n",
            maskVal, threshold, xNum, yNum, border);
    PS_ASSERT_IMAGE_NON_NULL(image, NULL);
    PS_ASSERT_IMAGE_NON_EMPTY(image, NULL);
    PS_ASSERT_IMAGE_TYPE(image, PS_TYPE_F32, NULL);
    if (mask != NULL) {
        PS_ASSERT_IMAGES_SIZE_EQUAL(image, mask, NULL);
        PS_ASSERT_IMAGE_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_INT_POSITIVE(xNum, NULL);
    PS_ASSERT_INT_POSITIVE(yNum, NULL);
    PS_ASSERT_INT_POSITIVE(border, NULL);
    PS_ASSERT_INT_LARGER_THAN(image->numCols, xNum, NULL);
    PS_ASSERT_INT_LARGER_THAN(image->numRows, yNum, NULL);
    PS_ASSERT_INT_LARGER_THAN(image->numCols, (2 * border), NULL);
    PS_ASSERT_INT_LARGER_THAN(image->numRows, (2 * border), NULL);

    if (stamps != NULL) {
        PS_ASSERT_INT_EQUAL(stamps->n, (xNum * yNum), NULL);
        //
        // Ensure that a pmStamp struct exists at each psArray location.
        //
        for (psS32 s = 0 ; s < (xNum * yNum) ; s++) {
            if (NULL == stamps->data[s]) {
                stamps->data[s] = (psPtr *) p_pmStampAlloc(PM_STAMP_REJECTED);
            }
        }
    } else {
        stamps = (psArray *) psArrayAlloc(xNum * yNum);
        for (psS32 s = 0 ; s < (xNum * yNum) ; s++) {
            stamps->data[s] = (psPtr *) p_pmStampAlloc(PM_STAMP_REJECTED);
        }
    }
    psS32 numRows = image->numRows;
    psS32 numCols = image->numCols;

    //
    // Iterate over the image sections
    //
    // XXX: Must handle cases where image size is not an even multiple of xNum or yNum
    // they are currently ignored.
    //
    psS32 num = 0;
    for (psS32 j = 0; j < yNum; j++) {
        for (psS32 i = 0; i < xNum; i++) {
            pmStamp *stamp = (pmStamp *) stamps->data[num];
            //
            // Only find a new stamp if we need to
            //
            if (stamp->status == PM_STAMP_REJECTED) {
                //
                // Find maximum non-masked value in the image section,
                // but don't include a footprint around the edge
                //
                psF32 max = -INFINITY;
                psS32 bestx = 0;
                psS32 besty = 0;
                //
                // The following nested loop iterates over every pixel in the mask
                // associated with this (i, j).  It ignores pixels within a
                // border of pixels from the image edge.
                //
                // XXX: verify (numX, numY), then get rid of it.
                //
                psS32 numX = xNum;
                psS32 numY = yNum;
                psS32 yMin = border + j * (numCols - 2.0 * border) / numY;
                psS32 yMax = (border + (j + 1) * (numCols - 2.0 * border) / numY) - 1;
                psS32 xMin = border + i * (numRows - 2.0 * border) / numX;
                psS32 xMax = (border + (i + 1) * (numRows - 2.0 * border) / numX) - 1;

                if ((yMax >= image->numRows) ||
                        (xMax >= image->numCols) ||
                        (yMin < 0) ||
                        (xMin < 0)) {
                    // XXX: We skip this stamp since its borders extends beyond the image.
                    // XXX: This is here mainly as a safeguard.  We need to redefine the above
                    // min/max pixels calculation to ensure that all stamps are legitimate.

                    stamp->x = -1;
                    stamp->y = -1;
                    stamp->status = PM_STAMP_NONE;
                } else {
                    stamp->p_xSize = 1 + (xMax - xMin);
                    stamp->p_ySize = 1 + (yMax - yMin);
                    stamp->p_xMin = xMin;
                    stamp->p_xMax = xMax;
                    stamp->p_yMin = yMin;
                    stamp->p_yMax = yMax;

                    for (psS32 y = yMin; y <= yMax ; y++) {
                        for (psS32 x = xMin; x <= xMax ; x++) {
                            // Determine if this pixel is larger than the max, and unmasked.
                            if (image->data.F32[y][x] > max) {
                                if ((mask == NULL) || !((mask->data.U8[y][x]) & maskVal)) {
                                    max = image->data.F32[y][x];
                                    bestx = x;
                                    besty = y;
                                }
                            }
                        }
                    }

                    //
                    // If the max pixel is larger than the threshold, we keep this stamp.
                    // Otherwise, mark the stamp as PM_STAMP_NONE
                    //
                    if (image->data.F32[besty][bestx] >= threshold) {
                        stamp->x = bestx;
                        stamp->y = besty;
                        stamp->status = PM_STAMP_RECALC;
                    } else {
                        stamp->x = bestx;
                        stamp->y = besty;
                        stamp->status = PM_STAMP_NONE;
                    }
                }
            }
            num++;
        }
    }
    psTrace("ImageSubtract.pmSubtractionFindStamps", 3,
            "Exiting pmSubtractionFindStamps(%d, %f, %d, %d, %d)\n",
            maskVal, threshold, xNum, yNum, border);
    return(stamps);
}

/*******************************************************************************
GenSpatialOrder(spatialOrder, x, y): generates and returns a psImage in which
the [i][j] location is calculated as (x^i * y^j).
 
XXX: Modify loop so that terms higher than spatialOrder are not computed.
 
XXX: Modify this so that [i][j] location is calculated as (x^j * y^i)?
 ******************************************************************************/
static psImage *GenSpatialOrder(psS32 spatialOrder,
                                psF32 x,
                                psF32 y)
{
    psTrace("ImageSubtract.GenSpatialOrder", 4,
            "Calling GenSpatialOrder(%d, %f, %f)\n", spatialOrder, x, y);

    psImage *polyValues = psImageAlloc(spatialOrder+1, spatialOrder+1, PS_TYPE_F64);

    psF64 xSum = 1.0;
    psF64 ySum = 1.0;
    for (psS32 i = 0; i < spatialOrder + 1; i++) {
        ySum = xSum;
        for (psS32 j = 0; j < spatialOrder + 1; j++) {
            polyValues->data.F64[i][j] = ySum;
            ySum*= y;
        }
        xSum*= x;
    }

    psTrace("ImageSubtract.GenSpatialOrder", 4,
            "Exiting GenSpatialOrder(%d, %f, %f)\n", spatialOrder, x, y);

    return(polyValues);
}


/*******************************************************************************
IsisKernelConvolve(input, kernels, kernelID, col, row): This routine
convolves a single kernel basis function with a pixel in an image.
  ******************************************************************************/
static psF32 IsisKernelConvolve(const psImage *input,
                                const psSubtractionKernels *kernels,
                                psS32 kernelID,
                                psS32 col,
                                psS32 row)
{


    psTrace("ImageSubtract.IsisKernelConvolve", 4,
            "Calling IsisKernelConvolve(%d, %d, %d)\n", kernelID, col, row);
    psS32 spatialOrder = kernels->spatialOrder;
    psS32 kernelSize = kernels->size;
    psS32 xOrder = (psS32) kernels->xOrder->data.F32[kernelID];
    psS32 yOrder = (psS32) kernels->yOrder->data.F32[kernelID];
    psF32 numColsHalf = 0.5 * (psF32) input->numCols;
    psF32 numRowsHalf = 0.5 * (psF32) input->numRows;
    psF32 imageX = (((psF32) col) - numColsHalf) / numColsHalf; // Normalised position
    psF32 imageY = (((psF32) row) - numRowsHalf) / numRowsHalf; // Normalised position

    psImage *polyValues = GenSpatialOrder(spatialOrder, imageX, imageY);

    psF64 polyVal = polyValues->data.F64[yOrder][xOrder];

    psImage *preCalc = (psImage *) kernels->preCalc->data[kernelID];

    // XXX: Are the following asserts really necessary?
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(row-kernelSize, 0, NAN);
    PS_ASSERT_INT_LESS_THAN(row+kernelSize, input->numRows, NAN);
    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(col-kernelSize, 0, NAN);
    PS_ASSERT_INT_LESS_THAN(col+kernelSize, input->numCols, NAN);
    psF32 conv = 0.0;
    for (psS32 yy = -kernelSize ; yy < kernelSize ; yy++) {
        for (psS32 xx = -kernelSize ; xx < kernelSize ; xx++) {
            conv += input->data.F32[yy+row][xx+col] *
                    preCalc->data.F32[yy+kernelSize][xx+kernelSize] *
                    polyVal;
        }
    }
    psFree(polyValues);

    psTrace("ImageSubtract.IsisKernelConvolve", 4,
            "Exiting IsisKernelConvolve(%d, %d, %d)\n", kernelID, col, row);
    return(conv);
}

/*******************************************************************************
ConvolvePixelPois(input, mask, badStampMaskVal, solution, kernels, col, row):
 
This routine takes a single pixel in the psImage input and convolves it with
the set of kernel basis functions and their appropriate weights in solution.
It returns the value of the convolved pixel.
 
XXX: Static structure for polyValues?
 ******************************************************************************/
static psF32 ConvolvePixelPois(const psImage *input,
                               const psImage *mask,
                               psU32 badStampMaskVal,
                               const psVector *solution,
                               const psSubtractionKernels *kernels,
                               psS32 col,
                               psS32 row)
{
    psTrace("ImageSubtract.ConvolvePixelPois", 4,
            "Calling ConvolvePixelPois(%d, %d)\n", col, row);
    psS32 nBF = kernels->u->n;
    psF32 numColsHalf = 0.5 * (psF32) input->numCols;
    psF32 numRowsHalf = 0.5 * (psF32) input->numRows;
    psF32 background = solution->data.F64[solution->n-1];
    psS32 spatialOrder = kernels->spatialOrder;
    psF32 conv = background; // Initial convolved value

    if ((mask == NULL) || !(mask->data.U8[row][col] & badStampMaskVal)) {
        psF32 imageX = (((psF32) col) - numColsHalf) / numColsHalf; // Normalised position
        psF32 imageY = (((psF32) row) - numRowsHalf) / numRowsHalf; // Normalised position
        psImage *polyValues = GenSpatialOrder(spatialOrder, imageX, imageY);

        // Iterate over the kernel basis functions
        for (psS32 k = 0; k < nBF; k++) {
            psS32 u = (psS32) kernels->u->data.F32[k];
            psS32 v = (psS32) kernels->v->data.F32[k];

            // XXX: What's the story with this?
            #if 0

            psS32 xOrder = (psS32) kernels->xOrder->data.F32[k];
            psS32 yOrder = (psS32) kernels->yOrder->data.F32[k];
            psF64 polyVal = polyValues->data.F64[yOrder][xOrder];
            #else

            psF32 polyVal = 1.0;
            #endif

            // XXX: Why this?
            if (k == 0) {
                conv += solution->data.F64[k] * input->data.F32[row - v][col - u] * polyVal;
            } else {
                conv += solution->data.F64[k] *
                        (input->data.F32[row - v][col - u] * polyVal - input->data.F32[row][col]);
            }
        }
        psFree(polyValues);
    }

    psTrace("ImageSubtract.ConvolvePixelPois", 4,
            "Exiting ConvolvePixelPois(%d, %d)\n", col, row);
    return(conv);
}



/*******************************************************************************
ConvolvePixelIsis(input, mask, badStampMaskVal, solution, kernels, col, row):
 
This routine takes a single pixel in the psImage input and convolves it with
the set of kernel basis functions and their appropriate weights in solution.
It returns the value of the convolved pixel.
 
XXX: Static structure for polyValues?
 ******************************************************************************/
static psF32 ConvolvePixelIsis(const psImage *input,
                               const psImage *mask,
                               psU32 badStampMaskVal,
                               const psVector *solution,
                               const psSubtractionKernels *kernels,
                               psS32 col,
                               psS32 row)
{
    psTrace("ImageSubtract.ConvolvePixelIsis", 4,
            "Calling ConvolvePixelIsis(%d, %d)\n", col, row);
    psF32 background = solution->data.F64[solution->n-1];
    psF32 conv = background; // Initial convolved value

    if ((mask == NULL) || !(mask->data.U8[row][col] & badStampMaskVal)) {
        // Iterate over the kernel basis functions
        for (psS32 k = 0; k < kernels->u->n; k++) {
            conv += IsisKernelConvolve(input, kernels, k, col, row);
        }
    }

    psTrace("ImageSubtract.ConvolvePixelIsis", 4,
            "Exiting ConvolvePixelIsis(%d, %d)\n", col, row);
    return(conv);
}

/*******************************************************************************
ConvolveImage(input, mask, badStampMaskVal, solution, kernels): convolves an
arbitrary image with either an ISIS or POIS set of kernel basis functions.
 ******************************************************************************/
static psImage *ConvolveImage(const psImage *input,
                              const psImage *mask,
                              psU32 badStampMaskVal,
                              const psVector *solution,
                              const psSubtractionKernels *kernels)
{
    psTrace("ImageSubtract.ConvolveImage", 4, "Calling ConvolveImage()\n");
    PS_ASSERT_IMAGE_NON_NULL(input, NULL);
    PS_ASSERT_IMAGE_NON_EMPTY(input, NULL);
    PS_ASSERT_IMAGE_TYPE(input, PS_TYPE_F32, NULL);
    if (mask != NULL) {
        PS_ASSERT_IMAGES_SIZE_EQUAL(input, mask, NULL);
        PS_ASSERT_IMAGE_TYPE(mask, PS_TYPE_U8, NULL);
    }
    PS_ASSERT_VECTOR_NON_NULL(solution, NULL);
    PS_ASSERT_VECTOR_TYPE(solution, PS_TYPE_F64, NULL);
    PS_ASSERT_PTR_NON_NULL(kernels, NULL);
    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->preCalc != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->preCalc, NULL);
    } else {
        if (kernels->type != PM_SUBTRACTION_KERNEL_POIS) {
            psError(PS_ERR_BAD_PARAMETER_NULL, true,
                    "Unallowable operation: kernels->preCalc == NULL and kernels->type != PM_SUBTRACTION_KERNEL_POIS.\n");
            return(NULL);
        }
    }
    psS32 nBF = kernels->u->n;
    PS_ASSERT_VECTOR_SIZE(solution, nBF+1, NULL);

    psS32 numCols = input->numCols;
    psS32 numRows = input->numRows;
    psS32 kernelSize = kernels->size;

    psImage *convolved = psImageAlloc(numCols, numRows, PS_TYPE_F32);

    for (psS32 y = kernelSize; y < numRows - kernelSize; y++) {
        for (psS32 x = kernelSize; x < numCols - kernelSize; x++) {
            if (kernels->type == PM_SUBTRACTION_KERNEL_POIS) {
                convolved->data.F32[y][x] = ConvolvePixelPois(input, mask, badStampMaskVal,
                                            solution, kernels, x, y);
            } else if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
                convolved->data.F32[y][x] = ConvolvePixelIsis(input, mask, badStampMaskVal,
                                            solution, kernels, x, y);
            } else {
                psLogMsg(__func__, PS_LOG_WARN, "WARNING: unknown kernel type.  Returning NULL\n");
                return(NULL);
            }
        }
    }

    //
    // Pad the rest of the convolved image with 0.0
    //
    for (psS32 y = kernelSize; y < numRows - kernelSize; y++) {
        for (psS32 x = 0; x < kernelSize; x++) {
            convolved->data.F32[y][x] = 0.0;
        }
        for (psS32 x = numCols - kernelSize; x < numCols; x++) {
            convolved->data.F32[y][x] = 0.0;
        }
    }
    for (psS32 y = 0; y < kernelSize; y++) {
        for (psS32 x = 0; x < numCols; x++) {
            convolved->data.F32[y][x] = 0.0;
        }
    }
    for (psS32 y = numRows - kernelSize; y < numRows; y++) {
        for (psS32 x = 0; x < numCols; x++) {
            convolved->data.F32[y][x] = 0.0;
        }
    }

    psTrace("ImageSubtract.ConvolveImage", 4, "Exiting ConvolveImage()\n");
    return convolved;
}





/*******************************************************************************
XXX: We should assert that the (footprint, kernelSize, imageSize) stuff
ensures that all data is accessed in bounds?
 ******************************************************************************/
bool pmSubtractionCalculateEquation(psArray *stamps,          ///< The stamps for which to calculate the equation,
                                    const psImage *reference, ///< Reference image
                                    const psImage *input,     ///< Input image
                                    const psSubtractionKernels *kernels, ///< The kernel basis functions
                                    psS32 footprint           ///< Half-size of region over which to calculate equation
                                   )
{
    psTrace("ImageSubtract.pmSubtractionCalculateEquation", 3,
            "Calling pmSubtractionCalculateEquation()\n");
    PS_ASSERT_PTR_NON_NULL(stamps, false);
    PS_ASSERT_IMAGE_NON_NULL(reference, false);
    PS_ASSERT_IMAGE_NON_EMPTY(reference, false);
    PS_ASSERT_IMAGE_NON_NULL(input, false);
    PS_ASSERT_IMAGE_NON_EMPTY(input, 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 (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->preCalc, false);
    }
    psS32 kernelSize = kernels->size;
    PS_ASSERT_INT_NONNEGATIVE(footprint, false);
    //
    // For each legitimate stamp, ensure that the footprint is small enough to perform
    // the full calculation.
    //
    // XXX: Verify with IfA that this is a reasonable action.
    //
    for (psS32 s = 0; s < stamps->n; s++) {
        pmStamp *stamp = (pmStamp *) stamps->data[s];
        if (stamp->status == PM_STAMP_RECALC) {
            // XXX: trace message
            // printf("stamp %d (x, y) is (%d, %d).  Footprint is %d.  kernelSize is %d.\n", s, stamp->x, stamp->y, footprint, kernelSize);
            if (((stamp->y - (footprint + kernelSize)) < 0) ||
                    ((stamp->x - (footprint + kernelSize)) < 0) ||
                    ((stamp->y + footprint + kernelSize) >= input->numRows) ||
                    ((stamp->x + footprint + kernelSize) >= input->numCols)) {
                stamp->status = PM_STAMP_NONE;
                psLogMsg(__func__, PS_LOG_WARN,
                         "WARNING: stamp %d will be ignored.  It exceeds image size: access columns (%d to %d) and rows (%d to %d)\n",
                         s,
                         stamp->x - (footprint + kernelSize),
                         (stamp->x + footprint + kernelSize) - 1,
                         stamp->y - (footprint + kernelSize),
                         (stamp->y + footprint + kernelSize) - 1);
            }
        }
    }

    psS32 numHalfRows = reference->numRows;
    psS32 numHalfCols = reference->numCols;
    psS32 spatialOrder = kernels->spatialOrder;

    //
    // The numSolveParams incorporates the additional parameter for the
    // background value, which we must solve for.
    //
    psS32 numKernels = kernels->u->n;
    int numSolveParams = numKernels + 1;
    int bgIndex = numKernels;        // Index in matrix for the background

    //
    // We iterate over each stamp, allocate the matrix and vectors if
    // necessary, and then calculate those matrix/vectors.
    //
    for (psS32 s = 0; s < stamps->n; s++) {
        pmStamp *stamp = (pmStamp *) stamps->data[s];
        psTrace("pmSubtractionCalculateEquation", 6, "subCalcEqn(): stamp %d\n", s);
        if (stamp->status == PM_STAMP_RECALC) {
            psTrace("pmSubtractionCalculateEquation", 6, "subCalcEqn(): stamp %d: status is PM_STAMP_RECALC.\n", s);
            psImage *stampMatrix = stamp->matrix;
            psVector *stampVector = stamp->vector;

            if (stampMatrix == NULL) {
                stampMatrix = psImageAlloc(numSolveParams, numSolveParams, PS_TYPE_F64);
                stamp->matrix = stampMatrix;
            } else {
                PS_ASSERT_IMAGE_TYPE(stampMatrix, PS_TYPE_F64, false);
                PS_ASSERT_IMAGE_SIZE(stampMatrix, numSolveParams, numSolveParams, false);
            }
            PS_IMAGE_SET_F64(stampMatrix, 0.0);

            if (stampVector == NULL) {
                stampVector = psVectorAlloc(numSolveParams, PS_TYPE_F64);
                stamp->vector = stampVector;
            } else {
                PS_ASSERT_VECTOR_TYPE(stampVector, PS_TYPE_F64, false);
                PS_ASSERT_VECTOR_SIZE(stampVector, numSolveParams, false);
            }
            PS_VECTOR_SET_F64(stampVector, 0.0);
            psTrace("pmSubtractionCalculateEquation", 6, "subCalcEqn(): stamp %d: allocate matrix and vector.\n", s);

            //
            // Evaluate the spatial-order polynomial.  The [i][j]-th element of
            // the psImage polyValues will hold (x^i * y^j) for the stamp.  The
            // (x, y) value are scaled to [-1:1]
            //
            psImage *polyValues = GenSpatialOrder(spatialOrder,
                                                  ((psF64) (stamp->x - numHalfCols)) / ((psF64) numHalfCols),
                                                  ((psF64) (stamp->y - numHalfRows)) / ((psF64) numHalfRows));

            psTrace("pmSubtractionCalculateEquation", 6, "subCalcEqn(): stamp %d: generated spatial order terms.\n", s);

            if (kernels->type == PM_SUBTRACTION_KERNEL_POIS) {
                //
                // Iterate over all pixels surrounding this stamp.
                //
                for (psS32 y = stamp->y - footprint; y < stamp->y + footprint; y++) {
                    for (psS32 x = stamp->x - footprint; x < stamp->x + footprint; x++) {
                        psTrace("pmSubtractionCalculateEquation", 6, "subCalcEqn(): pixel (%d, %d).\n", y, x);

                        // The inverse of the noise, squared.
                        psF32 invNoise2 = 1.0/reference->data.F32[y][x];

                        //
                        // Iterate over the first convolution */
                        //
                        for (psS32 k1 = 0; k1 < numKernels; k1++) {
                            psS32 u1 = kernels->u->data.F32[k1];        // Offset in x
                            psS32 v1 = kernels->v->data.F32[k1];        // Offset in y
                            psS32 i1 = kernels->xOrder->data.F32[k1];   // Polynomial order in x
                            psS32 j1 = kernels->yOrder->data.F32[k1];   // Polynomial order in y

                            //
                            // First convolution.  This will set the value for the stampVector.
                            //
                            // XXX: verify the [y-v2][x-u2] subscript.  This generated errors in
                            // testing, depending on kernel size and footprint.
                            //
                            psF32 conv1 = polyValues->data.F64[j1][i1] * reference->data.F32[y - v1][x - u1];

                            //
                            // Assuming that the first kernel component is 0 order in x and y, and 0 offset
                            // XXX: I don't understand this:
                            //
                            if (k1 != 0) {
                                conv1 -= reference->data.F32[y][x];
                            }

                            //
                            // Iterate over the second convolution
                            //
                            for (psS32 k2 = k1; k2 < numKernels; k2++) {
                                psS32 u2 = (psS32) kernels->u->data.F32[k2];        // Offset in x
                                psS32 v2 = (psS32) kernels->v->data.F32[k2];        // Offset in y
                                psS32 i2 = (psS32) kernels->xOrder->data.F32[k2];   // Polynomial order in x
                                psS32 j2 = (psS32) kernels->yOrder->data.F32[k2];   // Polynomial order in y
                                //
                                // XXX: verify the [y-v2][x-u2] subscript.  This generated errors in
                                // testing, depending on kernel size and footprint.
                                //
                                // Second convolution
                                //
                                psF32 conv2 = polyValues->data.F64[j2][i2] *
                                              reference->data.F32[y-v2][x-u2];
                                //
                                // Assuming that the first kernel component is 0 order in x and y, and 0 offset
                                // XXX: I don't understand this:
                                if (k2 != 0) {
                                    //
                                    conv2 -= reference->data.F32[y][x];
                                }

                                // Add into the matrix element
                                stampMatrix->data.F64[k1][k2] += conv1 * conv2 * invNoise2;

                            } // Iteration on second convolution

                            // Add into the vector element
                            stampVector->data.F64[k1] += input->data.F32[y][x] * conv1 * invNoise2;

                            /* Background term */
                            stampMatrix->data.F64[k1][bgIndex] += conv1 * invNoise2;

                        } // Iteration on first convolution

                        //
                        // Background only terms.
                        // XXX: understand this.
                        //
                        stampMatrix->data.F64[bgIndex][bgIndex] += invNoise2;
                        stampVector->data.F64[bgIndex] += input->data.F32[y][x] * invNoise2;
                    }
                }
            } else if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
                //
                // Iterate over all pixels surrounding this stamp.
                //
                // XXX: Why isn't there a polyValues term here?
                //

                for (psS32 y = stamp->y - footprint; y < stamp->y + footprint; y++) {
                    for (psS32 x = stamp->x - footprint; x < stamp->x + footprint; x++) {
                        psTrace("pmSubtractionCalculateEquation", 6, "subCalcEqn(): pixel (%d, %d).\n", y, x);
                        psF32 invNoise2 = 1.0/reference->data.F32[y][x]; // The inverse of the noise, squared.

                        for (psS32 k1 = 0; k1 < numKernels; k1++) {
                            psF32 conv1 = IsisKernelConvolve(reference, kernels, k1, x, y);

                            for (psS32 k2 = k1; k2 < numKernels; k2++) {
                                //printf("(k1, k2) is (%d, %d)\n", k1, k2);
                                psF32 conv2 = IsisKernelConvolve(reference, kernels, k2, x, y);
                                stampMatrix->data.F64[k1][k2]+= conv1 * conv2 * invNoise2;
                            }
                            stampVector->data.F64[k1]+= input->data.F32[y][x] * conv1 * invNoise2;
                            stampMatrix->data.F64[k1][bgIndex] += conv1 * invNoise2;
                        }
                        stampMatrix->data.F64[bgIndex][bgIndex] += invNoise2;
                        stampVector->data.F64[bgIndex] += input->data.F32[y][x] * invNoise2;
                    }
                }
            } else {
                psLogMsg(__func__, PS_LOG_WARN, "WARNING: unknown kernel->type.\n");
                return(false);
            }
            psFree(polyValues);

            // XXX: Generate psTrace()
            if (0) {
                for (psS32 s = 0; s < stamps->n; s++) {
                    pmStamp *stamp = (pmStamp *) stamps->data[s];
                    if (stamp->status == PM_STAMP_RECALC) {
                        psVector *stampVector = stamp->vector;
                        printf("STAMP: stamp %d vector:\n", s);
                        PS_VECTOR_PRINT_F64(stampVector);
                    }
                }
            }

            //
            // Fill in other side of symmetric matrix
            //
            // XXX: understand this.
            // XXX: Why aren't they using numSolveParams instead of numKernels?
            // XXX: is this POIS specific?
            //
            for (psS32 k1 = 0; k1 < numKernels; k1++) {
                for (psS32 k2 = 0; k2 < k1; k2++) {
                    stampMatrix->data.F64[k1][k2] = stampMatrix->data.F64[k2][k1];
                }
                stampMatrix->data.F64[bgIndex][k1] = stampMatrix->data.F64[k1][bgIndex];
            }

            //
            // XXX: Why aren't they using numSolveParams instead of numKernels?
            // XXX: is this POIS specific?
            //
            #define XXX_CONFIG_PENALTY 1.0
            for (psS32 k = 0; k < numKernels; k++)
            {
                psS32 u = kernels->u->data.F32[k];  // Offset in x
                psS32 v = kernels->v->data.F32[k];  // Offset in y
                stampMatrix->data.F64[k][k] += XXX_CONFIG_PENALTY * (psF32)(u*u + v*v);
            }
            stamp->status = PM_STAMP_USED;
        } else {
            // Stamp is ignored since it's not PM_STAMP_RECALC
        }
    }
    psTrace("ImageSubtract.pmSubtractionCalculateEquation", 3,
            "Exiting pmSubtractionCalculateEquation()\n");
    return(true);
}




/*******************************************************************************
 ******************************************************************************/
psVector *pmSubtractionSolveEquation(psVector *solution, ///< Solution vector, or NULL
                                     const psArray *stamps      ///< Array of stamps
                                    )
{
    psTrace("ImageSubtract.pmSubtractionSolveEquation", 3,
            "Calling pmSubtractionSolveEquation()\n");
    PS_ASSERT_PTR_NON_NULL(stamps, NULL);
    psS32 size = -1;
    psS32 s = 0;

    //
    // Determine the size of the stamp vectors and matrix.
    // We iterate until we find the first acceptable stamp.
    //
    while ((size == -1) && (s < stamps->n)) {
        pmStamp *stamp = (pmStamp *) stamps->data[s];
        PS_ASSERT_PTR_NON_NULL(stamp, NULL);
        if (stamp->status == PM_STAMP_USED) {
            size = ((pmStamp *) stamps->data[s])->vector->n;
            PS_ASSERT_INT_POSITIVE(size, NULL);
        }
        s++;
    }
    if (size == -1) {
        psLogMsg(__func__, PS_LOG_WARN, "WARNING: no acceptable stamps.  Returning NULL\n");
        return(NULL);
    }

    if (solution != NULL) {
        PS_ASSERT_VECTOR_TYPE(solution, PS_TYPE_F64, NULL);
        PS_ASSERT_VECTOR_SIZE(solution, size, NULL);
    } else {
        solution = psVectorAlloc(size, PS_TYPE_F64);
    }

    //
    // Create the solution matrix and vector.
    //
    // XXX: Test these functions with size=-1.  This caused seg faults during test.
    //      This should be done in the psImage.c and psVector.c test files.  It
    //      should never occur here.
    //
    psImage *sumMatrix = psImageAlloc(size, size, PS_TYPE_F64);
    psVector *sumVector = psVectorAlloc(size, PS_TYPE_F64);
    PS_VECTOR_SET_F64(sumVector, 0.0);
    PS_IMAGE_SET_F64(sumMatrix, 0.0);

    //
    // Verify that all stamps have similar sizes.
    // Compute the sum matrix and vector.
    //
    for (psS32 s = 0; s < stamps->n; s++) {
        pmStamp *stamp = (pmStamp *) stamps->data[s];

        if (stamp->status == PM_STAMP_USED) {
            PS_ASSERT_INT_EQUAL(((pmStamp *) stamps->data[s])->vector->n, size, NULL);

            psImage *stampMatrix = stamp->matrix;
            psVector *stampVector = stamp->vector;
            PS_ASSERT_VECTOR_TYPE(stampVector, PS_TYPE_F64, NULL);
            PS_ASSERT_VECTOR_SIZE(stampVector, size, NULL);
            PS_ASSERT_IMAGE_TYPE(stampMatrix, PS_TYPE_F64, NULL);
            PS_ASSERT_IMAGE_SIZE(stampMatrix, size, size, NULL);

            (void)psBinaryOp(sumMatrix, sumMatrix, "+", stampMatrix);
            (void)psBinaryOp(sumVector, sumVector, "+", stampVector);
        }
    }
    psVector *permutation = NULL;
    // XXX: Check output from these routines.

    // XXX: psTrace()
    if (0) {
        PS_IMAGE_PRINT_F64(sumMatrix);
    }

    psImage *luMatrix = psMatrixLUD(NULL, &permutation, sumMatrix);
    if (luMatrix == NULL) {
        psError(PS_ERR_UNKNOWN, true, "Failed to LU-Decompose the matrix.\n");
        psFree(sumMatrix);
        psFree(sumVector);
        psFree(luMatrix);
        psFree(permutation);
        return(NULL);
    }
    // XXX: psTrace()
    if (0) {
        PS_IMAGE_PRINT_F64(luMatrix);
    }

    solution = psMatrixLUSolve(solution, luMatrix, sumVector, permutation);
    // XXX: psTrace()
    // XXX: should we be checking for NAN's in the solution vector?
    if (0) {
        PS_VECTOR_PRINT_F64(solution);
    }
    if (solution == NULL) {
        psError(PS_ERR_UNKNOWN, true, "Failed to solve the matrix.\n");
        psFree(sumMatrix);
        psFree(sumVector);
        psFree(luMatrix);
        psFree(permutation);
        return(NULL);
    }

    psFree(sumMatrix);
    psFree(sumVector);
    psFree(luMatrix);
    psFree(permutation);

    psTrace("ImageSubtract.pmSubtractionSolveEquation", 3,
            "Exiting pmSubtractionSolveEquation()\n");
    return(solution);
}


/*******************************************************************************
 ******************************************************************************/
static psVector *CalculateDeviations(psVector *deviations,
                                     psArray *stamps,
                                     psS32 footprint,
                                     const psImage *refImage,
                                     const psImage *inImage,
                                     const psImage *mask,
                                     psU32 badStampMaskVal,
                                     const psSubtractionKernels *kernels,
                                     const psVector *solution)
{
    psTrace("ImageSubtract.CalculateDeviations", 4,
            "Calling CalculateDeviations()\n");
    PS_ASSERT_PTR_NON_NULL(stamps, NULL);
    if (deviations != NULL) {
        PS_ASSERT_VECTOR_TYPE(deviations, PS_TYPE_F32, NULL);
        PS_ASSERT_VECTORS_SIZE_EQUAL(deviations, stamps, NULL);
    } else {
        deviations = psVectorAlloc(stamps->n, PS_TYPE_F32);
        // XXX: Probably not necessary.
        PS_VECTOR_SET_F32(deviations, 0.0);
    }
    PS_ASSERT_IMAGE_NON_NULL(refImage, NULL);
    PS_ASSERT_IMAGE_NON_EMPTY(refImage, NULL);
    PS_ASSERT_IMAGE_TYPE(refImage, PS_TYPE_F32, NULL);
    PS_ASSERT_IMAGE_NON_NULL(inImage, NULL);
    PS_ASSERT_IMAGE_NON_EMPTY(inImage, NULL);
    PS_ASSERT_IMAGE_TYPE(inImage, PS_TYPE_F32, NULL);
    PS_ASSERT_IMAGES_SIZE_EQUAL(refImage, inImage, NULL);
    PS_ASSERT_IMAGE_NON_NULL(mask, NULL);
    PS_ASSERT_IMAGE_NON_EMPTY(mask, NULL);
    PS_ASSERT_IMAGE_TYPE(mask, PS_TYPE_U8, NULL);
    PS_ASSERT_IMAGES_SIZE_EQUAL(refImage, mask, NULL);
    PS_ASSERT_VECTOR_NON_NULL(solution, NULL);
    PS_ASSERT_VECTOR_TYPE(solution, PS_TYPE_F64, NULL);
    PS_ASSERT_PTR_NON_NULL(kernels, NULL);
    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->preCalc != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->preCalc, NULL);
    } else {
        if (kernels->type != PM_SUBTRACTION_KERNEL_POIS) {
            psError(PS_ERR_BAD_PARAMETER_NULL, true,
                    "Unallowable operation: kernels->preCalc == NULL and kernels->type != PM_SUBTRACTION_KERNEL_POIS.\n");
            return(NULL);
        }
    }
    psS32 nBF = kernels->u->n;
    PS_ASSERT_VECTOR_SIZE(solution, nBF+1, NULL);

    psS32 kernelSize = kernels->size;
    int xSize = footprint + kernelSize;
    int ySize = footprint + kernelSize;
    psStats *stats = psStatsAlloc(PS_STAT_SAMPLE_MEAN); // Statistics
    psImage *subStamp = psImageAlloc(2 * xSize, 2 * ySize, PS_TYPE_F32); // Subtraction of stamp
    for (psS32 s = 0; s < stamps->n; s++) {
        pmStamp *stamp = stamps->data[s]; // The coordinates of the stamp of interest
        psS32 x = stamp->x;               // Stamp x coord
        psS32 y = stamp->y;               // Stamp y coord
        if (stamp->status == PM_STAMP_USED) {

            psRegion myReg = psRegionSet(x - xSize, x + xSize, y - ySize, y + ySize);
            psImage *refStamp = psImageSubset((psImage *) refImage, myReg);
            psImage *inStamp = psImageSubset((psImage *) inImage, myReg);
            psImage *maskStamp = psImageSubset((psImage *) mask, myReg);
            psImage *convRefStamp = ConvolveImage(refStamp, maskStamp, badStampMaskVal, solution, kernels);

            // Calculate chi^2
            (void)psBinaryOp(subStamp, inStamp, "-", convRefStamp);
            (void)psBinaryOp(subStamp, subStamp, "/", inStamp);
            (void)psBinaryOp(subStamp, subStamp, "*", subStamp);
            myReg = psRegionSet(kernelSize, kernelSize + 2 * footprint,
                                kernelSize, kernelSize + 2 * footprint);
            psImage *subStampTrim = psImageSubset((psImage *) subStamp, myReg);
            psImage *maskStampTrim = psImageSubset((psImage *) maskStamp, myReg);
            psImageStats(stats, subStampTrim, maskStampTrim, badStampMaskVal);

            deviations->data.F32[s] = stats->sampleMean * (psF32)footprint * (psF32)footprint * 4.0;
            // XXX: Allocate and free these elsewhere.
            psFree(refStamp);
            psFree(inStamp);
            psFree(maskStamp);
            psFree(convRefStamp);
            psFree(subStampTrim);
            psFree(maskStampTrim);
        }
    }

    psFree(stats);
    psFree(subStamp);

    psTrace("ImageSubtract.CalculateDeviations", 4,
            "Exiting CalculateDeviations()\n");
    return deviations;
}

/*******************************************************************************
 ******************************************************************************/
bool pmSubtractionRejectStamps(psArray *stamps,  ///< Array of stamps to check for rejection
                               psImage *mask,  ///< Mask image
                               psU32 badStampMaskVal, ///< Value to use in mask for bad stamp
                               psS32 footprint,  ///< Region to mask if stamp is bad
                               psF32 sigmaRej,  ///< Number of RMS deviations above zero at which to reject
                               const psImage *refImage, ///< Reference image
                               const psImage *inImage, ///< Input image
                               const psVector *solution, ///< Solution vector
                               const psSubtractionKernels *kernels ///< Array of kernel parameters
                              )
{
    psTrace("ImageSubtract.pmSubtractionRejectStamps", 3,
            "Calling pmSubtractionRejectStamps()\n");
    PS_ASSERT_PTR_NON_NULL(stamps, false);
    PS_ASSERT_IMAGE_NON_NULL(refImage, false);
    PS_ASSERT_IMAGE_NON_EMPTY(refImage, false);
    PS_ASSERT_IMAGE_TYPE(refImage, PS_TYPE_F32, false);
    PS_ASSERT_IMAGE_NON_NULL(inImage, false);
    PS_ASSERT_IMAGE_NON_EMPTY(inImage, false);
    PS_ASSERT_IMAGE_TYPE(inImage, PS_TYPE_F32, false);
    PS_ASSERT_IMAGES_SIZE_EQUAL(refImage, inImage, false);
    PS_ASSERT_IMAGE_NON_NULL(mask, false);
    PS_ASSERT_IMAGE_NON_EMPTY(mask, false);
    PS_ASSERT_IMAGE_TYPE(mask, PS_TYPE_U8, false);
    PS_ASSERT_IMAGES_SIZE_EQUAL(refImage, mask, 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_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->preCalc != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->preCalc, false);
    } else {
        if (kernels->type != PM_SUBTRACTION_KERNEL_POIS) {
            psError(PS_ERR_BAD_PARAMETER_NULL, true,
                    "Unallowable operation: kernels->preCalc == NULL and kernels->type != PM_SUBTRACTION_KERNEL_POIS.\n");
            return(false);
        }
    }

    psS32 nBF = kernels->u->n;
    PS_ASSERT_VECTOR_SIZE(solution, nBF+1, false);

    psVector *deviations = CalculateDeviations(NULL,
                           stamps,
                           footprint,
                           refImage,
                           inImage,
                           mask,
                           badStampMaskVal,
                           kernels,
                           solution);
    //
    // Calculate the deviation from zero.
    //
    psF64 meanDev = 0.0;
    psS32 numDev = 0;
    for (psS32 i = 0; i < deviations->n; i++) {
        pmStamp *stamp = stamps->data[i];
        if (stamp->status == PM_STAMP_USED) {
            meanDev += PS_SQR(deviations->data.F32[i]);
            numDev++;
        }
    }
    psF32 rmsDev = sqrtf(meanDev / (psF64)(numDev - 1));
    psF32 limit = rmsDev * sigmaRej;

    for (psS32 s = 0; s < stamps->n; s++) {
        pmStamp *stamp = (pmStamp *) stamps->data[s];
        if (stamp->status == PM_STAMP_USED && fabsf(deviations->data.F32[s]) > limit) {
            // Mask out the stamp in the image so you don't find it again
            for (psS32 y = stamp->y - footprint; y < stamp->y + footprint; y++) {
                for (psS32 x = stamp->x - footprint; x < stamp->x + footprint; x++) {
                    mask->data.U8[y][x] |= badStampMaskVal;
                }
            }

            // Set stamp for replacement
            stamp->x = 0;
            stamp->y = 0;
            stamp->status = PM_STAMP_REJECTED;
        }
    }

    psFree(deviations);
    psTrace("ImageSubtract.pmSubtractionRejectStamps", 3,
            "Exiting pmSubtractionRejectStamps()\n");
    return(true);
}

/*******************************************************************************
 ******************************************************************************/
psImage *pmSubtractionKernelImage(psImage *out,
                                  const psVector *solution,
                                  const psSubtractionKernels *kernels,
                                  psF32 x,
                                  psF32 y
                                 )
{
    psTrace("ImageSubtract.pmSubtractionKernelImage", 3,
            "Calling pmSubtractionKernelImage()\n");
    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_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->preCalc != NULL) {
        PS_ASSERT_VECTORS_SIZE_EQUAL(kernels->u, kernels->preCalc, NULL);
    } else {
        if (kernels->type != PM_SUBTRACTION_KERNEL_POIS) {
            psError(PS_ERR_BAD_PARAMETER_NULL, true,
                    "Unallowable operation: kernels->preCalc == NULL and kernels->type != PM_SUBTRACTION_KERNEL_POIS.\n");
            return(NULL);
        }
    }
    PS_ASSERT_INT_EQUAL(1+kernels->u->n, solution->n, NULL);

    psS32 nBF = kernels->u->n;
    psS32 spatialOrder = kernels->spatialOrder;
    psS32 kernelSize = kernels->size;

    if (out != NULL) {
        if ((out->numCols < (1+2*kernelSize)) || (out->numRows < (1+2*kernelSize))) {
            psLogMsg(__func__, PS_LOG_WARN, "WARNING: out image is not large enough.\n");
            return(out);
        }
    } else {
        out = psImageAlloc(1+2*kernelSize, 1+2*kernelSize, PS_TYPE_F32);
    }
    PS_IMAGE_SET_F32(out, 0.0);

    //
    // Generate the spatial-order polynomial.  The [i][j]-th element of
    // the psImage polyValues will hold (x^i * y^j) for the stamp.
    //
    psImage *polyValues = GenSpatialOrder(spatialOrder, x, y);

    // XXX: switch (i, j) so they correspond to (x, y).
    if (kernels->type == PM_SUBTRACTION_KERNEL_ISIS) {
        for (psS32 k = 0 ; k < nBF ; k++) {
            psS32 xOrder = (psS32) kernels->xOrder->data.F32[k];
            psS32 yOrder = (psS32) kernels->yOrder->data.F32[k];
            psF64 polyVal = polyValues->data.F64[yOrder][xOrder];

            // XXX: Verify that this is correct.
            for (psS32 i = -kernelSize ; i <= kernelSize ; i++) {
                for (psS32 j = -kernelSize ; j <= kernelSize ; j++) {
                    psImage *preCalc = (psImage *) kernels->preCalc->data[k];
                    out->data.F32[i+kernelSize][j+kernelSize]+=
                        solution->data.F64[k] *
                        preCalc->data.F32[i+kernelSize][j+kernelSize] *
                        polyVal;
                }
            }
        }
    } else if (kernels->type == PM_SUBTRACTION_KERNEL_POIS) {
        for (psS32 k = 0 ; k < nBF ; k++) {
            // XXX: Why don't we have compilation warnings on type here (if
            // we remove the (psS32) cast)?
            psS32 u = (psS32) kernels->u->data.F32[k];
            psS32 v = (psS32) kernels->v->data.F32[k];
            psS32 xOrder = (psS32) kernels->xOrder->data.F32[k];
            psS32 yOrder = (psS32) kernels->yOrder->data.F32[k];
            // XXX: Verify that this is correct.

            out->data.F32[kernelSize - v][kernelSize - u]+=
                solution->data.F64[k] * polyValues->data.F64[yOrder][xOrder];
        }
    }
    psFree(polyValues);

    psTrace("ImageSubtract.pmSubtractionKernelImage", 3,
            "Exiting pmSubtractionKernelImage()\n");
    return(out);
}
