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

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

#include "pmHDU.h"
#include "pmFPA.h"
#include "pmSubtractionParams.h"
#include "pmSubtractionKernels.h"
#include "pmSubtractionStamps.h"
#include "pmSubtractionEquation.h"
#include "pmSubtraction.h"
#include "pmSubtractionMask.h"
#include "pmSubtractionMatch.h"

#define KERNEL_MOSAIC 2                 // Half-number of kernel instances in the mosaic image

static bool useFFT = true;              // Do convolutions using FFT


//#define TESTING
//#define TESTING_MEMORY

// Output memory usage information
static void memCheck(const char *where)
{
#ifdef TESTING_MEMORY
    psMemBlock **leaks = NULL;
    int numLeaks = psMemCheckLeaks(0, &leaks, NULL, true);
    size_t largestSize = 0;
    psMemId largest = 0;
    size_t totalSize = 0;
    for (int i = 0; i < numLeaks; i++) {
        psMemBlock *mb = leaks[i];
        totalSize += mb->userMemorySize;
        if (mb->userMemorySize > largestSize) {
            largestSize = mb->userMemorySize;
            largest = mb->id;
        }
    }
    psFree(leaks);
    fprintf(stderr, "%s:\n", where);
    fprintf(stderr, "    Memory in use: %zd\n", totalSize);
    fprintf(stderr, "    Largest block: %ld\n", largest);
    fprintf(stderr, "    sbrk(): %zd\n", (size_t)sbrk(0));
#endif
    return;
}


static bool getStamps(pmSubtractionStampList **stamps, // Stamps to read
                      const pmReadout *ro1, // Readout 1
                      const pmReadout *ro2, // Readout 2
                      const psImage *subMask, // Mask for subtraction, or NULL
                      psImage *weight,  // Weight map
                      const psRegion *region, // Region of interest, or NULL
                      float threshold,  // Threshold for stamp finding
                      float stampSpacing, // Spacing between stamps
                      int size,         // Kernel half-size
                      int footprint,     // Convolution footprint for stamps
                      pmSubtractionMode mode // Mode for subtraction
    )
{
    psTrace("psModules.imcombine", 3, "Finding stamps...\n");
    *stamps = pmSubtractionStampsFind(*stamps, ro1->image, subMask, region, threshold, footprint,
                                      stampSpacing, mode);
    if (!*stamps) {
        psError(PS_ERR_UNKNOWN, false, "Unable to find stamps.");
        return false;
    }

    memCheck("  find stamps");

    psTrace("psModules.imcombine", 3, "Extracting stamps...\n");
    if (!pmSubtractionStampsExtract(*stamps, ro1->image, ro2 ? ro2->image : NULL, weight, size)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to extract stamps.");
        return false;
    }

    memCheck("   extract stamps");

    return true;
}





