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


# 最小演示启动完整闭环：车辆画像 + 外部定位 + 传感器 + 底盘/运控（桩节点或 WiFi 网关）+ 车间编排。
# 参数：
#   use_gateway（布尔值，默认 false）：true 时用 vehicle_agent_gateway 替换本地底盘/运控桩节点。
#   chassis_host / control_host：Windows 车端 IP（仅 use_gateway=true 时生效）。
#   profile_storage_path：vehicle_profile_manager 的画像持久化文件。
#   sensor_storage_root / sensor_registry / capture_pipeline_name / telemetry_topics：
#       传感器 readiness 的最小真实配置入口。
#   data_input_params_file：可选 ROS 参数文件，用于把真实采集/ingest 产物路径注入算法输入。
#   dataset_index_file：可选数据集索引文件；为空时主控会按 data_input_params_file 同目录自动查找。
def generate_launch_description():
    use_gateway_arg = DeclareLaunchArgument(
        "use_gateway",
        default_value="false",
        description="true: 使用 WiFi 网关节点连接 Windows 车端；false: 使用本地 stub 节点",
    )
    chassis_host_arg = DeclareLaunchArgument(
        "chassis_host", default_value="192.168.1.100",
        description="Windows 车端 IP（use_gateway=true 时生效）",
    )
    control_host_arg = DeclareLaunchArgument(
        "control_host", default_value="192.168.1.100",
        description="Windows 车端 IP（use_gateway=true 时生效）",
    )
    profile_storage_path_arg = DeclareLaunchArgument(
        "profile_storage_path",
        default_value="/tmp/agv_calib_vehicle_profiles.db",
        description="vehicle_profile_manager 的画像持久化文件路径",
    )
    sensor_storage_root_arg = DeclareLaunchArgument(
        "sensor_storage_root",
        default_value="/tmp/agv_sensor_calibration",
        description="sensor_calibration_service 的产物目录",
    )
    sensor_registry_arg = DeclareLaunchArgument(
        "sensor_registry",
        default_value="demo_front_camera",
        description="逗号分隔的可用传感器 ID 列表",
    )
    capture_pipeline_name_arg = DeclareLaunchArgument(
        "capture_pipeline_name",
        default_value="sensor_capture_pipeline",
        description="传感器采集链路名称",
    )
    telemetry_topics_arg = DeclareLaunchArgument(
        "telemetry_topics",
        default_value="/sensor_calibration/telemetry",
        description="逗号分隔的传感器遥测 topic 列表",
    )
    vehicle_safe_to_move_arg = DeclareLaunchArgument(
        "vehicle_safe_to_move",
        default_value="true",
        description="传感器任务若需要移动车辆时，当前是否允许移动",
    )
    arm_ready_arg = DeclareLaunchArgument(
        "arm_ready",
        default_value="true",
        description="当前机械臂是否就绪",
    )
    data_input_params_file_arg = DeclareLaunchArgument(
        "data_input_params_file",
        default_value="",
        description="可选 ROS 参数文件，包含各 service 的 data_input.* 数据文件引用配置",
    )
    dataset_index_file_arg = DeclareLaunchArgument(
        "dataset_index_file",
        default_value="",
        description="可选数据集索引文件，用于最终报告追溯本次真实采集数据",
    )

    def make_nodes(context):
        use_gateway = LaunchConfiguration("use_gateway").perform(context).lower() == "true"
        chassis_host = LaunchConfiguration("chassis_host").perform(context)
        control_host = LaunchConfiguration("control_host").perform(context)
        profile_storage_path = LaunchConfiguration("profile_storage_path").perform(context)
        sensor_storage_root = LaunchConfiguration("sensor_storage_root").perform(context)
        sensor_registry = LaunchConfiguration("sensor_registry").perform(context)
        capture_pipeline_name = LaunchConfiguration("capture_pipeline_name").perform(context)
        telemetry_topics = LaunchConfiguration("telemetry_topics").perform(context)
        vehicle_safe_to_move = LaunchConfiguration("vehicle_safe_to_move").perform(context).lower() == "true"
        arm_ready = LaunchConfiguration("arm_ready").perform(context).lower() == "true"
        data_input_params_file = LaunchConfiguration("data_input_params_file").perform(context).strip()
        dataset_index_file = LaunchConfiguration("dataset_index_file").perform(context).strip()

        def with_data_input_params(params=None):
            merged = []
            if data_input_params_file:
                merged.append(data_input_params_file)
            if params is not None:
                merged.append(params)
            return merged

        common_nodes = [
            Node(
                package="vehicle_profile_manager",
                executable="vehicle_profile_manager_node",
                name="vehicle_profile_manager",
                output="screen",
                parameters=[{
                    "profile_storage_path": profile_storage_path,
                }],
            ),
            Node(
                package="external_localization_service",
                executable="external_localization_service_node",
                name="external_localization_service",
                output="screen",
                parameters=with_data_input_params(),
            ),
            Node(
                package="sensor_calibration_service",
                executable="sensor_calibration_service_node",
                name="sensor_calibration_service",
                output="screen",
                parameters=with_data_input_params({
                    "sensor_storage_root": sensor_storage_root,
                    "sensor_registry": sensor_registry,
                    "capture_pipeline_name": capture_pipeline_name,
                    "telemetry_topics": telemetry_topics,
                    "vehicle_safe_to_move": vehicle_safe_to_move,
                    "arm_ready": arm_ready,
                }),
            ),
            Node(
                package="workshop_orchestrator_v2",
                executable="workshop_orchestrator_v2_node",
                name="workshop_orchestrator_v2",
                output="screen",
                parameters=[{
                    "data_input_params_file": data_input_params_file,
                    "dataset_index_file": dataset_index_file,
                }],
            ),
        ]

        if use_gateway:
            # WiFi 网关模式：底盘和运控通过统一 TCP 端口转发到 Windows 车端。
            chassis_control_nodes = [
                Node(
                    package="vehicle_agent_gateway",
                    executable="vehicle_agent_gateway_node",
                    output="screen",
                    parameters=[{
                        "chassis_host": chassis_host,
                        "chassis_port": 9000,
                        "chassis_timeout_ms": 5000,
                        "control_host": control_host,
                        "control_port": 9000,
                        "control_timeout_ms": 5000,
                    }],
                ),
            ]
        else:
            # 本地桩节点模式：底盘和运控使用本地桩节点，无需 Windows 连接。
            chassis_control_nodes = [
                Node(
                    package="chassis_calibration_service",
                    executable="chassis_calibration_service_node",
                    name="chassis_calibration_service",
                    output="screen",
                    parameters=with_data_input_params(),
                ),
                Node(
                    package="control_calibration_service",
                    executable="control_calibration_service_node",
                    name="control_calibration_service",
                    output="screen",
                    parameters=with_data_input_params(),
                ),
            ]

        return common_nodes + chassis_control_nodes

    return LaunchDescription([
        use_gateway_arg,
        chassis_host_arg,
        control_host_arg,
        profile_storage_path_arg,
        sensor_storage_root_arg,
        sensor_registry_arg,
        capture_pipeline_name_arg,
        telemetry_topics_arg,
        vehicle_safe_to_move_arg,
        arm_ready_arg,
        data_input_params_file_arg,
        dataset_index_file_arg,
        OpaqueFunction(function=make_nodes),
    ])
