"""Moving the platform to a simulated pose, and stopping it.

Three things are being protected here, in the order they matter:

1. **The guards refuse before anything moves.** The service is the only place that
   decides whether a pose move may run, so every refusal is tested there — and a
   refusal must arrive *before* the first axis is commanded, because a sequence that
   stops halfway leaves the machine in a pose nobody simulated.
2. **The order is the model's order.** The kinematics composes ``R_pitch · R_yaw``
   (yaw acts on the body first), so the machine yaws before it pitches; table X/Y
   last. A different order would sweep the sensor through poses the operator never
   saw in the preview.
3. **Stop is a real stop.** The mover is driven by snapshots, not by a loop, so a
   stop runs at the next tick and no further axis is commanded. A mover that sat in
   a loop could not be interrupted at all.
"""

from __future__ import annotations

import time

import pytest

from orisys_autocal.motion import (
    MotionResult,
    MotionService,
    Snapshot,
    plan_pose_move,
    plan_return_to_initial,
)
from orisys_autocal.motion.models import AxisState, AxisStatusBits, BusStatus
from orisys_autocal.motion.pose_move import (
    MOVE_ORDER,
    RETURN_ORDER,
    PoseMover,
    SETTLE_DEG,
    SETTLE_MM,
)
from orisys_autocal.motion.transport import DemoTransport
from orisys_autocal.preferences import default_preferences

ROLES = {"x": 0, "y": 1, "pitch": 2, "yaw": 3, "z": 4}
LIMITS = default_preferences()["safety_limits"]


def make_service(**kwargs) -> MotionService:
    """A connected, initialised platform — the state a move is allowed in."""
    service = MotionService(
        DemoTransport(),
        axis_roles=kwargs.pop("roles", ROLES),
        limits=kwargs.pop("limits", LIMITS),
        **kwargs,
    )
    service._spec = object()  # noqa: SLF001 - connected, without opening anything
    service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5)  # noqa: SLF001
    return service


class FakePoller:
    """Records submissions and runs them, so a plan's commands can be inspected."""

    def __init__(self, service: MotionService) -> None:
        self.service = service
        self.commands: list = []

    def submit(self, action):
        self.commands.append(action)
        # Not executed here: the mover is driven by snapshots, and a real poller runs
        # one command per tick. Running them synchronously would hide the ordering
        # this test exists to check.
        return len(self.commands)


def reading(axis: int, dpos: float, *, idle: bool = True, alarm: bool = False) -> AxisState:
    return AxisState(
        axis=axis,
        dpos=dpos,
        idle=idle,
        status=AxisStatusBits(raw=512 if alarm else 0),
    )


def snapshot_of(service: MotionService, positions: dict[int, float], **kwargs) -> Snapshot:
    return Snapshot(
        connected=True,
        bus=service.bus,
        axes=tuple(
            reading(axis, value, **kwargs) for axis, value in positions.items()
        ),
    )


# --------------------------------------------------------------------- planning


def test_the_plan_uses_the_roles_and_each_role_own_unit():
    service = make_service()
    plan = plan_pose_move(
        service, {"x": 1.5, "y": -2.0, "yaw": 10.0, "pitch": -20.0}, hold=("z",)
    )
    by_role = {step.role: step for step in plan.steps}
    assert by_role["x"].axis == 0
    assert by_role["yaw"].axis == 3
    assert by_role["pitch"].axis == 2
    # 500 units per mm on the bench, and the measured rotary constants.
    assert by_role["x"].units == pytest.approx(750.0)
    assert by_role["yaw"].units == pytest.approx(10.0 * 29500.0 / 90.0)
    assert by_role["pitch"].units == pytest.approx(-20.0 * 12500.0 / 90.0)
    assert by_role["x"].unit == "mm"
    assert by_role["yaw"].unit == "°"


def test_the_move_order_yaws_before_it_pitches_and_slides_last():
    """The model composes R_pitch · R_yaw, so the machine does the same."""
    service = make_service()
    plan = plan_pose_move(service, {"x": 1.0, "y": 1.0, "yaw": 1.0, "pitch": 1.0})
    assert [step.role for step in plan.steps] == ["yaw", "pitch", "x", "y"]
    assert MOVE_ORDER.index("yaw") < MOVE_ORDER.index("pitch")
    assert MOVE_ORDER.index("pitch") < MOVE_ORDER.index("x")


def test_the_plan_holds_back_what_it_is_not_given():
    """z is not commanded by a pose move — expressed by absence, not by a no-op."""
    service = make_service()
    plan = plan_pose_move(service, {"x": 1.0, "y": 1.0}, hold=("z",))
    assert "z" not in {step.role for step in plan.steps}
    assert plan.held == ("z",)
    assert "z" in plan.describe()


