aima_phone_catcher/tests/test_controller.py

import threading
import time

import pytest

from lingkong_motor_service.config import Settings
from lingkong_motor_service.controller import (
    MotorCommunicationError,
    MotorController,
    MotorNotEnabledError,
    MotorState,
)
from lingkong_motor_service.protocol import CONTROL_FRAME_ID, MIXED_COMMAND_FRAME_ID, CanFrame
from lingkong_motor_service.transport import CanTransportError

from .fakes import FakeTransport


class BlockingSecondControlSendTransport(FakeTransport):
    """Hold the sender's first refresh while allowing the synchronous send."""

    def __init__(self) -> None:
        super().__init__()
        self.background_send_entered = threading.Event()
        self.release_background_send = threading.Event()
        self._nonzero_sends = 0
        self._count_lock = threading.Lock()

    def send(self, frame: CanFrame, *, tx_queue_timeout_s: float | None = None) -> None:
        if frame.arbitration_id == CONTROL_FRAME_ID and frame.data != bytes(8):
            with self._count_lock:
                self._nonzero_sends += 1
                should_block = self._nonzero_sends == 2
            if should_block:
                self.background_send_entered.set()
                self.release_background_send.wait(timeout=1.0)
        super().send(frame, tx_queue_timeout_s=tx_queue_timeout_s)


class FailOnThirdZeroTransport(FakeTransport):
    """Fail the strict zero frame during an open-to-close transition."""

    def __init__(self) -> None:
        super().__init__()
        self._zero_count = 0

    def send(self, frame: CanFrame, *, tx_queue_timeout_s: float | None = None) -> None:
        if frame.arbitration_id == CONTROL_FRAME_ID and frame.data == bytes(8):
            self._zero_count += 1
            if self._zero_count == 3:
                raise CanTransportError("injected transition stop failure")
        super().send(frame, tx_queue_timeout_s=tx_queue_timeout_s)


def enabled_settings(**overrides: object) -> Settings:
    values = {
        "hardware_enabled": True,
        "motor_ids": (1, 2),
        "broadcast_frequency_hz": 500,
    }
    values.update(overrides)
    return Settings(**values)


def frame_dump(transport: FakeTransport) -> list[tuple[int, str]]:
    with transport.lock:
        return [(frame.arbitration_id, frame.data.hex()) for frame in transport.frames]


def wait_for_payload_count(transport: FakeTransport, payload: bytes, count: int) -> None:
    deadline = time.monotonic() + 0.2
    while time.monotonic() < deadline:
        with transport.lock:
            found = sum(frame.data == payload for frame in transport.frames)
        if found >= count:
            return
        time.sleep(0.002)
    raise AssertionError(f"did not observe {count} copies of {payload.hex()}")


def status_2_reply(motor_id: int, temperature_c: int) -> CanFrame:
    return CanFrame(
        arbitration_id=0x140 + motor_id,
        data=bytes([0x9C, temperature_c & 0xFF, 0, 0, 0, 0, 0, 0]),
    )


def test_requires_enable_before_motion() -> None:
    controller = MotorController(enabled_settings(), FakeTransport())

    with pytest.raises(MotorNotEnabledError):
        controller.open()


