/** @file  psImage.c
 *
 *  @brief Contains basic image definitions and operations.
 *
 *  This file defines the basic type for an image struct and functions useful
 *  in manupulating images.
 *
 *  @author Robert DeSonia, MHPCC
 *  @author Ross Harman, MHPCC
 *
 *  @version $Revision: 1.114 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-08-26 04:34:28 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 *
 *  That is the routine used to generate matrices.
 */

#include <string.h>
#include <math.h>
#include <unistd.h>

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



#define SQUARE(x) ((x)*(x))
#define MIN(x,y) (((x) > (y)) ? (y) : (x))
#define MAX(x,y) (((x) > (y)) ? (x) : (y))

static void imageFree(psImage* image)
{
    if (image == NULL) {
        return;
    }

    if (image->parent != NULL) {
        psMemBlock* ptr = ((psMemBlock*)image)-1;
        int ref = ptr->refCounter;
        ptr->refCounter = 2;  // make sure psFree is not retriggered
        psArrayRemove(image->parent->children,image);
        ptr->refCounter = ref; // restore previous count (not assuming zero, but should be)

        image->parent = NULL;
    }

    psImageFreeChildren(image);

    psFree(image->p_rawDataBuffer);
    psFree(image->data.V);
}

psImage* psImageAlloc(int numCols,
                      int numRows,
                      psElemType type)
{
    int elementSize = PSELEMTYPE_SIZEOF(type);  // element size in bytes
    int rowSize = numCols * elementSize;        // row size in bytes.

    if (numRows < 1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                _("Specified number of rows (%d) or columns (%d) is invalid."),
                numRows, numCols);
        return NULL;
    }
    if (numCols < 1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                _("Specified number of rows (%d) or columns (%d) is invalid."),
                numRows, numCols);
        return NULL;
    }

    long numBytes = numRows * numCols * elementSize;

    psImage* image = (psImage* ) psAlloc(sizeof(psImage));

    psMemSetDeallocator(image, (psFreeFunc) imageFree);

    image->data.V = psAlloc(sizeof(psPtr ) * numRows);

    image->p_rawDataBuffer = psAlloc(numBytes);

    // set the row pointers.
    image->data.V[0] = image->p_rawDataBuffer;
    for (psS32 i = 1; i < numRows; i++) {
        image->data.V[i] = (psPtr )((int8_t *) image->data.V[i - 1] + rowSize);
    }

    image->col0 = 0;
    image->row0 = 0;
    P_PSIMAGE_SET_NUMCOLS (image, numCols);
    P_PSIMAGE_SET_NUMROWS (image, numRows);

    psMathType imageType;
    imageType.dimen = PS_DIMEN_IMAGE;
    imageType.type = type;
    P_PSIMAGE_SET_TYPE (image, imageType);

    image->parent = NULL;
    image->children = NULL;

    return image;
}


bool psMemCheckImage(psPtr ptr)
{
    if (!is_psType(ptr)) {
        return false;
    }
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)imageFree );
}

