from dataclasses import dataclass, field
from typing import Any, Callable, Dict, List, Optional, Sequence, Tuple

import math
import os
import sys
import threading
import time

import cv2
import numpy as np
import pyrealsense2 as rs
import pyaubo_sdk
from pyaubo_agvc_sdk import RpcClient
from ultralytics import YOLO

"""
番茄采摘控制核心实现。

这个模块只负责内部逻辑拆分，`control.py` 作为兼容层保留给 UI 调用。
"""


M_PI = math.pi

# 可修改的设备、运行、位姿、夹爪参数统一放在 control.py 顶部。
# control_core.py 只保留核心内部常量，避免出现第二套可调默认值。


# -------------------- 外部配置缓存 --------------------
_runtime_config: Dict[str, Any] = {}


PICK_ZONE_LEFT_RATIO = 1.0 / 3.0
PICK_ZONE_RIGHT_RATIO = 7.0 / 10.0
PICK_KEYPOINT_CONF_THRESHOLD = 0.8  # 关键点置信度低于该值时不参与采摘。
PICK_CUTPOINT_KEYPOINT_INDEX = 0  # YOLO pose 第 0 个点为采摘点 cutpoint。
PICK_ENDPOINT_KEYPOINT_INDEX = 1  # YOLO pose 第 1 个点为方向端点 endpoint。
PICK_KEYPOINT_BOX_MARGIN_RATIO = 0.15  # 关键点允许略超出 bbox 的比例。
PICK_CUTPOINT_MAX_REL_Y = 0.70  # 采摘点在 bbox 内的最大相对高度，防止误取下方点。
PICK_KEYPOINT_MIN_DISTANCE_PX = 8.0  # cutpoint 与 endpoint 的最小像素距离。
PICK_ENDPOINT_Y_TOLERANCE_PX = 4  # endpoint 允许比 cutpoint 略高的像素容差。
PICK_DEPTH_WINDOW_RADIUS = 5  # 首次稳健取深度的窗口半径，5 表示 11x11。
PICK_DEPTH_FALLBACK_WINDOW_RADIUS = 10  # 首次深度不足时的扩大窗口半径，10 表示 21x21。
PICK_DEPTH_MIN_VALID_PIXELS = 5  # 深度窗口内至少需要的有效深度点数量。
PICK_DEPTH_MIN_M = 0.25  # 有效深度下限，单位 m。
PICK_DEPTH_MAX_M = 0.6  # 有效深度上限，单位 m。
PICK_DEPTH_MEDIAN_TOLERANCE_M = 0.035  # 深度中位数滤波容差，单位 m。

# R_tc = np.array(
#     [
#         [-0.9996, -0.0270, -0.0103],
#         [0.0267, -0.7993, 0.0245],
#         [-0.0109, 0.0242, 0.7996],
#     ],
#     dtype=float,
# )
# T_tc = np.array([0.0, 0.0, 0.0], dtype=float)

R_tc = np.array(
    [
        [-0.9998, -0.0104, 0.0152],
        [0.0099, -0.9995, -0.0316],
        [0.0155, -0.0315, 0.9994],
    ],
    dtype=float,
)

T_tc = np.array([-0.0026, 0.0388, -0.0632], dtype=float)

POSITION_TOLERANCE = 0.01
ORIENTATION_TOLERANCE = 0.05
JOINT_TOLERANCE = 0.02


# -------------------- 运行时常量 --------------------
FRAME_SHAPE = (480, 640, 3)
AGV_SUCCESS_CODE = 10100000
EXEC_ID_MAX_RETRIES = 20
MOTION_COMMAND_RETRIES = 2
MOTION_START_DELAY = 0.2
HOME_RETRY_COUNT = 1
HOME_RETRY_DELAY = 1.0
STARTUP_HOME_DELAY = 2.0
DEFAULT_PLACE_POSITION = [0.1, 0.7, 0.5, 1.5708, 1.5708, 0]


# -------------------- 全局运行状态 --------------------
# `running`: 总运行开关，两个工作线程都以它作为退出条件。
# `has_tomato`: 视觉线程在 AGV 行进时发现候选成熟番茄后置位，通知 AGV 先停下。
# `picking_done`: 一次抓取放置完成后置位，视觉线程会在下一轮把本轮状态清掉，让 AGV 继续走。
running = threading.Event()
has_tomato = threading.Event()
picking_done = threading.Event()

# 这三个事件变量共同组成跨线程状态机：
# running 控制系统总开关；
# has_tomato 表示视觉线程发现了候选番茄，需要 AGV 暂停；
# picking_done 表示本轮抓取放置完成，系统可以回到继续巡检。

start_time = 0.0
agv_stop_time = 0.0

ui_callback: Callable[[np.ndarray], None] = lambda frame: None
robot_rpc_client = pyaubo_sdk.RpcClient()


def configure(config: Dict[str, Any]) -> None:
    """接收 control.py 同步过来的可调参数。"""
    _runtime_config.update(config)


def _cfg(name: str) -> Any:
    control_module = sys.modules.get("control")
    if control_module is not None and hasattr(control_module, name):
        value = getattr(control_module, name)
    elif name in _runtime_config:
        value = _runtime_config[name]
    else:
        raise RuntimeError(f"缺少配置参数 {name}，请先通过 control.py 启动或同步配置")
    if isinstance(value, list):
        return [list(item) if isinstance(item, list) else item for item in value]
    return value


def __getattr__(name: str) -> Any:
    control_module = sys.modules.get("control")
    if name in _runtime_config or (control_module is not None and hasattr(control_module, name)):
        return _cfg(name)
    raise AttributeError(f"module {__name__!r} has no attribute {name!r}")


@dataclass
class PlacementManager:
    """按顺序循环取放置位，始终读取 control.py 中的 `place_positions`。"""

    lock: threading.Lock = field(default_factory=threading.Lock)
    next_index: int = 0

    def get_next_position(self) -> Tuple[List[float], int]:
        with self.lock:
            positions = _cfg("place_positions") or [DEFAULT_PLACE_POSITION]
            used_index = self.next_index % len(positions)
            position = list(positions[used_index])
            self.next_index = (used_index + 1) % len(positions)
            return position, used_index

    def get_current_index(self) -> int:
        with self.lock:
            return self.next_index

    def reset(self) -> None:
        with self.lock:
            self.next_index = 0


@dataclass
class DetectedTomato:
    x1: int
    y1: int
    x2: int
    y2: int
    pixel_x: int
    pixel_y: int
    depth: float
    point_cam: List[float]
    point_base: List[float]
    robot_pose: List[float]
    angle_rad: float
    cutpoint: List[int] = field(default_factory=list)
    end_point: List[int] = field(default_factory=list)


@dataclass
class TomatoCandidate:
    box: Any
    box_index: int
    cutpoint: Optional[Tuple[int, int]]
    end_point: Optional[Tuple[int, int]]


placement_manager = PlacementManager()
_current_arm_controller: Optional["RobotArmController"] = None
_current_agv_controller: Optional["AgvController"] = None


def blank_frame() -> np.ndarray:
    return np.zeros(FRAME_SHAPE, dtype=np.uint8)


def set_ui_callback(callback: Callable[[np.ndarray], None]) -> None:
    global ui_callback
    if callable(callback):
        ui_callback = callback
        print("UI 回调函数已设置")
    else:
        print("警告：传入的 UI 回调不可调用，继续使用默认空回调")


def safe_ui_update(frame: Any, overlay: Optional[Dict[str, Any]] = None) -> None:
    try:
        payload = frame
        if overlay is not None:
            payload = {"frame": frame, **overlay}
        ui_callback(payload)
    except Exception as exc:
        print(f"UI 回调失败: {exc}")


def reset_runtime_state() -> None:
    global start_time, agv_stop_time
    running.clear()
    has_tomato.clear()
    picking_done.clear()
    start_time = 0.0
    agv_stop_time = 0.0


def start_runtime_state() -> None:
    global start_time, agv_stop_time
    has_tomato.clear()
    picking_done.clear()
    agv_stop_time = 0.0
    start_time = time.time()
    running.set()


def runtime_expired() -> bool:
    if start_time <= 0:
        return False
    return (time.time() - start_time) > _cfg("TOTAL_DURATION")


