from os.path import join
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.actions import ExecuteProcess
import yaml

def get_file(path):
    with open(path, 'r') as f:
        return f.read()

def get_yaml(path):
    with open(path, 'r') as f:
        return yaml.safe_load(f)


def generate_launch_description():
    launch_root = get_package_share_directory('ar3_config')
    description_root = get_package_share_directory('ar3_description')
    config_root = join(launch_root, 'config')

    robot_description  = {'robot_description': get_file(join(description_root, 'ar3.urdf'))}
    robot_description_semantic = {'robot_description_semantic': get_file(join(description_root, 'ar3.srdf'))}
    robot_description_kinematics = get_yaml(join(config_root, 'kinematics.yaml'))

    return LaunchDescription([
        # RViz
        Node(
            package='rviz2',
            executable='rviz2',
            output='log',
            arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
        ),

        # Attatch robot to map
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            output='screen',
            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
        ),

        Node(
            package='move_group_interface',
            executable='move_group_interface_ndde',
            output='screen',
        ),
        Node(
            package='move_group_interface',
            executable='client_node',
            output='screen',
        ),
        Node(
            package='joystickmove',
            executable='move',
            output='screen',
        ),

        # Create joystick controller
        ComposableNodeContainer(
            name="joystick_container",
            namespace="/",
            package="rclcpp_components",
            executable="component_container",
            composable_node_descriptions=[
                ComposableNode(
                    package="moveit_servo",
                    plugin="moveit_servo::ServoServer",
                    name="servo_server",
                    parameters=[
                        {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
                        robot_description,
                        robot_description_semantic,
                        robot_description_kinematics,
                    ],
                    extra_arguments=[{"use_intra_process_comms": True}],
                ),
                ComposableNode(
                    package="joy",
                    plugin="joy::Joy",
                    name="joy_node",
                    extra_arguments=[{"use_intra_process_comms": True}],
                )
            ],
            output="screen",
        ),

        # Controller
        Node(
            package='controller_manager',
            executable='ros2_control_node',
            output='screen',
            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
        ),

        # Start controllers
        ExecuteProcess(
            cmd=['ros2 run controller_manager spawner.py joint_trajectory_controller'],
            shell=True,
            output='screen'),
        ExecuteProcess(
            cmd=['ros2 run controller_manager spawner.py joint_state_broadcaster'],
            shell=True,
            output='screen'),

        # Run platform
        Node(
            package='cobot_platform',
            executable='move',
            output='screen',
        ),

        # Start the actual move_group node/action server
        Node(
            package="moveit_ros_move_group",
            executable="move_group",
            output="screen",
            parameters=[
                robot_description,
                robot_description_semantic,
                robot_description_kinematics,
                get_yaml(join(config_root, 'ompl_planning.yaml')),
                get_yaml(join(config_root, 'trajectory.yaml')),
                get_yaml(join(config_root, 'planning_scene.yaml')),
                get_yaml(join(config_root, "ar3_controllers.yaml")),
            ],
        ),

        # Robot state publisher
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            output='screen',
            parameters=[robot_description]
        ),
    ])