import numpy as np
import cv2

def conv(image, kernel, mode='same'):

    if mode == 'fill':  #选择是否进行边缘填充
        h = kernel.shape[0] // 2   #卷积核的列除以2取整
        w = kernel.shape[1] // 2   #卷积核的行除以2取整
        #在原始图像边缘进行填充，常数填充，填数值0，假设原始图像600*600，卷积核大小5*5，则填充后图像大小604*604
        image = np.pad(image, ((h, h), (w, w), (0, 0)), 'constant')  

    #进行卷积运算
    # conv_b = _convolve(image[:, :, 0], kernel)
    # conv_g = _convolve(image[:, :, 1], kernel)
    # conv_r = _convolve(image[:, :, 2], kernel)
    # res = np.dstack([conv_b, conv_g, conv_r])
    res = _convolve(image, kernel)
    return res


def _convolve(image, kernel):
    h_kernel, w_kernel = kernel.shape  #获取卷积核的长宽，也就是行数和列数

    h_image, w_image = image.shape   #获取欲处理图片的长宽

    #计算卷积核中心点开始运动的点，因为图片边缘不能为空
    res_h = h_image - h_kernel + 1
    res_w = w_image - w_kernel + 1

    #生成一个0矩阵，用于保存处理后的图片
    res = np.zeros((res_h, res_w), np.uint8)

    for i in range(res_h):   #行
        for j in range(res_w):   #列
            #image处传入的是一个与卷积核一样大小矩阵，这个矩阵取自于欲处理图片的一部分
            #这个矩阵与卷核进行运算，用i与j来进行卷积核滑动
            res[i, j] = normal(image[i:i + h_kernel, j:j + w_kernel], kernel) 

    return res

def normal(image, kernel):
    #np.multiply()函数：数组和矩阵对应位置相乘，输出与相乘数组/矩阵的大小一致（点对点相乘）
    
    res = np.multiply(image, kernel).sum() #点对点相乘后进行累加
    if res > 255:
        return 255
    elif res<0:
        return 0
    else:
        return res


class DLGMD_class():
    def __init__(self):
        self.framegap = 2
        self.frame_cnt = 0
        self.img_t_2 = None
        self.img_t_1 = None
        self.img_t_0 = None
        self.DNS_Cube=[0,0,0,0,0,0,0,0,0,0]
        self.args_kernal_G = np.array([
                            [1,	1,	1],
                            [1,	1,	1],
                            [1,	1,	1]
        ])

        self.args_kernel_E = np.array([
        [0,	0,	0,	0,	0,	0,	0],
        [0,	1,	3,	0,	3,	1,	0],
        [0,	3,	4,	12,	4,	3,	0],
        [0,	0,	12,	27,	12,	0,	0],
        [0,	3,	4,	12,	4,	3,	0],
        [0,	1,	3,	0,	3,	1,	0],
        [0,	0,	0,	0,	0,	0,	0]
        ])
        self.args_kernel_I = np.array([
        [3,	5,	6,	6,	6,	5,	3],
        [5,	7,	9,	0,	9,	7,	5],
        [6,	9,	0,	0,	0,	9,	6],
        [6,	0,	0,	0,	0,	0,	6],
        [6,	9,	0,	0,	0,	9,	6],
        [5,	7,	9,	0,	9,	7,	5],
        [3,	5,	6,	6,	6,	5,	3]
        ])

    def init(self, first_image):
        print("Initializing LGMD filter with sensor size:", first_image.shape)

        self.resolution = first_image.shape  # The resolution of the image

        # Allocations
        if self.img_t_0 is not None:
            self.img_t_1 = self.img_t_0.copy()
        if self.img_t_1 is not None:
            self.img_t_2 = self.img_t_1.copy()

        self.img_t_0 = first_image.copy()
        self.LGMD_OutputMP = 0
        self.LGMD_OutputFFI = 0

    def LGMD_callback(self, new_image):
        self.frame_cnt += 1 
        if self.frame_cnt <3:
            self.init(new_image)
            return None, None
        
        np.copyto(self.img_t_0, new_image)

        diff_t_1 = cv2.absdiff(self.img_t_0,self.img_t_1)
        diff_t_2 = cv2.absdiff(self.img_t_1,self.img_t_2)
        


        np.copyto(self.img_t_2, self.img_t_1)
        np.copyto(self.img_t_1, self.img_t_0)

        E = conv(diff_t_1, self.args_kernel_E, 'same')
        I = conv(diff_t_2, self.args_kernel_I, 'same')
        S = cv2.absdiff(E,I)

        for i in range (0,10):
            self.DNS_Cube[i] = S[i*48 : (i+1)*48-1, :].sum()

        maxLocation = self.DNS_Cube.index(max(self.DNS_Cube))

        return S, self.DNS_Cube