def remaining_seconds() -> int:
    total_duration = _cfg("TOTAL_DURATION")
    if start_time <= 0:
        return total_duration
    return max(0, int(total_duration - (time.time() - start_time)))


def start_agv_stop_window() -> bool:
    global agv_stop_time
    if agv_stop_time == 0:
        agv_stop_time = time.time()
        return True
    return False


def clear_agv_stop_window() -> None:
    global agv_stop_time
    agv_stop_time = 0.0


def agv_stop_elapsed() -> float:
    if agv_stop_time == 0:
        return 0.0
    return time.time() - agv_stop_time


def clear_pick_cycle() -> None:
    # 清空这一轮“发现目标 -> 停车 -> 抓取”的中间状态。
    has_tomato.clear()
    picking_done.clear()
    clear_agv_stop_window()


def mark_pick_complete() -> None:
    # 抓取完成后先打完成标记，再由搜索阶段统一恢复为继续前进。
    picking_done.set()
    has_tomato.clear()
    clear_agv_stop_window()


def get_next_placement() -> Tuple[List[float], int]:
    return placement_manager.get_next_position()


def get_placement_index() -> int:
    return placement_manager.get_current_index()


def create_placement_manager():
    """兼容旧接口，返回当前放置位访问器。"""
    return get_next_placement, get_placement_index


def exampleState(robot_name: str) -> Optional[List[float]]:
    """读取并打印当前机械臂关节角。"""
    try:
        joints = robot_rpc_client.getRobotInterface(robot_name).getRobotState().getJointPositions()
        joints = list(joints)
        print("机械臂关节角:", joints)
        return joints
    except Exception as exc:
        print(f"读取关节角失败: {exc}")
        return None


def get_robot_end_effector_pose(robot_name: str) -> Optional[List[float]]:
    """读取并打印当前 TCP 位姿。"""
    try:
        pose = robot_rpc_client.getRobotInterface(robot_name).getRobotState().getTcpPose()
        pose = list(pose)
        print("机械臂末端位姿:", pose)
        return pose
    except Exception as exc:
        print(f"读取末端位姿失败: {exc}")
        return None


def exampleInverseK(
    robot_name: str, pose: Sequence[float], reference_q: Sequence[float]
) -> Tuple[Optional[List[float]], Optional[int]]:
    """执行逆解计算。"""
    if not isinstance(pose, (list, tuple)) or len(pose) != 6:
        print(f"逆解目标位姿格式错误，需要 6 维列表: {pose}")
        return None, None

    try:
        result_joints, status = robot_rpc_client.getRobotInterface(robot_name).getRobotAlgorithm().inverseKinematics(
            list(reference_q), list(pose)
        )
        result_joints = list(result_joints)
        print("逆解输入位姿:", list(pose))
        print("逆解输出关节角:", result_joints)
        if status != 0:
            print(f"逆解失败，错误码: {status}")
            return None, status
        return result_joints, status
    except Exception as exc:
        print(f"逆解计算失败: {exc}")
        return None, None


def exampleStartup() -> Optional[str]:
    """机械臂上电、启动，并等待进入 Running。"""
    try:
        robot_names = robot_rpc_client.getRobotNames()
        if not robot_names:
            print("机械臂启动失败：未找到机器人名称")
            return None

        robot_name = robot_names[0]
        robot_manage = robot_rpc_client.getRobotInterface(robot_name).getRobotManage()
        if robot_manage.poweron() != 0:
            print("机械臂上电失败")
            return None
        print("机械臂上电请求已发送")

        if robot_manage.startup() != 0:
            print("机械臂启动失败")
            return None
        print("机械臂启动请求已发送")

        while True:
            robot_mode = robot_rpc_client.getRobotInterface(robot_name).getRobotState().getRobotModeType()
            print(f"机械臂当前模式: {robot_mode.name}")
            if robot_mode == pyaubo_sdk.RobotModeType.Running:
                return robot_name
            time.sleep(2)
    except Exception as exc:
        print(f"机械臂启动异常: {exc}")
        return None


def quaternion_to_rotation_matrix(q: Sequence[float]) -> np.ndarray:
    """四元数转旋转矩阵。"""
    w, x, y, z = q
    return np.array(
        [
            [1 - 2 * y**2 - 2 * z**2, 2 * x * y - 2 * w * z, 2 * x * z + 2 * w * y],
            [2 * x * y + 2 * w * z, 1 - 2 * x**2 - 2 * z**2, 2 * y * z - 2 * w * x],
            [2 * x * z - 2 * w * y, 2 * y * z + 2 * w * x, 1 - 2 * x**2 - 2 * y**2],
        ],
        dtype=float,
    )


def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray:
    """欧拉角转旋转矩阵，旋转顺序为 ZYX。"""
    r_x = np.array(
        [
            [1, 0, 0],
            [0, math.cos(roll), -math.sin(roll)],
            [0, math.sin(roll), math.cos(roll)],
        ],
        dtype=float,
    )
    r_y = np.array(
        [
            [math.cos(pitch), 0, math.sin(pitch)],
            [0, 1, 0],
            [-math.sin(pitch), 0, math.cos(pitch)],
        ],
        dtype=float,
    )
    r_z = np.array(
        [
            [math.cos(yaw), -math.sin(yaw), 0],
            [math.sin(yaw), math.cos(yaw), 0],
            [0, 0, 1],
        ],
        dtype=float,
    )
    return r_z @ r_y @ r_x


def get_robot_end_matrix(robot_name: str) -> np.ndarray:
    """获取末端相对基座的 4x4 齐次变换矩阵。"""
    pose = get_robot_end_effector_pose(robot_name)
    if pose is None:
        return np.eye(4, dtype=float)

    try:
        position = pose[:3]
        if len(pose) >= 7:
            rotation = quaternion_to_rotation_matrix(pose[3:7])
        elif len(pose) == 6:
            rotation = euler_to_rotation_matrix(*pose[3:])
        else:
            print(f"末端位姿格式错误: {pose}")
            return np.eye(4, dtype=float)

        matrix = np.eye(4, dtype=float)
        matrix[:3, :3] = rotation
        matrix[:3, 3] = position
        return matrix
    except Exception as exc:
        print(f"计算末端矩阵失败: {exc}")
        return np.eye(4, dtype=float)


def camera_to_base(point_cam: Sequence[float], robot_name: str) -> Optional[np.ndarray]:
    """将相机坐标系中的三维点换算到机械臂基坐标系。"""
    try:
        base_t_tool = get_robot_end_matrix(robot_name)
        tool_t_camera = np.eye(4, dtype=float)
        tool_t_camera[:3, :3] = R_tc
        tool_t_camera[:3, 3] = T_tc
        point_h = np.append(np.array(point_cam, dtype=float), 1.0)
        point_base = base_t_tool @ tool_t_camera @ point_h
        return point_base[:3]
    except Exception as exc:
        print(f"坐标转换失败: {exc}")
        return None


def check_joint_position(robot_name: str, target_joints: Sequence[float]) -> bool:
    """校验当前关节位置是否到达目标位置。"""
    current_joints = exampleState(robot_name)
    if current_joints is None or len(current_joints) != len(target_joints):
        print("无法读取关节位置，或关节数量不匹配")
        return False

    errors = [abs(current - target) for current, target in zip(current_joints, target_joints)]
    max_error = max(errors) if errors else float("inf")
    print(f"关节最大误差: {max_error:.4f} rad，阈值: {JOINT_TOLERANCE:.4f} rad")
    return max_error < JOINT_TOLERANCE


def check_tcp_pose(robot_name: str, target_pose: Sequence[float]) -> bool:
    """校验当前 TCP 位姿是否到达目标位姿。"""
    current_pose = get_robot_end_effector_pose(robot_name)
    if current_pose is None or len(current_pose) < 6 or len(target_pose) < 6:
        print("无法读取 TCP 位姿，或目标位姿格式错误")
        return False

    pos_error = np.linalg.norm(np.array(current_pose[:3], dtype=float) - np.array(target_pose[:3], dtype=float))
    ori_error = np.linalg.norm(np.array(current_pose[3:6], dtype=float) - np.array(target_pose[3:6], dtype=float))
    print(
        f"TCP 位姿误差: 位置={pos_error:.4f} m，姿态={ori_error:.4f} rad | "
        f"阈值: 位置={POSITION_TOLERANCE:.4f} m，姿态={ORIENTATION_TOLERANCE:.4f} rad"
    )
    return pos_error < POSITION_TOLERANCE and ori_error < ORIENTATION_TOLERANCE


