#define JOIN2(a, b) a ## _ ## b
#define FUNC2(name, stencil) JOIN2(name, stencil)
#define FUNC(name) FUNC2(name, STENCIL)

void FUNC(interp2d)(const double *src_start, const double *src_step,
                    const double *src_val,
                    const int *src_stride, const int *src_len,
                    int nb_points, const double *x_vals, const double *y_vals,
                    double *dst)
{
    for (int pt = 0; pt < nb_points; pt++) {
        const double x = x_vals[pt];
        const double y = y_vals[pt];

        const int    idx_x = (x - src_start[0]) / src_step[0] - (STENCIL / 2 - 1);
        const int    idx_y = (y - src_start[1]) / src_step[1] - (STENCIL / 2 - 1);

        const double src_x = idx_x * src_step[0] + src_start[0];
        const double src_y = idx_y * src_step[1] + src_start[1];

        double fact[2][STENCIL];

        double ret = 0.0;

        if (idx_x < 0 || idx_x > src_len[0] - STENCIL ||
            idx_y < 0 || idx_y > src_len[1] - STENCIL) {
            ret = NAN;
            goto output;
        }

        for (int i = 0; i < STENCIL; i++) {
            double fact_x = 1.0;
            double fact_y = 1.0;

            for (int j = 0; j < STENCIL; j++) {
                if (i == j)
                    continue;

                fact_x *= (x - (src_x + j * src_step[0])) / (src_step[0] * (i - j));
                fact_y *= (y - (src_y + j * src_step[1])) / (src_step[1] * (i - j));
            }

            fact[0][i] = fact_x;
            fact[1][i] = fact_y;
        }

        for (int j = 0; j < STENCIL; j++) {
            double val = 0.0;

            for (int i = 0; i < STENCIL; i++)
                val += fact[0][i] * src_val[(idx_y + j) * src_stride[1] + (idx_x + i) * src_stride[0]];

            ret += fact[1][j] * val;
        }
output:
        dst[pt] = ret;
    }
}