bool pmSubtractionMatch(pmReadout *conv1, pmReadout *conv2, const pmReadout *ro1, const pmReadout *ro2,
                        int footprint, float regionSize, float stampSpacing, float threshold,
                        const psArray *sources, const char *stampsName,
                        pmSubtractionKernelsType type, int size, int spatialOrder,
                        const psVector *isisWidths, const psVector *isisOrders,
                        int inner, int ringsOrder, int binning, bool optimum, const psVector *optFWHMs,
                        int optOrder, float optThreshold, int iter, float rej, psMaskType maskBad,
                        psMaskType maskBlank, float badFrac, pmSubtractionMode mode)
{
    PS_ASSERT_PTR_NON_NULL(conv1, false);
    if (mode == PM_SUBTRACTION_MODE_DUAL) {
        PS_ASSERT_PTR_NON_NULL(conv2, false);
    }
    PS_ASSERT_PTR_NON_NULL(ro1, false);
    PS_ASSERT_IMAGE_NON_NULL(ro1->image, false);
    PS_ASSERT_IMAGE_TYPE(ro1->image, PS_TYPE_F32, false);
    if (ro1->mask) {
        PS_ASSERT_IMAGE_NON_NULL(ro1->mask, false);
        PS_ASSERT_IMAGE_TYPE(ro1->mask, PS_TYPE_MASK, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(ro1->mask, ro1->image, false);
    }
    if (ro1->weight) {
        PS_ASSERT_IMAGE_NON_NULL(ro1->weight, false);
        PS_ASSERT_IMAGE_TYPE(ro1->weight, PS_TYPE_F32, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(ro1->weight, ro1->image, false);
    }
    if (ro2) {
        PS_ASSERT_IMAGE_NON_NULL(ro2->image, false);
        PS_ASSERT_IMAGE_TYPE(ro2->image, PS_TYPE_F32, false);
        PS_ASSERT_IMAGES_SIZE_EQUAL(ro2->image, ro1->image, false);
        if (ro2->mask) {
            PS_ASSERT_IMAGE_NON_NULL(ro2->mask, false);
            PS_ASSERT_IMAGE_TYPE(ro2->mask, PS_TYPE_MASK, false);
            PS_ASSERT_IMAGES_SIZE_EQUAL(ro2->mask, ro1->image, false);
        }
        if (ro2->weight) {
            PS_ASSERT_IMAGE_NON_NULL(ro2->weight, false);
            PS_ASSERT_IMAGE_TYPE(ro2->weight, PS_TYPE_F32, false);
            PS_ASSERT_IMAGES_SIZE_EQUAL(ro2->weight, ro1->image, false);
        }
    } else if (!stampsName && !sources) {
        psError(PS_ERR_UNEXPECTED_NULL, true,
                "A list of sources is required for convolving to a target PSF.");
        return false;
    }
    PS_ASSERT_INT_NONNEGATIVE(footprint, false);
    // regionSize can be just about anything (except maybe negative, but it can be NAN)
    PS_ASSERT_FLOAT_LARGER_THAN(stampSpacing, 0.0, false);
    // Don't care what threshold is
    if (sources) {
        PS_ASSERT_ARRAY_NON_NULL(sources, false);
    }
    // stampsName may be anything
    // We'll check kernel type when we allocate the kernels
    PS_ASSERT_INT_POSITIVE(size, false);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, false);
    if (isisWidths || isisOrders) {
        PS_ASSERT_VECTOR_NON_NULL(isisWidths, false);
        PS_ASSERT_VECTOR_TYPE(isisWidths, PS_TYPE_F32, false);
        PS_ASSERT_VECTOR_NON_NULL(isisOrders, false);
        PS_ASSERT_VECTOR_TYPE(isisOrders, PS_TYPE_S32, false);
        PS_ASSERT_VECTORS_SIZE_EQUAL(isisWidths, isisOrders, false);
    }
    PS_ASSERT_INT_NONNEGATIVE(inner, false);
    PS_ASSERT_INT_NONNEGATIVE(ringsOrder, false);
    PS_ASSERT_INT_POSITIVE(binning, false);
    if (optimum) {
        PS_ASSERT_VECTOR_NON_NULL(optFWHMs, false);
        PS_ASSERT_INT_NONNEGATIVE(optOrder, false);
        PS_ASSERT_FLOAT_LARGER_THAN(optThreshold, 0.0, false);
        PS_ASSERT_FLOAT_LESS_THAN_OR_EQUAL(optThreshold, 1.0, false);
    }
    PS_ASSERT_INT_POSITIVE(iter, false);
    PS_ASSERT_FLOAT_LARGER_THAN(rej, 0.0, false);
    // Don't care about maskBad
    // Don't care about maskBlank
    if (isfinite(badFrac)) {
        PS_ASSERT_FLOAT_LARGER_THAN(badFrac, 0.0, NULL);
        PS_ASSERT_FLOAT_LESS_THAN_OR_EQUAL(badFrac, 1.0, NULL);
    }

    // If the stamp footprint is smaller than the kernel size, then we won't get much signal in the outer
    // parts of the kernel, which can result in bad matching artifacts.
    if (footprint < size) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "Stamp footprint (%d) should be larger than or equal to the kernel size (%d)",
                footprint, size);
        return false;
    }

    // Reset the output readout, just in case
    if (conv1->image) {
        psFree(conv1->image);
        conv1->image = NULL;
    }
    if (conv1->mask) {
        psFree(conv1->mask);
        conv1->mask = NULL;
    }
    if (conv1->weight) {
        psFree(conv1->weight);
        conv1->weight = NULL;
    }

    // Where does our weight map come from?
    psImage *weight = NULL;             // Weight image to use
    if (ro1->weight && ro2 && ro2->weight) {
        weight = (psImage*)psBinaryOp(NULL, ro1->weight, "+", ro2->weight);
    } else if (ro1->weight) {
        weight = psMemIncrRefCounter(ro1->weight);
    } else if (ro2) {
        if (ro2->weight) {
            weight = psMemIncrRefCounter(ro2->weight);
        } else {
            weight = (psImage*)psBinaryOp(NULL, ro1->image, "+", ro2->image);
        }
    } else {
        weight = psMemIncrRefCounter(ro1->image);
    }

    // Putting important variable declarations here, since they are freed after a "goto" if there is an error.
    psImage *subMask = NULL;            // Mask for subtraction
    psRegion *region = NULL;            // Iso-kernel region
    psString regionString = NULL;       // String for region
    pmSubtractionStampList *stamps = NULL; // Stamps for matching PSF
    pmSubtractionKernels *kernels = NULL; // Kernel basis functions
    int numCols = ro1->image->numCols, numRows = ro1->image->numRows; // Image dimensions

    memCheck("start");

    subMask = pmSubtractionMask(ro1->mask, ro2 ? ro2->mask : NULL, maskBad, size, footprint,
                                badFrac, useFFT);
    if (!subMask) {
        psError(PS_ERR_UNKNOWN, false, "Unable to generate subtraction mask.");
        psFree(weight);
        return false;
    }

    memCheck("mask");

    // Get region of interest
    int xRegions = 1, yRegions = 1;     // Number of iso-kernel regions
    float xRegionSize = 0, yRegionSize = 0; // Size of iso-kernel regions
    if (isfinite(regionSize) && regionSize != 0.0) {
        xRegions = numCols / regionSize + 1;
        yRegions = numRows / regionSize + 1;
        xRegionSize = (float)numCols / (float)xRegions;
        yRegionSize = (float)numRows / (float)yRegions;
        region = psRegionAlloc(NAN, NAN, NAN, NAN);
    }

    // Iterate over iso-kernel regions
    for (int j = 0; j < yRegions; j++) {
        for (int i = 0; i < xRegions; i++) {
            psTrace("psModules.imcombine", 1, "Subtracting region %d of %d...\n",
                    j * xRegions + i + 1, xRegions * yRegions);
            if (region) {
                *region = psRegionSet((int)(i * xRegionSize), (int)((i + 1) * xRegionSize),
                                      (int)(j * yRegionSize), (int)((j + 1) * yRegionSize));
                psFree(regionString);
                regionString = psRegionToString(*region);
                psTrace("psModules.imcombine", 3, "Iso-kernel region: %s out of %d,%d\n",
                        regionString, numCols, numRows);
            }

            if (sources) {
                stamps = pmSubtractionStampsSetFromSources(sources, subMask, region, footprint,
                                                           stampSpacing, mode);
            } else if (stampsName && strlen(stampsName) > 0) {
                stamps = pmSubtractionStampsSetFromFile(stampsName, ro1->image, subMask, region, footprint,
                                                        stampSpacing, mode);
            }

            // We get the stamps here; we will also attempt to get stamps at the first iteration, but it
            // doesn't matter.
            if (!getStamps(&stamps, ro1, ro2, subMask, weight, NULL, threshold, stampSpacing,
                           size, footprint, mode)) {
                goto MATCH_ERROR;
            }

            if (mode == PM_SUBTRACTION_MODE_UNSURE || mode == PM_SUBTRACTION_MODE_TARGET) {
                pmSubtractionMode newMode = pmSubtractionOrder(stamps, footprint); // Subtraction mode
                switch (newMode) {
                  case PM_SUBTRACTION_MODE_1:
                    psLogMsg("psModules.imcombine", PS_LOG_INFO, "Convolving image 1 to match image 2.");
                    break;
                  case PM_SUBTRACTION_MODE_2:
                    if (mode == PM_SUBTRACTION_MODE_TARGET) {
                        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                                "Input PSF is larger than target PSF --- can't match image.");
                        goto MATCH_ERROR;
                    }
                    psLogMsg("psModules.imcombine", PS_LOG_INFO, "Convolving image 2 to match image 1.");
                    break;
                  default:
                    psError(PS_ERR_UNKNOWN, false, "Unable to determine subtraction order.");
                    goto MATCH_ERROR;
                }
                mode = newMode;
            }

            // Define kernel basis functions
            if (optimum && (type == PM_SUBTRACTION_KERNEL_ISIS || type == PM_SUBTRACTION_KERNEL_GUNK)) {
                kernels = pmSubtractionKernelsOptimumISIS(type, size, inner, spatialOrder, optFWHMs, optOrder,
                                                          stamps, footprint, optThreshold, mode);
                if (!kernels) {
                    psErrorClear();
                    psWarning("Unable to derive optimum ISIS kernel --- switching to default.");
                }
            }
            if (kernels == NULL) {
                // Not an ISIS/GUNK kernel, or the optimum kernel search failed
                kernels = pmSubtractionKernelsGenerate(type, size, spatialOrder, isisWidths, isisOrders,
                                                       inner, binning, ringsOrder, mode);
            }

            // Add analysis metadata
            {
                psMetadataAddPtr(conv1->analysis, PS_LIST_TAIL, PM_SUBTRACTION_ANALYSIS_KERNEL,
                                 PS_DATA_UNKNOWN | PS_META_DUPLICATE_OK, "Subtraction kernels", kernels);
                if (conv2) {
                    psMetadataAddPtr(conv2->analysis, PS_LIST_TAIL, PM_SUBTRACTION_ANALYSIS_KERNEL,
                                     PS_DATA_UNKNOWN | PS_META_DUPLICATE_OK, "Subtraction kernels", kernels);
                }
                psMetadataAddS32(conv1->analysis, PS_LIST_TAIL, PM_SUBTRACTION_ANALYSIS_MODE,
                                 PS_META_DUPLICATE_OK, "Subtraction kernels", mode);
                if (conv2) {
                    psMetadataAddS32(conv2->analysis, PS_LIST_TAIL, PM_SUBTRACTION_ANALYSIS_MODE,
                                     PS_META_DUPLICATE_OK, "Subtraction kernels", mode);
                }
                psRegion *subRegion;
                if (region) {
                    subRegion = psMemIncrRefCounter(region);
                } else {
                    subRegion = psRegionAlloc(0, numCols, 0, numRows);
                }
                psMetadataAddPtr(conv1->analysis, PS_LIST_TAIL, PM_SUBTRACTION_ANALYSIS_REGION,
                                  PS_DATA_REGION | PS_META_DUPLICATE_OK,
                                 "Region over which subtraction was performed", subRegion);
                if (conv2) {
                    psMetadataAddPtr(conv2->analysis, PS_LIST_TAIL, PM_SUBTRACTION_ANALYSIS_REGION,
                                     PS_DATA_REGION | PS_META_DUPLICATE_OK,
                                     "Region over which subtraction was performed", subRegion);
                }
                psFree(subRegion);
            }

            memCheck("kernels");

            int numRejected = -1;               // Number of rejected stamps in each iteration
            for (int k = 0; k < iter && numRejected != 0; k++) {
                psLogMsg("psModules.imcombine", PS_LOG_INFO, "Iteration %d.", k);

                if (!getStamps(&stamps, ro1, ro2, subMask, weight, region, threshold, stampSpacing,
                               size, footprint, mode)) {
                    goto MATCH_ERROR;
                }

                psTrace("psModules.imcombine", 3, "Calculating equation...\n");
                if (!pmSubtractionCalculateEquation(stamps, kernels)) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to calculate least-squares equation.");
                    goto MATCH_ERROR;
                }

                memCheck("  calculate equation");

                psTrace("psModules.imcombine", 3, "Solving equation...\n");

                if (!pmSubtractionSolveEquation(kernels, stamps)) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to calculate least-squares equation.");
                    goto MATCH_ERROR;
                }

                memCheck("  solve equation");

                psVector *deviations = pmSubtractionCalculateDeviations(stamps, kernels); // Stamp deviations
                if (!deviations) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to calculate deviations.");
                    goto MATCH_ERROR;
                }

                memCheck("   calculate deviations");

                psTrace("psModules.imcombine", 3, "Rejecting stamps...\n");
                numRejected = pmSubtractionRejectStamps(stamps, deviations, subMask, rej, footprint);
                if (numRejected < 0) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to reject stamps.");
                    psFree(deviations);
                    goto MATCH_ERROR;
                }
                psFree(deviations);

                memCheck("  reject stamps");
            }

            if (numRejected > 0) {
                psTrace("psModules.imcombine", 3, "Solving equation...\n");
                if (!pmSubtractionSolveEquation(kernels, stamps)) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to calculate least-squares equation.");
                    goto MATCH_ERROR;
                }
                psVector *deviations = pmSubtractionCalculateDeviations(stamps, kernels); // Stamp deviations
                if (!deviations) {
                    psError(PS_ERR_UNKNOWN, false, "Unable to calculate deviations.");
                    goto MATCH_ERROR;
                }
                pmSubtractionRejectStamps(stamps, deviations, subMask, NAN, footprint);
                psFree(deviations);
            }
            psFree(stamps);
            stamps = NULL;

            memCheck("solution");

            {
                psTrace("psModules.imcombine", 2, "Generating diagnostics...\n");
                // Generate image with convolution kernels
                int fullSize = 2 * size + 1 + 1;    // Full size of kernel
                psImage *convKernels = psImageAlloc(5 * fullSize - 1, 5 * fullSize - 1, PS_TYPE_F32);
                psImageInit(convKernels, NAN);
                for (int j = -KERNEL_MOSAIC; j <= KERNEL_MOSAIC; j++) {
                    for (int i = -KERNEL_MOSAIC; i <= KERNEL_MOSAIC; i++) {
                        psImage *kernel = pmSubtractionKernelImage(kernels, (float)i / (float)KERNEL_MOSAIC,
                                                                   (float)j / (float)KERNEL_MOSAIC,
                                                                   false); // Image of the kernel
                        if (!kernel) {
                            psError(PS_ERR_UNKNOWN, false, "Unable to generate kernel image.");
                            psFree(convKernels);
                            goto MATCH_ERROR;
                        }

                        if (psImageOverlaySection(convKernels, kernel, (i + KERNEL_MOSAIC) * fullSize,
                                                  (j + KERNEL_MOSAIC) * fullSize, "=") == 0) {
                            psError(PS_ERR_UNKNOWN, false, "Unable to overlay kernel image.");
                            psFree(kernel);
                            psFree(convKernels);
                            goto MATCH_ERROR;
                        }
                        psFree(kernel);
                    }
                }

                psString comment = NULL; // Comment for metadata
                psStringAppend(&comment, "Subtraction kernel for region %s", regionString);
                psMetadataAddImage(conv1->analysis, PS_LIST_TAIL, "SUBTRACTION.KERNEL.IMAGE",
                                   PS_META_DUPLICATE_OK, comment, convKernels);
                psFree(comment);
                psFree(convKernels);
            }