def waitArrival(impl: Any, target_joints: Optional[Sequence[float]] = None, target_pose: Optional[Sequence[float]] = None) -> int:
    """
    等待运动完成并在结束后进行位置校验。

    返回：
    0 = 成功
    -1 = 获取执行 ID 失败
    -2 = 运动等待超时
    """
    retry_count = 0
    motion_control = impl.getMotionControl()
    while motion_control.getExecId() == -1:
        retry_count += 1
        if retry_count > EXEC_ID_MAX_RETRIES:
            robot_names = robot_rpc_client.getRobotNames()
            robot_name = robot_names[0] if robot_names else None
            if robot_name:
                joints_ok = target_joints is not None and check_joint_position(robot_name, target_joints)
                pose_ok = target_pose is not None and check_tcp_pose(robot_name, target_pose)
                if joints_ok or pose_ok:
                    print("未获取到执行 ID，但检测到机械臂已到达目标位，按成功处理")
                    return 0
            print("运动失败：获取执行 ID 超时")
            return -1
        time.sleep(0.5)

    exec_id = motion_control.getExecId()
    timeout = 0.0
    max_timeout = 30.0

    while True:
        current_exec_id = motion_control.getExecId()
        if exec_id != current_exec_id:
            break

        timeout += 0.5
        if timeout > max_timeout:
            print(f"运动等待超时：{max_timeout} 秒")
            return -2
        time.sleep(0.5)

    robot_names = robot_rpc_client.getRobotNames()
    robot_name = robot_names[0] if robot_names else None
    if robot_name:
        if target_joints is not None and not check_joint_position(robot_name, target_joints):
            print("警告：关节位置未达到目标值")
        if target_pose is not None and not check_tcp_pose(robot_name, target_pose):
            print("警告：TCP 位姿未达到目标值")

    print("机械臂运动完成")
    return 0


def control_tool_io(robot_name: str, io_index: int, state: bool) -> None:
    """设置工具端数字 IO。"""
    io_control = robot_rpc_client.getRobotInterface(robot_name).getIoControl()
    io_control.setToolDigitalOutput(io_index, state)
    output = io_control.getToolDigitalOutput(io_index)
    print(f"TOOL_IO[{io_index}] -> {'ON' if output else 'OFF'}")


@dataclass
class RobotArmController:
    """机械臂子系统。"""

    robot_name: str
    rpc_client: Any
    placement_manager: PlacementManager

    @property
    def interface(self) -> Any:
        return self.rpc_client.getRobotInterface(self.robot_name)

    @property
    def runtime_machine(self) -> Any:
        return self.rpc_client.getRuntimeMachine()

    def get_joint_positions(self) -> Optional[List[float]]:
        return exampleState(self.robot_name)

    def get_tcp_pose(self) -> Optional[List[float]]:
        return get_robot_end_effector_pose(self.robot_name)

    def is_at_home(self, joints: Optional[Sequence[float]] = None) -> bool:
        home_joints = _cfg("HOME_JOINTS")
        current_joints = list(joints) if joints is not None else self.get_joint_positions()
        if current_joints is None or len(current_joints) != len(home_joints):
            return False
        max_error = max(abs(current - target) for current, target in zip(current_joints, home_joints))
        return max_error < JOINT_TOLERANCE

    def keep_scissors_open(self) -> None:
        try:
            control_tool_io(self.robot_name, _cfg("GRIPPER_CLOSE_IO"), False)
            control_tool_io(self.robot_name, _cfg("GRIPPER_OPEN_IO"), True)
            print("Scissors disabled: keeping end effector open")
        except Exception as exc:
            print(f"Failed to keep scissors open: {exc}")

    def reset_scissors_outputs(self) -> None:
        try:
            control_tool_io(self.robot_name, _cfg("GRIPPER_CLOSE_IO"), False)
            control_tool_io(self.robot_name, _cfg("GRIPPER_OPEN_IO"), False)
            print("Scissors enabled: tool IO reset to neutral")
        except Exception as exc:
            print(f"Failed to reset scissors IO: {exc}")

    def close_scissors_for_pick(self) -> None:
        if not _cfg("SCISSORS_ENABLED"):
            self.keep_scissors_open()
            return
        control_tool_io(self.robot_name, _cfg("GRIPPER_OPEN_IO"), False)
        control_tool_io(self.robot_name, _cfg("GRIPPER_CLOSE_IO"), True)
        time.sleep(_cfg("GRIPPER_ACTION_DELAY"))
        control_tool_io(self.robot_name, _cfg("GRIPPER_CLOSE_IO"), False)

    def open_scissors_after_place(self) -> None:
        if not _cfg("SCISSORS_ENABLED"):
            self.keep_scissors_open()
            return
        control_tool_io(self.robot_name, _cfg("GRIPPER_CLOSE_IO"), False)
        control_tool_io(self.robot_name, _cfg("GRIPPER_OPEN_IO"), True)
        time.sleep(_cfg("GRIPPER_ACTION_DELAY"))
        control_tool_io(self.robot_name, _cfg("GRIPPER_OPEN_IO"), False)

    def _restart_runtime_machine(self) -> None:
        """重置运行态，尽量把控制器拉回可接收运动指令的状态。"""
        try:
            self.runtime_machine.stop()
        except Exception:
            pass
        time.sleep(MOTION_START_DELAY)
        self.runtime_machine.start()
        time.sleep(MOTION_START_DELAY)

    def move_joint(
        self,
        target_joints: Sequence[float],
        *,
        target_pose: Optional[Sequence[float]] = None,
        description: str = "机械臂运动",
    ) -> bool:
        """按关节目标执行运动。"""
        joints = list(target_joints)
        last_error: Optional[Exception] = None

        for attempt in range(1, MOTION_COMMAND_RETRIES + 1):
            try:
                if attempt > 1:
                    print(f"{description}：重新下发运动指令（第 {attempt}/{MOTION_COMMAND_RETRIES} 次）")

                self._restart_runtime_machine()
                self.interface.getMotionControl().moveJoint(joints, _cfg("ARM_SPEED"), _cfg("ARM_ACCEL"), 0, 0)
                result = waitArrival(self.interface, target_joints=joints, target_pose=target_pose)
                if result == 0:
                    print(f"{description}完成")
                    return True

                print(f"{description}失败，错误码: {result}")
            except Exception as exc:
                last_error = exc
                print(f"{description}失败: {exc}")
            finally:
                try:
                    self.runtime_machine.stop()
                except Exception:
                    pass

            time.sleep(MOTION_START_DELAY)

        if last_error is not None:
            print(f"{description}最终失败: {last_error}")
        return False

    def move_pose(
        self,
        target_pose: Sequence[float],
        reference_joints: Sequence[float],
        *,
        description: str,
    ) -> Optional[List[float]]:
        """按位姿目标执行逆解和关节运动。"""
        target_joints, _ = exampleInverseK(self.robot_name, target_pose, reference_joints)
        if target_joints is None:
            print(f"{description}失败：逆解无结果")
            self.return_home(retries=1)
            return None

        if not self.move_joint(target_joints, target_pose=target_pose, description=description):
            return None
        return target_joints

    def return_home(self, retries: int = HOME_RETRY_COUNT) -> bool:
        """返回 Home 关节位。"""
        current_joints = self.get_joint_positions()
        if self.is_at_home(current_joints):
            print("Robot arm is already at Home, skipping Home motion")
            return True

        attempts = max(1, retries)
        for attempt in range(1, attempts + 1):
            if attempts == 1:
                print("机械臂：返回 Home")
            else:
                print(f"机械臂：返回 Home（第 {attempt}/{attempts} 次）")
            if self.move_joint(_cfg("HOME_JOINTS"), description="返回 Home"):
                return True
            if attempt < attempts:
                time.sleep(HOME_RETRY_DELAY)
        print("机械臂：多次尝试后仍未能返回 Home")
        return False

    def execute_pick_and_place(self, pose: Sequence[float], angle_rad: float) -> bool:
        """
        执行抓取和放置动作。

        保持原有外部接口和基本动作顺序：
        1. 去抓取预备点
        2. 去抓取点
        3. 夹爪动作
        4. 回抓取预备点
        5. 回 Home
        6. 去放置点
        7. 释放
        8. 再回 Home
        """
        current_joints = self.get_joint_positions()
        if current_joints is None:
            print("获取关节位置失败，终止抓取并尝试返回 Home")
            self.return_home()
            return False

        target_pose = list(pose)
        print(f"机械臂：开始处理目标位姿 {target_pose}")
        print(f"机械臂：关键点连线角度 {math.degrees(angle_rad):.2f}°")

        approach_pose = target_pose.copy()
        # 先走到抓取预备点，避免直接扎向果实。
        approach_pose[1] += _cfg("APPROACH_Y_OFFSET")

        pickup_pose = target_pose.copy()
        # 再把末端抬到可抓取高度，和原代码保持一致使用 Z 偏移。
        pickup_pose[0] += _cfg("PICKUP_X_OFFSET")
        pickup_pose[1] += _cfg("PICKUP_Y_OFFSET")
        pickup_pose[2] += _cfg("LIFT_Z_OFFSET")

        angle_index = int(_cfg("PICK_ANGLE_ORIENTATION_INDEX"))
        if 3 <= angle_index < len(pickup_pose):
            signed_angle = float(_cfg("PICK_ANGLE_SIGN")) * angle_rad
            pickup_pose[angle_index] += signed_angle
            print(
                "机械臂：末端姿态旋转 "
                f"{math.degrees(signed_angle):.2f}° | 姿态索引: {angle_index}"
            )
        else:
            print(f"警告：PICK_ANGLE_ORIENTATION_INDEX={_cfg('PICK_ANGLE_ORIENTATION_INDEX')} 无效，跳过姿态旋转")

        approach_joints = self.move_pose(approach_pose, current_joints, description="移动到抓取预备点")
        if approach_joints is None:
            self.return_home()
            return False

        pickup_joints = self.move_pose(pickup_pose, approach_joints, description="旋转末端并移动到抓取点")
        if pickup_joints is None:
            self.return_home()
            return False

        pick_point_delay = float(_cfg("PICK_POINT_ARRIVAL_DELAY"))
        if pick_point_delay > 0:
            print(f"机械臂：已到达抓取点，等待 {pick_point_delay:.1f} 秒后执行夹爪动作")
            time.sleep(pick_point_delay)

        self.close_scissors_for_pick()

        retreat_joints = self.move_pose(approach_pose, pickup_joints, description="采摘后返回预抓取点")
        if retreat_joints is None:
            self.return_home()
            return False

        if not self.return_home():
            return False

        place_pose, place_index = self.placement_manager.get_next_position()
        print(f"机械臂：移动到放置位 {place_index} -> {place_pose}")

        reference_joints = self.get_joint_positions() or _cfg("HOME_JOINTS")
        place_joints = self.move_pose(place_pose, reference_joints, description=f"移动到放置位 {place_index}")
        if place_joints is None:
            self.return_home()
            return False

        self.open_scissors_after_place()

        if not self.return_home():
            return False

        print("机械臂：本次抓取放置流程完成")
        return True


