"""Launch RViz2 with X2 robot model for visualization."""
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command, FindExecutable
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    pkg_dir = get_package_share_directory('x2_description')
    pkg_dir_viz = get_package_share_directory('robot_viz')
    pkg_dir_moveit = get_package_share_directory('x2_moveit')

    use_sim_arg = DeclareLaunchArgument('use_sim', default_value='false')
    controllers_yaml = os.path.join(pkg_dir_moveit, 'config', 'ros2_controllers.yaml')

    robot_description = ParameterValue(
        Command([
            FindExecutable(name='xacro'), ' ',
            os.path.join(pkg_dir, 'xacro', 'robot.xacro'),
            ' use_sim:=', LaunchConfiguration('use_sim'),
            ' use_fake_hardware:=true',
        ]),
        value_type=str,
    )

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}],
    )

    # joint_state_publisher_gui = Node(
    #     package='joint_state_publisher_gui',
    #     executable='joint_state_publisher_gui',
    # )

    ros2_control_node = Node(
        package = 'controller_manager',
        executable = 'ros2_control_node',
        parameters = [{'robot_description': robot_description}, controllers_yaml],
        output = 'screen',
    )

    joint_state_broadcaster = Node(
        package = 'controller_manager',
        executable = 'spawner',
        arguments = ['joint_state_broadcaster'],
    )

    left_arm_controller = Node(
        package = 'controller_manager',
        executable = 'spawner',
        arguments = ['left_arm_controller'],
    )

    right_arm_controller = Node(
        package = 'controller_manager',
        executable = 'spawner',
        arguments = ['right_arm_controller'],
    )

    rviz = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', os.path.join(pkg_dir_viz, 'rviz', 'model.rviz')],
    )

    # move_group = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(
    #         os.path.join(pkg_dir_moveit, 'launch', 'move_group.launch.py')
    #     ),
    # )

    return LaunchDescription([
        use_sim_arg,
        robot_state_publisher,
        joint_state_broadcaster,
        ros2_control_node,
        left_arm_controller,
        right_arm_controller,
        rviz,
        #move_group,
    ])