#if 0
            {
                // Generate images of the kernel components
                psMetadata *header = psMetadataAlloc(); // Header
                for (int i = 0; i < solution->n; i++) {
                    psString name = NULL;       // Header keyword
                    psStringAppend(&name, "SOLN%04d", i);
                    psMetadataAddF64(header, PS_LIST_TAIL, name, 0, NULL, solution->data.F64[i]);
                    psFree(name);
                }
                psArray *kernelImages = pmSubtractionKernelSolutions(solution, kernels, 0.0, 0.0);
                psFits *kernelFile = psFitsOpen("kernels.fits", "w");
                (void)psFitsWriteImageCube(kernelFile, header, kernelImages, NULL);
                psFitsClose(kernelFile);
                psFree(kernelImages);
                psFree(header);
            }
#endif

            memCheck("diag outputs");

            psTrace("psModules.imcombine", 2, "Convolving...\n");
            if (!pmSubtractionConvolve(conv1, conv2, ro1, ro2, subMask, maskBlank, region, kernels,
                                       true, useFFT)) {
                psError(PS_ERR_UNKNOWN, false, "Unable to convolve image.");
                goto MATCH_ERROR;
            }
            psFree(kernels);
            kernels = NULL;

            // There is data in the readout now
            conv1->data_exists = true;
            if (conv1->parent) {
                conv1->parent->data_exists = true;
                conv1->parent->parent->data_exists = true;
            }
            if (mode == PM_SUBTRACTION_MODE_DUAL) {
                conv2->data_exists = true;
                if (conv2->parent) {
                    conv2->parent->data_exists = true;
                    conv2->parent->parent->data_exists = true;
                }
            }
        }
    }
    psFree(region);
    region = NULL;
    psFree(regionString);
    regionString = NULL;
    psFree(subMask);
    subMask = NULL;
    psFree(weight);
    weight = NULL;

    if (!pmSubtractionBorder(conv1->image, conv1->weight, conv1->mask, size, maskBlank)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to set border of convolved image.");
        goto MATCH_ERROR;
    }

    memCheck("convolution");


