/** @file  psImageInterpolate.c
 *
 *  @brief Contains functions for interpolating an image
 *
 *  @author Robert DeSonia, MHPCC
 *  @author Ross Harman, MHPCC
 *  @author Paul Price, IfA
 *
 *  @version $Revision: 1.20 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2008-09-04 20:11:00 $
 *
 *  Copyright 2004-2007 Institute for Astronomy, University of Hawaii
 *
 */

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

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

#include "psAbort.h"
#include "psMemory.h"
#include "psError.h"
#include "psAssert.h"
#include "psString.h"
#include "psImage.h"
#include "psImageInterpolate.h"

static void imageInterpolateOptionsFree(psImageInterpolateOptions *options)
{
    // Casting away const
    psFree((psImage*)options->image);
    psFree((psImage*)options->mask);
    psFree((psImage*)options->variance);
}

psImageInterpolateOptions *psImageInterpolateOptionsAlloc(psImageInterpolateMode mode,
                                                          const psImage *image, const psImage *variance,
                                                          const psImage *mask, psMaskType maskVal,
                                                          double badImage, double badVariance,
                                                          psMaskType badMask, psMaskType poorMask,
                                                          float poorFrac)
{
    psImageInterpolateOptions *options = psAlloc(sizeof(psImageInterpolateOptions)); // Options, to return
    psMemSetDeallocator(options, (psFreeFunc)imageInterpolateOptionsFree);

    options->mode = mode;
    // Casting away const to add to options
    options->image = psMemIncrRefCounter((psImage*)image);
    options->variance = psMemIncrRefCounter((psImage*)variance);
    options->mask = psMemIncrRefCounter((psImage*)mask);
    options->maskVal = maskVal;
    options->badImage = badImage;
    options->badVariance = badVariance;
    options->badMask = badMask;
    options->poorMask = poorMask;
    options->poorFrac = poorFrac;
    options->shifting = true;

    return options;
}

// Interpolation engine for flat mode (nearest pixel)
static inline psImageInterpolateStatus interpolateFlat(double *imageValue, double *varianceValue,
                                                       psMaskType *maskValue, float x, float y,
                                                       const psImageInterpolateOptions *options)
{
    // Parameters have been checked by psImageInterpolate()

    const psImage *image = options->image; // Image of interest
    int xInt = round(x - 0.5 + FLT_EPSILON); // Pixel closest to point of interest in x
    int yInt = round(y - 0.5 + FLT_EPSILON); // Pixel closest to point of interest in y
    int xLast = image->numCols - 1;     // Last pixel in x
    int yLast = image->numRows - 1;     // Last pixel in y

    if (xInt < 0 || xInt > xLast || yInt < 0 || yInt > yLast) {
        // At least one pixel of the interpolation kernel is off the image
        if (imageValue) {
            *imageValue = options->badImage;
        }
        if (varianceValue) {
            *varianceValue = options->badVariance;
        }
        if (maskValue) {
            *maskValue = options->badMask;
        }

        return PS_INTERPOLATE_STATUS_OFF;
    } else {

        // Image and variance 'interpolation' according to image type
        #define FLAT_CASE(TYPE) \
          case PS_TYPE_##TYPE: { \
            if (imageValue) { \
                *imageValue = options->image->data.TYPE[yInt][xInt]; \
            } \
            if (varianceValue && options->variance) { \
                *varianceValue = options->variance->data.TYPE[yInt][xInt]; \
            } \
            break; \
        }

        switch (options->image->type.type) {
            FLAT_CASE(U8);
            FLAT_CASE(U16);
            FLAT_CASE(U32);
            FLAT_CASE(U64);
            FLAT_CASE(S8);
            FLAT_CASE(S16);
            FLAT_CASE(S32);
            FLAT_CASE(S64);
            FLAT_CASE(F32);
            FLAT_CASE(F64);
          default:
            psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Unrecognised type for image: %x",
                    options->image->type.type);
            return PS_INTERPOLATE_STATUS_ERROR;
        }

        if (maskValue) {
            if (options->mask) {
                *maskValue = options->mask->data.PS_TYPE_MASK_DATA[yInt][xInt];
            } else {
                *maskValue = 0;
            }
        }
    }
    return PS_INTERPOLATE_STATUS_GOOD;
}

// Setup for interpolation by kernel
static inline void interpolateKernelSetup(int *xNum, int *yNum, // Size of interpolation kernel, returned
                                          int *xCentral, int *yCentral, // Central pixel of convolution
                                          float x, float y, // Coordinates of interest
                                          psImageInterpolateMode mode // Mode for interpolation
                                          )
{
    switch (mode) {
      case PS_INTERPOLATE_BILINEAR:
        *xNum = *yNum = 2;
        // Central pixel is the pixel below the point of interest
        *xCentral = floor(x - 0.5 + FLT_EPSILON);
        *yCentral = floor(y - 0.5 + FLT_EPSILON);
        break;
      case PS_INTERPOLATE_BICUBE:
      case PS_INTERPOLATE_GAUSS:
        *xNum = *yNum = 3;
        // Central pixel is the closest pixel to the point of interest
        *xCentral = x;
        *yCentral = y;
        break;
      case PS_INTERPOLATE_FLAT:
      case PS_INTERPOLATE_LANCZOS2:
      case PS_INTERPOLATE_LANCZOS3:
      case PS_INTERPOLATE_LANCZOS4:
      default:
        psAbort("Invalid interpolation mode.");
    }
}

