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_world_path = os.path.join(urdf_tutorial_path, 'world', 'table.world')
    default_model_path = urdf_tutorial_path + '/urdf/hiveBot0/hive_core_bot0.urdf.xacro'

    # 声明模型路径参数
    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.55',  # 初始位置z (高于地面)
            '-Y', '0.0'   # 初始朝向
        ]
    )
    
    # 加载并激活 load_joint_state_controller 控制器
    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
            'bot0_joint_state_broadcaster'],
        output='screen'
    )

    # 加载并激活 load_bot0_position_controller 控制器
    load_bot0_position_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','bot0_position_controller'], 
        output='screen')
    
    load_bot0_wheel_velocity_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','bot0_wheel_velocity_controller'], 
        output='screen')

    action_node_arm_control = launch_ros.actions.Node(
        package='hive_core_r0',
        executable='arm_control_node',
        name='arm_control_node',
        output='screen'
    )
    
    return LaunchDescription([
        action_declare_arg_mode_path,
        
        # 先启动Gazebo
        launch_gazebo,
        
        # 然后启动状态发布器
        robot_state_publisher_node,
        
        # 延迟后生成机器人（确保Gazebo完全启动）
        TimerAction(
            period= 5.0, 
            actions=[spawn_entity_node]
        ),

        # 事件动作，当加载机器人结束后执行    
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=spawn_entity_node,
                on_exit=[load_joint_state_controller, load_bot0_position_controller, load_bot0_wheel_velocity_controller],)
            ),

        launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=load_bot0_position_controller,
            on_exit=[action_node_arm_control],)
            ),

    ])