"""Show what the pad is doing, with no dataflow and no arm.

`dora-teleop-probe` opens the gamepad and prints, live: the raw axes SDL
reports, the buttons it sees pressed, and the velocity this node would send.

It exists because the axis and button indices in :mod:`dora_teleop.mapping` are
SDL's documented layout rather than an observed one, and a pad that disagrees is
otherwise discovered by watching a real arm move the wrong way. It goes through
:class:`~dora_teleop.gamepad.Gamepad` and :func:`~dora_teleop.mapping.velocity`
— the same code the node runs — so what it prints is what the arm would get.

Nothing here can move anything. There is no dora node and no connection to the
arm; this only reads.
"""

import argparse
import time

from dora_teleop import mapping
from dora_teleop.config import Config
from dora_teleop.gamepad import Gamepad
from dora_teleop.mapping import LAYOUT

# What each velocity component is, in the order the arm takes them.
COMPONENTS = (
    ("vx", "mm/s", "forward / back"),
    ("vy", "mm/s", "left / right"),
    ("vz", "mm/s", "up / down"),
    ("wx", "deg/s", "roll"),
    ("wy", "deg/s", "pitch"),
    ("wz", "deg/s", "yaw"),
)

CLEAR = "\033[H\033[J"


def axis_names(layout=LAYOUT) -> dict[int, str]:
    """Name each axis index the layout uses."""
    return {
        layout.left_x: "left stick X",
        layout.left_y: "left stick Y",
        layout.left_trigger: "left trigger",
        layout.right_x: "right stick X",
        layout.right_y: "right stick Y",
        layout.right_trigger: "right trigger",
    }


def button_names(layout=LAYOUT) -> dict[int, str]:
    """Name each button index the layout uses."""
    return {
        layout.grasp: "grasp",
        layout.release: "release",
        layout.clear: "clear",
        layout.freedrive: "freedrive",
        layout.yaw_left: "yaw left",
        layout.yaw_right: "yaw right",
        layout.estop: "E-STOP",
    }


def bar(value: float, width: int = 21) -> str:
    """Draw a centred bar for a value between -1 and 1.

    Seeing the bar sit off-centre with the stick released is how a worn pad
    announces itself, which is most of the point of looking at raw numbers.
    """
    middle = width // 2
    position = middle + round(max(-1.0, min(1.0, value)) * middle)
    cells = ["-"] * width
    cells[middle] = "|"
    cells[position] = "#"
    return "".join(cells)


def render(sample, velocity: list[float], config: Config) -> str:
    """Build the whole display for one reading."""
    lines = ["dora-teleop probe — Ctrl-C to stop", ""]

    if not sample.present:
        lines += [
            "  no gamepad found.",
            "",
            f"  looking for joystick index {config.joystick_index};",
            "  plug one in and this will pick it up.",
        ]
        return "\n".join(lines)

    names = axis_names()
    lines.append(f"  axes ({len(sample.axes)} reported)")
    for index, value in enumerate(sample.axes):
        name = names.get(index, "unused")
        lines.append(f"    {index}  {name:<15} {value:+.3f}  {bar(value)}")

    labels = button_names()
    held = [f"{index}:{labels.get(index, 'unused')}" for index, down in enumerate(sample.buttons) if down]
    lines += ["", f"  buttons ({len(sample.buttons)} reported)", f"    {', '.join(held) or 'none pressed'}"]

    lines += ["", "  velocity this would send to the arm"]
    for (name, units, meaning), value in zip(COMPONENTS, velocity, strict=True):
        marker = "  <--" if value else ""
        lines.append(f"    {name}  {value:+8.2f} {units:<6} {meaning:<14}{marker}".rstrip())

    moving = "yes" if mapping.is_moving(velocity) else "no — the arm would be told to hold still"
    lines += ["", f"  moving: {moving}"]
    return "\n".join(lines)


def main() -> None:
    """Open the pad and print what it is doing until interrupted."""
    parser = argparse.ArgumentParser(description="Show what the gamepad is doing. Moves nothing.")
    parser.add_argument("--hz", type=float, default=10.0, help="Redraw rate. Defaults to 10.")
    args = parser.parse_args()

    config = Config.from_env()
    gamepad = Gamepad(config.joystick_index)
    gamepad.start()

    try:
        while True:
            sample = gamepad.read()
            velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
            print(CLEAR + render(sample, velocity, config), flush=True)
            time.sleep(1.0 / args.hz)
    except KeyboardInterrupt:
        print()
    finally:
        gamepad.shutdown()


if __name__ == "__main__":
    main()