"""Keep commissioned command numbers; reject unverified physical-unit moves."""

from dataclasses import replace
import math

import pytest

from orisys_autocal.motion.models import (
    AxisParams,
    AxisState,
    AxisStatusBits,
    BusStatus,
    ConnectionSpec,
    MotionResult,
    Snapshot,
)
from orisys_autocal.motion.pose_move import PoseMover, plan_pose_move
from orisys_autocal.motion.service import MotionService
from orisys_autocal.motion.transport import DemoTransport
from orisys_autocal.motion.units import deg_to_user_units, user_units_to_deg
from orisys_app_shared.zmotion import ZAuxError


class RecordingTransport(DemoTransport):
    def __init__(self):
        super().__init__()
        self.moves = []

    def move_abs(self, axis, value):
        self.moves.append((axis, value))
        return super().move_abs(axis, value)


def service():
    transport = RecordingTransport()
    instance = MotionService(
        transport,
        language="en_US",
        limits={"max_travel_mm": 100.0, "max_yaw_deg": 180.0, "max_pitch_deg": 90.0},
    )
    instance.connect(ConnectionSpec(mode="demo"))
    instance.poll()
    instance._bus = BusStatus(init_status=1, total_axes=5)
    return instance, transport


def mover_and_actions(instance, targets):
    actions = []

    def submit(action):
        actions.append(action)
        return len(actions)

    mover = PoseMover(instance, submit)
    plan = plan_pose_move(instance, targets, together=True)
    assert mover.start(plan).ok
    return mover, actions


def test_commissioned_command_numbers_and_order_remain_exact(qt_app):
    instance, transport = service()
    mover, actions = mover_and_actions(
        instance, {"x": -10.0, "y": 2.0, "yaw": 90.0, "pitch": -45.0, "z": 3.0}
    )
    assert actions[0](instance).ok
    assert transport.moves == [
        (2, 12500.0),
        (3, -14750.0),
        (0, -5000.0),
        (1, 1000.0),
        (4, 1500.0),
    ]


def test_readback_mismatch_refuses_instead_of_changing_the_command():
    instance, transport = service()
    transport.set_units(0, 5.0)
    instance.poll([0])
    instance._bus = BusStatus(init_status=1, total_axes=5)
    refusal = instance.check_axes_move({"x": 2.0})
    assert refusal is not None and "UNITS" in refusal.message
    assert math.isnan(instance.to_mm(0, 1000.0))
    assert transport.moves == []


def test_explicit_matching_linear_units_are_supported_without_rescaling_other_axes():
    instance, transport = service()
    assert instance.set_axis_params(0, AxisParams(units=5.0)).ok
    instance.poll([0])
    instance._bus = BusStatus(init_status=1, total_axes=5)
    assert instance.check_axes_move({"x": 2.0}) is None
    assert instance.to_units("x", 2.0) == 2000.0
    assert instance.to_mm(0, 1000.0) == 1.0
    assert instance.to_units("yaw", 90.0) == 12500.0


@pytest.mark.parametrize("units", [0.0, -1.0, math.nan, math.inf])
def test_invalid_unit_readback_is_not_a_physical_measurement(units):
    instance, transport = service()
    transport.set_units(0, units)
    instance.poll([0])
    instance._bus = BusStatus(init_status=1, total_axes=5)
    assert instance.check_axes_move({"x": 2.0}) is not None
    assert math.isnan(instance.to_mm(0, 1000.0))


def test_missing_or_failed_readback_cannot_reuse_the_last_good_scale():
    instance, transport = service()
    instance._last_axes.clear()
    assert instance.check_axes_move({"x": 2.0}) is not None
    instance.poll([0])
    instance._last_axes[0] = replace(instance._last_axes[0], read_ok=False)
    assert instance.check_axes_move({"x": 2.0}) is not None
    assert math.isnan(instance.to_mm(0, 1000.0))


@pytest.mark.parametrize("axis,role", [(2, "yaw"), (3, "pitch")])
def test_changed_rotary_pulse_equivalent_requires_new_calibration(axis, role):
    instance, transport = service()
    assert instance.set_axis_params(axis, AxisParams(units=5.0)).ok
    instance.poll([axis])
    instance._bus = BusStatus(init_status=1, total_axes=5)
    refusal = instance.check_axes_move({role: 45.0})
    assert refusal is not None and "calibrat" in refusal.message
    assert math.isnan(instance.to_units(role, 45.0))
    assert math.isnan(instance.to_deg(axis, 1000.0))
    assert math.isnan(user_units_to_deg(axis, 1000.0, units=5.0))
    assert math.isnan(deg_to_user_units(axis, 45.0, units=5.0))


def test_change_after_queueing_is_refused_before_any_axis_moves(qt_app):
    instance, transport = service()
    mover, actions = mover_and_actions(instance, {"x": 2.0, "yaw": 10.0})
    # No poll between the external parameter change and worker execution.
    transport.set_units(0, 5.0)
    result = actions[0](instance)
    assert not result.ok
    mover.on_command_finished(1, result)
    assert not mover.running and "UNITS" in mover.state.failure
    assert transport.moves == []


def test_unrelated_command_result_does_not_cancel_a_pose_move(qt_app):
    from orisys_autocal.motion.models import MotionResult

    instance, transport = service()
    mover, actions = mover_and_actions(instance, {"x": 2.0})
    mover.on_command_finished(99, MotionResult.refused("another command"))
    assert mover.running
    assert actions[0](instance).ok
    mover.on_command_failed(1, "read failed")
    assert not mover.running and mover.state.failure == "read failed"


