"""Facts about the xArm controller: its modes, its codes, and the pose maths.

Nothing tunable lives here — the limits and timeouts are configuration, read
from the environment in :mod:`dora_ulite6.config`. These are the constants the
SDK itself defines and the helpers that read its answers.
"""

from xarm.core.config.x_code import ControllerErrorCodeMap
from xarm.x3.code import APIState

MODE_POSITION = 0
MODE_TEACH = 2
MODE_CARTESIAN_VELOCITY = 5

# Controller states as the arm reports them. From 4 on the arm is stopped and
# refuses to move until enabled again — where the emergency stop leaves it.
STATE_STOPPED = 4

# Return codes meaning the command never ran because the arm was not usable. The
# SDK returns them instead of raising, so a driver that only logged them would
# have every caller believe a motion happened that never did.
UNAVAILABLE_CODES = frozenset(
    {APIState.NOT_CONNECTED, APIState.NOT_READY, APIState.HAS_ERROR, APIState.STATE_NOT_READY}
)

# The one controller error a motion of ours can clear: C23, joints angle exceed
# limit, whose documented remedy is to send the arm to its initial position.
# Every other error is answered by a person — releasing a button, dragging the
# arm out of a collision — and homing one would move an arm somebody stopped.
HOMEABLE_ERROR = 23

# How often to re-ask the controller while waiting. Cadence, not a limit.
ARRIVAL_POLL = 0.1
READY_POLL = 0.2


def format_pose(pose) -> str:
    """Render a pose for a log line or an error message."""
    x, y, z, roll, pitch, yaw = pose
    return f"({x:.0f}, {y:.0f}, {z:.0f} mm; {roll:.0f}, {pitch:.0f}, {yaw:.0f}°)"


def shortest_turn(start: float, target: float) -> float:
    """Return the signed degrees from ``start`` to ``target``, the short way round.

    Euler angles wrap: an arm reporting roll −178 and asked for roll 180 has two
    degrees to turn, but subtracting gives 358 — a sweep through roll 0 with the
    tool pointing sideways, which no arm passes through.
    """
    return (target - start + 180) % 360 - 180


def interpolate(start: list, target: list, fraction: float) -> list:
    """Return the pose ``fraction`` of the way from ``start`` to ``target``.

    Position moves in a straight line and each rotation turns the short way
    round, which is what the controller does along a linear move.
    """
    pose = [a + (b - a) * fraction for a, b in zip(start[:3], target[:3], strict=True)]
    pose += [a + shortest_turn(a, b) * fraction for a, b in zip(start[3:], target[3:], strict=True)]
    return pose


def describe_controller_error(code: int) -> str:
    """Name a controller error, so a log reader does not have to look the number up."""
    if not code:
        return "no controller error"
    entry = ControllerErrorCodeMap.get(code)
    if entry is None:
        return f"controller error {code}"
    return f"controller error {code} ({entry['en']['title']})"