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

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

#include "pmSubtraction.h"
#include "pmSubtractionKernels.h"

#define RINGS_BUFFER 10                 // Buffer size for RINGS data


// Free function for pmSubtractionKernels
static void subtractionKernelsFree(pmSubtractionKernels *kernels)
{
    psFree(kernels->description);
    psFree(kernels->u);
    psFree(kernels->v);
    psFree(kernels->widths);
    psFree(kernels->uStop);
    psFree(kernels->vStop);
    psFree(kernels->preCalc);
    psFree(kernels->penalties);
    psFree(kernels->solution1);
    psFree(kernels->solution2);
}

// Raise an integer to an integer power
static inline long power(int value,     // Value
                         int exp        // Exponent
    )
{
    if (exp == 0) {
        return 1.0;
    }
    long result = value;               // Result to return
    for (int i = 2; i <= exp; i++) {
        result *= value;
    }
    return result;
}

// Generate 1D convolution kernel for ISIS
static psVector *subtractionKernelISIS(float sigma, // Gaussian width
                                       int order, // Polynomial order
                                       int size // Kernel half-size
    )
{
    int fullSize = 2 * size + 1;        // Full size of kernel
    psVector *kernel = psVectorAlloc(fullSize, PS_TYPE_F32); // Kernel to return

    float expNorm = -0.5 / PS_SQR(sigma); // Normalisation for exponential
    float norm = 1.0 / (M_2_PI * sqrtf(sigma)); // Normalisation for Gaussian
    for (int i = 0, x = -size; x <= size; i++, x++) {
        kernel->data.F32[i] = norm * power(x, order) * expf(expNorm * PS_SQR(x));
    }

    return kernel;
}

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

bool p_pmSubtractionKernelsAddGrid(pmSubtractionKernels *kernels, int start, int size)
{
    PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, false);
    PS_ASSERT_INT_NONNEGATIVE(start, false);
    PS_ASSERT_INT_NONNEGATIVE(size, false);

    int numNew = PS_SQR(2 * size + 1) - 1;  // Number of new kernel parameters to add

    // Ensure the sizes match
    kernels->widths = psVectorRealloc(kernels->widths, start + numNew);
    kernels->u = psVectorRealloc(kernels->u, start + numNew);
    kernels->v = psVectorRealloc(kernels->v, start + numNew);
    kernels->preCalc = psArrayRealloc(kernels->preCalc, start + numNew);
    kernels->penalties = psVectorRealloc(kernels->penalties, start + numNew);
    kernels->inner = start;

    // Generate a set of kernels for each (u,v)
    for (int v = - size, index = start; v <= size; v++) {
        for (int u = - size; u <= size; u++, index++) {
            if (v == 0 && u == 0) {
                // Skip normalisation component: added explicitly
                index--;
                continue;
            }
            kernels->widths->data.F32[index] = NAN;
            kernels->u->data.S32[index] = u;
            kernels->v->data.S32[index] = v;
            kernels->preCalc->data[index] = NULL;
            kernels->penalties->data.F32[index] = kernels->penalty * (PS_SQR(u) + PS_SQR(v));

            psTrace("psModules.imcombine", 7, "Kernel %d: %d %d\n", index, u, v);
        }
    }

    return true;
}