// Generate the interpolation kernel
// No need to normalise to unity
static inline void interpolateKernelGenerate(int xNum, int yNum, // Size of interpolation kernel
                                             double kernel[xNum][yNum], // Kernel, to be set
                                             bool *xExact, bool *yExact, // Exact shift?
                                             int xCentral, int yCentral, // Central pixel of convolution
                                             float x, float y, // Coordinates of interest
                                             psImageInterpolateMode mode, // Mode for interpolation
                                             bool allowExact // Allow exact shifts?
                                             )
{
    double xFrac = x - 0.5 - xCentral; // Fraction of pixel in x
    double yFrac = y - 0.5 - yCentral; // Fraction of pixel in y
    *xExact = (allowExact && fabs(xFrac) < DBL_EPSILON);
    *yExact = (allowExact && fabs(yFrac) < DBL_EPSILON);

    switch (mode) {
      case PS_INTERPOLATE_BILINEAR: {
          kernel[0][0] = (1.0 - xFrac) * (1.0 - yFrac);
          kernel[0][1] = xFrac * (1.0 - yFrac);
          kernel[1][0] = yFrac * (1.0 - xFrac);
          kernel[1][1] = xFrac * yFrac;
          break;
      }
      case PS_INTERPOLATE_BICUBE: {
          double xFrac = x - 0.5 - xCentral; // Fraction of pixel in x
          double yFrac = y - 0.5 - yCentral; // Fraction of pixel in y
          // Calculation variables
          double xxFrac = xFrac * xFrac / 6.0;
          double yyFrac = yFrac * yFrac / 6.0;
          double xyFrac = 0.25 * xFrac * yFrac;
          xFrac /= 6.0;
          yFrac /= 6.0;
          kernel[0][0] = - 1.0/9.0 - xFrac - yFrac + xxFrac + yyFrac + xyFrac;
          kernel[0][1] = 2.0/9.0 - yFrac - 2.0 * xxFrac + yyFrac;
          kernel[0][2] = - 1.0/9.0 + xFrac - yFrac + xxFrac + yyFrac - xyFrac;
          kernel[1][0] = 2.0/9.0 - xFrac + xxFrac - 2.0 * yyFrac;
          kernel[1][1] = 5.0/9.0 - 2.0 * xxFrac - 2.0 * yyFrac;
          kernel[1][2] = 2.0/9.0 + xFrac + xxFrac - 2.0 * yyFrac;
          kernel[2][0] = - 1.0/9.0 - xFrac + yFrac + xxFrac + yyFrac - xyFrac;
          kernel[2][1] = 2.0/9.0 + yFrac - 2.0 * xxFrac + yyFrac;
          kernel[2][2] = - 1.0/9.0 + xFrac + yFrac + xxFrac + yyFrac + xyFrac;
          break;
      }
      case PS_INTERPOLATE_GAUSS: {
          double xFrac = x - xCentral - 0.5; // Fraction of pixel in x
          double yFrac = y - yCentral - 0.5; // Fraction of pixel in y
          double sigma = 0.5;           // Gaussian sigma
          for (int j = 0, yPos = - (yNum - 1) / 2; j < yNum; j++, yPos++) {
              for (int i = 0, xPos = - (xNum - 1) / 2; i < xNum; i++, xPos++) {
                  kernel[j][i] = exp(-0.5 / PS_SQR(sigma) * (PS_SQR(xPos - xFrac) +
                                                             PS_SQR(yPos - yFrac)));
              }
          }
          break;
      }
      case PS_INTERPOLATE_FLAT:
      case PS_INTERPOLATE_LANCZOS2:
      case PS_INTERPOLATE_LANCZOS3:
      case PS_INTERPOLATE_LANCZOS4:
      default:
        psAbort("Invalid interpolation mode.");
    }
    return;
}

// Can't interpolate: kernel is off the image
#define INTERPOLATE_OFF() { \
        *imageValue = options->badImage; \
        if (varianceValue) { \
            *varianceValue = options->badVariance; \
        } \
        if (maskValue) { \
            *maskValue = options->badMask; \
        } \
        return PS_INTERPOLATE_STATUS_OFF; \
    }

// Set up and check interpolation bounds
// This macro defines many useful values
#define INTERPOLATE_BOUNDS() \
    int xLast = image->numCols - 1, yLast = image->numRows - 1; /* Last pixels of image */ \
    /* Start and stop of kernel on image */ \
    int xStart = xCentral - (xNum - 1) / 2, xStop = xCentral + xNum / 2; \
    int yStart = yCentral - (yNum - 1) / 2, yStop = yCentral + yNum / 2; \
    if (xStart > xLast || xStop < 0 || yStart > yLast || yStop < 0) { \
        /* Interpolation kernel is entirely off the image */ \
        INTERPOLATE_OFF(); \
    } \
    int xMin, xMax, yMin, yMax; /* Minimum and maximum valid pixels on image */ \
    int iMin, iMax, jMin, jMax; /* Minimum and maximum valid pixels on kernel */ \
    bool offImage = false; /* At least one pixel of the kernel is off the image */ \
    if (xStart < 0) { \
        xMin = 0; \
        iMin = -xStart; \
        offImage = true; \
    } else { \
        xMin = xStart; \
        iMin = 0; \
    } \
    if (xStop > xLast) { \
        xMax = xLast; \
        iMax = xLast - xStart + 1; \
        offImage = true; \
    } else { \
        xMax = xStop; \
        iMax = xNum; \
    } \
    if (yStart < 0) { \
        yMin = 0; \
        jMin = -yStart; \
        offImage = true; \
    } else { \
        yMin = yStart; \
        jMin = 0; \
    } \
    if (yStop > yLast) { \
        yMax = yLast; \
        jMax = yLast - yStart + 1; \
        offImage = true; \
    } else { \
        yMax = yStop; \
        jMax = yNum; \
    }