bool psImageInit (psImage *image,...)
{

    va_list argp;

    if (image == NULL)
        return (false);

    va_start (argp, image);

    switch (image->type.type) {
        #define IMAGEINIT_INTCASE(TYPE,NATIVETYPE) \
    case PS_TYPE_##TYPE: { \
            NATIVETYPE temp = va_arg(argp, NATIVETYPE); \
            if ( temp < PS_MIN_##TYPE || temp > PS_MAX_##TYPE ) { \
                psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Error: Value %d out of Range.\n", temp); \
                return false; \
            } \
            ps##TYPE value = temp; \
            if (value == 0) { \
                size_t numBytes = image->numCols * sizeof(ps##TYPE); \
                for (int y = 0; y < image->numRows; y++) { \
                    memset(image->data.TYPE[y], 0, numBytes); \
                } \
            } else { \
                for (int iy = 0; iy < image->numRows; iy++) { \
                    ps##TYPE *row = image->data.TYPE[iy]; \
                    for (int ix = 0; ix < image->numCols; ix++) { \
                        row[ix] = value; \
                    } \
                } \
            } \
            return true; \
        }

        IMAGEINIT_INTCASE(U8,unsigned int)
        IMAGEINIT_INTCASE(U16,unsigned int)
        IMAGEINIT_INTCASE(U32,unsigned int)
        IMAGEINIT_INTCASE(S8,int)
        IMAGEINIT_INTCASE(S16,int)
        IMAGEINIT_INTCASE(S32,int)

        #define IMAGEINIT_LONGCASE(TYPE,NATIVETYPE) \
    case PS_TYPE_##TYPE: { \
            ps##TYPE value = va_arg(argp, NATIVETYPE); \
            if (value == 0) { \
                size_t numBytes = image->numCols * sizeof(ps##TYPE); \
                for (int y = 0; y < image->numRows; y++) { \
                    memset(image->data.TYPE[y], 0, numBytes); \
                } \
            } else { \
                for (int iy = 0; iy < image->numRows; iy++) { \
                    ps##TYPE *row = image->data.TYPE[iy]; \
                    for (int ix = 0; ix < image->numCols; ix++) { \
                        row[ix] = value; \
                    } \
                } \
            } \
            return true; \
        }

        IMAGEINIT_LONGCASE(U64,unsigned long)
        IMAGEINIT_LONGCASE(S64,long)

        #define IMAGEINIT_FLOATCASE(TYPE) \
    case PS_TYPE_##TYPE: { \
            ps##TYPE value = va_arg(argp, psF64); \
            if (value == 0) { \
                size_t numBytes = image->numCols * sizeof(ps##TYPE); \
                for (int y = 0; y < image->numRows; y++) { \
                    memset(image->data.TYPE[y], 0, numBytes); \
                } \
            } else { \
                for (int iy = 0; iy < image->numRows; iy++) { \
                    ps##TYPE *row = image->data.TYPE[iy]; \
                    for (int ix = 0; ix < image->numCols; ix++) { \
                        row[ix] = value; \
                    } \
                } \
            } \
            return true; \
        }

        IMAGEINIT_FLOATCASE(F32)
        IMAGEINIT_FLOATCASE(F64)

    default:
        psError(PS_ERR_BAD_PARAMETER_TYPE, true, "datatype not defined in psImageInit\n");
    }
    return (false);
}

bool psImageSet(psImage *image,
                int x,
                int y,
                double complex value)
{
    PS_ASSERT_IMAGE_NON_NULL(image, false);
    PS_ASSERT_INT_NONNEGATIVE(image->col0, false);
    PS_ASSERT_INT_NONNEGATIVE(image->row0, false);
    PS_ASSERT_INT_POSITIVE(image->numCols, false);
    PS_ASSERT_INT_POSITIVE(image->numRows, false);
    if ( x >= (image->col0 + image->numCols) ) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid x-position %d.  Position out of range (%d-%d)\n",
                x, image->col0, image->numCols+image->col0-1 );
        return false;
    } else if ( y >= (image->row0 + image->numRows) ) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid y-position %d.  Position out of range (%d-%d)\n",
                y, image->row0, image->numRows+image->row0-1 );
        return false;
    } else if (x < image->col0 && x >= 0) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid x-position %d.  Position out of range (%d-%d)\n",
                x, image->col0, image->numCols+image->col0-1 );
        return false;
    } else if (y < image->row0 && y >= 0) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid y-position %d.  Position out of range (%d-%d)\n",
                y, image->row0, image->numRows+image->row0-1 );
        return false;
    } else if (x < 0 || y < 0) {
        if (x < 0) {
            x += image->numCols;
        }
        if (x < 0) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Invalid x-position %d.  Position out of range (%d-%d)\n",
                    (x+image->col0), image->col0, image->numCols+image->col0-1 );
            return false;
        }
        if (y < 0) {
            y += image->numRows;
        }
        if (y < 0) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Invalid y-position %d.  Position out of range (%d-%d)\n",
                    (y+image->row0), image->row0, image->numRows+image->row0-1 );
            return false;
        }
    } else {
        x -= image->col0;
        y -= image->row0;
    }

    #define IMAGE_SET_CASE(TYPE) \