def init_camera() -> Tuple[Any, Any, Any]:
    """初始化 RealSense 相机。"""
    try:
        pipeline = rs.pipeline()
        config = rs.config()
        config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        profile = pipeline.start(config)

        depth_sensor = profile.get_device().first_depth_sensor()
        depth_scale = depth_sensor.get_depth_scale()
        depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
        depth_intrinsics = depth_profile.get_intrinsics()
        align = rs.align(rs.stream.color)

        print(f"相机初始化完成 | 深度比例尺: {depth_scale}")
        return pipeline, align, depth_intrinsics
    except Exception as exc:
        print(f"相机初始化失败: {exc}")
        raise


def init_tomato_detector(model_path: Optional[str] = None) -> Any:
    """初始化 YOLO 模型。"""
    use_path = model_path or _cfg("YOLO_MODEL_PATH") or "best.pt"
    print(f"加载 YOLO 模型 | 路径: {use_path}")

    if not os.path.exists(use_path):
        raise FileNotFoundError(f"YOLO 模型文件不存在: {use_path}")
    if not use_path.endswith(".pt"):
        raise TypeError(f"模型文件格式错误: {use_path}，仅支持 .pt")

    try:
        model = YOLO(use_path)
        print("YOLO 模型加载成功")
        return model
    except Exception as exc:
        print(f"YOLO 模型加载失败: {exc}")
        raise


def filter_ripe_tomatoes(results: Any, confidence_threshold: Optional[float] = None) -> List[Any]:
    """筛选成熟番茄目标。"""
    threshold = _cfg("PICK_CONFIDENCE_THRESHOLD") if confidence_threshold is None else confidence_threshold
    ripe_tomatoes: List[Any] = []
    for box in results.boxes:
        confidence = float(box.conf)
        if int(box.cls) == 0 and confidence >= threshold:
            ripe_tomatoes.append(box)
    return ripe_tomatoes


def draw_tomato_tilt_line(color_image: np.ndarray, x1: int, y1: int, x2: int, y2: int, pixel_x: int) -> float:
    """在目标区域内估计番茄串倾角。"""
    _ = pixel_x
    if x2 <= x1 or y2 <= y1:
        return 0.0

    roi = color_image[y1:y2, x1:x2]
    if roi.size == 0:
        return 0.0

    gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
    edges = cv2.Canny(gray, 50, 150)
    lines = cv2.HoughLinesP(edges, 1, np.pi / 180, threshold=50, minLineLength=50, maxLineGap=10)

    angle = 0.0
    if lines is None:
        return angle

    longest_line = max(
        lines,
        key=lambda line: np.linalg.norm(np.array(line[0][:2], dtype=float) - np.array(line[0][2:], dtype=float)),
    )
    x1_line, y1_line, x2_line, y2_line = [int(v) for v in longest_line[0]]
    x1_line += x1
    x2_line += x1
    y1_line += y1
    y2_line += y1

    vector_tilt = np.array([x2_line - x1_line, y2_line - y1_line], dtype=float)
    vector_vertical = np.array([0.0, y2 - y1], dtype=float)
    norm_tilt = np.linalg.norm(vector_tilt)
    norm_vertical = np.linalg.norm(vector_vertical)

    if norm_tilt > 0 and norm_vertical > 0:
        cos_angle = np.dot(vector_tilt, vector_vertical) / (norm_tilt * norm_vertical)
        cos_angle = max(-1.0, min(1.0, float(cos_angle)))
        angle = math.degrees(math.acos(cos_angle))
        angle = angle if x2_line > x1_line else -angle
        angle = max(-30.0, min(30.0, angle))

    cv2.putText(
        color_image,
        f"Angle: {angle:.2f}deg",
        (x1, y2 + 20),
        cv2.FONT_HERSHEY_SIMPLEX,
        0.6,
        (0, 255, 0),
        2,
    )
    print(f"检测到番茄串倾角: {angle:.2f}°")
    return angle


def fit_red_line(crop_image: np.ndarray, x1: int, y1: int) -> Optional[Tuple[Tuple[int, int], Tuple[int, int]]]:
    """在目标区域中拟合红色主轴。"""
    if crop_image.size == 0:
        return None

    lower_red1 = np.array([0, 120, 70])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([160, 120, 70])
    upper_red2 = np.array([180, 255, 255])

    hsv = cv2.cvtColor(crop_image, cv2.COLOR_BGR2HSV)
    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
    mask = cv2.bitwise_or(mask1, mask2)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    all_points = np.vstack(contours).squeeze()
    if len(np.atleast_1d(all_points)) < 2:
        return None

    vx, vy, x0, y0 = [float(value) for value in cv2.fitLine(all_points, cv2.DIST_L2, 0, 0.01, 0.01)]
    if abs(vx) < 1e-6:
        p1 = (x1 + int(x0), y1)
        p2 = (x1 + int(x0), y1 + crop_image.shape[0])
        return p1, p2

    left_y = int((-x0 * vy / vx) + y0)
    right_y = int((((crop_image.shape[1] - x0) * vy) / vx) + y0)
    p1 = (x1, y1 + left_y)
    p2 = (x1 + crop_image.shape[1], y1 + right_y)
    return p1, p2