// Refine limits if the interpolation is exact
#define INTERPOLATE_EXACT() { \
    if (xExact) { \
        if (iMin > 0 || iMax < 1) { \
            INTERPOLATE_OFF(); /* Returns */ \
        } \
        iMin = (xNum - 1) / 2; \
        iMax = iMin + 1; \
    } \
    if (yExact) { \
        if (jMin > 0 || jMax < 1) { \
            INTERPOLATE_OFF(); /* Returns */ \
        } \
        jMin = (yNum - 1) / 2; \
        jMax = jMin + 1; \
    } \
    if (xExact && yExact) { \
        return interpolateFlat(imageValue, varianceValue, maskValue, x, y, options); \
    } \
}

// Interpolation engine using interpolation kernel
static psImageInterpolateStatus interpolateKernel(double *imageValue, double *varianceValue,
                                                  psMaskType *maskValue, float x, float y,
                                                  const psImageInterpolateOptions *options)
{
    // Parameters have been checked by psImageInterpolate()

    int xNum, yNum;                     // Number of interpolation kernel pixels
    int xCentral, yCentral;             // Central pixel of the convolution
    interpolateKernelSetup(&xNum, &yNum, &xCentral, &yCentral, x, y, options->mode);

    const psImage *image = options->image; // Image of interest
    const psImage *mask = options->mask; // Image mask
    const psImage *variance = options->variance; // Image variance

    INTERPOLATE_BOUNDS();

    double kernel[yNum][xNum];          // Interpolation kernel for straight interpolation
    bool xExact, yExact;                // Exact shifts?
    interpolateKernelGenerate(xNum, yNum, kernel, &xExact, &yExact, xCentral, yCentral,
                              x, y, options->mode, options->shifting);

    float sumKernel = 0.0;              // Sum of the kernel
    double sumKernel2 = 0.0;            // Sum of the kernel-squared
    float sumBad = 0.0;                 // Sum of bad kernel-squared contributions
    psMaskType maskVal = options->maskVal; // Value to mask
    double sumImage = 0.0;              // Sum of image multiplied by kernel
    double sumVariance = 0.0;           // Sum of variance multiplied by kernel-squared

    bool wantVariance = variance && varianceValue; // Does the user want the variance value?
    bool haveMask = mask && maskVal; // Does the user want the variance value?

    INTERPOLATE_EXACT();

    // Add contributions in an area outside the image
#define INTERPOLATE_KERNEL_ADD_OFFIMAGE() { \
        float kernelValue = kernel[j][i]; /* Value of kernel */ \
        double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel */ \
        sumBad += kernelValue2; \
        sumKernel2 += kernelValue2; \
    }

    // Measure kernel contribution from outside image
    if (offImage) {
        if (!yExact) {
            // Bottom rows
            for (int j = 0; j < jMin; j++) {
                for (int i = 0; i < xNum; i++) {
                    INTERPOLATE_KERNEL_ADD_OFFIMAGE();
                }
            }
        }
        if (!xExact) {
            // Two sides
            for (int j = jMin; j < jMax; j++) {
                for (int i = 0; i < iMin; i++) {
                    INTERPOLATE_KERNEL_ADD_OFFIMAGE();
                }
                for (int i = iMax + 1; i < xNum; i++) {
                    INTERPOLATE_KERNEL_ADD_OFFIMAGE();
                }
            }
        }
        if (!yExact) {
            // Top rows
            for (int j = jMax + 1; j < yNum; j++) {
                for (int i = 0; i < xNum; i++) {
                    INTERPOLATE_KERNEL_ADD_OFFIMAGE();
                }
            }
        }
    }