case PS_TYPE_##TYPE: \
    image->data.TYPE[y][x] = (ps##TYPE)value; \
    break;

    switch (image->type.type) {
        IMAGE_SET_CASE(U8);
        IMAGE_SET_CASE(U16);
        IMAGE_SET_CASE(U32);
        IMAGE_SET_CASE(U64);
        IMAGE_SET_CASE(S8);
        IMAGE_SET_CASE(S16);
        IMAGE_SET_CASE(S32);
        IMAGE_SET_CASE(S64);
        IMAGE_SET_CASE(F32);
        IMAGE_SET_CASE(F64);
        IMAGE_SET_CASE(C32);
        IMAGE_SET_CASE(C64);
    default:
        psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Invalid psImage Data Type\n");
        return false;
    }

    return true;
}

double complex psImageGet(const psImage *image,
                          int x,
                          int y)
{
    PS_ASSERT_IMAGE_NON_NULL(image, NAN);
    PS_ASSERT_INT_NONNEGATIVE(image->col0, NAN);
    PS_ASSERT_INT_NONNEGATIVE(image->row0, NAN);
    PS_ASSERT_INT_POSITIVE(image->numCols, NAN);
    PS_ASSERT_INT_POSITIVE(image->numRows, NAN);
    if ( x >= (image->col0 + image->numCols) ) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid x-position %d.  Position out of range (%d-%d)\n",
                x, image->col0, image->numCols+image->col0-1 );
        return NAN;
    } else if ( y >= (image->row0 + image->numRows) ) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid y-position %d.  Position out of range (%d-%d)\n",
                y, image->row0, image->numRows+image->row0-1 );
        return NAN;
    } else if (x < image->col0 && x >= 0) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid x-position %d.  Position out of range (%d-%d)\n",
                x, image->col0, image->numCols+image->col0-1 );
        return NAN;
    } else if (y < image->row0 && y >= 0) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Invalid y-position %d.  Position out of range (%d-%d)\n",
                y, image->row0, image->numRows+image->row0-1 );
        return NAN;
    } else if (x < 0 || y < 0) {
        if (x < 0) {
            x += image->numCols;
        }
        if (x < 0) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Invalid x-position %d.  Position out of range (%d-%d)\n",
                    (x+image->col0), image->col0, image->numCols+image->col0-1 );
            return NAN;
        }
        if (y < 0) {
            y += image->numRows;
        }
        if (y < 0) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Invalid y-position %d.  Position out of range (%d-%d)\n",
                    (y+image->row0), image->row0, image->numRows+image->row0-1 );
            return NAN;
        }
    } else {
        x -= image->col0;
        y -= image->row0;
    }

    #define IMAGE_GET_CASE(TYPE) \
case PS_TYPE_##TYPE: \
    return image->data.TYPE[y][x];

    switch (image->type.type) {
        IMAGE_GET_CASE(U8);
        IMAGE_GET_CASE(U16);
        IMAGE_GET_CASE(U32);
        IMAGE_GET_CASE(U64);
        IMAGE_GET_CASE(S8);
        IMAGE_GET_CASE(S16);
        IMAGE_GET_CASE(S32);
        IMAGE_GET_CASE(S64);
        IMAGE_GET_CASE(F32);
        IMAGE_GET_CASE(F64);
        IMAGE_GET_CASE(C32);
        IMAGE_GET_CASE(C64);
    default:
        psError(PS_ERR_BAD_PARAMETER_TYPE, true, "Invalid psImage Data Type\n");
        return NAN;
    }
}

psImage* psImageRecycle(psImage* old,
                        int numCols,
                        int numRows,
                        const psElemType type)
{
    psS32 elementSize = PSELEMTYPE_SIZEOF(type);  // element size in bytes
    psS32 rowSize = numCols * elementSize;        // row size in bytes.

    if (old == NULL) {
        old = psImageAlloc(numCols, numRows, type);
        return old;
    }

    if (old->type.dimen != PS_DIMEN_IMAGE) {
        psFree(old);
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                _("The input psImage must have a PS_DIMEN_IMAGE dimension type."));
        return NULL;
    }

    /* image already the right size/type? */
    if (numCols == old->numCols && numRows == old->numRows &&
            type == old->type.type) {
        return old;
    }
    // Resize the image buffer
    old->p_rawDataBuffer = psRealloc(old->data.V[0],
                                     numCols * numRows * elementSize);
    old->data.V = (psPtr *)psRealloc(old->data.V, numRows * sizeof(psPtr ));

    // recreate the row pointers
    old->data.V[0] = old->p_rawDataBuffer;
    for (psS32 i = 1; i < numRows; i++) {
        old->data.V[i] = (psPtr )((int8_t *) old->data.V[i - 1] + rowSize);
    }

    *(psU32 *)&old->numCols = numCols;
    *(psU32 *)&old->numRows = numRows;
    *(psElemType* ) & old->type.type = type;

    return old;
}