#ifdef TESTING
    {
        psFits *fits = psFitsOpen("convolved1.fits", "w");
        psFitsWriteImage(fits, NULL, conv1->image, 0, NULL);
        psFitsClose(fits);

        if (mode == PM_SUBTRACTION_MODE_DUAL) {
            psFits *fits = psFitsOpen("convolved2.fits", "w");
            psFitsWriteImage(fits, NULL, conv2->image, 0, NULL);
            psFitsClose(fits);
        }
    }
#endif

    return true;

MATCH_ERROR:
    psFree(region);
    psFree(regionString);
    psFree(subMask);
    psFree(kernels);
    psFree(stamps);
    psFree(weight);
    return false;
}

// Calculate the second order moments for an image
static float subtractionOrderMoment(const psKernel *kernel, // Image for which to measure moments
                                    int radius   // Maximum radius
                                    )
{
    assert(kernel && kernel->kernel);

    int xMin = PS_MAX(kernel->xMin, -radius), xMax = PS_MIN(kernel->xMax, radius); // Bounds in x
    int yMin = PS_MAX(kernel->yMin, -radius), yMax = PS_MIN(kernel->yMax, radius); // Bounds in y

    float xCentroid = 0.0, yCentroid = 0.0; // Centroid (first moment)
    float sum = 0.0;       // Sum (zero-th moment)
    for (int y = yMin; y <= yMax; y++) {
        for (int x = xMin; x <= xMax; x++) {
            xCentroid += kernel->kernel[y][x] * x;
            yCentroid += kernel->kernel[y][x] * y;
            sum += kernel->kernel[y][x];
        }
    }
    xCentroid /= sum;
    yCentroid /= sum;

    float eta20 = 0.0, eta02 = 0.0;     // Second moments
    for (int y = yMin; y <= yMax; y++) {
        float yDiff = y - yCentroid;
        for (int x = xMin; x <= xMax; x++) {
            float xDiff = x - xCentroid;
            eta20 += PS_SQR(xDiff) * kernel->kernel[y][x];
            eta02 += PS_SQR(yDiff) * kernel->kernel[y][x];
        }
    }

    // Normalise to calculate the scale-invariant
    float sum2 = PS_SQR(sum);
    eta20 /= sum2;
    eta02 /= sum2;
    // eta11 /= sum2;

    return eta20 + eta02;
}

