#cython: cdivision=True
#cython: boundscheck=False
#cython: nonecheck=False
#cython: wraparound=False
cimport numpy as np
from libc.math cimport ceil, floor


ctypedef fused IMAGE_TYPES:
    float
    double
    np.uint8_t
    np.uint16_t


cdef inline Py_ssize_t round(IMAGE_TYPES r):
    return <Py_ssize_t>((r + 0.5) if (r > 0.0) else (r - 0.5))


cdef inline IMAGE_TYPES nearest_neighbour_interpolation(IMAGE_TYPES* image,
                                                        Py_ssize_t rows,
                                                        Py_ssize_t cols,
                                                        double r,
                                                        double c,
                                                        char mode,
                                                        double cval):
    """Nearest neighbour interpolation at a given position in the image.

    Parameters
    ----------
    image : double array
        Input image.
    rows, cols : int
        Shape of image.
    r, c : double
        Position at which to interpolate.
    mode : {'C', 'W', 'R', 'N'}
        Wrapping mode. Constant, Wrap, Reflect or Nearest.
    cval : double
        Constant value to use for constant mode.

    Returns
    -------
    value : double
        Interpolated value.
    """

    return get_pixel2d(image, rows, cols, round(r), round(c), mode, cval)


cdef inline IMAGE_TYPES bilinear_interpolation(IMAGE_TYPES* image,
                                               Py_ssize_t rows,
                                               Py_ssize_t cols,
                                               double r, double c,
                                               char mode, double cval):
    """Bilinear interpolation at a given position in the image.

    Parameters
    ----------
    image : double array
        Input image.
    rows, cols : int
        Shape of image.
    r, c : double
        Position at which to interpolate.
    mode : {'C', 'W', 'R', 'N'}
        Wrapping mode. Constant, Wrap, Reflect or Nearest.
    cval : double
        Constant value to use for constant mode.

    Returns
    -------
    value : double
        Interpolated value.
    """
    cdef double dr, dc, top, bottom
    cdef Py_ssize_t minr, minc, maxr, maxc

    minr = <Py_ssize_t>floor(r)
    minc = <Py_ssize_t>floor(c)
    maxr = <Py_ssize_t>ceil(r)
    maxc = <Py_ssize_t>ceil(c)

    dr = r - minr
    dc = c - minc
    top = (1 - dc) * get_pixel2d(image, rows, cols, minr, minc, mode, cval) \
          + dc * get_pixel2d(image, rows, cols, minr, maxc, mode, cval)
    bottom = (1 - dc) * get_pixel2d(image, rows, cols, maxr, minc, mode, cval) \
             + dc * get_pixel2d(image, rows, cols, maxr, maxc, mode, cval)

    return <IMAGE_TYPES>((1 - dr) * top + dr * bottom)


cdef inline double quadratic_interpolation(double x, double[3] f):
    """Quadratic interpolation.

    Parameters
    ----------
    x : double
        Position in the interval [-1, 1].
    f : double[4]
        Function values at positions [-1, 0, 1].

    Returns
    -------
    value : double
        Interpolated value.
    """
    return f[1] - 0.25 * (f[0] - f[2]) * x


cdef inline IMAGE_TYPES biquadratic_interpolation(IMAGE_TYPES* image,
                                                  Py_ssize_t rows,
                                                  Py_ssize_t cols,
                                                  double r, double c,
                                                  char mode, double cval):
    """Biquadratic interpolation at a given position in the image.

    Parameters
    ----------
    image : double array
        Input image.
    rows, cols : int
        Shape of image.
    r, c : double
        Position at which to interpolate.
    mode : {'C', 'W', 'R', 'N'}
        Wrapping mode. Constant, Wrap, Reflect or Nearest.
    cval : double
        Constant value to use for constant mode.

    Returns
    -------
    value : double
        Interpolated value.
    """

    cdef Py_ssize_t r0 = round(r)
    cdef Py_ssize_t c0 = round(c)
    if r < 0:
        r0 -= 1
    if c < 0:
        c0 -= 1
    # scale position to range [-1, 1]
    cdef double xr = (r - r0) - 1
    cdef double xc = (c - c0) - 1
    if r == r0:
        xr += 1
    if c == c0:
        xc += 1

    cdef double fc[3]
    cdef double fr[3]

    cdef Py_ssize_t pr, pc

    # row-wise cubic interpolation
    for pr in range(r0, r0 + 3):
        for pc in range(c0, c0 + 3):
            fc[pc - c0] = get_pixel2d(image, rows, cols, pr, pc, mode, cval)
        fr[pr - r0] = quadratic_interpolation(xc, fc)

    # cubic interpolation for interpolated values of each row
    return <IMAGE_TYPES>quadratic_interpolation(xr, fr)