def test_returning_to_zero_retracts_the_press_axis_first():
    service = make_service()
    plan = plan_return_to_initial(service)
    assert [step.role for step in plan.steps] == list(RETURN_ORDER)
    assert plan.steps[0].role == "z"
    assert all(step.target == 0.0 for step in plan.steps)


# ----------------------------------------------------------------------- guards


def test_a_move_is_refused_with_no_controller():
    # English, so the assertion reads as the sentence the operator would see. The
    # service localises its refusals; the wording is not the point, the refusal is.
    service = MotionService(
        DemoTransport(), axis_roles=ROLES, limits=LIMITS, language="en_US"
    )
    refusal = service.check_axes_move({"x": 1.0})
    assert refusal is not None and "Not connected" in refusal.message


def test_a_move_is_refused_before_the_bus_is_initialised():
    service = make_service(language="en_US")
    service._bus = BusStatus(bus_type="unknown", init_status=0)  # noqa: SLF001
    refusal = service.check_axes_move({"x": 1.0})
    assert refusal is not None and "initialis" in refusal.message


def test_a_target_past_the_travel_limit_is_refused_with_the_numbers():
    service = make_service()
    refusal = service.check_axes_move({"x": LIMITS["max_travel_mm"] + 0.5})
    assert refusal is not None
    assert f"{LIMITS['max_travel_mm'] + 0.5:.3f}" in refusal.message


def test_an_angle_is_checked_against_its_own_limit_not_the_millimetre_one():
    """A 400° yaw is past the stage envelope however roomy the table is."""
    service = make_service()
    assert service.check_axes_move({"yaw": 400.0}) is not None
    assert service.check_axes_move({"yaw": 170.0}) is None
    # And a millimetre target is not checked against the degree limit.
    assert service.check_axes_move({"x": 200.0}) is not None


def test_a_limit_that_was_never_set_refuses_rather_than_meaning_unlimited():
    service = make_service(limits={})
    refusal = service.check_axes_move({"x": 1.0})
    assert refusal is not None and "P0" in refusal.message


def test_a_duplicated_drive_is_refused():
    """Two roles on one axis means one of them silently does not move."""
    service = make_service(roles={"x": 0, "y": 0, "pitch": 2, "yaw": 3, "z": 4})
    problem = service.roles_problem()
    assert problem
    refusal = service.check_axes_move({"x": 1.0, "y": 1.0})
    assert refusal is not None and refusal.message == problem


def test_an_incomplete_mapping_is_refused():
    service = make_service(roles={"x": 0})
    refusal = service.check_axes_move({"x": 1.0})
    assert refusal is not None and "yaw" in refusal.message


def test_a_rotary_role_needs_a_calibrated_axis():
    """Axis 0 has no user-units-per-degree constant, so it cannot be a yaw."""
    service = make_service(roles={"x": 4, "y": 1, "pitch": 2, "yaw": 0, "z": 3})
    refusal = service.check_axes_move({"yaw": 1.0})
    assert refusal is not None and "yaw" in refusal.message


def test_a_non_finite_target_is_refused():
    service = make_service()
    for value in (float("nan"), float("inf")):
        refusal = service.check_axes_move({"x": value})
        assert refusal is not None


def test_the_guard_names_an_unknown_direction():
    service = make_service()
    refusal = service.check_axes_move({"roll": 1.0})
    assert refusal is not None and "roll" in refusal.message


# ------------------------------------------------------------------------ mover


def test_an_execute_plan_moves_the_four_axes_together_and_leaves_z():
    """Calibration execute starts yaw, pitch and X/Y in one command. Z stays put."""
    service = make_service()
    called: list[list[tuple[int, float]]] = []

    def record(moves):
        called.append(list(moves))
        return MotionResult.success()

    service.move_abs_many = record  # type: ignore[method-assign]
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    plan = plan_pose_move(
        service,
        {"x": 2.0, "y": 1.0, "yaw": 10.0, "pitch": -5.0},
        hold=("z",),
        together=True,
    )
    assert plan.together
    assert [step.role for step in plan.steps] == ["yaw", "pitch", "x", "y"]
    assert mover.start(plan).ok
    assert len(poller.commands) == 1
    poller.commands[0](service)
    assert [axis for axis, _value in called[0]] == [3, 2, 0, 1]

    mover.on_snapshot(
        snapshot_of(service, {0: 1000.0, 1: 0.0, 2: 0.0, 3: 0.0}, idle=False)
    )
    assert mover.running
    assert mover.progress_text().startswith("同时")
    assert len(poller.commands) == 1, "a later axis was queued instead of moving with the first"

    mover.on_snapshot(
        snapshot_of(
            service,
            {
                0: 1000.0,
                1: 500.0,
                2: -5.0 * 12500.0 / 90.0,
                3: 10.0 * 29500.0 / 90.0,
            },
        )
    )
    assert mover.state.finished