pmSubtractionKernels *p_pmSubtractionKernelsRawISIS(int size, int spatialOrder,
                                                    const psVector *fwhms, const psVector *orders,
                                                    float penalty, pmSubtractionMode mode)
{
    PS_ASSERT_VECTOR_NON_NULL(fwhms, NULL);
    PS_ASSERT_VECTOR_TYPE(fwhms, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(orders, NULL);
    PS_ASSERT_VECTOR_TYPE(orders, PS_TYPE_S32, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(fwhms, orders, NULL);
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, NULL);

    int numGaussians = fwhms->n;       // Number of Gaussians

    int num = 0;                        // Number of basis functions
    psString params = NULL;             // List of parameters
    for (int i = 0; i < numGaussians; i++) {
        int gaussOrder = orders->data.S32[i]; // Polynomial order to apply to Gaussian
        psStringAppend(&params, "(%.1f,%d)", fwhms->data.F32[i], orders->data.S32[i]);
        num += (gaussOrder + 1) * (gaussOrder + 2) / 2;
    }

    pmSubtractionKernels *kernels = pmSubtractionKernelsAlloc(num, PM_SUBTRACTION_KERNEL_ISIS, size,
                                                              spatialOrder, penalty, mode); // The kernels
    psStringAppend(&kernels->description, "ISIS(%d,%s,%d,%.2e)", size, params, spatialOrder, penalty);

    psLogMsg("psModules.imcombine", PS_LOG_INFO, "ISIS kernel: %s,%d --> %d elements",
             params, spatialOrder, num);
    psFree(params);

    // Set the kernel parameters
    int fullSize = 2 * size + 1;        // Full size of kernels
    for (int i = 0, index = 0; i < numGaussians; i++) {
        float sigma = fwhms->data.F32[i] / (2.0 * sqrtf(2.0 * logf(2.0))); // Gaussian sigma
        // Iterate over (u,v) order
        for (int uOrder = 0; uOrder <= orders->data.S32[i]; uOrder++) {
            for (int vOrder = 0; vOrder <= orders->data.S32[i] - uOrder; vOrder++, index++) {
                psArray *preCalc = psArrayAlloc(2); // Array to hold precalculated values
                psVector *xKernel = preCalc->data[0] = subtractionKernelISIS(sigma, uOrder, size); // x Kernel
                psVector *yKernel = preCalc->data[1] = subtractionKernelISIS(sigma, vOrder, size); // y Kernel

                // Calculate moments
                double sum = 0.0;       // Sum of kernel component, for normalisation
                double moment = 0.0;    // Moment, for penalty
                for (int v = -size, y = 0; v <= size; v++, y++) {
                    for (int u = -size, x = 0; u <= size; u++, x++) {
                        double value = xKernel->data.F32[x] * yKernel->data.F32[y]; // Value of kernel
                        sum += value;
                        moment += value * (PS_SQR(u) + PS_SQR(v));
                    }
                }

                // Normalise sum of kernel component to unity for even functions
                if (uOrder % 2 == 0 && vOrder % 2 == 0) {
                    double sum = 0.0;   // Sum of kernel component
                    for (int v = 0; v < fullSize; v++) {
                        for (int u = 0; u < fullSize; u++) {
                            sum += xKernel->data.F32[u] * yKernel->data.F32[v];
                        }
                    }
                    sum = 1.0 / sum;
                    psBinaryOp(xKernel, xKernel, "*", psScalarAlloc(sum, PS_TYPE_F32));
                    psBinaryOp(yKernel, yKernel, "*", psScalarAlloc(sum, PS_TYPE_F32));
                    moment *= sum;
                }

                kernels->widths->data.F32[index] = fwhms->data.F32[i];
                kernels->u->data.S32[index] = uOrder;
                kernels->v->data.S32[index] = vOrder;
                if (kernels->preCalc->data[index]) {
                    psFree(kernels->preCalc->data[index]);
                }
                kernels->preCalc->data[index] = preCalc;
                kernels->penalties->data.F32[index] = kernels->penalty * fabsf(moment);

                psTrace("psModules.imcombine", 7, "Kernel %d: %f %d %d %f\n", index,
                        fwhms->data.F32[i], uOrder, vOrder, fabsf(moment));
            }
        }
    }

    return kernels;
}

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

pmSubtractionKernels *pmSubtractionKernelsAlloc(int numBasisFunctions, pmSubtractionKernelsType type,
                                                int size, int spatialOrder, float penalty,
                                                pmSubtractionMode mode)
{
    pmSubtractionKernels *kernels = psAlloc(sizeof(pmSubtractionKernels)); // Kernels, to return
    psMemSetDeallocator(kernels, (psFreeFunc)subtractionKernelsFree);

    kernels->type = type;
    kernels->description = NULL;
    kernels->num = numBasisFunctions;
    kernels->u = psVectorAlloc(numBasisFunctions, PS_TYPE_S32);
    kernels->v = psVectorAlloc(numBasisFunctions, PS_TYPE_S32);
    kernels->widths = psVectorAlloc(numBasisFunctions, PS_TYPE_F32);
    kernels->preCalc = psArrayAlloc(numBasisFunctions);
    kernels->penalty = penalty;
    kernels->penalties = psVectorAlloc(numBasisFunctions, PS_TYPE_F32);
    kernels->uStop = NULL;
    kernels->vStop = NULL;
    kernels->size = size;
    kernels->inner = 0;
    kernels->spatialOrder = spatialOrder;
    kernels->bgOrder = 0;
    kernels->mode = mode;
    kernels->numCols = 0;
    kernels->numRows = 0;
    kernels->solution1 = NULL;
    kernels->solution2 = NULL;

    return kernels;
}

pmSubtractionKernels *pmSubtractionKernelsPOIS(int size, int spatialOrder, float penalty,
                                               pmSubtractionMode mode)
{
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, NULL);

    int num = PS_SQR(2 * size + 1) - 1; // Number of basis functions

    pmSubtractionKernels *kernels = pmSubtractionKernelsAlloc(num, PM_SUBTRACTION_KERNEL_POIS, size,
                                                              spatialOrder, penalty, mode); // The kernels
    psStringAppend(&kernels->description, "POIS(%d,%d,%.2e)", size, spatialOrder, penalty);
    psLogMsg("psModules.imcombine", PS_LOG_INFO, "POIS kernel: %d,%d --> %d elements",
             size, spatialOrder, num);

    if (!p_pmSubtractionKernelsAddGrid(kernels, 0, size)) {
        psAbort("Should never get here.");
    }

    return kernels;
}