def test_open_and_close_can_switch_directions_without_manual_stop() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)

    enabled = controller.enable()
    assert enabled.state is MotorState.READY
    assert enabled.streaming is False
    assert frame_dump(transport) == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8800880000000000"),
    ]

    opening = controller.open()
    assert opening.control_raw == 400
    assert opening.control_raw_by_motor == {1: 400, 2: -400}
    assert opening.state is MotorState.OPENING
    assert opening.streaming is True
    wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3)

    transition_start = len(frame_dump(transport))
    closing = controller.close()
    assert closing.control_raw == -400
    assert closing.control_raw_by_motor == {1: -400, 2: 400}
    assert closing.state is MotorState.CLOSING
    assert closing.streaming is True
    wait_for_payload_count(transport, bytes.fromhex("70 FE 90 01 00 00 00 00"), 3)

    transition_frames = frame_dump(transport)[transition_start:]
    first_close = transition_frames.index((CONTROL_FRAME_ID, "70fe900100000000"))
    assert (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_close]
    assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_close]
    assert not any(
        frame == (CONTROL_FRAME_ID, "900170fe00000000") for frame in transition_frames[first_close:]
    )

    open_payload = bytes.fromhex("90 01 70 FE 00 00 00 00")
    open_count_before = sum(frame.data == open_payload for frame in transport.frames)
    transition_start = len(frame_dump(transport))
    reopened = controller.open()
    assert reopened.control_raw == 400
    assert reopened.control_raw_by_motor == {1: 400, 2: -400}
    assert reopened.state is MotorState.OPENING
    assert reopened.streaming is True
    wait_for_payload_count(transport, open_payload, open_count_before + 3)

    transition_frames = frame_dump(transport)[transition_start:]
    first_open = transition_frames.index((CONTROL_FRAME_ID, "900170fe00000000"))
    assert (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_open]
    assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_open]
    assert not any(
        frame == (CONTROL_FRAME_ID, "70fe900100000000") for frame in transition_frames[first_open:]
    )

    stopped = controller.stop()
    assert stopped.state is MotorState.STOPPED
    assert stopped.streaming is False
    count_after_stop = len(frame_dump(transport))
    time.sleep(0.01)
    assert len(frame_dump(transport)) == count_after_stop
    controller.shutdown()


def test_open_to_close_stop_failure_never_sends_close_control() -> None:
    transport = FailOnThirdZeroTransport()
    controller = MotorController(enabled_settings(), transport)
    controller.enable()
    controller.open()
    wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3)

    with pytest.raises(MotorCommunicationError, match="transition stop failure"):
        controller.close()

    status = controller.status()
    assert status.state is MotorState.FAULT
    assert status.last_error == "injected transition stop failure"
    assert not any(
        frame.data == bytes.fromhex("70 FE 90 01 00 00 00 00") for frame in transport.frames
    )
    controller.shutdown()


@pytest.mark.parametrize(
    ("motor_ids", "opening_values", "closing_values", "opening_payload", "closing_payload"),
    [
        ((1,), {1: 400}, {1: -400}, "9001000000000000", "70fe000000000000"),
        ((2,), {2: -400}, {2: 400}, "000070fe00000000", "0000900100000000"),
    ],
)
def test_single_motor_keeps_its_configured_open_and_close_direction(
    motor_ids: tuple[int, ...],
    opening_values: dict[int, int],
    closing_values: dict[int, int],
    opening_payload: str,
    closing_payload: str,
) -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(motor_ids=motor_ids), transport)
    controller.enable()

    assert controller.open().control_raw_by_motor == opening_values
    wait_for_payload_count(transport, bytes.fromhex(opening_payload), 2)
    controller.stop()
    assert controller.close().control_raw_by_motor == closing_values
    wait_for_payload_count(transport, bytes.fromhex(closing_payload), 2)
    controller.shutdown()


def test_open_keeps_broadcasting_until_explicit_stop() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)
    controller.enable()
    controller.open()
    payload = bytes.fromhex("90 01 70 FE 00 00 00 00")
    wait_for_payload_count(transport, payload, 3)
    count_before_wait = sum(frame.data == payload for frame in transport.frames)

    time.sleep(0.06)

    status = controller.status()
    count_after_wait = sum(frame.data == payload for frame in transport.frames)
    assert status.state is MotorState.OPENING
    assert status.streaming is True
    assert count_after_wait > count_before_wait
    assert not any(
        frame.arbitration_id == MIXED_COMMAND_FRAME_ID
        and frame.data == bytes.fromhex("81 00 81 00 00 00 00 00")
        for frame in transport.frames
    )

    assert controller.stop().state is MotorState.STOPPED
    controller.shutdown()