#define INTERPOLATE_KERNEL_CASE(TYPE) \
  case PS_TYPE_##TYPE: { \
      if (wantVariance) { \
          if (haveMask) { \
              /* Variance and mask */ \
              for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
                  for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                      float kernelValue = kernel[j][i]; /* Value of kernel */ \
                      double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel */ \
                      if (mask->data.PS_TYPE_MASK_DATA[yPix][xPix] & maskVal) { \
                          sumBad += kernelValue2; \
                      } else { \
                          sumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                          sumVariance += kernelValue2 * variance->data.TYPE[yPix][xPix]; \
                          sumKernel += kernelValue; \
                      } \
                      sumKernel2 += PS_SQR(kernelValue); \
                  } \
              } \
              \
          } else { \
              /* Variance, no mask */ \
              for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
                  for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                      float kernelValue = kernel[j][i]; /* Value of kernel */ \
                      double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel */ \
                      sumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                      sumVariance += kernelValue2 * variance->data.TYPE[yPix][xPix]; \
                      sumKernel += kernelValue; \
                      sumKernel2 += kernelValue2; \
                  } \
              } \
          } \
      } else if (haveMask) { \
          /* Mask, no variance */ \
          for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
              for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                  float kernelValue = kernel[j][i]; /* Value of kernel */ \
                  if (mask->data.PS_TYPE_MASK_DATA[yPix][xPix] & maskVal) { \
                      sumBad += PS_SQR(kernelValue); \
                  } else { \
                      sumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                      sumKernel += kernelValue; \
                  } \
                  sumKernel2 += PS_SQR(kernelValue); \
              } \
          } \
      } else { \
          /* Neither variance nor mask */ \
          for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
              for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                  float kernelValue = kernel[j][i]; /* Value of kernel */ \
                  sumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                  sumKernel += kernelValue; \
              } \
          } \
      } \
    } \
    break;


    switch (image->type.type) {
        INTERPOLATE_KERNEL_CASE(U8);
        INTERPOLATE_KERNEL_CASE(U16);
        INTERPOLATE_KERNEL_CASE(U32);
        INTERPOLATE_KERNEL_CASE(U64);
        INTERPOLATE_KERNEL_CASE(S8);
        INTERPOLATE_KERNEL_CASE(S16);
        INTERPOLATE_KERNEL_CASE(S32);
        INTERPOLATE_KERNEL_CASE(S64);
        INTERPOLATE_KERNEL_CASE(F32);
        INTERPOLATE_KERNEL_CASE(F64);
      default:
        psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Unrecognised type for image: %x",
                image->type.type);
        return PS_INTERPOLATE_STATUS_ERROR;
    }

    psImageInterpolateStatus status = PS_INTERPOLATE_STATUS_ERROR; // Status of interpolation
    *imageValue = sumImage / sumKernel;
    if (wantVariance) {
        *varianceValue = sumVariance * PS_SQR(sumKernel) / sumKernel2;
    }
    if (sumBad == 0) {
        // Completely good pixel
        status = PS_INTERPOLATE_STATUS_GOOD;
    } else if (sumBad / sumKernel2 < PS_SQR(options->poorFrac)) {
        // Some pixels masked: poor pixel
        if (haveMask && maskValue) {
            *maskValue |= options->poorMask;
        }
        status = PS_INTERPOLATE_STATUS_POOR;
    } else {
        // Many pixels (or a few important pixels) masked: bad pixel
        if (haveMask && maskValue) {
            *maskValue |= options->badMask;
        }
        status = PS_INTERPOLATE_STATUS_BAD;
    }

    return status;
}


// Generate Lanczos interpolation kernels
static void lanczos(double values[],    // Interpolation kernel to generate
                    bool *exact,        // Exact shift?
                    int num,            // Number of values in the kernel
                    float frac,         // Sub-pixel position
                    bool allowExact     // Allow exact shift?
    )
{
    if (allowExact && fabs(frac) < DBL_EPSILON) {
        *exact = true;
        // No real shift
        for (int i = 0; i < (num - 1) / 2; i++) {
            values[i] = 0.0;
        }
        values[(num - 1) / 2] = 1.0;
        for (int i = (num - 1) / 2 + 1; i < num; i++) {
            values[i] = 0.0;
        }
    } else {
        *exact = false;
        double norm1 = 2.0 / PS_SQR(M_PI); // Normalisation for laczos
        double norm2 = M_PI * 4.0 / (float)num; // Normalisation for sinc function 1
        double norm3 =M_PI_2 * 4.0 / (float)num; // Normalisation for sinc function 2
        double pos = - (num - 1)/2 - frac;  // Position of interest
        for (int i = 0; i < num; i++, pos += 1.0) {
            if (fabs(pos) < DBL_EPSILON) {
                values[i] = 1.0;
            } else {
                values[i] = norm1 * sin(pos * norm2) * sin(pos * norm3) / PS_SQR(pos);
            }
        }
    }

    return;
}

// Setup for interpolation by separable kernels
static inline void interpolateSeparateSetup(int *xNum, int *yNum, // Size of interpolation kernel, returned
                                            int *xCentral, int *yCentral, // Central pixel of convolution
                                            float x, float y, // Coordinates of interest
                                            psImageInterpolateMode mode // Mode for interpolation
                                            )
{
    // Central pixel is the pixel below the point of interest
    *xCentral = floor(x - 0.5);
    *yCentral = floor(y - 0.5);
    switch (mode) {
      case PS_INTERPOLATE_LANCZOS2:
        *xNum = *yNum = 4;
        break;
      case PS_INTERPOLATE_LANCZOS3:
        *xNum = *yNum = 6;
        break;
      case PS_INTERPOLATE_LANCZOS4:
        *xNum = *yNum = 8;
        break;
      case PS_INTERPOLATE_FLAT:
      case PS_INTERPOLATE_BILINEAR:
      case PS_INTERPOLATE_BICUBE:
      case PS_INTERPOLATE_GAUSS:
      default:
        psAbort("Invalid interpolation mode.");
    }
}

