/*
 * The Python Imaging Library
 * $Id$
 *
 * apply convolution kernel to image
 *
 * history:
 * 1995-11-26 fl   Created, supports 3x3 kernels
 * 1995-11-27 fl   Added 5x5 kernels, copy border
 * 1999-07-26 fl   Eliminated a few compiler warnings
 * 2002-06-09 fl   Moved kernel definitions to Python
 * 2002-06-11 fl   Support floating point kernels
 * 2003-09-15 fl   Added ImagingExpand helper
 *
 * Copyright (c) Secret Labs AB 1997-2002.  All rights reserved.
 * Copyright (c) Fredrik Lundh 1995.
 *
 * See the README file for information on usage and redistribution.
 */

/*
 * FIXME: Support RGB and RGBA/CMYK modes as well
 * FIXME: Expand image border (current version leaves border as is)
 * FIXME: Implement image processing gradient filters
 */

#include <stdint.h>
#include "Imaging.h"

#define INT32_MAX_F 2147483647.0F

static inline UINT8
clip8(float in) {
    // Branchless clamp to [0, 255].
    in = in < 0.0f ? 0.0f : in;
    in = in > 255.0f ? 255.0f : in;
    return (UINT8)in;
}

static inline int
clip16(float in) {
    // Branchless clamp to [0, 65535].
    in = in < 0.0f ? 0.0f : in;
    in = in > 65535.0f ? 65535.0f : in;
    return (int)in;
}

static inline INT32
clip32(float in) {
    // Clamp the low bound branchlessly (maxss).
    in = in < 0.0f ? 0.0f : in;
    // Must explicitly return INT32_MAX for the high bound
    // to avoid incorrect overflow to INT_MIN.
    return in >= INT32_MAX_F ? INT32_MAX : (INT32)in;
}

Imaging
ImagingExpand(Imaging imIn, int margin) {
    Imaging imOut;
    int x, y;
    ImagingSectionCookie cookie;

    if (margin < 0) {
        return (Imaging)ImagingError_ValueError("bad kernel size");
    }
    if (margin > 0 && margin > INT_MAX / (margin * (int)sizeof(FLOAT32))) {
        return (Imaging)ImagingError_ValueError("filter size too large");
    }

    imOut =
        ImagingNewDirty(imIn->mode, imIn->xsize + 2 * margin, imIn->ysize + 2 * margin);
    if (!imOut) {
        return NULL;
    }

#define EXPAND_LINE(type, image, yin, yout)                       \
    {                                                             \
        for (x = 0; x < margin; x++) {                            \
            imOut->image[yout][x] = imIn->image[yin][0];          \
        }                                                         \
        for (x = 0; x < imIn->xsize; x++) {                       \
            imOut->image[yout][x + margin] = imIn->image[yin][x]; \
        }                                                         \
        for (x = 0; x < margin; x++) {                            \
            imOut->image[yout][margin + imIn->xsize + x] =        \
                imIn->image[yin][imIn->xsize - 1];                \
        }                                                         \
    }

#define EXPAND(type, image)                                                      \
    {                                                                            \
        for (y = 0; y < margin; y++) {                                           \
            EXPAND_LINE(type, image, 0, y);                                      \
        }                                                                        \
        for (y = 0; y < imIn->ysize; y++) {                                      \
            EXPAND_LINE(type, image, y, y + margin);                             \
        }                                                                        \
        for (y = 0; y < margin; y++) {                                           \
            EXPAND_LINE(type, image, imIn->ysize - 1, margin + imIn->ysize + y); \
        }                                                                        \
    }

    ImagingSectionEnter(&cookie);
    if (imIn->image8) {
        EXPAND(UINT8, image8);
    } else {
        EXPAND(INT32, image32);
    }
    ImagingSectionLeave(&cookie);

    ImagingCopyPalette(imOut, imIn);

    return imOut;
}

float
kernel_i16(int size, UINT8 *in0, int x, const float *kernel, int bigendian) {
    int i;
    float result = 0;
    int half_size = (size - 1) / 2;
    for (i = 0; i < size; i++) {
        int x1 = x + i - half_size;
        result += (float)(in0[x1 * 2 + (bigendian ? 1 : 0)] +
                          (in0[x1 * 2 + (bigendian ? 0 : 1)] << 8)) *
                  kernel[i];
    }
    return result;
}