pmSubtractionKernels *pmSubtractionKernelsISIS(int size, int spatialOrder,
                                               const psVector *fwhms, const psVector *orders,
                                               float penalty, pmSubtractionMode mode)
{
    pmSubtractionKernels *kernels = p_pmSubtractionKernelsRawISIS(size, spatialOrder, fwhms, orders,
                                                                  penalty, mode); // Kernels
    if (!kernels) {
        return NULL;
    }

    if (psTraceGetLevel("psModules.imcombine.kernel") >= 10) {
        for (int i = 0; i < kernels->num; i++) {
            psKernel *kernel = kernels->preCalc->data[i]; // Kernel of interest
            psString kernelName = NULL;
            psStringAppend(&kernelName, "kernel%03d.fits", i);
            psFits *kernelFile = psFitsOpen(kernelName, "w");
            psFree(kernelName);
            psFitsWriteImage(kernelFile, NULL, kernel->image, 0, NULL);
            psFitsClose(kernelFile);
            double sum = 0.0;
            for (int y = 0; y < kernel->image->numRows; y++) {
                for (int x = 0; x < kernel->image->numCols; x++) {
                    sum += kernel->image->data.F32[y][x];
                }
            }
            psTrace("psModules.imcombine.kernel", 10, "Kernel %d sum: %le\n", i, sum);
        }
    }

    return kernels;
}

