/** @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.103 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2006-04-20 01:13:11 $
 *
 *  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 "psImage.h"
#include "psString.h"

#include "psErrorText.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)
{
    psS32 area = 0;
    psS32 elementSize = PSELEMTYPE_SIZEOF(type);  // element size in bytes
    psS32 rowSize = numCols * elementSize;        // row size in bytes.

    area = numCols * numRows;

    if (area < 1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                PS_ERRORTEXT_psImage_AREA_NEGATIVE,
                numRows, numCols);
        return NULL;
    }

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

    psMemSetDeallocator(image, (psFreeFunc) imageFree);

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

    image->p_rawDataBuffer = psAlloc(area * elementSize);

    // 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);
    }

    *(psS32 *)&image->col0 = 0;
    *(psS32 *)&image->row0 = 0;
    *(psU32 *)&image->numCols = numCols;
    *(psU32 *)&image->numRows = numRows;
    *(psDimen* ) & image->type.dimen = PS_DIMEN_IMAGE;
    *(psElemType* ) & image->type.type = type;
    image->parent = NULL;
    image->children = NULL;

    return image;
}


bool psMemCheckImage(psPtr ptr)
{
    return ( psMemGetDeallocator(ptr) == (psFreeFunc)imageFree );
}


// XXX these have been moved to psRegion* in src/math/
# if 0
psRegion psRegionSet(float x0,
                     float x1,
                     float y0,
                     float y1)
{
    psRegion out;

    out.x0 = x0;
    out.y0 = y0;
    out.x1 = x1;
    out.y1 = y1;

    return out;
}

psRegion psRegionFromString(const char* region)
{
    psS32 col0;
    psS32 col1;
    psS32 row0;
    psS32 row1;

    // section should be of the form '[col0:col1,row0:row1]'
    if (region == NULL) {
        psError(PS_ERR_BAD_PARAMETER_NULL, true,
                PS_ERRORTEXT_psImage_SUBSECTION_NULL);
        return psRegionSet(NAN,NAN,NAN,NAN);
    }

    if (sscanf(region,"[%d:%d,%d:%d]",&col0,&col1,&row0,&row1) < 4) {
        psError(PS_ERR_BAD_PARAMETER_NULL, true,
                PS_ERRORTEXT_psImage_SUBSECTION_INVALID,
                region);
        return psRegionSet(NAN,NAN,NAN,NAN);
    }

    if (col0 > col1 || row0 > row1) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                PS_ERRORTEXT_psImage_SUBSET_RANGE_MALFORMED,
                col0,col1,row0,row1);
        return psRegionSet(NAN,NAN,NAN,NAN);
    }

    return psRegionSet(col0-1,col1,row0-1,row1);
}

psString psRegionToString(const psRegion region)
{
    char tmpText[256]; // big enough to store any region as text

    snprintf(tmpText,256,"[%g:%g,%g:%g]",
             region.x0+1, region.x1,
             region.y0+1, region.y1);

    return psStringCopy(tmpText);
}

// set actual region based on image parameters:
// - compensate for negative upper limits
// - force range to be on this image
// - saturate on upper and lower limits of image
// - flip x0,x1 if x0>x1
// - flip y0,y1 if y0>y1
// XXX EAM : psRegion refers to coordinates in the *parent* image
psRegion psRegionForImage(psImage *image,
                          psRegion in)
{

    //    if (image == NULL) {
    //        return in;
    //    }
    PS_ASSERT_IMAGE_NON_NULL(image, in);
    //if the region is [0,0,0,0], the whole image (or subimage) is to be included.
    if (in.x0 == 0 && in.x1 == 0 && in.y0 == 0 && in.y1 == 0) {
        in.x0 = image->col0;
        in.x1 = image->col0 + image->numCols - 1;
        in.y0 = image->row0;
        in.y1 = image->row0 + image->numRows - 1;
        return (in);
    }

    // convert non-positive upper-limits
    // XXX note that the upper limit in these cases is defined relative to the subimage
    // also note that truncation limits to the valid subimage pixels
    in.x1 = (in.x1 <= 0) ? (image->col0 + image->numCols + in.x1) : in.x1;
    in.y1 = (in.y1 <= 0) ? (image->row0 + image->numRows + in.y1) : in.y1;

    // force the upper-limits to be on the subimage
    in.x1 = PS_MIN(image->col0 + image->numCols, in.x1);
    in.y1 = PS_MIN(image->row0 + image->numRows, in.y1);

    // force the lower-limits to be on the subimage
    in.x0 = PS_MAX(image->col0, in.x0);
    in.y0 = PS_MAX(image->row0, in.y0);
    in.x0 = PS_MIN(image->col0 + image->numCols, in.x0);
    in.y0 = PS_MIN(image->row0 + image->numRows, in.y0);

    // flip start and end if out of order
    if (in.x0 > in.x1) {
        psError (PS_ERR_BAD_PARAMETER_VALUE, true,
                 "Invalid region in psRegionForImage.  x0 > x1.  Values have been swapped.\n");
        PS_SWAP (in.x0, in.x1);
    }
    if (in.y0 > in.y1) {
        psError (PS_ERR_BAD_PARAMETER_VALUE, true,
                 "Invalid region in psRegionForImage.  y0 > y1.  Values have been swapped.\n");
        PS_SWAP (in.y0, in.y1);
    }

    return (in);
}

// define a square region centered on the given coordinate
psRegion psRegionForSquare(double x,
                           double y,
                           double radius)
{
    psRegion region;
    region = psRegionSet (x - radius, x + radius + 1,
                          y - radius, y + radius + 1);
    return (region);
}
# endif /* commented-out psRegion functions */

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

    va_list argp;

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

    va_start (argp, image);

    switch (image->type.type) {
        #define IMAGEINIT_INTCASE(TTT,NATIVETYPE) \
    case PS_TYPE_##TTT: { \
            NATIVETYPE temp = va_arg (argp, NATIVETYPE); \
            if ( temp < PS_MIN_##TTT || temp > PS_MAX_##TTT ) { \
                psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Error: Value out of Range.\n"); \
                return false; \
            } \
            ps##TTT value = temp; \
            for (int iy = 0; iy < image->numRows; iy++) { \
                ps##TTT *row = image->data.TTT[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(TTT,NATIVETYPE) \
    case PS_TYPE_##TTT: { \
            ps##TTT value = va_arg (argp, NATIVETYPE); \
            for (int iy = 0; iy < image->numRows; iy++) { \
                ps##TTT *row = image->data.TTT[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(TTT) \
    case PS_TYPE_##TTT: { \
            ps##TTT value = va_arg(argp, psF64); \
            \
            for (int iy = 0; iy < image->numRows; iy++) { \
                ps##TTT *row = image->data.TTT[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 %d not defined in psImageInit\n", image->type);
        return (false);
    }
    return (false);
}