def calculate_keypoint_line_angle_rad(
    cutpoint: Optional[Tuple[int, int]],
    end_point: Optional[Tuple[int, int]],
) -> float:
    """计算上/下关键点连线相对图像竖直方向的角度。"""
    if cutpoint is None or end_point is None:
        return 0.0

    dx = float(end_point[0] - cutpoint[0])
    dy = float(end_point[1] - cutpoint[1])
    if abs(dx) < 1e-6 and abs(dy) < 1e-6:
        return 0.0

    angle_rad = math.atan2(dx, dy)
    max_angle_rad = math.radians(float(_cfg("MAX_PICK_ANGLE_DEG")))
    return max(-max_angle_rad, min(max_angle_rad, angle_rad))


@dataclass
class VisionController:
    """视觉子系统。"""

    pipeline: Any
    align: Any
    depth_intrinsics: Any
    model: Any
    robot_name: str
    arm_controller: RobotArmController

    def capture_frame(self) -> Tuple[Optional[np.ndarray], Optional[Any]]:
        """采集并对齐相机画面。"""
        try:
            frames = self.pipeline.wait_for_frames(timeout_ms=5000)
            if not frames:
                print("未获取到相机帧，重试...")
                return None, None

            aligned_frames = self.align.process(frames)
            depth_frame = aligned_frames.get_depth_frame()
            color_frame = aligned_frames.get_color_frame()
            if not depth_frame or not color_frame:
                print("深度帧或彩色帧为空，重试...")
                return None, None

            color_image = np.asanyarray(color_frame.get_data())
            return color_image, depth_frame
        except Exception as exc:
            print(f"获取相机帧失败: {exc}")
            return None, None

    @staticmethod
    def get_pick_zone_bounds(frame_width: int) -> Tuple[int, int]:
        left = int(frame_width * PICK_ZONE_LEFT_RATIO)
        right = int(frame_width * PICK_ZONE_RIGHT_RATIO)
        return max(0, left), max(left + 1, min(frame_width - 1, right))

    def annotate_pick_zone(self, frame: np.ndarray) -> None:
        frame_h, frame_w = frame.shape[:2]
        left, right = self.get_pick_zone_bounds(frame_w)
        overlay = frame.copy()
        cv2.rectangle(overlay, (left, 0), (right, frame_h - 1), (0, 180, 0), -1)
        cv2.addWeighted(overlay, 0.12, frame, 0.88, 0, frame)
        cv2.rectangle(frame, (left, 0), (right, frame_h - 1), (0, 220, 0), 2)
        cv2.putText(
            frame,
            "Pick Zone",
            (left + 8, max(24, int(frame_h * 0.08))),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.65,
            (0, 220, 0),
            2,
        )

    @staticmethod
    def box_xyxy(box: Any) -> Tuple[int, int, int, int]:
        return tuple(int(value) for value in box.xyxy[0].cpu().numpy())

    @staticmethod
    def box_confidence(box: Any) -> float:
        return float(box.conf[0]) if hasattr(box.conf, "__len__") else float(box.conf)

    def box_center_in_pick_zone(self, box: Any, frame_width: int) -> bool:
        x1, _y1, x2, _y2 = self.box_xyxy(box)
        center_x = int((x1 + x2) / 2)
        left, right = self.get_pick_zone_bounds(frame_width)
        return left <= center_x <= right

    @staticmethod
    def as_numpy_array(value: Any) -> Optional[np.ndarray]:
        if value is None:
            return None
        if hasattr(value, "cpu"):
            value = value.cpu()
        if hasattr(value, "numpy"):
            value = value.numpy()
        return np.asarray(value)

    @staticmethod
    def clamp_pixel(px: float, py: float, frame_shape: Tuple[int, int, int]) -> Tuple[int, int]:
        frame_h, frame_w = frame_shape[:2]
        pixel_x = max(0, min(frame_w - 1, int(round(px))))
        pixel_y = max(0, min(frame_h - 1, int(round(py))))
        return pixel_x, pixel_y

    def extract_keypoint_by_index(
        self,
        current_points: np.ndarray,
        current_conf: Optional[np.ndarray],
        point_index: int,
        frame_shape: Tuple[int, int, int],
    ) -> Optional[Tuple[int, int]]:
        if point_index >= len(current_points):
            return None

        point = current_points[point_index]
        px, py = float(point[0]), float(point[1])
        if px <= 0 and py <= 0:
            return None
        if current_conf is not None and point_index < len(current_conf):
            if float(current_conf[point_index]) < PICK_KEYPOINT_CONF_THRESHOLD:
                return None
        return self.clamp_pixel(px, py, frame_shape)

    def validate_candidate_keypoints(
        self,
        box: Any,
        cutpoint: Tuple[int, int],
        end_point: Optional[Tuple[int, int]],
    ) -> bool:
        x1, y1, x2, y2 = self.box_xyxy(box)
        box_w = max(1, x2 - x1)
        box_h = max(1, y2 - y1)
        margin = int(max(box_w, box_h) * PICK_KEYPOINT_BOX_MARGIN_RATIO)

        if not (x1 - margin <= cutpoint[0] <= x2 + margin and y1 - margin <= cutpoint[1] <= y2 + margin):
            print(f"Pick candidate skipped: semantic cutpoint outside bbox @ {cutpoint}")
            return False

        cut_rel_y = (cutpoint[1] - y1) / float(box_h)
        if cut_rel_y > PICK_CUTPOINT_MAX_REL_Y:
            print(
                "Pick candidate skipped: semantic cutpoint too low "
                f"(rel_y={cut_rel_y:.2f}) @ {cutpoint}"
            )
            return False

        if end_point is None:
            return True

        if not (x1 - margin <= end_point[0] <= x2 + margin and y1 - margin <= end_point[1] <= y2 + margin):
            print(f"Pick candidate skipped: endpoint outside bbox @ {end_point}")
            return False

        distance = float(np.linalg.norm(np.array(end_point, dtype=float) - np.array(cutpoint, dtype=float)))
        if distance < PICK_KEYPOINT_MIN_DISTANCE_PX:
            print(f"Pick candidate skipped: keypoints too close ({distance:.1f}px)")
            return False

        if end_point[1] + PICK_ENDPOINT_Y_TOLERANCE_PX < cutpoint[1]:
            print(f"Pick candidate skipped: endpoint appears above cutpoint @ cut={cutpoint}, end={end_point}")
            return False

        return True

    def extract_candidate_keypoints(
        self,
        result: Any,
        box_index: int,
        frame_shape: Tuple[int, int, int],
    ) -> Tuple[Optional[Tuple[int, int]], Optional[Tuple[int, int]]]:
        keypoints = getattr(result, "keypoints", None)
        keypoint_xy = self.as_numpy_array(getattr(keypoints, "xy", None))
        if keypoint_xy is None or box_index >= len(keypoint_xy):
            return None, None

        current_points = keypoint_xy[box_index]
        keypoint_conf = self.as_numpy_array(getattr(keypoints, "conf", None))
        current_conf = keypoint_conf[box_index] if keypoint_conf is not None and box_index < len(keypoint_conf) else None

        cutpoint = self.extract_keypoint_by_index(
            current_points,
            current_conf,
            PICK_CUTPOINT_KEYPOINT_INDEX,
            frame_shape,
        )
        end_point = self.extract_keypoint_by_index(
            current_points,
            current_conf,
            PICK_ENDPOINT_KEYPOINT_INDEX,
            frame_shape,
        )
        return cutpoint, end_point

    def build_pick_candidates(self, result: Any, frame_shape: Tuple[int, int, int]) -> List[TomatoCandidate]:
        frame_w = frame_shape[1]
        candidates: List[TomatoCandidate] = []
        boxes = getattr(result, "boxes", None)
        if boxes is None:
            return candidates

        for box_index, box in enumerate(boxes):
            confidence = self.box_confidence(box)
            if int(box.cls) != 0 or confidence < _cfg("PICK_CONFIDENCE_THRESHOLD"):
                continue
            if not self.box_center_in_pick_zone(box, frame_w):
                continue

            cutpoint, end_point = self.extract_candidate_keypoints(result, box_index, frame_shape)
            if cutpoint is None:
                print("Pick candidate skipped: missing semantic cutpoint keypoint")
                continue
            if not self.validate_candidate_keypoints(box, cutpoint, end_point):
                continue
            candidates.append(TomatoCandidate(box=box, box_index=box_index, cutpoint=cutpoint, end_point=end_point))

        return candidates

    def run_detection(self, image: np.ndarray) -> Tuple[np.ndarray, List[TomatoCandidate]]:
        """执行 YOLO 检测并返回带标注画面。"""
        results = self.model(image, classes=0, conf=_cfg("YOLO_DETECT_CONF"), verbose=False)
        result = results[0]
        annotated_image = result.plot()
        self.annotate_pick_zone(annotated_image)
        ripe_tomatoes = self.build_pick_candidates(result, image.shape)
        return annotated_image, ripe_tomatoes

    def serialize_detection_boxes(self, boxes: Sequence[Any]) -> List[Dict[str, Any]]:
        """把 YOLO 检测框整理成可供 UI 叠加显示的结构化信息。"""
        serialized: List[Dict[str, Any]] = []
        for index, item in enumerate(boxes, start=1):
            try:
                box = item.box if isinstance(item, TomatoCandidate) else item
                x1, y1, x2, y2 = self.box_xyxy(box)
                center_x = int((x1 + x2) / 2)
                center_y = int((y1 + y2) / 2)
                cutpoint = list(item.cutpoint) if isinstance(item, TomatoCandidate) and item.cutpoint else []
                end_point = list(item.end_point) if isinstance(item, TomatoCandidate) and item.end_point else []
                serialized.append(
                    {
                        "label": f"Tomato-{index}",
                        "confidence": self.box_confidence(box),
                        "bbox": [x1, y1, x2, y2],
                        "center": [center_x, center_y],
                        "cutpoint": cutpoint,
                        "end_point": end_point,
                    }
                )
            except Exception as exc:
                print(f"检测框序列化失败: {exc}")
        return serialized

    def annotate_target(self, frame: np.ndarray, target: DetectedTomato) -> None:
        """在画面上绘制目标核心信息。"""
        cutpoint = (target.pixel_x, target.pixel_y)
        cv2.drawMarker(frame, cutpoint, (0, 255, 255), cv2.MARKER_CROSS, 18, 2)
        cv2.circle(frame, cutpoint, 6, (0, 255, 255), -1)
        cv2.circle(frame, cutpoint, 11, (0, 80, 255), 2)
        if len(target.end_point) == 2:
            end_point = tuple(int(v) for v in target.end_point)
            cv2.circle(frame, end_point, 5, (255, 215, 0), -1)
            cv2.line(frame, cutpoint, end_point, (255, 255, 0), 2)
            angle_text = f"Angle: {math.degrees(target.angle_rad):.2f}deg"
            (angle_w, _angle_h), angle_baseline = cv2.getTextSize(
                angle_text,
                cv2.FONT_HERSHEY_SIMPLEX,
                0.5,
                2,
            )
            angle_x = max(6, min(end_point[0] + 8, frame.shape[1] - angle_w - 6))
            angle_y = max(18, min(end_point[1] + 18, frame.shape[0] - angle_baseline - 6))
            cv2.putText(
                frame,
                angle_text,
                (angle_x, angle_y),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.5,
                (0, 255, 0),
                2,
            )

    @staticmethod
    def collect_depth_samples(
        depth_frame: Any,
        pixel_x: int,
        pixel_y: int,
        frame_shape: Tuple[int, int, int],
        radius: int,
    ) -> List[float]:
        frame_h, frame_w = frame_shape[:2]
        x_start = max(0, pixel_x - radius)
        x_end = min(frame_w - 1, pixel_x + radius)
        y_start = max(0, pixel_y - radius)
        y_end = min(frame_h - 1, pixel_y + radius)

        samples: List[float] = []
        for sample_y in range(y_start, y_end + 1):
            for sample_x in range(x_start, x_end + 1):
                depth = float(depth_frame.get_distance(sample_x, sample_y))
                if PICK_DEPTH_MIN_M <= depth <= PICK_DEPTH_MAX_M:
                    samples.append(depth)
        return samples

    def get_robust_depth(
        self,
        depth_frame: Any,
        pixel_x: int,
        pixel_y: int,
        frame_shape: Tuple[int, int, int],
    ) -> Optional[float]:
        center_depth = float(depth_frame.get_distance(pixel_x, pixel_y))
        for radius in (PICK_DEPTH_WINDOW_RADIUS, PICK_DEPTH_FALLBACK_WINDOW_RADIUS):
            samples = self.collect_depth_samples(depth_frame, pixel_x, pixel_y, frame_shape, radius)
            if len(samples) < PICK_DEPTH_MIN_VALID_PIXELS:
                continue

            sample_array = np.array(samples, dtype=float)
            median_depth = float(np.median(sample_array))
            stable_samples = sample_array[np.abs(sample_array - median_depth) <= PICK_DEPTH_MEDIAN_TOLERANCE_M]
            if len(stable_samples) >= PICK_DEPTH_MIN_VALID_PIXELS:
                depth = float(np.median(stable_samples))
            else:
                depth = median_depth

            if not (PICK_DEPTH_MIN_M <= center_depth <= PICK_DEPTH_MAX_M):
                print(
                    "采摘点中心深度无效，使用邻域稳健深度 "
                    f"{depth:.3f}m @ ({pixel_x}, {pixel_y}), radius={radius}"
                )
            elif abs(center_depth - depth) > PICK_DEPTH_MEDIAN_TOLERANCE_M:
                print(
                    "采摘点中心深度跳变，使用邻域稳健深度 "
                    f"{depth:.3f}m 替代 {center_depth:.3f}m @ ({pixel_x}, {pixel_y})"
                )
            return depth

        print(
            "采摘点邻域深度不足 "
            f"center={center_depth:.3f}m @ ({pixel_x}, {pixel_y})"
        )
        return None

    def build_detected_tomato(
        self,
        candidate: TomatoCandidate,
        depth_frame: Any,
        frame: np.ndarray,
        tcp_pose: Sequence[float],
    ) -> Optional[DetectedTomato]:
        """把检测框转换成可供机械臂执行的抓取目标。"""
        box = candidate.box
        x1, y1, x2, y2 = self.box_xyxy(box)
        if candidate.cutpoint is None:
            print("Pick target skipped: missing upper YOLO keypoint")
            return None
        pixel_x, pixel_y = candidate.cutpoint

        depth = self.get_robust_depth(depth_frame, pixel_x, pixel_y, frame.shape)
        if depth is None:
            print(f"无效深度值 @ ({pixel_x}, {pixel_y})")
            return None

        point_cam = rs.rs2_deproject_pixel_to_point(self.depth_intrinsics, [pixel_x, pixel_y], depth)
        point_base = camera_to_base(point_cam, self.robot_name)
        if point_base is None:
            print(f"坐标转换失败 @ ({pixel_x}, {pixel_y})，本次目标跳过")
            self.arm_controller.return_home()
            return None

        angle_rad = calculate_keypoint_line_angle_rad(candidate.cutpoint, candidate.end_point)
        print(f"关键点连线角度: {math.degrees(angle_rad):.2f}°")
        robot_pose = list(point_base) + list(tcp_pose[3:6])

        target = DetectedTomato(
            x1=x1,
            y1=y1,
            x2=x2,
            y2=y2,
            pixel_x=pixel_x,
            pixel_y=pixel_y,
            depth=depth,
            point_cam=list(point_cam),
            point_base=list(point_base),
            robot_pose=robot_pose,
            angle_rad=angle_rad,
            cutpoint=[int(pixel_x), int(pixel_y)],
            end_point=list(candidate.end_point) if candidate.end_point else [],
        )
        self.annotate_target(frame, target)
        return target

    def log_target(self, index: int, target: DetectedTomato) -> None:
        """打印目标信息。"""
        print(f"检测到目标 #{index}:")
        print(f"  像素坐标: ({target.pixel_x}, {target.pixel_y})")
        print(
            "  相机坐标: "
            f"({target.point_cam[0]:.3f}, {target.point_cam[1]:.3f}, {target.point_cam[2]:.3f}) m"
        )
        print(
            "  机械臂基坐标: "
            f"({target.point_base[0]:.3f}, {target.point_base[1]:.3f}, {target.point_base[2]:.3f}) m"
        )

    def handle_search_phase(self, raw_image: np.ndarray) -> np.ndarray:
        """AGV 行进时的快速检测阶段。"""
        annotated_image, ripe_tomatoes = self.run_detection(raw_image)

        if ripe_tomatoes and not picking_done.is_set():
            # 这里只负责“发现目标并请求停车”，真正抓取发生在 AGV 停稳后的下一阶段。
            has_tomato.set()
            print(f"视觉检测：识别到 {len(ripe_tomatoes)} 个成熟番茄 | 触发 AGV 暂停")
        elif picking_done.is_set():
            # 一次采摘结束后，把状态机恢复到“边走边看”的初始态。
            clear_pick_cycle()
            print("视觉检测：采摘完成 | 允许 AGV 继续前进")

        return annotated_image

    def handle_pick_phase(self, raw_image: np.ndarray, depth_frame: Any) -> np.ndarray:
        """AGV 停止后的精确检测与抓取阶段。"""
        if start_agv_stop_window():
            # AGV 刚停下时画面可能还在抖，这里故意留一个稳定窗口再做精检和抓取。
            pick_settle_delay = _cfg("AGV_PICK_SETTLE_DELAY")
            print(f"AGV 已停止，等待 {pick_settle_delay:.1f} 秒后开始精确检测...")
            frozen_image, frozen_targets = self.run_detection(raw_image)
            safe_ui_update(
                frozen_image,
                {
                    "freeze": True,
                    "stage": "pick_wait",
                    "detections": self.serialize_detection_boxes(frozen_targets),
                },
            )
            time.sleep(pick_settle_delay)
            return frozen_image

        agv_stop_timeout = _cfg("AGV_STOP_TIMEOUT")
        if agv_stop_elapsed() > agv_stop_timeout:
            print(f"AGV 停止超时 {agv_stop_timeout} 秒，允许继续前进")
            clear_pick_cycle()
            timeout_image = raw_image.copy()
            self.annotate_pick_zone(timeout_image)
            return timeout_image

        annotated_image, ripe_tomatoes = self.run_detection(raw_image)
        if not ripe_tomatoes:
            print("视觉检测：停止窗口内未找到成熟番茄，允许 AGV 继续前进")
            clear_pick_cycle()
            return annotated_image

        tcp_pose = self.arm_controller.get_tcp_pose()
        if tcp_pose is None:
            print("获取末端位姿失败，跳过本次抓取并返回 Home")
            self.arm_controller.return_home()
            clear_pick_cycle()
            return annotated_image

        for index, candidate in enumerate(ripe_tomatoes, start=1):
            try:
                # 逐个候选目标尝试构造成机械臂可执行的抓取目标，成功一个就结束本轮。
                target = self.build_detected_tomato(candidate, depth_frame, annotated_image, tcp_pose)
                if target is None:
                    continue

                detections = self.serialize_detection_boxes([candidate])
                if detections:
                    detections[0]["pick_xyz"] = [float(value) for value in target.point_cam[:3]]
                    detections[0]["pick_xyz_frame"] = "d405_camera"
                    detections[0]["angle_deg"] = float(math.degrees(target.angle_rad))

                safe_ui_update(
                    annotated_image,
                    {
                        "freeze": True,
                        "stage": "pick_target",
                        "detections": detections,
                    },
                )
                self.log_target(index, target)
                if self.arm_controller.execute_pick_and_place(target.robot_pose, target.angle_rad):
                    mark_pick_complete()
                else:
                    clear_pick_cycle()
                break
            except Exception as exc:
                print(f"处理目标时出错: {exc}")
                self.arm_controller.return_home()
                clear_pick_cycle()
                continue
        else:
            print("视觉检测：候选目标均不可用，允许 AGV 继续前进")
            clear_pick_cycle()

        return annotated_image

    def annotate_runtime_status(self, frame: np.ndarray) -> None:
        """绘制运行时状态文本。"""
        cv2.putText(
            frame,
            f"Time Left: {remaining_seconds()}s",
            (10, 30),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.8,
            (0, 0, 255),
            2,
        )
        cv2.putText(
            frame,
            f"Tomato: {'Yes' if has_tomato.is_set() else 'No'}",
            (10, 70),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.8,
            (0, 255, 0),
            2,
        )
        cv2.putText(
            frame,
            f"Picking: {'Yes' if picking_done.is_set() else 'No'}",
            (10, 110),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.8,
            (255, 0, 0),
            2,
        )

    def shutdown(self) -> None:
        """释放视觉资源。"""
        try:
            self.pipeline.stop()
        except Exception as exc:
            print(f"释放相机失败: {exc}")
        safe_ui_update(blank_frame())
        print("视觉线程停止 | 相机已释放")

    def run(self) -> None:
        """视觉主线程。"""
        print("视觉线程启动 | 开始检测番茄...")
        while running.is_set():
            if runtime_expired():
                print("视觉检测：总时长已到，准备停止...")
                running.clear()
                break

            color_image, depth_frame = self.capture_frame()
            if color_image is None or depth_frame is None:
                safe_ui_update(blank_frame())
                time.sleep(0.1)
                continue

            try:
                # 视觉线程本质上是个两阶段状态机：
                # 1. 搜索态：AGV 行进，找候选番茄；
                # 2. 抓取态：AGV 停住，在停止窗口内做精检并触发抓取。
                if has_tomato.is_set() and not picking_done.is_set():
                    annotated_image = self.handle_pick_phase(color_image, depth_frame)
                else:
                    annotated_image = self.handle_search_phase(color_image)
            except Exception as exc:
                print(f"视觉检测逻辑出错: {exc}")
                self.arm_controller.return_home()
                annotated_image = color_image.copy()

            self.annotate_runtime_status(annotated_image)
            safe_ui_update(annotated_image)
            time.sleep(0.05)

        self.shutdown()


