from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def _as_bool(value: str) -> bool:
    return value.strip().lower() in ("1", "true", "yes", "on")


def _config_file(name: str) -> PathJoinSubstitution:
    return PathJoinSubstitution([
        FindPackageShare("xr_rm_bringup"),
        "config",
        name,
    ])


def _udp_receiver_node() -> Node:
    return Node(
        package="xr_rm_input",
        executable="udp_controller_receiver",
        name="udp_controller_receiver",
        output="screen",
        parameters=[{
            "udp_host": LaunchConfiguration("udp_host"),
            "udp_port": LaunchConfiguration("udp_port"),
            "left_topic": "/xr/left_controller",
            "right_topic": "/xr/right_controller",
        }],
    )


def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node:
    config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
    robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
    return Node(
        package="xr_rm_teleop",
        executable="single_arm_velocity_teleop",
        name="single_arm_velocity_teleop",
        output="screen",
        parameters=[
            _config_file(config_name),
            {
                "use_mock": use_mock,
                "robot_ip": robot_ip,
                "robot_port": LaunchConfiguration("robot_port"),
                "move_to_initial_pose_on_connect": move_to_initial_pose,
            },
        ],
    )


def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
    config_file = _config_file("dual_arm_rm75.yaml")
    return [
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="left_arm_teleop",
            output="screen",
            parameters=[
                config_file,
                {
                    "use_mock": use_mock,
                    "robot_ip": LaunchConfiguration("left_robot_ip"),
                    "robot_port": LaunchConfiguration("robot_port"),
                    "move_to_initial_pose_on_connect": move_to_initial_pose,
                },
            ],
        ),
        Node(
            package="xr_rm_teleop",
            executable="single_arm_velocity_teleop",
            name="right_arm_teleop",
            output="screen",
            parameters=[
                config_file,
                {
                    "use_mock": use_mock,
                    "robot_ip": LaunchConfiguration("right_robot_ip"),
                    "robot_port": LaunchConfiguration("robot_port"),
                    "move_to_initial_pose_on_connect": move_to_initial_pose,
                },
            ],
        ),
    ]


def _launch_setup(context, *args, **kwargs):
    del args, kwargs
    arm = LaunchConfiguration("arm").perform(context).strip().lower()
    use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
    move_to_initial_pose = _as_bool(
        LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
    )

    if arm not in ("left", "right", "both"):
        raise ValueError("arm must be one of: left, right, both")

    nodes = [_udp_receiver_node()]
    if arm == "both":
        nodes.extend(_dual_arm_nodes(use_mock, move_to_initial_pose))
    else:
        nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose))
    return nodes


def generate_launch_description() -> LaunchDescription:
    return LaunchDescription([
        DeclareLaunchArgument("arm", default_value="right"),
        DeclareLaunchArgument("use_mock", default_value="true"),
        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
        DeclareLaunchArgument("udp_port", default_value="15000"),
        DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
        DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
        DeclareLaunchArgument("robot_port", default_value="8080"),
        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
        OpaqueFunction(function=_launch_setup),
    ])