cdef inline double cubic_interpolation(double x, double[4] f):
    """Cubic interpolation.

    Parameters
    ----------
    x : double
        Position in the interval [0, 1].
    f : double[4]
        Function values at positions [0, 1/3, 2/3, 1].

    Returns
    -------
    value : double
        Interpolated value.
    """
    return \
        f[1] + 0.5 * x * \
            (f[2] - f[0] + x * \
                (2.0 * f[0] - 5.0 * f[1] + 4.0 * f[2] - f[3] + x * \
                    (3.0 * (f[1] - f[2]) + f[3] - f[0])))


cdef inline IMAGE_TYPES bicubic_interpolation(IMAGE_TYPES* image,
                                              Py_ssize_t rows, Py_ssize_t cols,
                                              double r, double c,
                                              char mode, double cval):
    """Bicubic interpolation at a given position in the image.

    Parameters
    ----------
    image : double array
        Input image.
    rows, cols : int
        Shape of image.
    r, c : double
        Position at which to interpolate.
    mode : {'C', 'W', 'R', 'N'}
        Wrapping mode. Constant, Wrap, Reflect or Nearest.
    cval : double
        Constant value to use for constant mode.

    Returns
    -------
    value : double
        Interpolated value.
    """

    cdef Py_ssize_t r0 = <Py_ssize_t>r - 1
    cdef Py_ssize_t c0 = <Py_ssize_t>c - 1
    if r < 0:
        r0 -= 1
    if c < 0:
        c0 -= 1
    # scale position to range [0, 1]
    cdef double xr = (r - r0) / 3
    cdef double xc = (c - c0) / 3

    cdef double fc[4]
    cdef double fr[4]

    cdef Py_ssize_t pr, pc

    # row-wise cubic interpolation
    for pr in range(r0, r0 + 4):
        for pc in range(c0, c0 + 4):
            fc[pc - c0] = get_pixel2d(image, rows, cols, pr, pc, mode, cval)
        fr[pr - r0] = cubic_interpolation(xc, fc)

    # cubic interpolation for interpolated values of each row
    return <IMAGE_TYPES>cubic_interpolation(xr, fr)


cdef inline IMAGE_TYPES get_pixel2d(IMAGE_TYPES* image, Py_ssize_t rows,
                                    Py_ssize_t cols, Py_ssize_t r, Py_ssize_t c,
                                    char mode, double cval):
    """Get a pixel from the image, taking wrapping mode into consideration.

    Parameters
    ----------
    image : double array
        Input image.
    rows, cols : int
        Shape of image.
    r, c : int
        Position at which to get the pixel.
    mode : {'C', 'W', 'R', 'N'}
        Wrapping mode. Constant, Wrap, Reflect or Nearest.
    cval : double
        Constant value to use for constant mode.

    Returns
    -------
    value : double
        Pixel value at given position.
    """
    if mode == 'C':
        if (r < 0) or (r > rows - 1) or (c < 0) or (c > cols - 1):
            return <IMAGE_TYPES>cval
        else:
            return image[r * cols + c]
    else:
        return image[coord_map(rows, r, mode) * cols + coord_map(cols, c, mode)]


cdef inline Py_ssize_t coord_map(Py_ssize_t dim, Py_ssize_t coord, char mode):
    """
    Wrap a coordinate, according to a given mode.

    Parameters
    ----------
    dim : int
        Maximum coordinate.
    coord : int
        Coord provided by user.  May be < 0 or > dim.
    mode : {'W', 'R', 'N'}
        Whether to wrap or reflect the coordinate if it
        falls outside [0, dim).
    """
    dim = dim - 1
    if mode == 'R': # reflect
        if coord < 0:
            # How many times times does the coordinate wrap?
            if <Py_ssize_t>(-coord / dim) % 2 != 0:
                return dim - <Py_ssize_t>(-coord % dim)
            else:
                return <Py_ssize_t>(-coord % dim)
        elif coord > dim:
            if <Py_ssize_t>(coord / dim) % 2 != 0:
                return <Py_ssize_t>(dim - (coord % dim))
            else:
                return <Py_ssize_t>(coord % dim)
    elif mode == 'W': # wrap
        if coord < 0:
            return <Py_ssize_t>(dim - (-coord % dim))
        elif coord > dim:
            return <Py_ssize_t>(coord % dim)
    elif mode == 'N': # nearest
        if coord < 0:
            return 0
        elif coord > dim:
            return dim

    return coord