bool p_psImageCopyToRawBuffer(void* buffer,
                              const psImage* input,
                              psElemType type)
{
    psElemType inDatatype;
    psS32 numRows;
    psS32 numCols;

    if (input == NULL || input->data.V == NULL) {
        psError(PS_ERR_BAD_PARAMETER_NULL, true,
                _("Can not operate on a NULL psImage."));
        return false;
    }

    if (input->type.dimen != PS_DIMEN_IMAGE) {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                _("The input psImage must have a PS_DIMEN_IMAGE dimension type."));
        return false;
    }

    inDatatype = input->type.type;
    numRows = input->numRows;
    numCols = input->numCols;

    // cover the trival case of copy of the same
    // datatype.
    if (type == inDatatype) {
        int rowSize = PSELEMTYPE_SIZEOF(inDatatype)*numCols;
        for (psS32 row=0;row<numRows;row++) {
            memcpy(&((psS8*)buffer)[row*rowSize], input->data.V[row], rowSize);
        }
        return true;
    }

    #define PSIMAGE_BUFFER_COPY(INTYPE,OUTTYPE) { \
        ps##INTYPE *in; \
        ps##OUTTYPE *out = buffer; \
        for(psS32 row=0;row<numRows;row++) { \
            in = input->data.INTYPE[row]; \
            for (psS32 col=0;col<numCols;col++) { \
                *(out++) = *(in++); \
            } \
        } \
    }

    #define PSIMAGE_BUFFER_COPY_CASE(OUT,OUTTYPE) { \
        switch (inDatatype) { \
        case PS_TYPE_S8: \
            PSIMAGE_BUFFER_COPY(S8,OUTTYPE); \
            break; \
        case PS_TYPE_S16: \
            PSIMAGE_BUFFER_COPY(S16,OUTTYPE); \
            break; \
        case PS_TYPE_S32: \
            PSIMAGE_BUFFER_COPY(S32,OUTTYPE); \
            break; \
        case PS_TYPE_S64: \
            PSIMAGE_BUFFER_COPY(S64,OUTTYPE); \
            break; \
        case PS_TYPE_U8: \
            PSIMAGE_BUFFER_COPY(U8,OUTTYPE); \
            break; \
        case PS_TYPE_U16: \
            PSIMAGE_BUFFER_COPY(U16,OUTTYPE); \
            break; \
        case PS_TYPE_U32: \
            PSIMAGE_BUFFER_COPY(U32,OUTTYPE); \
            break; \
        case PS_TYPE_U64: \
            PSIMAGE_BUFFER_COPY(U64,OUTTYPE); \
            break; \
        case PS_TYPE_F32: \
            PSIMAGE_BUFFER_COPY(F32,OUTTYPE); \
            break; \
        case PS_TYPE_F64: \
            PSIMAGE_BUFFER_COPY(F64,OUTTYPE); \
            break; \
        case PS_TYPE_C32: \
            PSIMAGE_BUFFER_COPY(C32,OUTTYPE); \
            break; \
        case PS_TYPE_C64: \
            PSIMAGE_BUFFER_COPY(C64,OUTTYPE); \
            break; \
        default: \
            break; \
        } \
    }

    switch (type) {
    case PS_TYPE_S8:
        PSIMAGE_BUFFER_COPY_CASE(output, S8);
        break;
    case PS_TYPE_S16:
        PSIMAGE_BUFFER_COPY_CASE(output, S16);
        break;
    case PS_TYPE_S32:
        PSIMAGE_BUFFER_COPY_CASE(output, S32);
        break;
    case PS_TYPE_S64:
        PSIMAGE_BUFFER_COPY_CASE(output, S64);
        break;
    case PS_TYPE_U8:
        PSIMAGE_BUFFER_COPY_CASE(output, U8);
        break;
    case PS_TYPE_U16:
        PSIMAGE_BUFFER_COPY_CASE(output, U16);
        break;
    case PS_TYPE_U32:
        PSIMAGE_BUFFER_COPY_CASE(output, U32);
        break;
    case PS_TYPE_U64:
        PSIMAGE_BUFFER_COPY_CASE(output, U64);
        break;
    case PS_TYPE_F32:
        PSIMAGE_BUFFER_COPY_CASE(output, F32);
        break;
    case PS_TYPE_F64:
        PSIMAGE_BUFFER_COPY_CASE(output, F64);
        break;
    case PS_TYPE_C32:
        PSIMAGE_BUFFER_COPY_CASE(output, C32);
        break;
    case PS_TYPE_C64:
        PSIMAGE_BUFFER_COPY_CASE(output, C64);
        break;
    default: {
            char* typeStr;
            PS_TYPE_NAME(typeStr,type);
            psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                    _("Specified psImage type, %s, is not supported."),
                    typeStr);
            break;
        }
    }
    return true;
}


