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 Obstacle():
    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
        

    def init(self, first_image):
        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()

        self.img_t_0 = first_image.copy()
        self.obstacle_height = 0

    def Diff_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)

        index = np.where(diff_t_1>250)
        Location_height = np.mean(index[0])

        np.copyto(self.img_t_1, self.img_t_0)
        return Location_height