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

# Helper functions to make passing parameters less disgusting
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():
    # Get package locations
    launch_root = get_package_share_directory('ar3_config')
    description_root = get_package_share_directory('ar3_description')
    config_root = join(launch_root, 'config')

    # Load files for the urdf(robot model), srdf (semantic model), and the kinematics
    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([
        # Start RViz (ui)
        Node(
            package='rviz2',
            executable='rviz2',
            output='log',
            arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
            parameters=[robot_description, robot_description_semantic, robot_description_kinematics, get_yaml(join(config_root, 'ompl_planning.yaml'))]
        ),

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

        # creates the cpp interface so you can use /move_pose
        Node(
            package='move_group_interface',
            executable='move_group_interface_node',
            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")),
                #{ "default_planning_pipeline": "ompl" },
                #{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
        ),

        # Our joystick node
        #Node(
        #    package='joystickmove',
        #    executable='move',
        #    output='screen',
        #),

        # Create joystick controller and ComposableNodes for servo_server
        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",
        ),

        # Setup the controller manager
        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'),

        Node(
                package='voice_move',
                executable='voice_listener',
                output='screen'),
        Node(
                package='high_lvl_cmd_srv',
                executable='high_lvl_cmd_srv',
                output='screen'),

        # Run platform for interface between teensy
        Node(
            package='cobot_platform',
            executable='move',
            output='screen',
        ),

        # Start the actual move_group node/action server
        # TODO: AAAAAAAAAAAA
        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]
        ),
    ])