def test_return_to_zero_stays_one_axis_at_a_time():
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    plan = plan_return_to_initial(service)
    assert not plan.together
    assert mover.start(plan).ok
    assert len(poller.commands) == 1
    assert mover.state.step.role == "z"


def test_the_mover_commands_one_axis_and_waits_for_it_to_arrive():
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
    assert mover.start(plan).ok
    assert len(poller.commands) == 1, "more than the first axis was commanded at once"

    # x is at its target and idle -> the second axis goes out.
    mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 0.0}))
    assert len(poller.commands) == 2
    assert mover.state.step.role == "y"

    # y arrives -> the plan is done.
    mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
    assert not mover.running
    assert mover.state.finished


def test_the_mover_waits_while_an_axis_is_still_moving():
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
    mover.on_snapshot(snapshot_of(service, {0: 990.0, 1: 0.0}, idle=False))
    assert len(poller.commands) == 1
    assert mover.state.index == 0


def test_a_long_yaw_is_not_cut_off_while_it_is_still_moving(monkeypatch):
    """A yaw past 90° can take longer than a minute. Keep it while the axis moves.

    The old step cap abandoned the plan after 20s and the window then stopped
    every axis, so a 180° yaw never arrived.
    """
    clock = {"t": 1_000.0}
    monkeypatch.setattr(
        "orisys_autocal.motion.pose_move.time.monotonic", lambda: clock["t"]
    )
    service = make_service(
        limits={**LIMITS, "max_yaw_deg": 180.0, "max_pitch_deg": 360.0}
    )
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    assert mover.start(plan_pose_move(service, {"yaw": 180.0})).ok
    clock["t"] += 120.0
    mover.on_snapshot(snapshot_of(service, {3: 1_000.0}, idle=False))
    assert mover.running
    assert mover.state.failure == ""
    assert len(poller.commands) == 1


def test_the_mover_fails_when_an_axis_stops_short():
    """A drive that has stopped is not going to arrive: say so, do not wait."""
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
    mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
    assert mover.running, "gave up before the stall grace elapsed"
    mover.state._idle_since = time.monotonic() - 10.0  # noqa: SLF001
    mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
    assert not mover.running
    assert "stopped short" in mover.state.failure
    assert len(poller.commands) == 1, "a further axis was commanded after the failure"


def test_the_mover_stops_on_an_alarm():
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    mover.start(plan_pose_move(service, {"x": 2.0}))
    mover.on_snapshot(snapshot_of(service, {0: 0.0}, alarm=True))
    assert not mover.running
    assert "alarm" in mover.state.failure


def test_a_tolerance_accepts_an_axis_that_reports_its_final_position_late():
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
    mover.start(plan)
    # 1 unit inside SETTLE_MM of 1000 units for 2 mm.
    mover.on_snapshot(snapshot_of(service, {0: 1000.0 - 1.0, 1: 0.0}))
    assert len(poller.commands) == 2, "a step within tolerance was treated as a miss"
    assert SETTLE_MM > 0 and SETTLE_DEG > 0


def test_stopping_cancels_the_rest_of_the_plan():
    """The stop is a separate command, so the mover must stop queueing first."""
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
    assert mover.running
    mover.cancel("stopped")
    assert not mover.running
    assert mover.state.cancelled
    # Nothing further, even if the axis happens to arrive afterwards.
    mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
    assert len(poller.commands) == 1


def test_a_second_plan_cannot_start_on_top_of_a_running_one():
    service = make_service()
    poller = FakePoller(service)
    mover = PoseMover(service, poller.submit)
    mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
    second = mover.start(plan_pose_move(service, {"x": 3.0}))
    assert not second.ok
    assert len(poller.commands) == 1


def test_the_mover_refuses_a_plan_it_would_not_be_allowed_to_run():
    service = MotionService(DemoTransport(), axis_roles=ROLES, limits=LIMITS)
    mover = PoseMover(service, FakePoller(service).submit)
    result = mover.start(plan_pose_move(service, {"x": 1.0}))
    assert not result.ok
    assert not mover.running


def test_the_progress_line_names_the_step_it_is_on():
    service = make_service()
    mover = PoseMover(service, FakePoller(service).submit)
    mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
    text = mover.progress_text()
    assert "1/2" in text and "x" in text


def test_the_units_round_trip_through_the_service():
    """A target planned in mm must arrive at the drive as the same mm."""
    service = make_service()
    for role, value in (("x", 3.5), ("y", -2.25), ("yaw", 45.0), ("pitch", -30.0)):
        units = service.to_units(role, value)
        axis = service.role_axis(role)
        if role in ("x", "y"):
            assert service.to_mm(axis, units) == pytest.approx(value)
        else:
            from orisys_autocal.motion import user_units_to_deg

            assert user_units_to_deg(axis, units) == pytest.approx(value)