license_plate_recognition/modules/radar/radar.py

import math
import time
from typing import List, Optional

import serial

from modules.audio.speaker import IpCast
from modules.display.screen import Screen, FlashFile


class RadarData:
    """对应Go的RadarData结构体"""

    def __init__(self):
        self.Valid: bool = False  # 数据是否有效
        self.Direction: str = ""  # 方向：in(来向)/out(去向)
        self.Speed: float = 0.0  # 速度（xxx.x格式）


class DeviceInitData:
    """模拟Go的DeviceInitData，存储速度阈值（需根据实际业务赋值）"""
    LowSpeed: float = 0.0  # 速度阈值，比如5.0（速度>5触发回调）


# ========== 新增1：超时清空状态管理（修复初始化+超大时间差） ==========
class ScreenClearManager:
    """屏幕超时清空管理器（修复初始化问题）"""

    def __init__(self, timeout_seconds: float = 2.0):
        self.timeout_seconds = timeout_seconds  # 超时时间（秒）
        # 初始化：使用当前时间，避免超大时间差
        self.last_valid_time = time.time()
        self.screen_cleared = False  # 屏幕是否已清空

    def update_valid_time(self):
        """更新有效数据时间"""
        self.last_valid_time = time.time()
        self.screen_cleared = False

    def check_and_clear(self, radar_screen: Optional[Screen]) -> bool:
        """
        检查是否需要清空屏幕（修复清空逻辑+错误处理）
        :return: 是否清空了屏幕
        """
        # 1. 屏幕实例不存在/未连接，直接返回
        if radar_screen is None or not hasattr(radar_screen, 'get_live_state') or not radar_screen.get_live_state():
            return False

        current_time = time.time()
        time_since_last_valid = current_time - self.last_valid_time

        # 2. 仅在超时且未清空时执行（修复超大时间差显示）
        if time_since_last_valid > self.timeout_seconds and not self.screen_cleared:
            # 限制最大显示时间差为timeout_seconds*2，避免超大数值
            display_seconds = min(time_since_last_valid, self.timeout_seconds * 2)
            # 获取屏幕地址（兼容不同Screen类实现）
            screen_addr = ""
            try:
                screen_addr = radar_screen.conn.getpeername() if hasattr(radar_screen, 'conn') else (
                radar_screen.ip, radar_screen.port)
            except:
                screen_addr = "未知地址"

            print(f"雷达超时：{display_seconds:.1f}秒无有效数据，清空屏幕 {screen_addr}")

            try:
                # 修复清空逻辑：优先使用安全的del_ram_text，兼容不同Screen实现
                if hasattr(radar_screen, 'del_ram_text'):
                    radar_screen.del_ram_text(0)
                # 备选方案：显示空内容（兼容无del_ram_text的情况）
                elif hasattr(radar_screen, 'text_ram'):
                    ff = FlashFile()
                    ff.set_msg("", 0)
                    ff.set_mode(4, 2)
                    ff.set_origin(0, True, 0)
                    ff.set_area(128, True, 96)
                    radar_screen.text_ram(ff, False)

                self.screen_cleared = True
                return True
            except Exception as e:
                # 精准捕获错误，避免'dyna_area_num'报错刷屏
                if 'dyna_area_num' in str(e):
                    print(f"清空屏幕失败：屏幕实例无'dyna_area_num'属性（非致命错误）")
                else:
                    print(f"清空屏幕失败：{str(e)[:50]}")  # 限制错误信息长度
            return False

        # 3. 未超时但已清空（恢复数据后重置）
        elif time_since_last_valid <= self.timeout_seconds and self.screen_cleared:
            self.screen_cleared = False

        return False


# 初始化全局的超时清空管理器（确保启动时就初始化）
screen_clear_manager = ScreenClearManager(timeout_seconds=2.0)


# ========== 核心：解析单帧雷达数据（完全复刻原Go逻辑） ==========
def parse_radar_frame(frame: bytes) -> RadarData:
    """
    解析单帧雷达数据（兼容前缀空白、帧尾不完整）
    :param frame: 原始帧字节数据
    :return: 解析后的RadarData对象
    """
    data = RadarData()

    # ========== 1. 预处理帧数据：清理前缀空白字符 ==========
    trim_prefix_chars = {0x00, 0x0A, 0x0D, 0x20, 0x09}
    trimmed_frame = bytearray(frame)

    while len(trimmed_frame) > 0:
        if trimmed_frame[0] in trim_prefix_chars:
            trimmed_frame.pop(0)
        else:
            break

    # ========== 2. 基础校验：清理后至少保留7字节（V+001.9） ==========
    if len(trimmed_frame) < 7:
        return data

    # ========== 3. 校验帧头（首字节必须是ASCII的'V'，0x56） ==========
    if trimmed_frame[0] != ord('V'):
        return data

    # ========== 4. 适配帧尾：兼容仅含\r（0x0D）或完整\r\n（0x0D+0x0A） ==========
    frame_tail_valid = False
    if len(trimmed_frame) >= 8 and trimmed_frame[7] == 0x0D:
        frame_tail_valid = True
    elif len(trimmed_frame) >= 9 and trimmed_frame[7] == 0x0D and trimmed_frame[8] == 0x0A:
        frame_tail_valid = True

    if not frame_tail_valid:
        return data

    # ========== 5. 解析方向（第2字节：+ 来向/in / - 去向/out） ==========
    direction_char = chr(trimmed_frame[1])
    if direction_char == '+':
        data.Direction = "in"
    elif direction_char == '-':
        data.Direction = "out"
    else:
        return data

    # ========== 6. 校验小数点位置（第6字节必须是'.'，0x2E） ==========
    if trimmed_frame[5] != ord('.'):
        return data

    # ========== 7. ASCII字符转数字（如'1'→1） ==========
    try:
        hundreds = int(chr(trimmed_frame[2]))
        tens = int(chr(trimmed_frame[3]))
        units = int(chr(trimmed_frame[4]))
        decimal = int(chr(trimmed_frame[6]))
    except ValueError:
        return data

    # ========== 8. 计算最终速度值（xxx.x格式） ==========
    data.Speed = float(hundreds * 100 + tens * 10 + units) + float(decimal) / 10.0
    data.Valid = True

    # ========== 调试用：打印解析到的原始数据 ==========
    # print(f"📡 雷达解析成功：方向={data.Direction}，速度={data.Speed:.1f}，原始帧={frame!r}")

    return data