pmSubtractionKernels *pmSubtractionKernelsSPAM(int size, int spatialOrder, int inner, int binning,
                                               float penalty, pmSubtractionMode mode)
{
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, NULL);
    PS_ASSERT_INT_NONNEGATIVE(inner, NULL);
    PS_ASSERT_INT_LARGER_THAN(size, inner, NULL);
    PS_ASSERT_INT_POSITIVE(binning, NULL);

    // The outer region should be divisible by the "binning"; otherwise allocate remainder to the inner region
    int numOuter = (size - inner) / binning; // Number of summed pixels in the outer region
    int numInner = inner + (size - inner) % binning; // Number of pixels in the inner region
    assert(numOuter * binning + numInner == size);
    int numTotal = numOuter + numInner; // Total number of summed pixels

    psTrace("psModules.imcombine", 3, "Inner: %d Outer: %d\n", numInner, numOuter);

    int num = PS_SQR(2 * numTotal + 1) - 1; // Number of basis functions

    psTrace("psModules.imcombine", 3, "Number of basis functions: %d\n", num);

    pmSubtractionKernels *kernels = pmSubtractionKernelsAlloc(num, PM_SUBTRACTION_KERNEL_SPAM, size,
                                                              spatialOrder, penalty, mode); // The kernels
    kernels->inner = inner;
    psStringAppend(&kernels->description, "SPAM(%d,%d,%d,%d,%.2e)", size, inner, binning, spatialOrder,
                   penalty);

    psLogMsg("psModules.imcombine", PS_LOG_INFO, "SPAM kernel: %d,%d,%d,%d --> %d elements",
             size, inner, binning, spatialOrder, num);

    kernels->uStop = psVectorAlloc(num, PS_TYPE_S32);
    kernels->vStop = psVectorAlloc(num, PS_TYPE_S32);

    psVector *locations = psVectorAlloc(2 * numTotal + 1, PS_TYPE_S32); // Locations for each kernel element
    psVector *widths = psVectorAlloc(2 * numTotal + 1, PS_TYPE_S32); // Widths for each kernel element
    locations->data.S32[numTotal] = 0;
    widths->data.S32[numTotal] = 0;
    for (int i = 1; i <= numInner; i++) {
        locations->data.S32[numTotal + i] = i;
        widths->data.S32[numTotal + i] = 0;
        locations->data.S32[numTotal - i] = - i;
        widths->data.S32[numTotal - i] = 0;
    }
    for (int i = numInner + 1; i <= numTotal; i++) {
        locations->data.S32[numTotal + i] = locations->data.S32[numTotal + i - 1] +
            widths->data.S32[numTotal + i - 1] + 1;
        widths->data.S32[numTotal + i] = binning - 1;
        locations->data.S32[numTotal - i] = locations->data.S32[numTotal - i + 1] - binning;
        widths->data.S32[numTotal - i] = binning - 1;
    }

    if (psTraceGetLevel("psModules.imcombine") >= 10) {
        for (int i = 0; i < 2 * numTotal + 1; i++) {
            psTrace("psModules.imcombine", 10, "%d: %d -> %d\n", i, locations->data.S32[i],
                    locations->data.S32[i] + widths->data.S32[i]);
        }
    }

    // Set the kernel parameters
    for (int i = - numTotal, index = 0; i <= numTotal; i++) {
        int u = locations->data.S32[numTotal + i]; // Location of pixel
        int uStop = u + widths->data.S32[numTotal + i]; // Width of pixel

        for (int j = - numTotal; j <= numTotal; j++, index++) {
            if (i == 0 && j == 0) {
                // Skip normalisation component: added explicitly
                index--;
                continue;
            }
            int v = locations->data.S32[numTotal + j]; // Location of pixel
            int vStop = v + widths->data.S32[numTotal + j]; // Width of pixel

            kernels->u->data.S32[index] = u;
            kernels->v->data.S32[index] = v;
            kernels->uStop->data.S32[index] = uStop;
            kernels->vStop->data.S32[index] = vStop;

            psTrace("psModules.imcombine", 7, "Kernel %d: %d %d %d %d\n", index,
                    u, uStop, v, vStop);
        }
    }

    psFree(locations);
    psFree(widths);

    psWarning("Kernel penalty for dual-convolution is not configured for SPAM kernels.");
    psVectorInit(kernels->penalties, 0.0);

    return kernels;
}


