import os
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument("sim_gazebo",
                              default_value="false",
                              description="Gazebo Simulation")
    )
    declared_arguments.append(
        DeclareLaunchArgument("gazebo_gui",
                              default_value="false",
                              description="Launch env_manager?")
    )
    declared_arguments.append(
        DeclareLaunchArgument("debugger",
                              default_value="false",
                              description="launch Gazebo with debugger?")
    )
    declared_arguments.append(
        DeclareLaunchArgument("launch_env_manager",
                              default_value="false",
                              description="Launch env_manager?")
    )
    declared_arguments.append(
        DeclareLaunchArgument("rgbd_camera",
                              default_value="true",
                              description="Camera are used?")
    )
   
    sim_gazebo = LaunchConfiguration("sim_gazebo")
    gazebo_gui = LaunchConfiguration("gazebo_gui")
    debugger = LaunchConfiguration("debugger")
    rgbd_camera = LaunchConfiguration("rgbd_camera")
    launch_env_manager = LaunchConfiguration("launch_env_manager")
    gazebo_world_filename = LaunchConfiguration("gazebo_world_filename")

    world_config_file = PathJoinSubstitution(
        [FindPackageShare("rbs_simulation"), "worlds", gazebo_world_filename]
    )
    gazebo_server = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [os.path.join(get_package_share_directory('ros_gz_sim'),
                            'launch', 'gz_sim.launch.py')]),
            launch_arguments={
                'gz_args': [' -s ', '-r ', world_config_file],
                "debugger": debugger,
            }.items(),
            condition=UnlessCondition(gazebo_gui))
    
    gazebo = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [os.path.join(get_package_share_directory('ros_gz_sim'),
                            'launch', 'gz_sim.launch.py')]),
            launch_arguments={
                'gz_args': [' -r ', world_config_file],
                "debugger": debugger,
            }.items(),
            condition=IfCondition(gazebo_gui))

    env_manager = Node(
        package="env_manager",
        executable="run_env_manager",
        parameters=[{'use_sim_time': True}],
        condition=IfCondition(launch_env_manager)
    )

    rgbd_sensor_bridge = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=[
            '/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
            '/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
            '/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
            '/rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
        ],
        output='screen',
        condition=IfCondition(rgbd_camera)
    )
    
   
    clock_bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'
        ],
        output='screen',
        condition=IfCondition(sim_gazebo))

    nodes_to_start = [
        gazebo,
        gazebo_server,
        clock_bridge,
        rgbd_sensor_bridge,
        env_manager
    ]
    return LaunchDescription(declared_arguments + nodes_to_start)