int psImageFreeChildren(psImage* image)
{
    psS32 numFreed = 0;

    if (image == NULL) {
        return numFreed;
    }

    if (image->children != NULL) {
        psImage** children = (psImage**)image->children->data;
        numFreed = image->children->n;

        // orphan the children first
        // (so psFree doesn't try to modify the parent's children array while I'm using it)
        for (psS32 i=0;i<numFreed;i++) {
            children[i]->parent = NULL;
        }

        psFree(image->children);
        image->children = NULL;
    }

    return numFreed;
}

bool p_psImagePrint (int fd,
                     psImage *a,
                     char *name)
{
    write(fd,"matrix: ",8);
    write(fd,name,strlen(name));
    write(fd,"\n",1);

    char buffer[20];
    //    for (int j = 0; j < a[0].numRows; j++) {
    //        for (int i = 0; i < a[0].numCols; i++) {
    for (int j = 0; j < a->numRows; j++) {
        for (int i = 0; i < a->numCols; i++) {
            snprintf (buffer,20, "%f  ", p_psImageGetElementF64(a, i, j));
            write(fd,buffer,strlen(buffer));
        }
        write(fd,"\n",1);
    }
    write(fd,"\n",1);
    return (true);
}

double complex psImagePixelInterpolate(const psImage* input,
                                       float x,
                                       float y,
                                       const psImage* mask,
                                       psMaskType maskVal,
                                       double complex unexposedValue,
                                       psImageInterpolateMode mode)
{
    PS_ASSERT_IMAGE_NON_NULL(input, unexposedValue);

    #define PSIMAGE_PIXEL_INTERPOLATE_CASE(TYPE)                             \
case PS_TYPE_##TYPE:                                                 \
    switch (mode) {                                                  \
    case PS_INTERPOLATE_FLAT:                                        \
        return p_psImagePixelInterpolateFLAT_##TYPE(                 \
                input,                                               \
                x,                                                   \
                y,                                                   \
                mask,                                                \
                maskVal,                                             \
                unexposedValue);                                     \
        break;                                                       \
    case PS_INTERPOLATE_BILINEAR:                                    \
        return p_psImagePixelInterpolateBILINEAR_##TYPE(             \
                input,                                               \
                x,                                                   \
                y,                                                   \
                mask,                                                \
                maskVal,                                             \
                unexposedValue);                                     \
        break;                                                       \
    case PS_INTERPOLATE_BILINEAR_VARIANCE:                           \
        return p_psImagePixelInterpolateBILINEAR_VARIANCE_##TYPE(    \
                input,                                               \
                x,                                                   \
                y,                                                   \
                mask,                                                \
                maskVal,                                             \
                unexposedValue);                                     \
        break;                                                       \
    default:                                                         \
        psError(PS_ERR_BAD_PARAMETER_VALUE,true,                     \
                _("Specified interpolation method (%d) is not supported."),     \
                mode);                                               \
    }                                                                \
    break;

    switch (input->type.type) {
        PSIMAGE_PIXEL_INTERPOLATE_CASE(U8);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(U16);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(U32);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(U64);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(S8);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(S16);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(S32);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(S64);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(F32);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(F64);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(C32);
        PSIMAGE_PIXEL_INTERPOLATE_CASE(C64);
    default: {
            char* typeStr;
            PS_TYPE_NAME(typeStr,input->type.type);
            psError(PS_ERR_BAD_PARAMETER_TYPE,true,
                    _("Specified psImage type, %s, is not supported."),
                    typeStr);
        }
    }

    return unexposedValue;
}