pmSubtractionKernels *pmSubtractionKernelsFRIES(int size, int spatialOrder, int inner, float penalty,
                                                pmSubtractionMode mode)
{
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, NULL);
    PS_ASSERT_INT_NONNEGATIVE(inner, NULL);
    PS_ASSERT_INT_LARGER_THAN(size, inner, NULL);

    int fibNum = 0;                     // Number of Fibonacci values
    int fibLast = 1, fibTotal = 2;      // Fibonacci sequence
    while (fibTotal < size - inner) {
        int temp = fibTotal;
        fibTotal += fibLast;
        fibLast = temp;
        fibNum++;
    }

    int numInner = inner;               // Number of pixels in the inner region
    int numOuter = fibNum;              // Number of summed pixels in the outer region
    int numTotal = numOuter + numInner; // Total number of summed pixels

    psTrace("psModules.imcombine", 3, "Inner: %d Outer: %d\n", numInner, numOuter);

    int num = PS_SQR(2 * numTotal + 1) - 1; // Number of basis functions

    psTrace("psModules.imcombine", 3, "Number of basis functions: %d\n", num);

    pmSubtractionKernels *kernels = pmSubtractionKernelsAlloc(num, PM_SUBTRACTION_KERNEL_FRIES, size,
                                                              spatialOrder, penalty, mode); // The kernels
    kernels->inner = inner;
    psStringAppend(&kernels->description, "FRIES(%d,%d,%d,%.2e)", size, inner, spatialOrder, penalty);

    psLogMsg("psModules.imcombine", PS_LOG_INFO, "FRIES kernel: %d,%d,%d --> %d elements",
             size, inner, spatialOrder, num);

    kernels->uStop = psVectorAlloc(num, PS_TYPE_S32);
    kernels->vStop = psVectorAlloc(num, PS_TYPE_S32);

    psVector *start = psVectorAlloc(2 * numTotal + 1, PS_TYPE_S32);
    psVector *stop = psVectorAlloc(2 * numTotal + 1, PS_TYPE_S32);
    start->data.S32[numTotal] = 0;
    stop->data.S32[numTotal] = 0;
    for (int i = 1; i <= numInner; i++) {
        start->data.S32[numTotal + i] = i;
        stop->data.S32[numTotal + i] = i;
        start->data.S32[numTotal - i] = -i;
        stop->data.S32[numTotal - i] = -i;
    }
    for (int i = numInner + 1, fibLast = 1, fib = 2, temp; i <= numTotal;
         i++, fib = (temp = fib) + fibLast, fibLast = temp) {
        start->data.S32[numTotal + i] = stop->data.S32[numTotal + i - 1] + 1;
        stop->data.S32[numTotal + i] = PS_MIN(start->data.S32[numTotal + i] + fib - 1, size);
        start->data.S32[numTotal - i] = - stop->data.S32[numTotal + i];
        stop->data.S32[numTotal - i] = - start->data.S32[numTotal + i];
    }

    if (psTraceGetLevel("psModules.imcombine") >= 10) {
        for (int i = 0; i < 2 * numTotal + 1; i++) {
            psTrace("psModules.imcombine", 10, "%d: %d -> %d\n", i, start->data.S32[i], stop->data.S32[i]);
        }
    }

    // Set the kernel parameters
    for (int i = - numTotal, index = 0; i <= numTotal; i++) {
        int u = start->data.S32[numTotal + i]; // Location of pixel
        int uStop = stop->data.S32[numTotal + i]; // Width of pixel
        for (int j = - numTotal; j <= numTotal; j++, index++) {
            if (i == 0 && j == 0) {
                // Skip normalisation component: added explicitly
                index--;
                continue;
            }
            int v = start->data.S32[numTotal + j]; // Location of pixel
            int vStop = stop->data.S32[numTotal + j]; // Width of pixel

            kernels->u->data.S32[index] = u;
            kernels->v->data.S32[index] = v;
            kernels->uStop->data.S32[index] = uStop;
            kernels->vStop->data.S32[index] = vStop;

            psTrace("psModules.imcombine", 7, "Kernel %d: %d %d %d %d\n", index,
                    u, uStop, v, vStop);
        }
    }

    psFree(start);
    psFree(stop);

    psWarning("Kernel penalty for dual-convolution is not configured for FRIES kernels.");
    psVectorInit(kernels->penalties, 0.0);

    return kernels;
}

// Grid United with Normal Kernel
pmSubtractionKernels *pmSubtractionKernelsGUNK(int size, int spatialOrder, const psVector *fwhms,
                                               const psVector *orders, int inner, float penalty,
                                               pmSubtractionMode mode)
{
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, NULL);
    PS_ASSERT_VECTOR_NON_NULL(fwhms, NULL);
    PS_ASSERT_VECTOR_TYPE(fwhms, PS_TYPE_F32, NULL);
    PS_ASSERT_VECTOR_NON_NULL(orders, NULL);
    PS_ASSERT_VECTOR_TYPE(orders, PS_TYPE_S32, NULL);
    PS_ASSERT_VECTORS_SIZE_EQUAL(fwhms, orders, NULL);
    PS_ASSERT_INT_NONNEGATIVE(inner, NULL);
    PS_ASSERT_INT_LESS_THAN(inner, size, NULL);

    // XXX GUNK doesn't seem to work --- doesn't add the POIS components, or at least, they're not noticed

    pmSubtractionKernels *kernels = p_pmSubtractionKernelsRawISIS(size, spatialOrder, fwhms, orders,
                                                                  penalty, mode); // Kernels
    psStringPrepend(&kernels->description, "GUNK=");
    psStringAppend(&kernels->description, "+POIS(%d,%d)", inner, spatialOrder);

    int numISIS = kernels->num;         // Number of ISIS kernels

    if (!p_pmSubtractionKernelsAddGrid(kernels, numISIS, inner)) {
        psAbort("Should never get here.");
    }

    return kernels;
}

