"""The driver's half of velocity control: what actually reaches the controller.

`FakeArm` stands in for this whole class everywhere else, so nothing else in the
suite can see what the driver says to the SDK. That is the one thing worth
pinning here: a velocity is a set-point the controller holds, and how often it is
re-sent is the difference between an arm that travels and an arm that steps.
"""

import pytest

from dora_ulite6.config import Config
from dora_ulite6.driver import ULite6

MOVE = [20.0, 0.0, 0.0, 0.0, 0.0, 0.0]
BACK = [-20.0, 0.0, 0.0, 0.0, 0.0, 0.0]
STILL = [0.0] * 6


class FakeSDK:
    """Just enough XArmAPI to build a driver and watch it command a velocity."""

    def __init__(self, *args, **kwargs):
        self.connected = True
        self.mode = None
        self.velocities = []

    def connect(self):
        self.connected = True

    def disconnect(self):
        self.connected = False

    def motion_enable(self, enable=True):
        pass

    def set_mode(self, mode):
        self.mode = mode

    def set_state(self, state):
        pass

    def vc_set_cartesian_velocity(self, speeds, is_radian=None, is_tool_coord=False, duration=-1):
        self.velocities.append((list(speeds), duration))
        return 0


@pytest.fixture
def robot(monkeypatch):
    """Return a driver over a fake SDK, with the calls it made on `arm`."""
    monkeypatch.setattr("dora_ulite6.driver.XArmAPI", FakeSDK)
    return ULite6(Config())


def commanded(robot):
    """Return every velocity the driver put on the controller."""
    return [speeds for speeds, _ in robot.arm.velocities]


def test_a_velocity_is_never_given_a_duration(robot):
    # `duration` makes the controller stop the arm on its own, which means every
    # message re-arms a timed segment and the controller re-plans the same
    # motion. The SDK reads 0 as "always effective, will not stop
    # automatically", which is what lets one command stand.
    robot.set_cartesian_velocity(MOVE)

    assert [duration for _, duration in robot.arm.velocities] == [0]


def test_a_held_velocity_reaches_the_controller_once(robot):
    # The arm holds it, so saying it again says nothing new — and saying it
    # again several times a second is what an operator feels as stepping.
    for _ in range(5):
        robot.set_cartesian_velocity(MOVE)

    assert commanded(robot) == [MOVE]


def test_a_changed_velocity_reaches_the_controller(robot):
    # Sending once is worth nothing if a new direction is taken for the old one.
    robot.set_cartesian_velocity(MOVE)
    robot.set_cartesian_velocity(BACK)

    assert commanded(robot) == [MOVE, BACK]


def test_a_zero_velocity_stops_the_arm_and_leaves_velocity_mode(robot):
    robot.set_cartesian_velocity(MOVE)

    robot.set_cartesian_velocity(STILL)

    assert commanded(robot) == [MOVE, STILL]
    assert robot._velocity_mode is False


def test_the_same_velocity_is_sent_again_after_leaving_velocity_mode(robot):
    # Leaving the mode clears what the controller was holding, so the velocity
    # it had before means nothing on the way back in. Remembering it across the
    # gap would leave the arm still while the operator held a direction.
    robot.set_cartesian_velocity(MOVE)
    robot.set_cartesian_velocity(STILL)

    robot.set_cartesian_velocity(MOVE)

    assert commanded(robot) == [MOVE, STILL, MOVE]