from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler,IncludeLaunchDescription
from launch.conditions import IfCondition,UnlessCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration

from launch_ros.actions import Node, PushRosNamespace
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    use_sim_time  = LaunchConfiguration('use_sim_time')
    use_hardware  = LaunchConfiguration('use_hardware')
    namespace     = LaunchConfiguration('namespace')
    enable_camera = LaunchConfiguration('enable_camera')

    declared_args = [
        DeclareLaunchArgument(
            'namespace', default_value='',
            description='ROS namespace for all robot nodes (e.g. "robot1").'
        ),
        DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation time.'
        ),
        DeclareLaunchArgument(
            'use_hardware', default_value='false',
            description='Load the real I2C hardware interface plugin (set true on physical robot).'
        ),
        DeclareLaunchArgument(
            'enable_camera', default_value='false',
            description='Enable the camera node.'
        ),
    ]

    
    robot_controllers = PathJoinSubstitution([
        FindPackageShare('raspbot_v2_control'), 'config', 'controllers.yaml'
    ])
    
    robot_description_content = Command([
        PathJoinSubstitution([FindExecutable(name='xacro')]),
        ' ',
        PathJoinSubstitution([
            FindPackageShare('raspbot_v2_description'), 'urdf', 'raspbot_v2.urdf.xacro'
        ]),
        ' use_sim_time:=', use_sim_time,
        ' use_hardware:=', use_hardware,
        ' controller_config:=', robot_controllers,
    ])
    robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
        
    control_node = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[robot_description, robot_controllers],
        output='both',
        remappings=[
            # ("~/robot_description", "robot_description"),
            ("mecanum_drive_controller/reference", "/cmd_vel"),
            ("mecanum_drive_controller/odometry", "/odom"),
            ("mecanum_drive_controller/tf_odometry", "/tf"),
        ],
        condition=UnlessCondition(use_sim_time),
    )
    
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='both',
        parameters=[robot_description],
    )
    
    joint_state_broadcaster_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster', '--controller-manager',
                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
                   '--controller-manager-timeout', '30',
        ],
    )

    robot_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['mecanum_drive_controller', '--controller-manager',
                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
                   '--controller-manager-timeout', '30',
        ]
    )
    
    # Delay starting the robot controller until the joint state broadcaster has finished starting up.
    delay_robot_controller_after_joint_state_broadcaster = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[robot_controller_spawner],
        ),
    )
    
    pan_tilt_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['pan_tilt_controller', '--controller-manager',
                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
                   '--controller-manager-timeout', '30',
        ],
        condition=IfCondition(enable_camera),
    )
    
    return LaunchDescription([
        *declared_args,
        PushRosNamespace(namespace),
        control_node,
        robot_state_publisher_node,
        joint_state_broadcaster_spawner,
        delay_robot_controller_after_joint_state_broadcaster,
        pan_tilt_controller_spawner,
    ])