// Generate the interpolation kernels for separable case; they should be normalised to unity
static inline void interpolateSeparateGenerate(int xNum, int yNum, // Size of interpolation kernel
                                               double xKernel[xNum], double yKernel[yNum], // Kernels
                                               bool *xExact, bool *yExact, // Exact shifts?
                                               int xCentral, int yCentral, // Central pixel of convolution
                                               float x, float y, // Coordinates of interest
                                               psImageInterpolateMode mode, // Mode for interpolation
                                               bool allowExact // Allow an exact shift?
                                               )
{
    // XXX Could put in an "exact shift" (i.e., xFrac = 0.0) version
    switch (mode) {
      case PS_INTERPOLATE_LANCZOS2:
      case PS_INTERPOLATE_LANCZOS3:
      case PS_INTERPOLATE_LANCZOS4: {
          double xFrac = x - xCentral - 0.5; // Fraction of pixel in x
          lanczos(xKernel, xExact, xNum, xFrac, allowExact);
          double yFrac = y - yCentral - 0.5; // Fraction of pixel in y
          lanczos(yKernel, yExact, yNum, yFrac, allowExact);
          break;
      }
      case PS_INTERPOLATE_FLAT:
      case PS_INTERPOLATE_BILINEAR:
      case PS_INTERPOLATE_BICUBE:
      case PS_INTERPOLATE_GAUSS:
      default:
        psAbort("Invalid interpolation mode.");
    }
}

// Interpolation engine for separable interpolation kernels (either for good reasons or for practical reasons)
static psImageInterpolateStatus interpolateSeparate(double *imageValue, double *varianceValue,
                                                    psMaskType *maskValue, float x, float y,
                                                    const psImageInterpolateOptions *options)
{
    // Parameters have been checked by psImageInterpolate()

    int xNum, yNum;                     // Number of interpolation kernel pixels
    int xCentral, yCentral; // Central pixel of the convolution
    interpolateSeparateSetup(&xNum, &yNum, &xCentral, &yCentral, x, y, options->mode);

    const psImage *image = options->image; // Image of interest
    const psImage *mask = options->mask; // Image mask
    const psImage *variance = options->variance; // Image variance

    INTERPOLATE_BOUNDS();

    double xKernel[xNum], yKernel[yNum]; // Interpolation kernels
    bool xExact, yExact;                // Exact shift?
    interpolateSeparateGenerate(xNum, yNum, xKernel, yKernel, &xExact, &yExact,
                                xCentral, yCentral, x, y, options->mode, options->shifting);

    float sumKernel = 0.0;              // Sum of the kernel
    double sumKernel2 = 0.0;            // Sum of the kernel-squared
    float sumBad = 0.0;                 // Sum of bad kernel-squared contributions
    psMaskType maskVal = options->maskVal; // Value to mask
    double sumImage = 0.0;              // Sum of image multiplied by kernel
    double sumVariance = 0.0;           // Sum of variance multiplied by kernel-squared

    bool wantVariance = variance && varianceValue; // Does the user want the variance value?
    bool haveMask = mask && maskVal; // Does the user want the variance value?

    INTERPOLATE_EXACT();

    // Add contributions in an area outside the image
#define INTERPOLATE_SEPARATE_SETUP_OFFIMAGE_COL() \
    float xSumBad = 0.0; \
    double xSumKernel2 = 0.0;
#define INTERPOLATE_SEPARATE_ADD_OFFIMAGE_COL() { \
        float kernelValue = xKernel[i]; /* Value of kernel */ \
        double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel */ \
        xSumBad += kernelValue2; \
        xSumKernel2 += kernelValue2; \
    }
#define INTERPOLATE_SEPARATE_ADD_OFFIMAGE_ROW() { \
        float kernelValue = yKernel[j]; /* Value of kernel */ \
        double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel */ \
        sumBad += kernelValue2 * xSumBad; \
        sumKernel2 += kernelValue2 * xSumKernel2; \
    }

    // Measure kernel contribution from outside image
    if (offImage) {
        // Bottom rows
        if (!yExact) {
            for (int j = 0; j < jMin; j++) {
                INTERPOLATE_SEPARATE_SETUP_OFFIMAGE_COL();
                for (int i = 0; i < xNum; i++) {
                    INTERPOLATE_SEPARATE_ADD_OFFIMAGE_COL();
                }
                INTERPOLATE_SEPARATE_ADD_OFFIMAGE_ROW();
            }
        }
        // Two sides
        if (!xExact) {
            for (int j = jMin; j < jMax; j++) {
                INTERPOLATE_SEPARATE_SETUP_OFFIMAGE_COL();
                for (int i = 0; i < iMin; i++) {
                    INTERPOLATE_SEPARATE_ADD_OFFIMAGE_COL();
                }
                for (int i = iMax + 1; i < xNum; i++) {
                    INTERPOLATE_SEPARATE_ADD_OFFIMAGE_COL();
                }
                INTERPOLATE_SEPARATE_ADD_OFFIMAGE_ROW();
            }
        }
        // Top rows
        if (!yExact) {
            for (int j = jMax + 1; j < yNum; j++) {
                if (!xExact) {
                    INTERPOLATE_SEPARATE_SETUP_OFFIMAGE_COL();
                    for (int i = 0; i < xNum; i++) {
                        INTERPOLATE_SEPARATE_ADD_OFFIMAGE_COL();
                    }
                    INTERPOLATE_SEPARATE_ADD_OFFIMAGE_ROW();
                }
            }
        }
    }