psF64 p_psImageGetElementF64(psImage* image,
                             int col,
                             int row)
{
    if (image == NULL) {
        return NAN;
    }
    if (col < 0 || col >= image->numCols) {
        return NAN;
    }
    if (row < 0 || row >= image->numRows) {
        return NAN;
    }

    switch (image->type.type) {
    case PS_TYPE_U8:
        return image->data.U8[row][col];
        break;
    case PS_TYPE_U16:
        return image->data.U16[row][col];
        break;
    case PS_TYPE_U32:
        return image->data.U32[row][col];
        break;
    case PS_TYPE_U64:
        return image->data.U64[row][col];
        break;
    case PS_TYPE_S8:
        return image->data.S8[row][col];
        break;
    case PS_TYPE_S16:
        return image->data.S16[row][col];
        break;
    case PS_TYPE_S32:
        return image->data.S32[row][col];
        break;
    case PS_TYPE_S64:
        return image->data.S64[row][col];
        break;
    case PS_TYPE_F32:
        return image->data.F32[row][col];
        break;
    case PS_TYPE_F64:
        return image->data.F64[row][col];
    default:
        return NAN;
    }
}

#define PSIMAGE_PIXEL_INTERPOLATE_FLAT(TYPE,RETURNTYPE) \
inline RETURNTYPE p_psImagePixelInterpolateFLAT_##TYPE( \
        const psImage* input, \
        float x, \
        float y, \
        const psImage* mask, \
        psU32 maskVal, \
        RETURNTYPE unexposedValue) \
{ \
    psS32 intX = (psS32) round((psF64)(x) - 0.5 + FLT_EPSILON); \
    psS32 intY = (psS32) round((psF64)(y) - 0.5 + FLT_EPSILON); \
    psS32 lastX = input->numCols - 1; \
    psS32 lastY = input->numRows - 1; \
    \
    if ((intX < 0) || \
            (intX > lastX) || \
            (intY < 0) || \
            (intY > lastY) || \
            ( (mask!=NULL) && \
              ((mask->data.PS_TYPE_MASK_DATA[intY][intX] & maskVal) != 0) ) ) { \
        return unexposedValue; \
    } \
    \
    return input->data.TYPE[intY][intX]; \
}

