"""
@file:    border_detect_2023e.py
@author:  榴莲派工作室(LLP STUDIO)SVP Peng
@version: V1.0
@date:    2023-08-02
@brief:   识别黑色矩形边框并返回4个顶点坐标
"""

import sensor
import image
import lcd
import gc
import utime
from fpioa_manager import fm
from machine import UART

start_time = utime.ticks_ms()
switch_time = start_time + 15000
fm.register(9, fm.fpioa.UART1_TX, force=True)
fm.register(10, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 115200, 8, 1, 0, timeout=1000, read_buf_len=4096)
lcd.init()
lcd.rotation(2)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)    # 160 * 120
sensor.set_auto_exposure(False, 50000)    # 调节曝光度以看清激光点
#sensor.set_auto_whitebal(True)
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)    # 关闭增益用于色块识别
sensor.set_brightness(3)
sensor.skip_frames(time=2000)

#RED_THRESHOLD = [(55, 96, 7, 17, -2, 4)]
RED_THRESHOLD = [(68, 88, 16, 40, 3, 15)]
#RED_THRESHOLD = [(56, 255, 45, 127, -128, 127)]
data = bytearray()
cx = 0
cy = 0
bottom_leftx = 0
bottom_lefty = 0
bottom_rightx = 0
bottom_righty = 0
top_rightx = 0
top_righty = 0
top_leftx = 0
top_lefty = 0
cnt = 0
pre_center = None
TH_DISTANCE = 100    # 中心偏差阈值的平方


def find_max(blobs):
    """寻找目标色块中最大的色块

    Args:
        blobs: 目标色块群

    Returns:
        一个blob类对象，返回目标色块群中的最大色块
    """
    max_size = 0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob


while True:
    current_time = utime.ticks_ms()
    img = sensor.snapshot()
    #img.rotation_corr(z_rotation=90.0)
    if (cnt < 5) and (current_time <= switch_time):
        # 在图像中寻找矩形
        for r in img.find_rects(threshold=10000):
            # 判断矩形边长是否符合要求
            if r.w() > 20 and r.h() > 20:
                center = (r.x() + r.w() // 2, r.y() + r.h() // 2)
                if pre_center != None:
                    distance = (center[0] - pre_center[0]) ** 2 + (center[1] - pre_center[1]) ** 2
                    if distance <= TH_DISTANCE:
                        cnt += 1
                        pre_center = center
                else:
                    pre_center = center
                # 在屏幕上框出矩形
                img.draw_rectangle(r.rect(), color=(0, 0, 255))
                # 获取矩形角点位置
                corner = r.corners()
                bottom_leftx = corner[0][0]
                bottom_lefty = corner[0][1]
                bottom_rightx = corner[1][0]
                bottom_righty = corner[1][1]
                top_rightx = corner[2][0]
                top_righty = corner[2][1]
                top_leftx = corner[3][0]
                top_lefty = corner[3][1]
                #print(corner)
                # 在屏幕上圈出矩形角点
                for c in corner:
                    img.draw_circle(c[0], c[1], 5, color=(0, 255, 0), thickness=2, fill=False)
                    img.draw_string(c[0], c[1], "{},{}".format(c[0], c[1]), color=(36, 255, 167))
                    #print(c[0], c[1])

    #elif (cnt < 5) and (current_time > switch_time):
        #sensor.set_framesize(sensor.QVGA)

    else:
        sensor.set_framesize(sensor.QVGA)
        blobs = img.find_blobs(RED_THRESHOLD, x_stride=1, y_stride=1, area_threshold=0,
            pixels_threshold=0, merge=False, margin=1)
        if blobs:
                blob = find_max(blobs)
                img.draw_rectangle(blob[0:4])
                cx = blob[5]
                cy = blob[6]
                #print(cx, cy)
                img.draw_cross(cx, cy, color=(0, 255, 0))
                img.draw_string(cx, cy, "{},{}".format(cx, cy), color=255)

    # 发送数据帧
    data = bytearray([0XA3, 0XB3, int(cx > 255), int(cy > 255), cx, cy, int(top_leftx > 255),
        int(top_lefty > 255), int(top_rightx > 255), int(top_righty > 255), int(bottom_rightx > 255),
        int(bottom_righty > 255), int(bottom_leftx > 255), int(bottom_lefty > 255), top_leftx,
        top_lefty, top_rightx, top_righty, bottom_rightx, bottom_righty, bottom_leftx, bottom_lefty,
        0X0D, 0X0A])
    #print(data)
    uart_A.write(data)
    lcd.display(img)
gc.collect()