"""A bench stand-in for the arm and the browser, so teleop can run with neither.

Run by `dataflow.yml`. It answers every service, publishes plausible telemetry,
and prints the velocities it is sent — which is enough to drive the node from a
real gamepad and watch what a real arm would have been told to do.

**It is not a simulator.** It does not integrate the velocity, move a pose, or
model the arm in any way; a jog is printed and forgotten. Its job is to close
the loop so the node's own behaviour can be watched, not to stand in for
`dora-ulite6`. Anything that needs the arm to actually respond needs the arm.
"""

import json

import pyarrow as pa
from dora import Node

REQUEST_ID = "request_id"

# Somewhere in front of the robot, tool pointing down. Fixed, because a moving
# pose here would look like the arm was responding, which it is not.
POSE = [250.0, 0.0, 200.0, 180.0, 0.0, 0.0]
JOINTS = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


def main() -> None:
    """Answer whatever teleop asks and report what it would have moved."""
    node = Node("bench")
    announced = False
    tool_engaged = False
    freedrive = False
    jogs = 0
    shown = None

    for event in node:
        if event["type"] != "INPUT":
            continue

        if event["id"] == "tick":
            if not announced:
                announced = True
                node.send_output("robot_state", pa.array(["ready"]))
            node.send_output("tcp_pose", pa.array(POSE, type=pa.float32()), {"units_position": "mm"})
            node.send_output("joint_pose", pa.array(JOINTS, type=pa.float32()), {"units": "deg"})
            # No browser is attached to this dataflow, and saying so is what
            # keeps teleop from believing one went away mid-jog.
            node.send_output("clients", pa.array([{"connected": 0, "event": "close"}]))

        elif event["id"] == "jog":
            velocity = event["value"][0].as_py()["velocity"]
            jogs += 1
            moving = any(velocity)
            shown = ", ".join(f"{v:+7.2f}" for v in velocity)
            print(f"jog {jogs:5d}  [{shown}]  {'' if moving else '<- stop'}", flush=True)

        elif event["id"] == "request":
            row = event["value"][0].as_py()
            service = row.get("service", "")
            request_id = event["metadata"].get(REQUEST_ID, "")
            if service == "grasp":
                tool_engaged = True
            elif service == "release":
                tool_engaged = False
            elif service == "freedrive":
                freedrive = bool(row.get("enabled", True))
            if service != "get_state":
                print(f"service {service} ({request_id})", flush=True)
            node.send_output(
                "response",
                pa.array([_answer(service, tool_engaged, freedrive)]),
                {REQUEST_ID: request_id},
            )

        elif event["id"] == "ui_state":
            # No browser is attached, so this is where the console's picture of
            # the arm would go. Printing it on change rather than on every tick
            # makes the things worth watching — the lease moving, the arm
            # starting and stopping — visible without 20 lines a second.
            snapshot = json.loads(event["value"][0].as_py())
            if snapshot.get("type") != "telemetry":
                continue
            current = (snapshot["arm_state"], snapshot["owner"], snapshot["estop"])
            if current != shown:
                shown = current
                state, owner, estop = current
                print(
                    f"console would show: {state}, owner {owner}" + (", E-STOPPED" if estop else ""),
                    flush=True,
                )


def _answer(service: str, tool_engaged: bool, freedrive: bool) -> dict:
    """Build the response row the arm node would have sent."""
    if service == "get_state":
        return {
            "ok": True,
            "service": service,
            "message": "ready",
            "connected": True,
            "ready": True,
            "tool_engaged": tool_engaged,
            "freedrive": freedrive,
            "pose": POSE,
            "joints": JOINTS,
        }
    return {"ok": True, "service": service, "message": f"{service} accepted by the bench"}


if __name__ == "__main__":
    main()