@pytest.mark.parametrize(
    ("action", "initial_payload", "derated_payload", "expected_values"),
    [
        ("open", "900170fe00000000", "64009cff00000000", {1: 100, 2: -100}),
        ("close", "70fe900100000000", "9cff640000000000", {1: -100, 2: 100}),
    ],
)
def test_motion_derates_after_configured_delay_without_changing_direction(
    action: str,
    initial_payload: str,
    derated_payload: str,
    expected_values: dict[int, int],
) -> None:
    transport = FakeTransport()
    settings = enabled_settings(
        broadcast_frequency_hz=200,
        derate_after_s=0.03,
        temperature_poll_interval_s=1,
    )
    controller = MotorController(settings, transport)
    controller.enable()

    getattr(controller, action)()
    wait_for_payload_count(transport, bytes.fromhex(initial_payload), 2)
    wait_for_payload_count(transport, bytes.fromhex(derated_payload), 2)

    status = controller.status()
    assert status.derated is True
    assert status.control_raw_by_motor == expected_values
    assert status.temperature_limited is False
    controller.stop()
    controller.shutdown()


def test_temperature_below_limit_is_reported_without_stopping() -> None:
    transport = FakeTransport()
    transport.queue_received(status_2_reply(1, 79))
    controller = MotorController(enabled_settings(temperature_poll_interval_s=0.05), transport)
    controller.enable()

    controller.open()
    deadline = time.monotonic() + 0.2
    while 1 not in controller.status().temperature_c_by_motor and time.monotonic() < deadline:
        time.sleep(0.002)

    status = controller.status()
    assert status.temperature_c_by_motor == {1: 79}
    assert status.temperature_limited is False
    assert status.state is MotorState.OPENING
    assert status.streaming is True
    controller.stop()
    controller.shutdown()


def test_temperature_at_limit_sets_both_outputs_to_zero_and_latches() -> None:
    transport = FakeTransport()
    transport.queue_received(status_2_reply(2, 80))
    controller = MotorController(enabled_settings(temperature_poll_interval_s=0.05), transport)
    controller.enable()

    controller.open()
    deadline = time.monotonic() + 0.2
    while not controller.status().temperature_limited and time.monotonic() < deadline:
        time.sleep(0.002)

    status = controller.status()
    assert status.temperature_c_by_motor == {2: 80}
    assert status.temperature_limited is True
    assert status.control_raw_by_motor == {1: 0, 2: 0}
    assert status.state is MotorState.STOPPED
    assert status.streaming is False
    assert "motor 2 temperature 80C" in (status.last_error or "")
    assert frame_dump(transport)[-2:] == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
    ]
    with pytest.raises(MotorNotEnabledError, match="temperature protection is latched"):
        controller.close()
    controller.shutdown()


def test_enable_sends_zero_and_enable() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)

    status = controller.enable()

    assert status.state is MotorState.READY
    assert frame_dump(transport) == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8800880000000000"),
    ]
    controller.shutdown()


def test_one_stop_request_sends_two_complete_rounds_with_tx_drain_wait() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)
    controller.enable()
    start = len(transport.frames)

    stopped = controller.stop()

    with transport.lock:
        frames = [(frame.arbitration_id, frame.data.hex()) for frame in transport.frames[start:]]
        times_ns = transport.frame_times_ns[start:]
        timeouts_s = transport.tx_queue_timeouts_s[start:]
    assert stopped.state is MotorState.STOPPED
    assert stopped.streaming is False
    assert stopped.control_raw_by_motor == {1: 0, 2: 0}
    assert frames == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
    ]
    assert timeouts_s == [None, 0.1, None, 0.1]
    assert times_ns[2] - times_ns[1] >= 15_000_000
    controller.shutdown()