@dataclass
class AgvController:
    """AGV 子系统。"""

    agv_client: Any

    def reacquire_priority(self) -> None:
        """重新获取 AGV 控制权。"""
        self.agv_client.release_priority()
        time.sleep(0.5)
        self.agv_client.set_priority("admin", _cfg("AGV_IP"))

    def send_speed(self, linear_speed: float, description: str) -> None:
        """发送 AGV 速度指令。"""
        ret = self.agv_client.set_control_speed(linear_speed, _cfg("AGV_SPEED_STOP"))
        if ret != AGV_SUCCESS_CODE:
            print(f"AGV 控制：{description}指令失败（码 {ret}），重新获取控制权...")
            self.reacquire_priority()

    def stop_motion(self) -> None:
        """停止 AGV。"""
        self.agv_client.set_control_speed(_cfg("AGV_SPEED_STOP"), _cfg("AGV_SPEED_STOP"))

    def shutdown(self) -> None:
        """线程结束时确保 AGV 停止。"""
        try:
            self.stop_motion()
        except Exception as exc:
            print(f"AGV 停止失败: {exc}")
        print("AGV 控制线程停止 | AGV 已停止")

    def run(self) -> None:
        """AGV 控制主线程。"""
        print("AGV 控制线程启动 | 等待视觉检测信号...")
        while running.is_set():
            try:
                if runtime_expired():
                    print("AGV 控制：总时长已到，停止 AGV...")
                    self.stop_motion()
                    running.clear()
                    break

                if has_tomato.is_set() and not picking_done.is_set():
                    # AGV 线程本身不做视觉/抓取，只消费状态位决定“停”还是“走”。
                    self.send_speed(_cfg("AGV_SPEED_STOP"), "停止")
                else:
                    self.send_speed(_cfg("AGV_SPEED_FORWARD"), "前进")
            except Exception as exc:
                print(f"AGV 控制线程出错: {exc}")
                time.sleep(0.5)
                continue

            time.sleep(0.1)

        self.shutdown()