// RINGS --- just what it says
pmSubtractionKernels *pmSubtractionKernelsRINGS(int size, int spatialOrder, int inner, int ringsOrder,
                                                float penalty, pmSubtractionMode mode)
{
    PS_ASSERT_INT_POSITIVE(size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(spatialOrder, NULL);
    PS_ASSERT_INT_NONNEGATIVE(inner, NULL);
    PS_ASSERT_INT_LESS_THAN_OR_EQUAL(inner, size, NULL);
    PS_ASSERT_INT_NONNEGATIVE(ringsOrder, NULL);

    int fibNum = 0;                     // Number of Fibonacci values
    {
        int fibIndex = 1, fibIndexMinus1 = 0; // Fibonnacci parameters
        int radius = inner;
        while (radius + fibIndex < size) {
            radius++;
            int fibNew = fibIndex + fibIndexMinus1;
            fibIndexMinus1 = fibIndex;
            fibIndex = fibNew;
            radius += fibIndex;
            fibNum++;
        }
    }

    int numInner = inner - 1;           // Number of pixels in the inner region
    int numOuter = fibNum;              // Number of summed pixels in the outer region

    int numRings = numOuter + numInner; // Number of rings (not including the central pixel)
    int numPoly = PM_SUBTRACTION_POLYTERMS(ringsOrder); // Number of polynomial variants of each ring

    int num = numRings * numPoly; // Total number of basis functions

    pmSubtractionKernels *kernels = pmSubtractionKernelsAlloc(num, PM_SUBTRACTION_KERNEL_RINGS, size,
                                                              spatialOrder, penalty, mode); // The kernels
    kernels->inner = inner;
    psStringAppend(&kernels->description, "RINGS(%d,%d,%d,%d,%.2e)", size, inner, ringsOrder, spatialOrder,
                   penalty);

    psLogMsg("psModules.imcombine", PS_LOG_INFO, "RINGS kernel: %d,%d,%d,%d --> %d elements",
             size, inner, ringsOrder, spatialOrder, num);

    // Set the Gaussian kernel parameters
    int fibIndex = 1, fibIndexMinus1 = 0; // Fibonnacci parameters
    int radiusLast = 1;                 // Last radius
    for (int i = 1, index = 0; i < numRings + 1; i++) {
        float lower2;                   // Lower limit of radius^2
        float upper2;                   // Upper limit of radius^2
        if (i <= inner) {
            // A ring every pixel width
            float radius = i;
            lower2 = PS_SQR(radius - 0.5);
            upper2 = PS_SQR(radius + 0.5);
            radiusLast = i;
        } else {
            // Rings Fibonacci distributed (2, 3, 5...)
            int fibNew = fibIndex + fibIndexMinus1;
            fibIndexMinus1 = fibIndex;
            fibIndex = fibNew;

            float radiusLower = radiusLast + 1;
            radiusLast = radiusLower + fibIndex;
            float radiusUpper = radiusLast;

            lower2 = PS_SQR(radiusLower - 0.5);
            upper2 = PS_SQR(radiusUpper + 0.5);
        }

        psTrace("psModules.imcombine", 8, "Radius limits: %f --> %f\n", sqrtf(lower2), sqrtf(upper2));

        // Iterate over (u,v) order
        for (int uOrder = 0; uOrder <= (i == 0 ? 0 : ringsOrder); uOrder++) {
            for (int vOrder = 0; vOrder <= (i == 0 ? 0 : ringsOrder - uOrder); vOrder++, index++) {

                psArray *data = psArrayAlloc(3); // Container for data
                psVector *uCoords = data->data[0] = psVectorAllocEmpty(RINGS_BUFFER, PS_TYPE_S32); // u coords
                psVector *vCoords = data->data[1] = psVectorAllocEmpty(RINGS_BUFFER, PS_TYPE_S32); // v coords
                psVector *poly = data->data[2] = psVectorAllocEmpty(RINGS_BUFFER, PS_TYPE_F32); // Polynomial
                double moment = 0.0;    // Moment, for penalty

                if (i == 0) {
                    // Central pixel is easy
                    uCoords->data.S32[0] = vCoords->data.S32[0] = 0;
                    poly->data.F32[0] = 1.0;
                    uCoords->n = vCoords->n = poly->n = 1;
                    radiusLast = 0;
                    moment = 0.0;
                } else {
                    int j = 0;          // Index for data
                    double norm = 0.0;  // Normalisation
                    for (int v = -size; v <= size; v++) {
                        int v2 = PS_SQR(v);   // Square of v
                        float vPoly = powf(v/(float)size, vOrder); // Value of v^vOrder

                        for (int u = -size; u <= size; u++) {
                            int u2 = PS_SQR(u); // Square of u
                            int distance2 = u2 + v2; // Distance from the centre
                            if (distance2 > lower2 && distance2 < upper2) {
                                float uPoly = powf(u/(float)size, uOrder); // Value of u^uOrder

                                float polyVal = uPoly * vPoly; // Value of polynomial
                                if (polyVal != 0) { // No point adding it otherwise
                                    uCoords->data.S32[j] = u;
                                    vCoords->data.S32[j] = v;
                                    poly->data.F32[j] = polyVal;
                                    norm += polyVal;
                                    moment += polyVal * (PS_SQR(u) + PS_SQR(v));

                                    psVectorExtend(uCoords, RINGS_BUFFER, 1);
                                    psVectorExtend(vCoords, RINGS_BUFFER, 1);
                                    psVectorExtend(poly, RINGS_BUFFER, 1);
                                    psTrace("psModules.imcombine", 9, "u = %d, v = %d, poly = %f\n",
                                            u, v, poly->data.F32[j]);
                                    j++;
                                }
                            }
                        }
                    }
                    // Normalise kernel component to unit sum
                    if (uOrder % 2 == 0 && vOrder % 2 == 0) {
                        psBinaryOp(poly, poly, "*", psScalarAlloc(1.0 / norm, PS_TYPE_F32));
                        // Add subtraction of 0,0 component to preserve photometric scaling
                        uCoords->data.S32[j] = 0;
                        vCoords->data.S32[j] = 0;
                        poly->data.F32[j] = -1.0;
                        psVectorExtend(uCoords, RINGS_BUFFER, 1);
                        psVectorExtend(vCoords, RINGS_BUFFER, 1);
                        psVectorExtend(poly, RINGS_BUFFER, 1);
                    } else {
                        norm = powf(size, uOrder) * powf(size, vOrder);
                        psBinaryOp(poly, poly, "*", psScalarAlloc(1.0 / norm, PS_TYPE_F32));
                    }
//                    moment /= norm;
                }

                psTrace("psModules.imcombine", 8, "%ld pixels in kernel\n", uCoords->n);

                kernels->preCalc->data[index] = data;
                kernels->u->data.S32[index] = uOrder;
                kernels->v->data.S32[index] = vOrder;
                kernels->penalties->data.F32[index] = kernels->penalty * fabsf(moment);

                psTrace("psModules.imcombine", 7, "Kernel %d: %d %d %d\n", index,
                        i, uOrder, vOrder);
            }
        }
    }

    return kernels;
}

pmSubtractionKernels *pmSubtractionKernelsGenerate(pmSubtractionKernelsType type, int size, int spatialOrder,
                                                   const psVector *fwhms, const psVector *orders, int inner,
                                                   int binning, int ringsOrder, float penalty,
                                                   pmSubtractionMode mode)
{
    switch (type) {
      case PM_SUBTRACTION_KERNEL_POIS:
        return pmSubtractionKernelsPOIS(size, spatialOrder, penalty, mode);
      case PM_SUBTRACTION_KERNEL_ISIS:
        return pmSubtractionKernelsISIS(size, spatialOrder, fwhms, orders, penalty, mode);
      case PM_SUBTRACTION_KERNEL_SPAM:
        return pmSubtractionKernelsSPAM(size, spatialOrder, inner, binning, penalty, mode);
      case PM_SUBTRACTION_KERNEL_FRIES:
        return pmSubtractionKernelsFRIES(size, spatialOrder, inner, penalty, mode);
      case PM_SUBTRACTION_KERNEL_GUNK:
        return pmSubtractionKernelsGUNK(size, spatialOrder, fwhms, orders, inner, penalty, mode);
      case PM_SUBTRACTION_KERNEL_RINGS:
        return pmSubtractionKernelsRINGS(size, spatialOrder, inner, ringsOrder, penalty, mode);
      default:
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Unknown kernel type: %x", type);
        return NULL;
    }
}


// Intermediate string parsing functions required because of different APIs for strtol and strtof
static inline int parseStringInt(const char *string)
{
    return strtol(string, NULL, 10);
}
static inline float parseStringFloat(const char *string)
{
    return strtof(string, NULL);
}


// Parse a string of a number, up to some delimiter, and advance past the delimiter
#define PARSE_STRING_NUMBER(TARGET, STRING, DELIM, PARSEFUNC) { \
    char *start = STRING;               /* Start of string */ \
    char *end = strchr(STRING, DELIM);  /* End of string */ \
    if (!end) { \
        psAbort("End of string encountered"); \
    } \
    int stringSize = end - STRING;      /* Size of string with value, NOT including \0 */ \
    char value[stringSize + 1];         /* String to parse */ \
    strncpy(value, start, stringSize); \
    value[stringSize] = '\0'; \
    TARGET = PARSEFUNC(value); \
    STRING += stringSize + 1;           /* Advance past delimiter */ \
}