static void
ImagingFilter3x3(Imaging imOut, Imaging im, const float *kernel, float offset) {
#define KERNEL1x3(in0, x, kernel, d)                                     \
    ((float)(in0[x - d]) * (kernel)[0] + (float)(in0[x]) * (kernel)[1] + \
     (float)(in0[x + d]) * (kernel)[2])

    int x = 0, y = 0;
    // restrict safety: assert im and imOut to be separate,
    // so we can safely `restrict` the copy pointers below
    assert(im->image != imOut->image);
    // Hoist for loop optimization.
    int xsize = im->xsize, ysize = im->ysize;

    memcpy(imOut->image[0], im->image[0], im->linesize);
    if (im->bands == 1) {
        // Add one time for rounding
        offset += 0.5;
        if (im->type == IMAGING_TYPE_INT32) {
            for (y = 1; y < ysize - 1; y++) {
                INT32 *restrict in_1 = (INT32 *)im->image[y - 1];
                INT32 *restrict in0 = (INT32 *)im->image[y];
                INT32 *restrict in1 = (INT32 *)im->image[y + 1];
                INT32 *restrict out = (INT32 *)imOut->image[y];

                out[0] = in0[0];
                for (x = 1; x < xsize - 1; x++) {
                    float ss = offset;
                    ss += KERNEL1x3(in1, x, &kernel[0], 1);
                    ss += KERNEL1x3(in0, x, &kernel[3], 1);
                    ss += KERNEL1x3(in_1, x, &kernel[6], 1);
                    out[x] = clip32(ss);
                }
                out[x] = in0[x];
            }
        } else {
            int bigendian = 0;
            if (im->type == IMAGING_TYPE_SPECIAL) {
                if (
                    im->mode == IMAGING_MODE_I_16B
#ifdef WORDS_BIGENDIAN
                    || im->mode == IMAGING_MODE_I_16N
#endif
                ) {
                    bigendian = 1;
                }
            }
            for (y = 1; y < ysize - 1; y++) {
                UINT8 *restrict in_1 = (UINT8 *)im->image[y - 1];
                UINT8 *restrict in0 = (UINT8 *)im->image[y];
                UINT8 *restrict in1 = (UINT8 *)im->image[y + 1];
                UINT8 *restrict out = (UINT8 *)imOut->image[y];

                out[0] = in0[0];
                if (im->type == IMAGING_TYPE_SPECIAL) {
                    out[1] = in0[1];
                }
                for (x = 1; x < xsize - 1; x++) {
                    float ss = offset;
                    if (im->type == IMAGING_TYPE_SPECIAL) {
                        ss += kernel_i16(3, in1, x, &kernel[0], bigendian);
                        ss += kernel_i16(3, in0, x, &kernel[3], bigendian);
                        ss += kernel_i16(3, in_1, x, &kernel[6], bigendian);
                        // NOT rounding here because `offset` already has a +0.5 bias.
                        int ss_int = clip16(ss);
                        out[x * 2 + (bigendian ? 1 : 0)] = (UINT8)(ss_int & 0xff);
                        out[x * 2 + (bigendian ? 0 : 1)] = (UINT8)(ss_int >> 8);
                    } else {
                        ss += KERNEL1x3(in1, x, &kernel[0], 1);
                        ss += KERNEL1x3(in0, x, &kernel[3], 1);
                        ss += KERNEL1x3(in_1, x, &kernel[6], 1);
                        out[x] = clip8(ss);
                    }
                }
                if (im->type == IMAGING_TYPE_SPECIAL) {
                    out[x * 2] = in0[x * 2];
                    out[x * 2 + 1] = in0[x * 2 + 1];
                } else {
                    out[x] = in0[x];
                }
            }
        }
    } else {
        // Add one time for rounding
        offset += 0.5;
        for (y = 1; y < ysize - 1; y++) {
            UINT8 *restrict in_1 = (UINT8 *)im->image[y - 1];
            UINT8 *restrict in0 = (UINT8 *)im->image[y];
            UINT8 *restrict in1 = (UINT8 *)im->image[y + 1];
            UINT8 *restrict out = (UINT8 *)imOut->image[y];

            memcpy(out, in0, sizeof(UINT32));
            if (im->bands == 2) {
                for (x = 1; x < xsize - 1; x++) {
                    float ss0 = offset;
                    float ss3 = offset;
                    UINT32 v;
                    ss0 += KERNEL1x3(in1, x * 4 + 0, &kernel[0], 4);
                    ss3 += KERNEL1x3(in1, x * 4 + 3, &kernel[0], 4);
                    ss0 += KERNEL1x3(in0, x * 4 + 0, &kernel[3], 4);
                    ss3 += KERNEL1x3(in0, x * 4 + 3, &kernel[3], 4);
                    ss0 += KERNEL1x3(in_1, x * 4 + 0, &kernel[6], 4);
                    ss3 += KERNEL1x3(in_1, x * 4 + 3, &kernel[6], 4);
                    v = MAKE_UINT32(clip8(ss0), 0, 0, clip8(ss3));
                    memcpy(out + x * sizeof(v), &v, sizeof(v));
                }
            } else if (im->bands == 3) {
                for (x = 1; x < xsize - 1; x++) {
                    float ss0 = offset;
                    float ss1 = offset;
                    float ss2 = offset;
                    UINT32 v;
                    ss0 += KERNEL1x3(in1, x * 4 + 0, &kernel[0], 4);
                    ss1 += KERNEL1x3(in1, x * 4 + 1, &kernel[0], 4);
                    ss2 += KERNEL1x3(in1, x * 4 + 2, &kernel[0], 4);
                    ss0 += KERNEL1x3(in0, x * 4 + 0, &kernel[3], 4);
                    ss1 += KERNEL1x3(in0, x * 4 + 1, &kernel[3], 4);
                    ss2 += KERNEL1x3(in0, x * 4 + 2, &kernel[3], 4);
                    ss0 += KERNEL1x3(in_1, x * 4 + 0, &kernel[6], 4);
                    ss1 += KERNEL1x3(in_1, x * 4 + 1, &kernel[6], 4);
                    ss2 += KERNEL1x3(in_1, x * 4 + 2, &kernel[6], 4);
                    v = MAKE_UINT32(clip8(ss0), clip8(ss1), clip8(ss2), 0);
                    memcpy(out + x * sizeof(v), &v, sizeof(v));
                }
            } else if (im->bands == 4) {
                for (x = 1; x < xsize - 1; x++) {
                    float ss0 = offset;
                    float ss1 = offset;
                    float ss2 = offset;
                    float ss3 = offset;
                    UINT32 v;
                    ss0 += KERNEL1x3(in1, x * 4 + 0, &kernel[0], 4);
                    ss1 += KERNEL1x3(in1, x * 4 + 1, &kernel[0], 4);
                    ss2 += KERNEL1x3(in1, x * 4 + 2, &kernel[0], 4);
                    ss3 += KERNEL1x3(in1, x * 4 + 3, &kernel[0], 4);
                    ss0 += KERNEL1x3(in0, x * 4 + 0, &kernel[3], 4);
                    ss1 += KERNEL1x3(in0, x * 4 + 1, &kernel[3], 4);
                    ss2 += KERNEL1x3(in0, x * 4 + 2, &kernel[3], 4);
                    ss3 += KERNEL1x3(in0, x * 4 + 3, &kernel[3], 4);
                    ss0 += KERNEL1x3(in_1, x * 4 + 0, &kernel[6], 4);
                    ss1 += KERNEL1x3(in_1, x * 4 + 1, &kernel[6], 4);
                    ss2 += KERNEL1x3(in_1, x * 4 + 2, &kernel[6], 4);
                    ss3 += KERNEL1x3(in_1, x * 4 + 3, &kernel[6], 4);
                    v = MAKE_UINT32(clip8(ss0), clip8(ss1), clip8(ss2), clip8(ss3));
                    memcpy(out + x * sizeof(v), &v, sizeof(v));
                }
            }
            memcpy(out + x * sizeof(UINT32), in0 + x * sizeof(UINT32), sizeof(UINT32));
        }
    }
    memcpy(imOut->image[y], im->image[y], im->linesize);
}