def get_arm_controller(robot_name: str) -> RobotArmController:
    global _current_arm_controller
    if _current_arm_controller is None or _current_arm_controller.robot_name != robot_name:
        _current_arm_controller = RobotArmController(robot_name, robot_rpc_client, placement_manager)
    return _current_arm_controller


def return_to_home(robot_name: str) -> None:
    """兼容旧接口：返回 Home。"""
    controller = get_arm_controller(robot_name)
    controller.return_home()


def control_robot(robot_name: str, pose: Sequence[float], angle_rad: float) -> bool:
    """兼容旧接口：执行抓取与放置。"""
    controller = get_arm_controller(robot_name)
    return controller.execute_pick_and_place(pose, angle_rad)


def vision_detection_thread(pipeline: Any, align: Any, depth_intrinsics: Any, model: Any, robot_name: str) -> None:
    """兼容旧接口：视觉线程入口。"""
    controller = VisionController(
        pipeline=pipeline,
        align=align,
        depth_intrinsics=depth_intrinsics,
        model=model,
        robot_name=robot_name,
        arm_controller=get_arm_controller(robot_name),
    )
    controller.run()


def agv_control_thread(agv_client: Any) -> None:
    """兼容旧接口：AGV 线程入口。"""
    global _current_agv_controller
    _current_agv_controller = AgvController(agv_client)
    _current_agv_controller.run()


