"""
@file:    laser_detect_2023e.py
@author:  榴莲派工作室(LLP STUDIO)SVP Peng
@version: V1.0
@date:    2023-08-02
@brief:   识别红绿两种激光点
"""

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

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.QVGA)
sensor.skip_frames(time=900)
sensor.set_auto_exposure(False, 2000)    # 在这里调节曝光度，调节完可以比较清晰地看清激光点
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)    # 关闭增益（色块识别时必须要关）

RED_THRESHOLD = [(55, 96, 7, 17, -2, 4)]
#GREEN_THRESHOLD = [(64, 98, -17, -8, -3, 7)]
#GREEN_THRESHOLD = [(58, 98, -25, -11, 2, 15)]
GREEN_THRESHOLD = [(73, 98, -47, -14, 13, 41)]
RED = (255, 0, 0)
GREEN = (0, 255, 0)


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


def laser_dot_detect(threshold, color):
    """追踪激光点位置并返回其坐标

    Args:
        threshold: 颜色阈值
        color:     屏幕字符串颜色

    Returns:
        一个tuple
        如果找到了激光点，返回其坐标(x, y)
        如果没有找到激光点，返回(0, 0)
    """
    blobs = img.find_blobs(threshold, x_stride=1, y_stride=1, area_threshold=20,
    pixels_threshold=15, 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=color)
        img.draw_string(cx, cy, "{},{}".format(cx, cy), color=255)
        return (int(cx), int(cy))
    else:
        return (0, 0)


while True:
    img = sensor.snapshot()
    img.rotation_corr(z_rotation=90.0)
    #(rcx, rcy) = laser_dot_detect(RED_THRESHOLD, RED)
    (gcx, gcy) = laser_dot_detect(GREEN_THRESHOLD, GREEN)
    print(gcx, gcy)
    # 12位数据帧，帧头2位，红色中心进位2位，红色中心(rcx, rcy)，绿色中心进位2位，绿色中心(gcx, gcy)，帧尾2位
    #data = bytearray([0xa3, 0xb3, int(rcx > 255), int(rcy > 255), rcx, rcy,
        #int(gcx > 255), int(gcy > 255), gcx, gcy, 0x0d, 0x0a])
    #print(data)
    #uart_A.write(data)
    lcd.display(img)
gc.collect()