"""Launch Franka fr3 in Gazebo with a joint_trajectory_controller for HPP tutorial 6.

Usage (inside the docker container):
    ros2 launch hpp_tutorial tutorial_7/launch_sim.py
"""

import os

import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchContext, LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    OpaqueFunction,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def get_robot_description_and_controllers(context: LaunchContext, robot_type):
    robot_type_str = context.perform_substitution(robot_type)

    tutorial_dir = os.path.join(
        get_package_share_directory("hpp_tutorial"), "tutorial_7"
    )
    controllers_yaml = os.path.join(tutorial_dir, "controllers.yaml")

    franka_xacro = os.path.join(
        get_package_share_directory("franka_gazebo_bringup"),
        "urdf",
        "franka_arm.gazebo.xacro",
    )

    robot_description_xml = xacro.process_file(
        franka_xacro,
        mappings={
            "robot_type": robot_type_str,
            "hand": "false",
            "ros2_control": "true",
            "gazebo": "true",
            "ee_id": "franka_hand",
        },
    ).toxml()

    # Replace the Franka default controllers YAML with our tutorial controllers
    franka_controllers = os.path.join(
        get_package_share_directory("franka_gazebo_bringup"),
        "config",
        "franka_gazebo_controllers.yaml",
    )
    robot_description_xml = robot_description_xml.replace(
        franka_controllers, controllers_yaml
    )

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[{"robot_description": robot_description_xml}],
    )

    # Spawn controllers using ros2_control spawner
    spawn_jsb = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
        output="screen",
    )

    spawn_jtc = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_trajectory_controller"],
        output="screen",
    )

    return [robot_state_publisher, spawn_jsb, spawn_jtc]


def generate_launch_description():
    robot_type = LaunchConfiguration("robot_type")

    # Gazebo Sim
    os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname(
        get_package_share_directory("franka_description")
    )
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory("ros_gz_sim"),
                "launch",
                "gz_sim.launch.py",
            )
        ),
        launch_arguments={"gz_args": "empty.sdf -r"}.items(),
    )

    spawn = Node(
        package="ros_gz_sim",
        executable="create",
        arguments=["-topic", "/robot_description"],
        output="screen",
    )

    bridge = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
        output="screen",
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument("robot_type", default_value="fr3"),
            gazebo,
            OpaqueFunction(
                function=get_robot_description_and_controllers, args=[robot_type]
            ),
            spawn,
            bridge,
        ]
    )