def connect_robot_arm() -> RobotArmController:
    """连接并启动机械臂。"""
    robot_rpc_client.connect(_cfg("ROBOT_IP"), _cfg("ROBOT_PORT"))
    robot_rpc_client.setRequestTimeout(1000)
    if not robot_rpc_client.hasConnected():
        raise RuntimeError("机械臂连接失败")
    print("机械臂连接成功")

    robot_rpc_client.login("rob1", "123456")
    if not robot_rpc_client.hasLogined():
        raise RuntimeError("机械臂登录失败")
    print("机械臂登录成功")

    robot_name = exampleStartup()
    if not robot_name:
        raise RuntimeError("机械臂启动失败")
    print(f"机械臂 {robot_name} 启动成功")

    return get_arm_controller(robot_name)


def connect_agv() -> Any:
    """连接 AGV 并获取控制权。"""
    agv_client = RpcClient()
    agv_client.setRequestTimeout(1000)
    agv_ip = _cfg("AGV_IP")
    agv_port = _cfg("AGV_PORT")
    print(f"正在连接 AGV | IP: {agv_ip}:{agv_port}")

    connect_ret = agv_client.connect(agv_ip, agv_port)
    if connect_ret != 0:
        raise RuntimeError(f"AGV 连接失败（返回码 {connect_ret}）")

    login_ret = agv_client.login("admin", "admin")
    if login_ret != 0:
        raise RuntimeError(f"AGV 登录失败（返回码 {login_ret}）")
    print("AGV 连接与登录成功")

    agv_client.release_priority()
    time.sleep(0.5)
    control_ret = agv_client.set_priority("admin", agv_ip)
    if control_ret != AGV_SUCCESS_CODE:
        raise RuntimeError(f"AGV 获取控制权失败（返回码 {control_ret}）")
    print("AGV 控制权获取成功")

    return agv_client


def release_agv(agv_client: Optional[Any]) -> None:
    """释放 AGV 资源。"""
    if agv_client is None:
        return
    try:
        agv_client.release_priority()
    except Exception as exc:
        print(f"释放 AGV 控制权失败: {exc}")
    try:
        agv_client.disconnect()
    except Exception as exc:
        print(f"断开 AGV 失败: {exc}")


def release_robot_arm() -> None:
    """释放机械臂 RPC 连接。"""
    try:
        if robot_rpc_client.hasConnected():
            robot_rpc_client.disconnect()
    except Exception as exc:
        print(f"断开机械臂连接失败: {exc}")


def main() -> None:
    """主程序入口。"""
    global _current_arm_controller, _current_agv_controller

    # 每次主流程启动前都把跨线程状态和放置位游标清零，避免继承上一次运行残留。
    reset_runtime_state()
    placement_manager.reset()

    print("=" * 50)
    print("开始初始化番茄采摘系统")
    print("当前参数：")
    print(f"  机械臂 IP: {_cfg('ROBOT_IP')}:{_cfg('ROBOT_PORT')}")
    print(f"  AGV IP: {_cfg('AGV_IP')}:{_cfg('AGV_PORT')}")
    print(f"  YOLO 模型: {_cfg('YOLO_MODEL_PATH')}")
    print(f"  总时长: {_cfg('TOTAL_DURATION')}s")
    print("=" * 50)

    arm_controller: Optional[RobotArmController] = None
    agv_client: Optional[Any] = None
    pipeline: Optional[Any] = None
    vision_thread: Optional[threading.Thread] = None
    agv_thread: Optional[threading.Thread] = None

    try:
        print("\n1. 初始化机械臂...")
        arm_controller = connect_robot_arm()
        if not _cfg("SCISSORS_ENABLED"):
            arm_controller.keep_scissors_open()
        else:
            arm_controller.reset_scissors_outputs()

        print("\n1.1 机械臂回到初始位置...")
        # 机械臂刚进入 Running 后，控制器有时还没完全准备好接收第一条运动指令。
        # 这里把“回 Home”作为尽力而为的预处理，失败时只告警，不阻塞整机启动。
        if arm_controller.is_at_home():
            print("Robot arm already at Home during startup, continuing initialization")
        else:
            time.sleep(STARTUP_HOME_DELAY)
            if not arm_controller.return_home():
                print("警告：机械臂启动后返回 Home 失败，继续启动系统，后续流程中再尝试回 Home")

        print("\n2. 初始化 AGV...")
        agv_client = connect_agv()

        print("\n3. 初始化视觉...")
        pipeline, align, depth_intrinsics = init_camera()
        model = init_tomato_detector()

        print("\n4. 启动核心线程...")
        start_runtime_state()
        _current_arm_controller = arm_controller
        _current_agv_controller = AgvController(agv_client)

        # 主线程只负责保活和兜底清理；
        # 真正并发执行的是视觉线程和 AGV 控制线程。
        vision_thread = threading.Thread(
            target=vision_detection_thread,
            args=(pipeline, align, depth_intrinsics, model, arm_controller.robot_name),
            daemon=True,
            name="VisionThread",
        )
        agv_thread = threading.Thread(
            target=agv_control_thread,
            args=(agv_client,),
            daemon=True,
            name="AGVThread",
        )

        vision_thread.start()
        agv_thread.start()
        print(f"所有线程启动完成 | 总运行时长：{_cfg('TOTAL_DURATION')} 秒")

        # 主线程不参与业务决策，只等待 `running` 被任一线程或异常路径清掉。
        while running.is_set():
            time.sleep(1)

    except KeyboardInterrupt:
        print("\n用户手动中断程序...")
        running.clear()
        if arm_controller is not None:
            arm_controller.return_home()
    except Exception as exc:
        print(f"系统初始化或运行失败: {exc}")
        running.clear()
        if arm_controller is not None:
            arm_controller.return_home()
    finally:
        print("\n释放系统资源...")
        running.clear()

        if vision_thread is not None and vision_thread.is_alive():
            vision_thread.join(timeout=2.0)
        elif pipeline is not None:
            try:
                pipeline.stop()
            except Exception:
                pass

        if agv_thread is not None and agv_thread.is_alive():
            agv_thread.join(timeout=2.0)

        release_agv(agv_client)
        release_robot_arm()
        reset_runtime_state()
        _current_arm_controller = None
        _current_agv_controller = None
        print("程序结束 | 所有设备已断开连接")


def signal_handler(sig: int, frame: Any) -> None:
    """信号处理：停止线程并尽量回 Home。"""
    _ = frame
    print(f"\n接收到信号 {sig}，正在终止程序并返回 Home...")
    running.clear()

    try:
        if _current_arm_controller is not None:
            _current_arm_controller.return_home()
        else:
            robot_names = robot_rpc_client.getRobotNames()
            if robot_names:
                RobotArmController(robot_names[0], robot_rpc_client, placement_manager).return_home()
    except Exception:
        pass

    time.sleep(1)
    sys.exit(0)