pmSubtractionKernels *pmSubtractionKernelsFromDescription(const char *description, int bgOrder,
                                                          pmSubtractionMode mode)
{
    PS_ASSERT_STRING_NON_EMPTY(description, NULL);

    if (bgOrder != 0) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Background order %d is not yet supported.", bgOrder);
        return false;
    }

    pmSubtractionKernelsType type = PM_SUBTRACTION_KERNEL_NONE; // Type of kernel
    int size = 0;                       // Half-size of kernel
    int spatialOrder = 0;               // Order of spatial variations
    const psVector *fwhms = NULL;       // FWHM of Gaussians
    const psVector *orders = NULL;      // Polynomial order for each FWHM
    int inner = 0;                      // Size of inner region
    int binning = 0;                    // Binning to use
    int ringsOrder = 0;                 // Polynomial order for rings
    float penalty = 0.0;                // Penalty for wideness

    if (strncmp(description, "ISIS", 4) == 0) {
        // XXX Support for GUNK
        if (strstr(description, "+POIS")) {
            type = PM_SUBTRACTION_KERNEL_GUNK;
            psAbort("Deciphering GUNK kernels (%s) is not currently supported.", description);
        } else {
            type = PM_SUBTRACTION_KERNEL_ISIS;
            char *ptr = (char*)description + 5;    // Eat "ISIS("
            PARSE_STRING_NUMBER(size, ptr, ',', parseStringInt);

            // Count the number of Gaussians
            int numGauss = 0;
            for (char *string = ptr; string; string = strchr(string + 1, '(')) {
                numGauss++;
            }

            fwhms = psVectorAlloc(numGauss, PS_TYPE_F32);
            orders = psVectorAlloc(numGauss, PS_TYPE_S32);

            for (int i = 0; i < numGauss; i++) {
                ptr++;                  // Eat the '('
                PARSE_STRING_NUMBER(fwhms->data.F32[i], ptr, ',', parseStringFloat); // Eat "1.234,"
                PARSE_STRING_NUMBER(orders->data.S32[i], ptr, ')', parseStringInt); // Eat "3)"
            }

            ptr++;                      // Eat ','
            PARSE_STRING_NUMBER(spatialOrder, ptr, ',', parseStringInt);
            penalty = parseStringFloat(ptr);
        }
    } else if (strncmp(description, "RINGS", 5) == 0) {
        type = PM_SUBTRACTION_KERNEL_RINGS;
        char *ptr = (char*)description + 6;
        PARSE_STRING_NUMBER(size, ptr, ',', parseStringInt);
        PARSE_STRING_NUMBER(inner, ptr, ',', parseStringInt);
        PARSE_STRING_NUMBER(ringsOrder, ptr, ',', parseStringInt);
        PARSE_STRING_NUMBER(spatialOrder, ptr, ',', parseStringInt);
        PARSE_STRING_NUMBER(penalty, ptr, ')', parseStringInt);
    } else {
        psAbort("Deciphering kernels other than ISIS and RINGS is not currently supported.");
    }


    return pmSubtractionKernelsGenerate(type, size, spatialOrder, fwhms, orders,
                                        inner, binning, ringsOrder, penalty, mode);
}


pmSubtractionKernelsType pmSubtractionKernelsTypeFromString(const char *type)
{
    if (strcasecmp(type, "POIS") == 0) {
        return PM_SUBTRACTION_KERNEL_POIS;
    }
    if (strcasecmp(type, "ISIS") == 0) {
        return PM_SUBTRACTION_KERNEL_ISIS;
    }
    if (strcasecmp(type, "SPAM") == 0) {
        return PM_SUBTRACTION_KERNEL_SPAM;
    }
    if (strcasecmp(type, "FRIES") == 0) {
        return PM_SUBTRACTION_KERNEL_FRIES;
    }
    if (strcasecmp(type, "GUNK") == 0) {
        return PM_SUBTRACTION_KERNEL_GUNK;
    }
    if (strcasecmp(type, "RINGS") == 0) {
        return PM_SUBTRACTION_KERNEL_RINGS;
    }

    psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Unrecognised kernel type: %s", type);
    return PM_SUBTRACTION_KERNEL_NONE;
}
