cimport c_numpy
cimport c_python
import numpy, sys

c_numpy.import_array()

cdef extern from 'math.h':
    cdef double fabs(double)

cdef extern from 'sys/param.h':
    cdef int MIN(int, int)
    cdef int MAX(int, int)    

cdef double calc_sad(c_numpy.ndarray ablock, int rs, int cs, c_numpy.ndarray refblock):
    cdef double sad
    cdef int i, j
    cdef char *p_a, *p_ref
    
    sad = 0.0
    for i from 0 <= i < refblock.dimensions[0]:
        p_a = ablock.data + (rs+i) * ablock.strides[0] + cs * ablock.strides[1]
        p_ref = refblock.data + i * refblock.strides[0]
        
        for j from 0 <= j < refblock.dimensions[1]:
            sad = sad + fabs((<double*>p_a)[0] - (<double*>p_ref)[0])
            p_a = p_a + ablock.strides[1]
            p_ref = p_ref + refblock.strides[1]

    return sad

def me(c_numpy.ndarray a, c_numpy.ndarray refblock, int rc, int cc, int sr):
    cdef int rs, rs1, rs2, cs, cs1, cs2
    cdef int min_r, min_c
    cdef double sad, min_sad
    cdef int bs
    
    bs = refblock.dimensions[0]
    min_sad = sys.maxint
    min_r, min_c = 0, 0

    rs1 = MAX(0,rc-sr)
    rs2 = MIN(a.dimensions[0]-bs,rc+sr)+1
    
    cs1 = MAX(0,cc-sr)
    cs2 = MIN(cc+sr,a.dimensions[1]-bs)+1
    
    for rs from rs1 <= rs < rs2:
        for cs from cs1 <= cs < cs2:
            sad = calc_sad(a, rs, cs, refblock)
            if sad < min_sad:
                # found new local block SAD minimum, store motion vector
                min_r, min_c, min_sad = rs, cs, sad
        
    return min_r, min_c, min_sad