#!/usr/bin/env python3
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    pkg_share = FindPackageShare('t_shape_robot')
    t_width_arg = DeclareLaunchArgument('t_width', default_value='3.0')
    t_height_arg = DeclareLaunchArgument('t_height', default_value='2.0')
    linear_speed_arg = DeclareLaunchArgument('linear_speed', default_value='0.3')
    angular_speed_arg = DeclareLaunchArgument('angular_speed', default_value='0.5')
    loop_enabled_arg = DeclareLaunchArgument('loop_enabled', default_value='true')
    
    urdf_file = PathJoinSubstitution([pkg_share, 'urdf', 'robot.urdf'])
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'robot_description': Command(['cat ', urdf_file])}]
    )
    
    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen'
    )
    
    t_shape_controller_node = Node(
        package='t_shape_robot',
        executable='t_shape_controller',
        name='t_shape_controller',
        output='screen',
        parameters=[{
            't_width': LaunchConfiguration('t_width'),
            't_height': LaunchConfiguration('t_height'),
            'linear_speed': LaunchConfiguration('linear_speed'),
            'angular_speed': LaunchConfiguration('angular_speed'),
            'loop_enabled': LaunchConfiguration('loop_enabled')
        }]
    )
    
    rviz_config_file = PathJoinSubstitution([pkg_share, 'rviz', 'config.rviz'])
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', rviz_config_file]
    )
    
    return LaunchDescription([
        t_width_arg,
        t_height_arg,
        linear_speed_arg,
        angular_speed_arg,
        loop_enabled_arg,
        robot_state_publisher_node,
        joint_state_publisher_node,
        t_shape_controller_node,
        rviz_node
    ])