bool psImageSet(const 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;
    }

    switch (image->type.type) {
    case PS_TYPE_U8:
        *(psU8*)&image->data.U8[x][y] = value;
        break;
    case PS_TYPE_U16:
        *(psU16*)&image->data.U16[x][y] = value;
        break;
    case PS_TYPE_U32:
        *(psU32*)&image->data.U32[x][y] = value;
        break;
    case PS_TYPE_U64:
        *(psU64*)&image->data.U64[x][y] = value;
        break;
    case PS_TYPE_S8:
        *(psS8*)&image->data.S8[x][y] = value;
        break;
    case PS_TYPE_S16:
        *(psS16*)&image->data.S16[x][y] = value;
        break;
    case PS_TYPE_S32:
        *(psS32*)&image->data.S32[x][y] = value;
        break;
    case PS_TYPE_S64:
        *(psS64*)&image->data.S64[x][y] = value;
        break;
    case PS_TYPE_F32:
        *(psF32*)&image->data.F32[x][y] = value;
        break;
    case PS_TYPE_F64:
        *(psF64*)&image->data.F64[x][y] = value;
        break;
    case PS_TYPE_C32:
        *(psC32*)&image->data.C32[x][y] = value;
        break;
    case PS_TYPE_C64:
        *(psC64*)&image->data.C64[x][y] = value;
        break;
    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 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;
    }


    switch (image->type.type) {
    case PS_TYPE_U8:
        return image->data.U8[x][y];
        break;
    case PS_TYPE_U16:
        return image->data.U16[x][y];
        break;
    case PS_TYPE_U32:
        return image->data.U32[x][y];
        break;
    case PS_TYPE_U64:
        return image->data.U64[x][y];
        break;
    case PS_TYPE_S8:
        return image->data.S8[x][y];
        break;
    case PS_TYPE_S16:
        return image->data.S16[x][y];
        break;
    case PS_TYPE_S32:
        return image->data.S32[x][y];
        break;
    case PS_TYPE_S64:
        return image->data.S64[x][y];
        break;
    case PS_TYPE_F32:
        return image->data.F32[x][y];
        break;
    case PS_TYPE_F64:
        return image->data.F64[x][y];
        break;
    case PS_TYPE_C32:
        return image->data.C32[x][y];
        break;
    case PS_TYPE_C64:
        return image->data.C64[x][y];
        break;
    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,
                PS_ERRORTEXT_psImage_NOT_AN_IMAGE);
        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,
                PS_ERRORTEXT_psImage_IMAGE_NULL);
        return false;
    }

    if (input->type.dimen != PS_DIMEN_IMAGE) {
        psError(PS_ERR_BAD_PARAMETER_TYPE, true,
                PS_ERRORTEXT_psImage_NOT_AN_IMAGE);
        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,
                    PS_ERRORTEXT_psImage_IMAGE_TYPE_UNSUPPORTED,
                    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++) {
            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,                     \
                PS_ERRORTEXT_psImage_INTERPOLATE_METHOD_INVALID,     \
                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,
                    PS_ERRORTEXT_psImage_IMAGE_TYPE_UNSUPPORTED,
                    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)