static void
ImagingFilter5x5(Imaging imOut, Imaging im, const float *kernel, float offset) {
#define KERNEL1x5(in0, x, kernel, d)                                             \
    ((float)(in0[x - d - d]) * (kernel)[0] + (float)(in0[x - d]) * (kernel)[1] + \
     (float)(in0[x]) * (kernel)[2] + (float)(in0[x + d]) * (kernel)[3] +         \
     (float)(in0[x + d + d]) * (kernel)[4])

    int x = 0, y = 0;

    // restrict safety: assert im and imOut to be separate,
    // so we can safely `restrict` the copy pointers below
    assert(im->image != imOut->image);
    // Hoist for loop optimization.
    int xsize = im->xsize, ysize = im->ysize;

    memcpy(imOut->image[0], im->image[0], im->linesize);
    memcpy(imOut->image[1], im->image[1], im->linesize);
    if (im->bands == 1) {
        // Add one time for rounding
        offset += 0.5;
        if (im->type == IMAGING_TYPE_INT32) {
            for (y = 2; y < ysize - 2; y++) {
                INT32 *restrict in_2 = (INT32 *)im->image[y - 2];
                INT32 *restrict in_1 = (INT32 *)im->image[y - 1];
                INT32 *restrict in0 = (INT32 *)im->image[y];
                INT32 *restrict in1 = (INT32 *)im->image[y + 1];
                INT32 *restrict in2 = (INT32 *)im->image[y + 2];
                INT32 *restrict out = (INT32 *)imOut->image[y];

                out[0] = in0[0];
                out[1] = in0[1];
                for (x = 2; x < xsize - 2; x++) {
                    float ss = offset;
                    ss += KERNEL1x5(in2, x, &kernel[0], 1);
                    ss += KERNEL1x5(in1, x, &kernel[5], 1);
                    ss += KERNEL1x5(in0, x, &kernel[10], 1);
                    ss += KERNEL1x5(in_1, x, &kernel[15], 1);
                    ss += KERNEL1x5(in_2, x, &kernel[20], 1);
                    out[x] = clip32(ss);
                }
                out[x + 0] = in0[x + 0];
                out[x + 1] = in0[x + 1];
            }
        } else {
            int bigendian = 0;
            if (im->type == IMAGING_TYPE_SPECIAL) {
                if (
                    im->mode == IMAGING_MODE_I_16B
#ifdef WORDS_BIGENDIAN
                    || im->mode == IMAGING_MODE_I_16N
#endif
                ) {
                    bigendian = 1;
                }
            }
            for (y = 2; y < ysize - 2; y++) {
                UINT8 *restrict in_2 = (UINT8 *)im->image[y - 2];
                UINT8 *restrict in_1 = (UINT8 *)im->image[y - 1];
                UINT8 *restrict in0 = (UINT8 *)im->image[y];
                UINT8 *restrict in1 = (UINT8 *)im->image[y + 1];
                UINT8 *restrict in2 = (UINT8 *)im->image[y + 2];
                UINT8 *restrict out = (UINT8 *)imOut->image[y];

                out[0] = in0[0];
                out[1] = in0[1];
                if (im->type == IMAGING_TYPE_SPECIAL) {
                    out[2] = in0[2];
                    out[3] = in0[3];
                }
                for (x = 2; x < xsize - 2; x++) {
                    float ss = offset;
                    if (im->type == IMAGING_TYPE_SPECIAL) {
                        ss += kernel_i16(5, in2, x, &kernel[0], bigendian);
                        ss += kernel_i16(5, in1, x, &kernel[5], bigendian);
                        ss += kernel_i16(5, in0, x, &kernel[10], bigendian);
                        ss += kernel_i16(5, in_1, x, &kernel[15], bigendian);
                        ss += kernel_i16(5, in_2, x, &kernel[20], bigendian);
                        // NOT rounding here because `offset` already has a +0.5 bias.
                        int ss_int = clip16(ss);
                        out[x * 2 + (bigendian ? 1 : 0)] = (UINT8)(ss_int & 0xff);
                        out[x * 2 + (bigendian ? 0 : 1)] = (UINT8)(ss_int >> 8);
                    } else {
                        ss += KERNEL1x5(in2, x, &kernel[0], 1);
                        ss += KERNEL1x5(in1, x, &kernel[5], 1);
                        ss += KERNEL1x5(in0, x, &kernel[10], 1);
                        ss += KERNEL1x5(in_1, x, &kernel[15], 1);
                        ss += KERNEL1x5(in_2, x, &kernel[20], 1);
                        out[x] = clip8(ss);
                    }
                }
                if (im->type == IMAGING_TYPE_SPECIAL) {
                    out[x * 2 + 0] = in0[x * 2 + 0];
                    out[x * 2 + 1] = in0[x * 2 + 1];
                    out[x * 2 + 2] = in0[x * 2 + 2];
                    out[x * 2 + 3] = in0[x * 2 + 3];
                } else {
                    out[x + 0] = in0[x + 0];
                    out[x + 1] = in0[x + 1];
                }
            }
        }
    } else {
        // Add one time for rounding
        offset += 0.5;
        for (y = 2; y < ysize - 2; y++) {
            UINT8 *restrict in_2 = (UINT8 *)im->image[y - 2];
            UINT8 *restrict in_1 = (UINT8 *)im->image[y - 1];
            UINT8 *restrict in0 = (UINT8 *)im->image[y];
            UINT8 *restrict in1 = (UINT8 *)im->image[y + 1];
            UINT8 *restrict in2 = (UINT8 *)im->image[y + 2];
            UINT8 *restrict out = (UINT8 *)imOut->image[y];

            memcpy(out, in0, sizeof(UINT32) * 2);
            if (im->bands == 2) {
                for (x = 2; x < xsize - 2; x++) {
                    float ss0 = offset;
                    float ss3 = offset;
                    UINT32 v;
                    ss0 += KERNEL1x5(in2, x * 4 + 0, &kernel[0], 4);
                    ss3 += KERNEL1x5(in2, x * 4 + 3, &kernel[0], 4);
                    ss0 += KERNEL1x5(in1, x * 4 + 0, &kernel[5], 4);
                    ss3 += KERNEL1x5(in1, x * 4 + 3, &kernel[5], 4);
                    ss0 += KERNEL1x5(in0, x * 4 + 0, &kernel[10], 4);
                    ss3 += KERNEL1x5(in0, x * 4 + 3, &kernel[10], 4);
                    ss0 += KERNEL1x5(in_1, x * 4 + 0, &kernel[15], 4);
                    ss3 += KERNEL1x5(in_1, x * 4 + 3, &kernel[15], 4);
                    ss0 += KERNEL1x5(in_2, x * 4 + 0, &kernel[20], 4);
                    ss3 += KERNEL1x5(in_2, x * 4 + 3, &kernel[20], 4);
                    v = MAKE_UINT32(clip8(ss0), 0, 0, clip8(ss3));
                    memcpy(out + x * sizeof(v), &v, sizeof(v));
                }
            } else if (im->bands == 3) {
                for (x = 2; x < xsize - 2; x++) {
                    float ss0 = offset;
                    float ss1 = offset;
                    float ss2 = offset;
                    UINT32 v;
                    ss0 += KERNEL1x5(in2, x * 4 + 0, &kernel[0], 4);
                    ss1 += KERNEL1x5(in2, x * 4 + 1, &kernel[0], 4);
                    ss2 += KERNEL1x5(in2, x * 4 + 2, &kernel[0], 4);
                    ss0 += KERNEL1x5(in1, x * 4 + 0, &kernel[5], 4);
                    ss1 += KERNEL1x5(in1, x * 4 + 1, &kernel[5], 4);
                    ss2 += KERNEL1x5(in1, x * 4 + 2, &kernel[5], 4);
                    ss0 += KERNEL1x5(in0, x * 4 + 0, &kernel[10], 4);
                    ss1 += KERNEL1x5(in0, x * 4 + 1, &kernel[10], 4);
                    ss2 += KERNEL1x5(in0, x * 4 + 2, &kernel[10], 4);
                    ss0 += KERNEL1x5(in_1, x * 4 + 0, &kernel[15], 4);
                    ss1 += KERNEL1x5(in_1, x * 4 + 1, &kernel[15], 4);
                    ss2 += KERNEL1x5(in_1, x * 4 + 2, &kernel[15], 4);
                    ss0 += KERNEL1x5(in_2, x * 4 + 0, &kernel[20], 4);
                    ss1 += KERNEL1x5(in_2, x * 4 + 1, &kernel[20], 4);
                    ss2 += KERNEL1x5(in_2, x * 4 + 2, &kernel[20], 4);
                    v = MAKE_UINT32(clip8(ss0), clip8(ss1), clip8(ss2), 0);
                    memcpy(out + x * sizeof(v), &v, sizeof(v));
                }
            } else if (im->bands == 4) {
                for (x = 2; x < xsize - 2; x++) {
                    float ss0 = offset;
                    float ss1 = offset;
                    float ss2 = offset;
                    float ss3 = offset;
                    UINT32 v;
                    ss0 += KERNEL1x5(in2, x * 4 + 0, &kernel[0], 4);
                    ss1 += KERNEL1x5(in2, x * 4 + 1, &kernel[0], 4);
                    ss2 += KERNEL1x5(in2, x * 4 + 2, &kernel[0], 4);
                    ss3 += KERNEL1x5(in2, x * 4 + 3, &kernel[0], 4);
                    ss0 += KERNEL1x5(in1, x * 4 + 0, &kernel[5], 4);
                    ss1 += KERNEL1x5(in1, x * 4 + 1, &kernel[5], 4);
                    ss2 += KERNEL1x5(in1, x * 4 + 2, &kernel[5], 4);
                    ss3 += KERNEL1x5(in1, x * 4 + 3, &kernel[5], 4);
                    ss0 += KERNEL1x5(in0, x * 4 + 0, &kernel[10], 4);
                    ss1 += KERNEL1x5(in0, x * 4 + 1, &kernel[10], 4);
                    ss2 += KERNEL1x5(in0, x * 4 + 2, &kernel[10], 4);
                    ss3 += KERNEL1x5(in0, x * 4 + 3, &kernel[10], 4);
                    ss0 += KERNEL1x5(in_1, x * 4 + 0, &kernel[15], 4);
                    ss1 += KERNEL1x5(in_1, x * 4 + 1, &kernel[15], 4);
                    ss2 += KERNEL1x5(in_1, x * 4 + 2, &kernel[15], 4);
                    ss3 += KERNEL1x5(in_1, x * 4 + 3, &kernel[15], 4);
                    ss0 += KERNEL1x5(in_2, x * 4 + 0, &kernel[20], 4);
                    ss1 += KERNEL1x5(in_2, x * 4 + 1, &kernel[20], 4);
                    ss2 += KERNEL1x5(in_2, x * 4 + 2, &kernel[20], 4);
                    ss3 += KERNEL1x5(in_2, x * 4 + 3, &kernel[20], 4);
                    v = MAKE_UINT32(clip8(ss0), clip8(ss1), clip8(ss2), clip8(ss3));
                    memcpy(out + x * sizeof(v), &v, sizeof(v));
                }
            }
            memcpy(
                out + x * sizeof(UINT32), in0 + x * sizeof(UINT32), sizeof(UINT32) * 2
            );
        }
    }
    memcpy(imOut->image[y], im->image[y], im->linesize);
    memcpy(imOut->image[y + 1], im->image[y + 1], im->linesize);
}

Imaging
ImagingFilter(Imaging im, int xsize, int ysize, const FLOAT32 *kernel, FLOAT32 offset) {
    Imaging imOut;
    ImagingSectionCookie cookie;

    if (im->type == IMAGING_TYPE_FLOAT32 ||
        (im->type == IMAGING_TYPE_SPECIAL && im->bands != 1)) {
        return (Imaging)ImagingError_ModeError();
    }

    if (im->xsize < xsize || im->ysize < ysize) {
        return ImagingCopy(im);
    }

    if ((xsize != 3 && xsize != 5) || xsize != ysize) {
        return (Imaging)ImagingError_ValueError("bad kernel size");
    }

    imOut = ImagingNewDirty(im->mode, im->xsize, im->ysize);
    if (!imOut) {
        return NULL;
    }

    ImagingSectionEnter(&cookie);
    if (xsize == 3) {
        /* 3x3 kernel. */
        ImagingFilter3x3(imOut, im, kernel, offset);
    } else {
        /* 5x5 kernel. */
        ImagingFilter5x5(imOut, im, kernel, offset);
    }
    ImagingSectionLeave(&cookie);
    return imOut;
}