def callback_with_data(
        radar_data: RadarData,
        radar_serial=None,
        speaker: Optional[IpCast] = None,
        radar_screen: Optional[Screen] = None
):
    """
    雷达数据回调（新增超时清空逻辑）
    """
    if radar_serial is None:
        import detect_plate
        radar_serial = detect_plate.radar_serial

    # 速度阈值判断（触发条件）
    if radar_data.Valid and radar_data.Speed > DeviceInitData.LowSpeed:
        color = 1
        rounded_speed = str(round(radar_data.Speed))

        if radar_data.Speed < 20:
            color = 2

        speak_text = f"注意来车"

        # 语音播报
        if speaker:
            try:
                speaker.Speak(speak_text)
            except Exception as e:
                print(f"语音播报失败：{e}")

        # 屏幕显示
        if radar_screen and hasattr(radar_screen, 'get_live_state') and radar_screen.get_live_state():
            try:
                ff = FlashFile()
                ff.set_msg(rounded_speed, color)
                ff.set_mode(4, 2)
                ff.set_origin(0, True, 0)
                ff.set_area(128, True, 96)
                radar_screen.text_ram(ff, False)

                # ========== 更新有效数据时间 ==========
                screen_clear_manager.update_valid_time()

                # print(f"雷达速度{rounded_speed}已发送到屏幕")
            except Exception as e:
                print(f"雷达屏幕显示失败：{e}")


def open_serial(
        port_name: str,
        speaker: Optional[IpCast] = None,
        radar_screen: Optional[Screen] = None,
        radar_serial_ref=None
):
    """打开雷达串口并监听（彻底解决残留数据+初始化问题）"""
    radar_serial = radar_serial_ref

    serial_options = {
        "port": port_name,
        "baudrate": 9600,
        "bytesize": serial.EIGHTBITS,
        "stopbits": serial.STOPBITS_ONE,
        "parity": serial.PARITY_NONE,
        "timeout": 0.1,
    }

    while True:
        try:
            radar_serial = serial.Serial(**serial_options)
            print(f"雷达串口 {port_name} 已打开")

            # 发送雷达配置指令
            radar_cmd = bytes([0x43, 0x46, 0x02, 0x01, 0x03, 0x00, 0x0d, 0x0a])
            send_len = radar_serial.write(radar_cmd)
            if send_len != len(radar_cmd):
                print(f"雷达配置指令发送不完整")

            time.sleep(0.1)

            read_buf: List[int] = []
            buf = bytearray(1024)
            last_clear_check = time.time()
            # 初始化有效数据时间（双重保障）
            last_valid_data_time = time.time()
            # 重置屏幕清空管理器（确保每次重连都初始化）
            screen_clear_manager.last_valid_time = time.time()
            screen_clear_manager.screen_cleared = False

            while True:
                try:
                    n = radar_serial.readinto(buf)

                    # 定期检查超时清空（0.2秒一次，避免高频检查）
                    current_time = time.time()
                    if current_time - last_clear_check > 0.2:
                        screen_clear_manager.check_and_clear(radar_screen)
                        last_clear_check = current_time

                    if n == 0:
                        continue

                    read_buf.extend(buf[:n])

                    while len(read_buf) > 0:
                        frame_end_idx = -1
                        for i, b in enumerate(read_buf):
                            if b == 0x0D or b == 0x0A:
                                frame_end_idx = i
                                break
                        if frame_end_idx == -1:
                            if len(read_buf) > 32:
                                read_buf = read_buf[-16:]
                            break

                        # 解析雷达帧
                        frame = bytes(read_buf[:frame_end_idx + 1])
                        read_buf = read_buf[frame_end_idx + 1:]
                        radar_data = parse_radar_frame(frame)

                        if radar_data.Valid:
                            data_time = time.time()
                            time_since_last = data_time - last_valid_data_time

                            # 更新时间
                            last_valid_data_time = data_time
                            last_clear_check = current_time
                            # 更新清空管理器的时间（核心修复：有数据时同步）
                            screen_clear_manager.update_valid_time()

                            # 日志显示
                            # local_time = time.strftime("%H:%M:%S", time.localtime(data_time))
                            # print(f"[{local_time}] 速度={radar_data.Speed:.1f} (距上次{time_since_last:.2f}秒)")
                            if radar_data.Speed < 5:
                                continue

                            # 触发回调
                            callback_with_data(radar_data, radar_serial, speaker, radar_screen)

                            read_buf = []
                            break  # 退出当前的帧处理循环

                except Exception as read_err:
                    print(f"雷达读取错误：{read_err}，3秒后重连")
                    time.sleep(3)
                    break
        except Exception as e:
            print(f"雷达串口打开失败：{e}，3秒后重试")
            time.sleep(3)
        finally:
            if 'radar_serial' in locals() and radar_serial and radar_serial.is_open:
                radar_serial.close()
                print(f"雷达串口 {port_name} 已关闭")