"""Turning stick deflection into a velocity the arm understands.

Pure functions, no state and no hardware, because this is the part that has to
be right and the part that is cheapest to check. Everything here is in the
arm node's units: `[vx, vy, vz, wx, wy, wz]`, mm/s and deg/s, in the robot's
**base** frame — the same frame `tcp_pose` reports.

The screen convention comes from the old web console, so the two agree: stick
up is +X, stick right is +Y.
"""

from dataclasses import dataclass

# The arm takes six numbers; these name them.
VX, VY, VZ, WX, WY, WZ = range(6)


@dataclass(frozen=True)
class Layout:
    """Which axis and button index does what, in SDL's numbering.

    The defaults are the layout SDL reports for an Xbox-style pad, which is also
    what it reports for most PlayStation-style ones. A pad that disagrees is a
    change here rather than a new environment variable: an index nobody can
    verify from the outside is worse than a line of code somebody can read.
    """

    left_x: int = 0
    left_y: int = 1
    left_trigger: int = 2
    right_x: int = 3
    right_y: int = 4
    right_trigger: int = 5

    yaw_left: int = 4  # LB
    yaw_right: int = 5  # RB
    grasp: int = 0  # A
    release: int = 1  # B
    clear: int = 2  # X
    freedrive: int = 3  # Y
    estop: int = 6  # Back


LAYOUT = Layout()


def deadzone(value: float, width: float) -> float:
    """Ignore travel near centre, and rescale what is left to the full range.

    Rescaling matters: without it the arm jumps to `width` of full speed the
    instant the stick leaves the dead band, which feels like a broken control
    rather than a gentle one.
    """
    if width <= 0.0:
        return value
    size = abs(value)
    if size < width:
        return 0.0
    scaled = (size - width) / (1.0 - width)
    return scaled if value > 0 else -scaled


def expo(value: float, factor: float) -> float:
    """Bend the response so small deflections move the arm slowly.

    Fine positioning happens near centre, and a linear stick gives it almost no
    resolution there. `factor` is how much of the curve is cubic: 0 is linear.
    """
    return (1.0 - factor) * value + factor * value**3


def trigger(value: float) -> float:
    """Return a trigger's travel as 0 at rest and 1 fully pressed.

    SDL reports a trigger from -1 to 1, unlike a stick, and a trigger that has
    never been touched sometimes reads 0 instead of -1 until it first moves.
    Clamping keeps that from reading as half pressed.
    """
    return max(0.0, min(1.0, (value + 1.0) / 2.0))


def axis(values: list[float], index: int) -> float:
    """Read one axis, treating a pad that does not report it as centred."""
    return values[index] if index < len(values) else 0.0


def pressed(buttons: list[bool], index: int) -> bool:
    """Read one button, treating a pad that does not report it as unpressed."""
    return bool(buttons[index]) if index < len(buttons) else False


def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
    """Map one gamepad reading to a base-frame velocity.

    Translation comes from the left stick and the triggers, rotation from the
    right stick and the bumpers. Several axes move at once on purpose: a stick
    is analog and blended, which is exactly what the console's four-sector dial
    pad could not do.
    """

    def shaped(value: float) -> float:
        return expo(deadzone(value, config.deadzone), config.expo)

    out = [0.0] * 6
    # Up on the stick is negative in SDL, and up should send the arm away from
    # the operator, along +X.
    out[VX] = shaped(-axis(axes, layout.left_y)) * config.speed_mm_s
    out[VY] = shaped(axis(axes, layout.left_x)) * config.speed_mm_s

    lift = trigger(axis(axes, layout.right_trigger)) - trigger(axis(axes, layout.left_trigger))
    out[VZ] = shaped(lift) * config.speed_mm_s

    out[WX] = shaped(axis(axes, layout.right_x)) * config.speed_deg_s
    out[WY] = shaped(-axis(axes, layout.right_y)) * config.speed_deg_s

    yaw = pressed(buttons, layout.yaw_right) - pressed(buttons, layout.yaw_left)
    out[WZ] = yaw * config.speed_deg_s
    return out


def is_moving(velocity_vector: list[float]) -> bool:
    """Whether this velocity would actually drive the arm."""
    return any(velocity_vector)