def test_background_send_failure_latches_fault_and_attempts_zero_stop() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)
    controller.enable()
    controller.open()
    transport.fail_next = True

    deadline = time.monotonic() + 0.2
    while controller.status().state is not MotorState.FAULT and time.monotonic() < deadline:
        time.sleep(0.002)

    assert controller.status().state is MotorState.FAULT
    assert controller.status().streaming is False
    while (
        frame_dump(transport)[-1] != (MIXED_COMMAND_FRAME_ID, "8100810000000000")
        and time.monotonic() < deadline
    ):
        time.sleep(0.002)
    assert frame_dump(transport)[-2:] == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
    ]
    controller.shutdown()


def test_shutdown_joins_sender_before_zero_stop_disable_and_transport_shutdown() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)
    controller.enable()
    controller.open()
    wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3)

    controller.shutdown()

    assert frame_dump(transport)[-5:] == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
        (MIXED_COMMAND_FRAME_ID, "8000800000000000"),
    ]
    assert transport.tx_queue_timeouts_s[-5:] == [None, 0.1, None, 0.1, 0.1]
    assert transport.is_open is False
    count_after_close = len(frame_dump(transport))
    time.sleep(0.01)
    assert len(frame_dump(transport)) == count_after_close


def test_send_failure_latches_local_fault() -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)
    transport.fail_next = True

    with pytest.raises(MotorCommunicationError):
        controller.enable()

    assert controller.status().state is MotorState.FAULT
    assert controller.status().last_error == "injected CAN failure"
    controller.shutdown()


def test_thread_start_failure_never_sends_nonzero_control(
    monkeypatch: pytest.MonkeyPatch,
) -> None:
    transport = FakeTransport()
    controller = MotorController(enabled_settings(), transport)
    controller.enable()

    def fail_start(thread: threading.Thread) -> None:
        del thread
        raise RuntimeError("injected thread start failure")

    monkeypatch.setattr(threading.Thread, "start", fail_start)

    with pytest.raises(MotorCommunicationError, match="could not start"):
        controller.open()

    assert controller.status().state is MotorState.FAULT
    assert not any(
        frame.arbitration_id == CONTROL_FRAME_ID and frame.data != bytes(8)
        for frame in transport.frames
    )
    controller.shutdown()


def test_join_timeout_still_ends_with_sender_zero_and_stop(
    monkeypatch: pytest.MonkeyPatch,
) -> None:
    transport = BlockingSecondControlSendTransport()
    controller = MotorController(enabled_settings(), transport)
    monkeypatch.setattr(controller, "_BROADCAST_JOIN_TIMEOUT_S", 0.01)
    controller.enable()
    controller.open()
    assert transport.background_send_entered.wait(timeout=0.2)

    with pytest.raises(MotorCommunicationError, match="did not stop"):
        controller.stop()

    transport.release_background_send.set()
    deadline = time.monotonic() + 0.2
    while frame_dump(transport)[-1] != (MIXED_COMMAND_FRAME_ID, "8100810000000000"):
        if time.monotonic() >= deadline:
            raise AssertionError("sender did not finish with zero and Stop")
        time.sleep(0.002)

    assert frame_dump(transport)[-2:] == [
        (CONTROL_FRAME_ID, "0000000000000000"),
        (MIXED_COMMAND_FRAME_ID, "8100810000000000"),
    ]
    controller.shutdown()


def test_shutdown_does_not_close_transport_while_sender_is_still_alive(
    monkeypatch: pytest.MonkeyPatch,
) -> None:
    transport = BlockingSecondControlSendTransport()
    controller = MotorController(enabled_settings(), transport)
    monkeypatch.setattr(controller, "_BROADCAST_JOIN_TIMEOUT_S", 0.01)
    controller.enable()
    controller.open()
    assert transport.background_send_entered.wait(timeout=0.2)

    with pytest.raises(MotorCommunicationError, match="did not stop"):
        controller.shutdown()
    assert transport.is_open is True

    transport.release_background_send.set()
    deadline = time.monotonic() + 0.2
    while controller._broadcast_thread is not None and time.monotonic() < deadline:
        time.sleep(0.002)
    controller.shutdown()
    assert transport.is_open is False