PSIMAGE_PIXEL_INTERPOLATE_FLAT(U8,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(U16,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(U32,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(U64,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(S8,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(S16,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(S32,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(S64,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(F32,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(F64,psF64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(C32,psC64)
PSIMAGE_PIXEL_INTERPOLATE_FLAT(C64,psC64)

#define PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(TYPE, RETURNTYPE, SUFFIX, FRACFUNC) \
inline RETURNTYPE p_psImagePixelInterpolateBILINEAR_##SUFFIX( \
        const psImage* input, \
        float x, \
        float y, \
        const psImage* mask, \
        psU32 maskVal, \
        RETURNTYPE unexposedValue) \
{ \
    double floorX = floor((psF64)(x) - 0.5); \
    double floorY = floor((psF64)(y) - 0.5); \
    psF64 fracX = x - 0.5 - floorX; \
    psF64 fracY = y - 0.5 - floorY; \
    psS32 intFloorX = (psS32) floorX; \
    psS32 intFloorY = (psS32) floorY; \
    psS32 lastX = input->numCols - 1; \
    psS32 lastY = input->numRows - 1; \
    ps##TYPE V00 = 0; \
    ps##TYPE V01 = 0; \
    ps##TYPE V10 = 0; \
    ps##TYPE V11 = 0; \
    psBool valid00 = false; \
    psBool valid01 = false; \
    psBool valid10 = false; \
    psBool valid11 = false; \
    \
    if (intFloorY >= 0 && intFloorY <= lastY) { \
        if (intFloorX >= 0 && intFloorX <= lastX) { \
            V00 = input->data.TYPE[intFloorY][intFloorX]; \
            valid00 = (mask == NULL) || \
                      ((mask->data.PS_TYPE_MASK_DATA[intFloorY][intFloorX] & maskVal) == 0); \
        } \
        if (intFloorX >= -1 && intFloorX < lastX) { \
            V10 = input->data.TYPE[intFloorY][intFloorX+1]; \
            valid10 = (mask == NULL) || \
                      ((mask->data.PS_TYPE_MASK_DATA[intFloorY][intFloorX+1] & maskVal) == 0); \
        } \
    } \
    if (intFloorY >= -1 && intFloorY < lastY) { \
        if (intFloorX >= 0 && intFloorX <= lastX) { \
            V01 = input->data.TYPE[intFloorY+1][intFloorX]; \
            valid01 = (mask == NULL) || \
                      ((mask->data.PS_TYPE_MASK_DATA[intFloorY+1][intFloorX] & maskVal) == 0); \
        } \
        if (intFloorX >= -1 && intFloorX < lastX) { \
            V11 = input->data.TYPE[intFloorY+1][intFloorX+1]; \
            valid11 = (mask == NULL) || \
                      ((mask->data.PS_TYPE_MASK_DATA[intFloorY+1][intFloorX+1] & maskVal) == 0); \
        } \
    } \
    \
    /* cover likely case of all pixels being valid more efficiently */  \
    if (valid00 && valid10 && valid01 && valid11) { \
        /* formula from the ADD */ \
        return V00*FRACFUNC((1.0-fracX)*(1.0-fracY)) + V10*FRACFUNC(fracX*(1.0-fracY)) + \
               V01*FRACFUNC(fracY*(1.0-fracX)) + V11*FRACFUNC(fracX*fracY); \
    } \
    \
    /* OK, at least one pixel is not valid - need to do it piecemeal */ \
    \
    RETURNTYPE V0 = 0.0; \
    psBool valid0 = true; \
    if (valid00 && valid10) { \
        V0 = V00*FRACFUNC(1-fracX)+V10*FRACFUNC(fracX); \
    } else if (valid00) { \
        V0 = V00; \
    } else if (valid10) { \
        V0 = V10; \
    } else { \
        valid0 = false; \
    } \
    \
    RETURNTYPE V1 = 0.0; \
    psBool valid1 = true; \
    if (valid01 && valid11) { \
        V1 = V01*FRACFUNC(1-fracX)+V11*FRACFUNC(fracX); \
    } else if (valid01) { \
        V1 = V01; \
    } else if (valid11) { \
        V1 = V11; \
    } else { \
        valid1 = false; \
    } \
    \
    if (valid0 && valid1) { \
        return V0*FRACFUNC(1-fracY) + V1*FRACFUNC(fracY); \
    } else if (valid0) { \
        return V0; \
    } else if (valid1) { \
        return V1; \
    } \
    \
    return unexposedValue; \
}

PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U8,psF64,U8,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U16,psF64,U16,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U32,psF64,U32,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U64,psF64,U64,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S8,psF64,S8,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S16,psF64,S16,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S32,psF64,S32,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S64,psF64,S64,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(F32,psF64,F32,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(F64,psF64,F64,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(C32,psC64,C32,)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(C64,psC64,C64,)

// Variance Version
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U8,psF64,VARIANCE_U8,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U16,psF64,VARIANCE_U16,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U32,psF64,VARIANCE_U32,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(U64,psF64,VARIANCE_U64,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S8,psF64,VARIANCE_S8,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S16,psF64,VARIANCE_S16,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S32,psF64,VARIANCE_S32,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(S64,psF64,VARIANCE_S64,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(F32,psF64,VARIANCE_F32,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(F64,psF64,VARIANCE_F64,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(C32,psC64,VARIANCE_C32,SQUARE)
PSIMAGE_PIXEL_INTERPOLATE_BILINEAR(C64,psC64,VARIANCE_C64,SQUARE)