#define INTERPOLATE_SEPARATE_CASE(TYPE) \
  case PS_TYPE_##TYPE: { \
      if (wantVariance) { \
          if (haveMask) { \
              /* Variance and mask */ \
              for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
                  double xSumImage = 0.0; /* Sum of image multiplied by kernel in x */ \
                  double xSumVariance = 0.0; /* Sum of image multiplied by kernel-squared in x */ \
                  float xSumKernel = 0.0; /* Sum of kernel in x */ \
                  double xSumKernel2 = 0.0; /* Sum of kernel-squared in x */ \
                  float xSumBad = 0.0; /* Sum of bad kernel-squared in x */ \
                  for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                      float kernelValue = xKernel[i]; /* Value of kernel in x */ \
                      double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel in x */ \
                      if (mask->data.PS_TYPE_MASK_DATA[yPix][xPix] & maskVal) { \
                          xSumBad += kernelValue2; \
                      } else { \
                          xSumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                          xSumVariance += kernelValue2 * variance->data.TYPE[yPix][xPix]; \
                          xSumKernel += kernelValue; \
                      } \
                      xSumKernel2 += kernelValue2; \
                  } \
                  float kernelValue = yKernel[j]; /* Value of kernel in y */ \
                  double kernelValue2 = PS_SQR(kernelValue); /* Value of kernel-squared in y */ \
                  sumImage += kernelValue * xSumImage; \
                  sumVariance += kernelValue2 * xSumVariance; \
                  sumBad += kernelValue2 * xSumBad; \
                  sumKernel += kernelValue * xSumKernel; \
                  sumKernel2 += kernelValue2 * xSumKernel2; \
              } \
          } else { \
              /* Variance, no mask */ \
              for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
                  double xSumImage = 0.0; /* Sum of image multiplied by kernel in x */ \
                  double xSumVariance = 0.0; /* Sum of image multiplied by kernel-squared in x */ \
                  float xSumKernel = 0.0; /* Sum of kernel in x */ \
                  double xSumKernel2 = 0.0; /* Sum of kernel-squared in x */ \
                  for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                      float kernelValue = xKernel[i]; /* Value of kernel */ \
                      double kernelValue2 = PS_SQR(kernelValue); /* Square of kernel */ \
                      xSumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                      xSumVariance += kernelValue2 * variance->data.TYPE[yPix][xPix]; \
                      xSumKernel += kernelValue; \
                      xSumKernel2 += kernelValue2; \
                  } \
                  float kernelValue = yKernel[j]; /* Value of kernel in y */ \
                  double kernelValue2 = PS_SQR(kernelValue); /* Value of kernel-squared in y */ \
                  sumImage += kernelValue * xSumImage; \
                  sumVariance += kernelValue2 * xSumVariance; \
                  sumKernel += kernelValue * xSumKernel; \
                  sumKernel2 += kernelValue2 * xSumKernel2; \
              } \
          } \
      } else if (haveMask) { \
          /* Mask, no variance */ \
          for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
              double xSumImage = 0.0; /* Sum of image multiplied by kernel in x */ \
              float xSumKernel = 0.0; /* Sum of kernel in x */ \
              double xSumKernel2 = 0.0; /* Sum of kernel-squared in x */ \
              float xSumBad = 0.0; /* Sum of bad kernel-squared in x */ \
              for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                  float kernelValue = xKernel[i]; /* Value of kernel */ \
                  double kernelValue2 = PS_SQR(kernelValue); /* Value of kernel-squared */ \
                  if (mask->data.PS_TYPE_MASK_DATA[yPix][xPix] & maskVal) { \
                      xSumBad += kernelValue2; \
                  } else { \
                      xSumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                      xSumKernel += kernelValue; \
                  } \
                  xSumKernel2 += kernelValue2; \
              } \
              float kernelValue = yKernel[j]; /* Value of kernel in y */ \
              double kernelValue2 = PS_SQR(kernelValue); /* Value of kernel-squared in y */ \
              sumImage += kernelValue * xSumImage; \
              sumBad += kernelValue2 * xSumBad; \
              sumKernel += kernelValue * xSumKernel; \
              sumKernel2 += kernelValue2 * xSumKernel2; \
          } \
      } else {\
          /* Neither variance nor mask */ \
          for (int j = jMin, yPix = yMin; j < jMax; j++, yPix++) { \
              double xSumImage = 0.0; /* Sum of image multiplied by kernel in x */ \
              float xSumKernel = 0.0; /* Sum of kernel in x */ \
              for (int i = iMin, xPix = xMin; i < iMax; i++, xPix++) { \
                  float kernelValue = xKernel[i]; /* Value of kernel */ \
                  xSumImage += kernelValue * image->data.TYPE[yPix][xPix]; \
                  xSumKernel += kernelValue; \
              } \
              float kernelValue = yKernel[j]; /* Value of kernel in y */ \
              sumImage += kernelValue * xSumImage; \
              sumKernel += kernelValue * xSumKernel; \
          } \
      } \
    } \
    break;


    switch (image->type.type) {
        INTERPOLATE_SEPARATE_CASE(U8);
        INTERPOLATE_SEPARATE_CASE(U16);
        INTERPOLATE_SEPARATE_CASE(U32);
        INTERPOLATE_SEPARATE_CASE(U64);
        INTERPOLATE_SEPARATE_CASE(S8);
        INTERPOLATE_SEPARATE_CASE(S16);
        INTERPOLATE_SEPARATE_CASE(S32);
        INTERPOLATE_SEPARATE_CASE(S64);
        INTERPOLATE_SEPARATE_CASE(F32);
        INTERPOLATE_SEPARATE_CASE(F64);
      default:
        psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Unrecognised type for image: %x",
                image->type.type);
        return PS_INTERPOLATE_STATUS_ERROR;
    }

    psImageInterpolateStatus status = PS_INTERPOLATE_STATUS_ERROR; // Status of interpolation
    *imageValue = sumImage / sumKernel;
    if (wantVariance) {
        *varianceValue = sumVariance * PS_SQR(sumKernel) / sumKernel2;
    }
    if (sumBad == 0) {
        // Completely good pixel
        status = PS_INTERPOLATE_STATUS_GOOD;
    } else if (sumBad / sumKernel2 < PS_SQR(options->poorFrac)) {
        // Some pixels masked: poor pixel
        if (haveMask && maskValue) {
            *maskValue |= options->poorMask;
        }
        status = PS_INTERPOLATE_STATUS_POOR;
    } else {
        // Many pixels (or a few important pixels) masked: bad pixel
        if (haveMask && maskValue) {
            *maskValue |= options->badMask;
        }
        status = PS_INTERPOLATE_STATUS_BAD;
    }

    return status;
}



