import math
from types import SimpleNamespace

from builtin_interfaces.msg import Time
from xr_rm_input.udp_controller_receiver import UdpControllerReceiver
from xr_rm_input.xrobotoolkit_to_udp_bridge import (
    _buttons_payload,
    _controller_payload,
    _stop_controller_payload,
)


def _receiver_without_socket() -> UdpControllerReceiver:
    receiver = object.__new__(UdpControllerReceiver)
    receiver._quat_order = "xyzw"
    receiver.get_clock = lambda: SimpleNamespace(
        now=lambda: SimpleNamespace(to_msg=lambda: Time())
    )
    return receiver


def test_bridge_payload_contains_only_selected_controller_inputs() -> None:
    buttons = _buttons_payload(
        primary=lambda: True,
        secondary=lambda: False,
    )
    payload = _controller_payload(
        hand="left",
        pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0],
        axis=[2.0, -2.0],
        buttons=buttons,
        grip_pressed=True,
        trigger_pressed=False,
    )

    assert payload == {
        "hand": "left",
        "grip": True,
        "trigger": 0.0,
        "pos": [1.0, 2.0, 3.0],
        "quat": [0.0, 0.0, 0.0, 1.0],
        "pose_valid": True,
        "pose_source": "xrobotoolkit",
        "axis": [1.0, -1.0],
        "buttons": {
            "primary": True,
            "secondary": False,
        },
    }


def test_stop_payload_uses_neutral_selected_inputs() -> None:
    payload = _stop_controller_payload("right")

    assert payload["axis"] == [0.0, 0.0]
    assert payload["buttons"] == {
        "primary": False,
        "secondary": False,
    }
    assert "grip_value" not in payload
    assert "trigger_value" not in payload


def test_receiver_publishes_selected_controller_inputs() -> None:
    msg = _receiver_without_socket()._payload_to_msg(
        {
            "grip": True,
            "trigger": 1.0,
            "axis": [2.0, -2.0],
            "buttons": {
                "primary": True,
                "secondary": False,
            },
            "pos": [1.0, 2.0, 3.0],
            "quat": [0.0, 0.0, 0.0, 1.0],
        },
        "left",
    )

    assert msg.primary is True
    assert msg.secondary is False
    assert list(msg.axis) == [1.0, -1.0]


def test_receiver_defaults_invalid_optional_inputs() -> None:
    msg = _receiver_without_socket()._payload_to_msg(
        {
            "grip": True,
            "trigger": 0.4,
            "axis": [math.nan, 0.0],
            "buttons": [],
            "pos": [1.0, 2.0, 3.0],
            "quat": [0.0, 0.0, 0.0, 1.0],
        },
        "right",
    )

    assert msg.primary is False
    assert msg.secondary is False
    assert list(msg.axis) == [0.0, 0.0]
    assert msg.grip is True
    assert abs(msg.trigger - 0.4) < 1e-6
    assert msg.pose.position.x == 1.0
    assert msg.pose.position.y == 2.0
    assert msg.pose.position.z == 3.0


def test_receiver_defaults_missing_legacy_optional_inputs() -> None:
    msg = _receiver_without_socket()._payload_to_msg(
        {
            "grip": True,
            "trigger": 0.0,
            "pos": [0.0, 1.0, 0.0],
            "quat": [0.0, 0.0, 0.0, 1.0],
        },
        "left",
    )

    assert msg.primary is False
    assert msg.secondary is False
    assert list(msg.axis) == [0.0, 0.0]
    assert msg.grip is True