#if 0
// Calculate the deviations for a particular subtraction order
static psVector *subtractionOrderDeviation(float *sumKernel, // Sum of the kernel
                                           pmSubtractionStampList *stamps, // Stamps to convolve
                                           const pmSubtractionKernels *kernels, // Kernel basis functions
                                           int footprint, // Stamp footprint
                                           pmSubtractionMode mode // Mode of subtraction
                                           )
{
    assert(stamps);
    assert(footprint >= 0);
    assert(mode == PM_SUBTRACTION_MODE_1 || mode == PM_SUBTRACTION_MODE_2);

    if (!pmSubtractionCalculateEquation(stamps, kernels, footprint, mode)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to calculate least-squares equation.");
        return NULL;
    }

    psVector *solution = pmSubtractionSolveEquation(NULL, stamps);
    if (!solution) {
        psError(PS_ERR_UNKNOWN, false, "Unable to calculate least-squares equation.");
        return NULL;
    }

    if (sumKernel) {
        float sum = 0.0;                // Sum of the kernel
        psImage *image = pmSubtractionKernelImage(solution, kernels, 0.0, 0.0); // Image of kernel
        for (int y = 0; y < image->numRows; y++) {
            for (int x = 0; x < image->numCols; x++) {
                sum += image->data.F32[y][x];
            }
        }
        psFree(image);
        *sumKernel = sum;
    }

    psVector *deviations = pmSubtractionCalculateDeviations(stamps, solution, footprint, kernels, mode);
    psFree(solution);
    if (!deviations) {
        psError(PS_ERR_UNKNOWN, false, "Unable to calculate deviations.");
        return NULL;
    }

    return deviations;
}
#endif