psImageInterpolateStatus psImageInterpolate(double *imageValue, double *varianceValue, psMaskType *maskValue,
                                            float x, float y, const psImageInterpolateOptions *options)
{
    PS_ASSERT_PTR_NON_NULL(imageValue, PS_INTERPOLATE_STATUS_ERROR);
    PS_ASSERT_PTR_NON_NULL(options, PS_INTERPOLATE_STATUS_ERROR);

    const psImage *image = options->image; // Image to interpolate
    const psImage *mask = options->mask; // Mask to interpolate
    const psImage *variance = options->variance; // Variance to interpolate

    PS_ASSERT_IMAGE_NON_NULL(image, PS_INTERPOLATE_STATUS_ERROR);
    if (varianceValue && variance) {
        PS_ASSERT_IMAGE_NON_NULL(variance, PS_INTERPOLATE_STATUS_ERROR);
        PS_ASSERT_IMAGE_TYPE(variance, image->type.type, PS_INTERPOLATE_STATUS_ERROR);
        psAssert(image->numCols == variance->numCols && image->numRows == variance->numRows,
                 "Image and variance sizes");
    }
    if (maskValue && mask) {
        PS_ASSERT_IMAGE_NON_NULL(mask, PS_INTERPOLATE_STATUS_ERROR);
        PS_ASSERT_IMAGE_TYPE(mask, PS_TYPE_MASK, PS_INTERPOLATE_STATUS_ERROR);
        psAssert(image->numCols == mask->numCols && image->numRows == mask->numRows, "Image and mask sizes");
    }

    PS_ASSERT_FLOAT_LARGER_THAN_OR_EQUAL(options->poorFrac, 0.0, PS_INTERPOLATE_STATUS_ERROR);

    switch (options->mode) {
      case PS_INTERPOLATE_FLAT:
        return interpolateFlat(imageValue, varianceValue, maskValue, x, y, options);
      case PS_INTERPOLATE_BILINEAR:
      case PS_INTERPOLATE_BICUBE:
      case PS_INTERPOLATE_GAUSS:
        return interpolateKernel(imageValue, varianceValue, maskValue, x, y, options);
      case PS_INTERPOLATE_LANCZOS2:
      case PS_INTERPOLATE_LANCZOS3:
      case PS_INTERPOLATE_LANCZOS4:
        return interpolateSeparate(imageValue, varianceValue, maskValue, x, y, options);
      default:
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                _("Specified interpolation method (%d) is not supported."),
                options->mode);
        return PS_INTERPOLATE_STATUS_ERROR;
    }

    psAbort("Should never reach here.");
    return PS_INTERPOLATE_STATUS_ERROR;
}


static float varianceFactorFlat(float x, float y, const psImageInterpolateOptions *options)
{
    // There's no smearing
    return 1.0;
}

