#!/usr/bin/env python3
"""在当前机械臂的彩色话题上启动 hole_detect。"""

from __future__ import annotations

import os
from pathlib import Path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def _ws_root() -> Path:
    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
        v = os.environ.get(key, "").strip()
        if v:
            return Path(v)
    return Path.home() / "ros2"


def _setup(context, *args, **kwargs):
    root = _ws_root()
    backend = LaunchConfiguration("backend").perform(context) or os.environ.get("SITE_HOLE_BACKEND") or "two_stage"
    weights = LaunchConfiguration("weights").perform(context) or os.environ.get("SITE_HOLE_WEIGHTS") or ""
    cfg = LaunchConfiguration("config_file").perform(context) or os.environ.get("SITE_HOLE_CONFIG") or ""
    cam = os.environ.get("SITE_CAM_NAME") or "d405"
    preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
    if not cfg:
        if backend in ("two_stage", "hough_roi"):
            cfg = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
        else:
            cfg = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
    if not weights:
        if backend in ("two_stage", "hough_roi"):
            weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or ""
        else:
            weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "yolo" / "gongjian0929.onnx")
    return [
        LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"),
        Node(
            package="arm_application",
            executable="hole_detect",
            name="hole_detect",
            output="screen",
            parameters=[
                {
                    "camera_source": cam,
                    "weights": weights,
                    "config_file": cfg,
                    "backend": backend,
                    "device": os.environ.get("SITE_YOLO_DEVICE") or "0",
                    "preview": preview,
                }
            ],
        ),
    ]


def generate_launch_description() -> LaunchDescription:
    return LaunchDescription(
        [
            DeclareLaunchArgument("backend", default_value=""),
            DeclareLaunchArgument("weights", default_value=""),
            DeclareLaunchArgument("config_file", default_value=""),
            DeclareLaunchArgument("preview", default_value="false"),
            OpaqueFunction(function=_setup),
        ]
    )