@pytest.mark.parametrize("change", ["parameters", "mapping"])
def test_a_queued_plan_cannot_be_reinterpreted_under_new_settings(qt_app, change):
    instance, transport = service()
    mover, actions = mover_and_actions(instance, {"x": 2.0, "y": 1.0})
    if change == "parameters":
        instance.set_axis_params(0, AxisParams(units=5.0))
    else:
        instance.set_axis_roles({"x": 1, "y": 0, "yaw": 2, "pitch": 3, "z": 4})
    assert not actions[0](instance).ok
    assert transport.moves == []


def test_a_stale_plan_is_refused_before_it_is_queued(qt_app):
    instance, transport = service()
    plan = plan_pose_move(instance, {"x": 2.0}, together=True)
    instance.set_axis_params(0, AxisParams(units=5.0))
    instance.poll([0])
    instance._bus = BusStatus(init_status=1, total_axes=5)
    actions = []
    mover = PoseMover(instance, lambda action: actions.append(action))
    assert not mover.start(plan).ok
    assert actions == [] and transport.moves == []


def test_unit_read_error_before_execution_refuses_the_entire_group(qt_app):
    instance, transport = service()
    mover, actions = mover_and_actions(instance, {"x": 2.0, "yaw": 10.0})

    def unreadable(axis):
        raise ZAuxError(30500, "unit read failed")

    transport.get_units = unreadable
    assert not actions[0](instance).ok
    assert transport.moves == []


def test_raw_debug_moves_keep_the_existing_user_unit_contract():
    instance, transport = service()
    transport.set_units(2, 5.0)
    assert instance.move_abs(2, 90.0).ok
    assert transport.moves == [(2, 90.0)]


@pytest.mark.parametrize("together", [False, True])
@pytest.mark.parametrize("failure", ["refused", "exception"])
def test_arrival_before_command_failure_never_reports_success(qt_app, together, failure):
    instance, transport = service()
    actions = []
    mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
    finished = []
    mover.finished.connect(finished.append)
    assert mover.start(plan_pose_move(instance, {"x": 0.0}, together=together)).ok
    mover.on_snapshot(Snapshot(axes=(instance._last_axes[0],)))
    assert not finished and mover.running

    if failure == "refused":
        transport.set_units(0, 5.0)
        result = actions[0](instance)
        assert not result.ok and "UNITS" in result.message
        mover.on_command_finished(1, result)
    else:
        mover.on_command_failed(1, "read failed")
    assert mover.state.failure and not mover.state.finished
    assert not finished and not mover.running
    assert transport.moves == []


@pytest.mark.parametrize("together", [False, True])
def test_only_matching_success_and_a_later_snapshot_allow_arrival(qt_app, together):
    instance, _transport = service()
    actions = []
    mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
    assert mover.start(plan_pose_move(instance, {"x": 0.0}, together=together)).ok
    snapshot = Snapshot(axes=(instance._last_axes[0],))
    mover.on_command_finished(99, MotionResult.success())
    mover.on_snapshot(snapshot)
    assert mover.running and not mover.state.finished
    result = actions[0](instance)
    assert result.ok
    mover.on_command_finished(1, result)
    assert mover.running, "a pre-acknowledgement snapshot was reused"
    mover.on_snapshot(Snapshot(axes=(instance._last_axes[0],)))
    assert mover.state.finished and not mover.running


def test_each_sequential_axis_waits_for_its_own_command_success(qt_app):
    instance, _transport = service()
    actions = []
    mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
    assert mover.start(plan_pose_move(instance, {"x": 0.0, "y": 0.0})).ok
    snapshot = Snapshot(axes=tuple(instance._last_axes[axis] for axis in (0, 1)))
    mover.on_snapshot(snapshot)
    assert len(actions) == 1 and mover.state.index == 0
    result = actions[0](instance)
    assert result.ok
    mover.on_command_finished(1, result)
    mover.on_snapshot(snapshot)
    assert len(actions) == 2 and mover.state.index == 1
    mover.on_command_finished(1, MotionResult.success())
    mover.on_snapshot(snapshot)
    assert mover.running and not mover.state.finished
    result = actions[1](instance)
    assert result.ok
    mover.on_command_finished(2, result)
    mover.on_snapshot(snapshot)
    assert mover.state.finished


@pytest.mark.parametrize("together", [False, True])
@pytest.mark.parametrize("fault", ["alarm", "read"])
def test_axis_faults_remain_visible_while_command_is_pending(qt_app, together, fault):
    instance, _transport = service()
    mover = PoseMover(instance, lambda _action: 1)
    targets = {"x": 0.0, "y": 0.0} if together else {"y": 0.0}
    assert mover.start(plan_pose_move(instance, targets, together=together)).ok
    bad_axis = AxisState(
        axis=1,
        units=10.0,
        status=AxisStatusBits(raw=512 if fault == "alarm" else 0),
        read_ok=fault != "read",
    )
    mover.on_snapshot(Snapshot(axes=(AxisState(axis=0, units=10.0), bad_axis)))
    assert not mover.running and not mover.state.finished
    assert ("alarm" if fault == "alarm" else "read failed") in mover.state.failure