import os
import launch
import launch_ros
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.event_handlers import OnProcessStart
from launch.actions import TimerAction

def generate_launch_description():
    # 声明参数
    robot_name_in_model = "HiveBot0"
    urdf_package = 'hive_core_r0'
    urdf_tutorial_path = get_package_share_directory(urdf_package)
    default_model_path = urdf_tutorial_path + '/urdf/hiveBot1/hive_core_bot1.urdf.xacro'
    default_world_path = os.path.join(urdf_tutorial_path, 'world', 'empty.world')

    # 声明模型路径参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value=str(default_model_path),
        description='URDF 的绝对路径')
    
    # 获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    
    # 声明机器人状态发布节点
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}],
    )

    # 启动Gazebo - 添加 paused=true 参数使仿真暂停
    launch_gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch',
            'gazebo.launch.py')
        ]),
        launch_arguments={
            'world': default_world_path,
            'verbose': 'true',
        }.items()
    )
    
    # 在Gazebo中生成机器人
    spawn_entity_node = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=[
            '-entity', robot_name_in_model,  # 模型名称
            '-topic', '/robot_description',  # 从ROS话题获取URDF
            '-x', '0.0',  # 初始位置x
            '-y', '0.0',  # 初始位置y
            '-z', '0.45',  # 初始位置z (高于地面)
            '-Y', '0.0'   # 初始朝向
        ]
    )

    # 1. 启动controller_manager（通常由gazebo_ros2_control自动启动，若未启动则显式添加）
    controller_manager_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            {'robot_description': robot_description},
            os.path.join(urdf_tutorial_path, 'config', 'bot1_ros2_controller.yaml')  # 显式指定配置文件
        ],
        output="screen"
    )
    
    # 加载并激活 load_joint_state_controller 控制器
    load_joint_state_controller = TimerAction(
        period=3.0,  # 延迟3秒，等待初始化
        actions=[ExecuteProcess(
            cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 
                'bot1_joint_state_broadcaster'],
            output='screen'
        )]
    )

    # 加载并激活 load_bot0_position_controller 控制器
    load_bot1_position_controller = TimerAction(
        period=2.0,  # 延迟2秒
        actions=[ExecuteProcess(
            cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 
                'bot1_position_controller'],
            output='screen'
        )]
    )


    action_node_hiveBot1_control = launch_ros.actions.Node(
        package='hive_core_r0',
        executable='hiveBot1_control_node',
        name='hiveBot1_control_node',
        output='screen'
    )

    # 4. 延迟启动控制节点（在位置控制器加载后）
    start_control_node = TimerAction(
        period=2.0,
        actions=[action_node_hiveBot1_control]
    )

    keyboard_control_node = launch_ros.actions.Node(
        package='hive_core_r0',
        executable='keyboard_control_node',
        name='keyboard_control_node',
        output='screen'
    )
    # 5. 延迟启动键盘控制节点（在控制节点启动后）
    start_keyboard_control_node = TimerAction(
        period=2.0,
        actions=[keyboard_control_node]
    )
    
    # 在LaunchDescription中按顺序添加
    return LaunchDescription([
        action_declare_arg_mode_path,
        launch_gazebo,
        robot_state_publisher_node,
        # 延迟后生成机器人（确保Gazebo完全启动）
        TimerAction(
            period= 5.0, 
            actions=[spawn_entity_node]
        ),
        controller_manager_node,  # 显式启动控制器管理器
        load_joint_state_controller,
        load_bot1_position_controller,
        start_control_node,
        # start_keyboard_control_node,
    ])