# serial_handler.py
import serial
import time
from serial_init import SerialSharedData  
from transform import transform_data

# 声明需要用到的全局变量（与主程序保持一致）            
speed_adjustment = 100                # 初始值设为100，在50-100范围内
Trajectory_angle = 0
compensate_queue = None               # 后续由主程序赋值

# 初始化全局共享数据实例
serial_shared = SerialSharedData()
# 串口接收线程函数，持续监听串口数据，解析0xFF坐标帧并更新补偿变量
def serial_receive_data(ser):
    """线程内持续接收串口数据，同步解析0xFF坐标帧并更新全局补偿变量"""
    frame_header = 0xFF
    frame_length = 7  # 0xFF + x(2) + y(2) + z(2)

    while True:
        try:
            if not ser or not isinstance(ser, serial.Serial) or not ser.is_open:
                time.sleep(0.1)
                continue

            if ser.in_waiting > 0:
                data = ser.read(ser.in_waiting)
                with serial_shared.lock:
                    serial_shared.buffer += data
                    serial_shared.cmd_buffer += data

                # 在接收线程中解析0xFF坐标帧
                with serial_shared.lock:
                    current_buf = serial_shared.buffer[:]

                if len(current_buf) >= frame_length:
                    header_index = current_buf.find(bytes([frame_header]))
                    if header_index != -1 and header_index + frame_length <= len(current_buf):
                        # 提取完整帧并更新缓冲区
                        frame = current_buf[header_index:header_index+frame_length]
                        with serial_shared.lock:
                            serial_shared.buffer = serial_shared.buffer[header_index+frame_length:]

                        # 坐标解码
                        x_coord = (frame[1] << 8) | frame[2]
                        x_coord = x_coord - 0x10000 if x_coord & 0x8000 else x_coord
                        if x_coord>90:
                            x_coord = 90
                        elif x_coord<-90:
                            x_coord = -90
                        y_coord = (frame[3] << 8) | frame[4]
                        y_coord = y_coord - 0x10000 if y_coord & 0x8000 else y_coord
                        if y_coord>50:
                            y_coord = 50
                        elif y_coord<-50:
                            y_coord = -50
                        z_coord = (frame[5] << 8) | frame[6]
                        z_coord = z_coord - 0x10000 if z_coord & 0x8000 else z_coord
                        if z_coord>90:
                            z_coord = 90
                        elif z_coord<-90:
                            z_coord = -90

                        inc_x, inc_y, inc_z = transform_data(x_coord, y_coord, z_coord)
                        # 存入队列（覆盖旧值，确保取到最新）
                        if compensate_queue is not None and not compensate_queue.empty():
                            compensate_queue.get_nowait()
                        if compensate_queue is not None:
                            compensate_queue.put((inc_x, inc_y, inc_z))

                        print(f"接收线程解析更新 - x:{x_coord:.1f}, y:{y_coord:.1f}, z:{z_coord:.1f}")

        except Exception as e:
            print(f"串口接收线程错误: {str(e)}")
        time.sleep(0.005)
# 从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）
def read_cmd_from_shared(timeout=1.0):
    """从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）"""
    frame_header = 0xAA
    cmd_length = 2  # 指令帧结构：0xAA（帧头） + 1字节指令值
    start_time = time.time()

    while (time.time() - start_time) < timeout:
        # 加锁读取指令缓冲区
        with serial_shared.lock:
            current_cmd_buf = serial_shared.cmd_buffer[:]

        if len(current_cmd_buf) >= cmd_length:
            header_index = current_cmd_buf.find(bytes([frame_header]))
            if header_index != -1:
                # 提取完整指令帧
                cmd_bytes = current_cmd_buf[header_index:header_index+cmd_length]
                # 更新共享缓冲区（移除已处理指令）
                with serial_shared.lock:
                    serial_shared.cmd_buffer = serial_shared.cmd_buffer[header_index+cmd_length:]
                return cmd_bytes
        time.sleep(0.01)
    return None  
# 处理特殊指令0xBB，更新全局参数并打印清晰信息
def handle_special_commands(PRESET_PARAMS):
    
    global speed_adjustment, Trajectory_angle

    bb_header = 0xBB
    bb_frame_length = 2  # 0xBB + 1字节配置选择值(0-11)
    # 初始化返回值（默认无选中配置）
    selected_delay_map = None
    selected_relay_config = None

    # 加锁读取缓冲区，避免线程冲突
    with serial_shared.lock:
        current_cmd_buf = serial_shared.cmd_buffer[:]

    # 处理0xBB指令（参数配置选择）
    bb_index = current_cmd_buf.find(bytes([bb_header]))
    if bb_index != -1 and bb_index + bb_frame_length <= len(current_cmd_buf):
        # 提取完整的0xBB帧
        bb_frame = current_cmd_buf[bb_index:bb_index+bb_frame_length]
        # 解析配置选择值（1-12有效，对应PRESET_PARAMS的0-11索引）
        config_select = bb_frame[1]
        
        # 校验配置值范围（仅处理1-12，共12组）
        if 1 <= config_select <= len(PRESET_PARAMS):
            # 匹配预设参数项（索引 = 配置值 - 1）
            selected_preset = PRESET_PARAMS[config_select - 1]
            # 提取params并更新全局变量
            speed_val, angle_val = selected_preset["params"]
            speed_adjustment = speed_val
            Trajectory_angle = angle_val
            # 提取当前配置项对应的delay和relay配置（赋值给返回变量）
            selected_delay_map = selected_preset["program_delay_map"]
            selected_relay_config = selected_preset["relay_config"]

            # 打印清晰的配置信息
            print(f"=== 0xBB指令处理成功 ===")
            print(f"配置选择值：{config_select}")
            print(f"speed_adjustment: {speed_adjustment}%")
            print(f"Trajectory_angle: {Trajectory_angle}")
            print(f"已加载对应program_delay_map和relay_config配置")
            
            # 移除已处理的帧（加锁更新缓冲区）
            with serial_shared.lock:
                serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:]
        else:
            # 无效配置值处理：清理帧但不更新参数
            with serial_shared.lock:
                serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:]
            print(f"⚠️  收到0xBB指令，配置选择值{config_select}无效（仅支持1-{len(PRESET_PARAMS)}）")

    # 返回选中的delay配置和relay配置（无选中则返回None）
    return selected_delay_map, selected_relay_config