import cv2
import numpy as np
import time
import serial

# ------------------------------参数集合------------------------------
trackbar_default = 250  # 滚动条灰度默认值
distance_list_len = 10  # 距离列表判定长度
distance_max = 350  # 距离上限阈值，灯光亮的图像帧数
distance_min = 30  # 距离下限阈值，灯光灭的图像帧数

# ------------------------------串口信号------------------------------
ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=0.5)

# ------------------------------创建窗口------------------------------
windowname = 'frame'
cv2.namedWindow(windowname)


# ------------------------------创建滑动条------------------------------
#为实现不同环境下阈值的调整，使用的滚动条调节阈值
def p(x):
    pass


cv2.createTrackbar('threshold', windowname, trackbar_default, 255, p)


# ------------------------------图像处理函数------------------------------
#入口参数：frame 摄像头读取的某帧图像，是个三维矩阵
#返回参数：binary处理后的图像，（x，y）是光源中心坐标
def Image_processing(frame):
    # 下采样并取单通道         （取第2通道，帧尺寸为500×500）
    single = cv2.resize(frame[:, :, 2], (500, 500))
    # 画面旋转                （得到以坐标（250，250）为旋转中心，旋转90°，缩放比例为1的旋转矩阵mat，使用仿射变换函数实现旋转）
    mat = cv2.getRotationMatrix2D((250, 250), 90, 1)
    single = cv2.warpAffine(single, mat, (500, 500))
    # 去噪 高斯模糊平滑处理      （高斯核尺寸（21，21），越大越模糊）
    gaus = cv2.GaussianBlur(single, ksize=(21, 21), sigmaX=0)
    # 降噪 形态学运算——开运算    （开运算先腐蚀后膨胀，用于消除毛刺亮点放大缝隙。OPEN开运算，核函数5×5全1矩阵，迭代次数3）
    open = cv2.morphologyEx(gaus, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8), iterations=3)
    # 二值处理                （由滚动条调节二值处理的阈值threshold；低于threshold的像素点被置为0黑色，高于阈值threshhold的像素点被置为255白色)
    threshold = cv2.getTrackbarPos('threshold', windowname)
    r, binary = cv2.threshold(open, threshold, 255, cv2.THRESH_BINARY)
    # 查找光源最大轮廓        （mode检索外轮廓；method获取每个轮廓的每个像素，相邻的两个点的像素位置差不超过1；输出第二个参数contours，检索到的轮廓数，类型为列表）
    contours = cv2.findContours(image=binary, mode=cv2.RETR_EXTERNAL, method=cv2.CHAIN_APPROX_NONE)[-2]
    x, y = 0, 0
    if len(contours):
        area = []
        #检测到的所有轮廓的面积存放area，并找到最大面积的轮廓的索引max_idx
        for i in range(len(contours)):
            area.append(cv2.contourArea(contours[i]))
            max_idx = np.argmax(np.array(area))
        # 寻找中心  m00所有白色区域的像素值的和，m10所有白色区域像素的x坐标的和，m00所有白色区域像素的y坐标的和
        M = cv2.moments(contours[max_idx])
        if M['m00'] != 0:
            (x, y) = int(M['m10'] / M['m00']), int(M['m01'] / M['m00'])
    #绘制中心
    cv2.circle(binary, (x, y), 10, (0, 0, 225))
    return binary, x, y


# ------------------------------状态判定函数(StatesIdentification)------------------------------
#入口参数：tt距离数据空列表，state空状态，（xb，yb）前一帧光源中心点，（xn，yn）当前帧光源中心点
#返回参数：tt距离判定列表（10个距离数据），state表示LED灯的状态，（xn，yn）状态判定结束后的光源中心坐标

def SI(tt, state, xb, yb, xn, yn):
    #前一帧光源中心与当前帧光源中心的欧氏距离
    distance_new = ((xb - xn) ** 2 + (yb - yn) ** 2) ** 0.5
    tt.append(distance_new)
    if len(tt) == distance_list_len:
        high, low, zero = 0, 0, 0
        for i in range(distance_list_len):
            if tt[i] > distance_max:
                high += 1
            elif 0 < tt[i] < distance_min:
                low += 1
            elif tt[i] == 0:
                zero += 1
        #当满足帧率条件且经过几次闪烁周期，则判断为闪烁灯光
        if high == distance_list_len and state != 'twinkle':
            state = 'twinkle'
            print('检测到闪烁状态!')
        elif xn != 0 and xb == xn and state != 'light':
            state = 'light'
            print('检测到常亮状态!')
        elif xn == 0 and state != 'nolight':
            state = 'nolight'
            print('未检测到目标!')
        tt = []
    return tt, state, xn, yn


# ------------------------------指令控制函数------------------------------
#入口参数：（xn，yn）当前帧光源中心坐标，bs前一帧运动状态
#返回参数：bs当前帧运动状态

def Control_Order(xn, yn, bs):
    if state == 'light':
        #（250，250）为树莓派单目摄像头视觉中心坐标，xn<200表示灯源在左侧，xn>300表示灯源在右侧
        if xn < 200 and bs != 3:  # 左转
            ser.write('W!5!-05!70!060'.encode())
            time.sleep(0.2)
            bs = 3
        elif xn > 300 and bs != 4:  # 右转
            ser.write('W!5!+05!70!060'.encode())
            time.sleep(0.2)
            bs = 4
        elif bs != 1:  # 前进
            ser.write('W!15!+00!00!060'.encode())
            time.sleep(0.2)
            bs = 1
    else:
        ser.write('W!00!+00!00!060'.encode())
    return bs


if __name__ == '__main__':
    cap = cv2.VideoCapture(0)
    x_before, y_before = 0, 0
    distance_list = []
    state = ''
    bs = None
    while cap.isOpened():
        # read()按帧读取，ret是布尔值（读取帧正确则返回True），frame是每一帧的图像，是个三维矩阵
        ret, frame = cap.read()
        if ret:
            binary, x_new, y_new = Image_processing(frame)
            distance_list, state, x_before, y_before = SI(distance_list, state, x_before, y_before, x_new, y_new)
            bs = Control_Order(x_new, y_new, bs)
            cv2.imshow(windowname, binary)
            #waitKey(1),表示按下键盘q，延时1ms切换到下一帧图像，对于视频而言
            if cv2.waitKey(1) == ord('q'):
                break
    cv2.destroyAllWindows()
    cap.release()