pmSubtractionMode pmSubtractionOrder(pmSubtractionStampList *stamps, int radius)
{
    PS_ASSERT_INT_POSITIVE(radius, PM_SUBTRACTION_MODE_ERR);

    psVector *mask = psVectorAlloc(stamps->num, PS_TYPE_MASK); // Mask for stamps
    psVector *moments = psVectorAlloc(stamps->num, PS_TYPE_F32); // Moments
    for (int i = 0; i < stamps->num; i++) {
        pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
        if (stamp->status != PM_SUBTRACTION_STAMP_CALCULATE && stamp->status != PM_SUBTRACTION_STAMP_USED) {
            mask->data.PS_TYPE_MASK_DATA[i] = 0xff;
            continue;
        }
        mask->data.PS_TYPE_MASK_DATA[i] = 0;
        moments->data.F32[i] = subtractionOrderMoment(stamp->image1, radius) /
            subtractionOrderMoment(stamp->image2, radius);
    }

    psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN);
    if (!psVectorStats(stats, moments, NULL, mask, 0xff)) {
        psError(PS_ERR_UNKNOWN, false, "Unable to calculate statistics for moments ratio.");
        psFree(mask);
        psFree(moments);
        psFree(stats);
        return PM_SUBTRACTION_MODE_ERR;
    }
    psFree(moments);
    psFree(mask);

    psLogMsg("psModules.imcombine", PS_LOG_INFO, "Median ratio of second moments: %lf", stats->robustMedian);
    pmSubtractionMode mode = (stats->robustMedian <= 1.0 ? PM_SUBTRACTION_MODE_1 : PM_SUBTRACTION_MODE_2);
    psFree(stats);

    return mode;
}