static float varianceFactorKernel(float x, float y, const psImageInterpolateOptions *options)
{
    int xNum, yNum;                     // Number of interpolation kernel pixels
    int xCentral, yCentral;             // Central pixel of the convolution
    interpolateKernelSetup(&xNum, &yNum, &xCentral, &yCentral, x, y, options->mode);

    const psImage *image = options->image; // Image of interest
    int xLast = image->numCols - 1;     // Last pixel in x
    int yLast = image->numRows - 1;     // Last pixel in y

    if (xCentral - (xNum - 1) / 2 < 0 || xCentral + xNum / 2 > xLast ||
        yCentral - (yNum - 1) / 2 < 0 || yCentral + yNum / 2 > yLast) {
        // At least one pixel of the interpolation kernel is off the image
        return NAN;
    }

    double kernel[yNum][xNum];          // Interpolation kernel for straight interpolation
    bool xExact, yExact;                // Exact shift?
    // Pretend we're not shifting pixels (i.e., we don't care about any exact shifting) because on the off
    // chance that a shift is integral, we don't want to get a trivial kernel back, but something that sort of
    // reflects what's going on.
    interpolateKernelGenerate(xNum, yNum, kernel, &xExact, &yExact, xCentral, yCentral,
                              x, y, options->mode, false);

    double sumKernel = 0.0;             // Sum of kernel
    double sumKernel2 = 0.0;            // Sum of kernel squares
    for (int j = 0; j < yNum; j++) {
        for (int i = 0; i < xNum; i++) {
            float value = kernel[j][i]; // Value of kernel
            sumKernel += value;
            sumKernel2 += PS_SQR(value);
        }
    }

    return sumKernel2 / PS_SQR(sumKernel);
}

static float varianceFactorSeparate(float x, float y, const psImageInterpolateOptions *options)
{
    int xNum, yNum;                     // Number of interpolation kernel pixels
    int xCentral, yCentral;             // Central pixel of the convolution
    interpolateSeparateSetup(&xNum, &yNum, &xCentral, &yCentral, x, y, options->mode);

    const psImage *image = options->image; // Image of interest
    int xLast = image->numCols - 1;     // Last pixel in x
    int yLast = image->numRows - 1;     // Last pixel in y

    if (xCentral - (xNum - 1) / 2 < 0 || xCentral + xNum / 2 > xLast ||
        yCentral - (yNum - 1) / 2 < 0 || yCentral + yNum / 2 > yLast) {
        // At least one pixel of the interpolation kernel is off the image
        return NAN;
    }

    double xKernel[xNum], yKernel[yNum]; // Interpolation kernels for separable interpolation
    bool xExact, yExact;                // Exact shift?
    // Pretend we're not shifting pixels (i.e., we don't care about any exact shifting) because on the off
    // chance that a shift is integral, we don't want to get a trivial kernel back, but something that sort of
    // reflects what's going on.
    interpolateSeparateGenerate(xNum, yNum, xKernel, yKernel, &xExact, &yExact,
                                xCentral, yCentral, x, y, options->mode, false);

    double ySumKernel = 0.0;            // Sum of kernel in y
    double ySumKernel2 = 0.0;           // Sum of kernel squared in y
    for (int j = 0; j < yNum; j++) {
        double xSumKernel = 0.0;        // Sum of kernel in x
        double xSumKernel2 = 0.0;       // Sum of kernel squared in x
        for (int i = 0; i < xNum; i++) {
            float value = xKernel[i];   // Value of kernel
            xSumKernel += value;
            xSumKernel2 += PS_SQR(value);
        }
        float value = yKernel[j];       // Value of kernel
        ySumKernel += xSumKernel * value;
        ySumKernel2 += xSumKernel2 * PS_SQR(value);
    }

    return ySumKernel2 / PS_SQR(ySumKernel);
}

float psImageInterpolateVarianceFactor(float x, float y, const psImageInterpolateOptions *options)
{
    PS_ASSERT_PTR_NON_NULL(options, PS_INTERPOLATE_STATUS_ERROR);

    switch (options->mode) {
      case PS_INTERPOLATE_FLAT:
        return varianceFactorFlat(x, y, options);
      case PS_INTERPOLATE_BILINEAR:
      case PS_INTERPOLATE_BICUBE:
      case PS_INTERPOLATE_GAUSS:
        return varianceFactorKernel(x, y, options);
      case PS_INTERPOLATE_LANCZOS2:
      case PS_INTERPOLATE_LANCZOS3:
      case PS_INTERPOLATE_LANCZOS4:
        return varianceFactorSeparate(x, y, options);
      default:
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                _("Specified interpolation method (%d) is not supported."),
                options->mode);
        return NAN;
    }

    psAbort("Should never reach here.");
    return NAN;
}


psImageInterpolateMode psImageInterpolateModeFromString(const char *name)
{
    PS_ASSERT_STRING_NON_EMPTY(name, PS_INTERPOLATE_NONE);

    if (!strcasecmp(name, "FLAT"))     return PS_INTERPOLATE_FLAT;
    if (!strcasecmp(name, "BILINEAR")) return PS_INTERPOLATE_BILINEAR;
    if (!strcasecmp(name, "BICUBE"))   return PS_INTERPOLATE_BICUBE;
    if (!strcasecmp(name, "GAUSS"))    return PS_INTERPOLATE_GAUSS;
    if (!strcasecmp(name, "LANCZOS2")) return PS_INTERPOLATE_LANCZOS2;
    if (!strcasecmp(name, "LANCZOS3")) return PS_INTERPOLATE_LANCZOS3;
    if (!strcasecmp(name, "LANCZOS4")) return PS_INTERPOLATE_LANCZOS4;

    psError(PS_ERR_BAD_PARAMETER_VALUE, true, _("Unknown interpolation type %s"), name);
    return PS_INTERPOLATE_NONE;
}
