import numpy as np

def interp1d(src_start, src_step, src_val, dst_coords, stencil):
    idx_src   = (dst_coords / src_step - src_start - (stencil / 2 - 1)).astype(np.int)
    src_coord = np.linspace(src_start, src_start + src_step * (src_val.shape[0] - 1),
                            src_val.shape[0], dtype = src_val.dtype)

    fact = np.zeros((stencil, ) + idx_src.shape, dtype = src_val.dtype) + 1.0
    for i in range(stencil):
        for j in range(stencil):
            if i == j:
                continue

            fact[i] *= (dst_coords - src_coord[idx_src + j]) / (src_coord[idx_src + i] - src_coord[idx_src + j])

    ret = np.zeros_like(dst_coords, dtype = src_val.dtype)
    for i in range(stencil):
        ret += fact[i] * src_val[idx_src + i]

    return ret

def interp2d(src_start, src_step, src_val, dst_coords, stencil):
    ret = np.zeros_like(dst_coords[0], dtype = src_val.dtype)

    idx_src = []
    fact    = []
    for dim in range(len(src_start)):
        src_coord = np.linspace(src_start[dim], src_start[dim] + src_step[dim] * (src_val.shape[dim] - 1),
                                src_val.shape[dim], dtype = src_val.dtype)

        idx = ((dst_coords[dim] - src_start[dim]) / src_step[dim] - (stencil / 2 - 1)).astype(np.int)
        f   = np.zeros((stencil, ) + idx.shape, dtype = src_val.dtype) + 1.0


        for i in range(stencil):
            for j in range(stencil):
                if i == j:
                    continue

                f[i] *= (dst_coords[dim] - src_coord[idx + j]) / (src_coord[idx + i] - src_coord[idx + j])

        idx_src.append(idx)
        fact.append(f)

    for i in range(stencil):
        val = np.zeros_like(ret)
        for j in range(stencil):
            val += fact[1][j] * src_val[idx_src[0] + i, idx_src[1] + j]

        ret += fact[0][i] * val

    return ret