#!/usr/bin/env python3
"""
G1 RECORD + REPLAY — one script for the arm and the Inspire hand.

RECORD
  Arms go limp after a 3s safe-start: move them BY HAND.
  The fingers cannot be posed by hand (the RH56 drive is not back-drivable), so how they
  get into a take depends on --fingers:
    touch     (default) TOUCH a finger to set how far it closes on replay, from Δbase —
              the force change from rest. The keys below do NOT move the hand in this mode.
    position  the keys drive the hand for real and the MEASURED angle is recorded.
    external  the recorder sends nothing; the dashboard sliders (:8088) drive all 12.
    follow    press a fingertip and that finger moves under your hand (f arms/disarms).
  Keys during a take: o/c = both open/close · [ ] = right · ; ' = left ·
                      1-9 = apply a saved dashboard shape · f = arm/disarm follow.
  Then it offers a preview (ARM MOVES), then saves.

REPLAY
  Drives the arm and the hand through the recording together.
  Press p (or space) to PAUSE and again to resume — the arm holds the frame it stopped
  on rather than going slack, so pausing mid-motion is safe.

  conda activate g1_env
  cd ~/Robotics_workspace/yslootahtech/Project/G1/G1_Lootah/Manual_Recorder    # symlink to
                                # Project/G1/DFX_inspire_service/recorder — one file

  python3 g1_record_replay.py enp3s0 record --output wave --seconds 20
  python3 g1_record_replay.py enp3s0 replay --input wave
  python3 g1_record_replay.py enp3s0 record --output arm_only --no-hand
  python3 g1_record_replay.py enp3s0 record --output hand_only --no-arm
  python3 g1_record_replay.py enp3s0 record --output grip --fingers position

  On the robot itself the same script lives in the container:
  docker exec -it -w /opt/recorder inspire-hand python3 g1_record_replay.py eth0 record \
      --output wave --seconds 20

Needs inspire_g1 running on the robot (the Docker container) for the hand.
Files go in DataG1/.  q: 1.00 = finger open, 0.00 = closed.

WHY REPLAY USED TO MISS THE RECORDED POSE
  You record with the arm LIMP (kp=0) — YOUR hand supplies the torque that holds it up.
  On replay a pure PD controller can only make that torque out of position error
  (err = tau_gravity / kp), so at kp=80 the arm settles 4-6° low at the shoulder,
  ~4-5cm at the fingertip, every time. Three things fix it here:
    1. gravity feedforward  — a clamped integral learns each joint's holding torque and
       sends it as tau, so the arm can sit ON the target instead of below it. No robot
       model needed; it also absorbs the hand's mass and anything you're carrying.
    2. dq feedforward       — the recorded velocity is sent too, so kd stops fighting
       the motion and adding lag.
    3. the waist is replayed — it used to be pinned to a single pose and the recorded
       torso motion thrown away. The balance controller leans the torso several degrees
       while you pose the arm; discarding it moved the hand 5-10cm in space even when
       every arm joint was perfect.
  Replay prints the measured tracking error at the end so you can see it working.
"""
import sys, os, time, json, argparse, termios, tty, select
import urllib.request
from pathlib import Path

from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_
from unitree_sdk2py.utils.crc import CRC
from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_
from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_

# ---- arm ----
G1_NUM_MOTOR = 29
ENABLE_ARM_SDK_INDEX = 29
LEG_INDICES = list(range(0, 12))
WAIST_INDICES = [12, 13, 14]
ARM_INDICES = list(range(15, 29))
SHOULDER_ELBOW = [15, 16, 17, 18, 22, 23, 24, 25]
WRIST_INDICES = [19, 20, 21, 26, 27, 28]
# wrist_ROLL is a 25 Nm joint like the shoulders; only wrist pitch/yaw are the small 5 Nm
# ones. Grouping all six "wrists" together capped roll at 3 Nm — an 8x under-estimate.
WRIST_ROLL = [19, 26]
WRIST_SMALL = [20, 21, 27, 28]
RECORD_HZ = 30.0
SAFE_START = 3.0
WAIST_KP, WAIST_KD = 120.0, 6.0     # was 60/4 — the torso sagged while you posed the arm,
                                    # and that sag is baked into the recording
HOLD_KP, HOLD_KD = 60.0, 4.0
TEACH_KP, TEACH_KD = 0.0, 2.0
PLAY_KP, PLAY_KD = 80.0, 3.0        # matches g1_replay so the arm reaches full height

# ---- gravity feedforward ----
# A clamped integral: tau += KI * (q_target - q_actual) * dt. It converges to exactly the
# torque each joint needs to hold its pose, with no robot model.
#
# The clamps bound a runaway integral — but set too LOW they starve a joint that legitimately
# needs the torque, and the error explodes instead of shrinking. Measured on a `fold` replay
# with the first guess (shoulders/elbow 12 Nm, all wrists 3 Nm):
#
#     joint         % of time at clamp   mean error AT clamp   vs NOT at clamp
#     L_sh_pitch          0.5%                 10.40 deg            1.06 deg
#     L_wr_pitch          2.5%                  5.04 deg            0.33 deg
#     L_wr_roll           0.8%                  3.48 deg            0.27 deg
#
# i.e. saturation was rare but produced ~10x the error, and was the entire source of the
# 12.54 deg / 8.84 deg peaks. Now sized from the URDF effort limits (shoulders, elbow and
# wrist_roll 25 Nm; wrist pitch/yaw 5 Nm) at ~80% of limit, leaving real headroom.
GRAV_KI = 60.0
TAU_CLAMP = {}
for _j in WAIST_INDICES:
    TAU_CLAMP[_j] = 20.0
for _j in SHOULDER_ELBOW:
    TAU_CLAMP[_j] = 20.0        # was 12.0; URDF limit 25 Nm
for _j in WRIST_ROLL:
    TAU_CLAMP[_j] = 12.0        # was 3.0;  URDF limit 25 Nm — this one was 8x too tight
for _j in WRIST_SMALL:
    TAU_CLAMP[_j] = 4.0         # was 3.0;  URDF limit 5 Nm

# ---- hand ----
HAND_MOTORS = 12
GRIP_FORCE = 500.0
# The RH56 refuses to drive a finger whose MEASURED force already exceeds its force limit.
# A drifted force-sensor zero (the G1's right R.mid reads ~856g, R.index ~570g at rest,
# untouched) therefore reads exactly like a dead motor: 0 current, no movement. We measure
# the resting offset during calibration and raise that hand's limit to clear it.
FORCE_MAX = 1000.0
FORCE_MARGIN = 250.0

# ---- force-follow (admittance) ----
# The RH56 cannot be back-driven, so pushing a finger moves it 0.000. Admittance control —
# the standard technique for non-backdrivable force-sensed devices — closes the loop in
# software: read the push, drive the finger that way, hold when you stop. The DEADBAND is
# what makes "touched but not moved" record nothing: a light contact never moves the finger,
# so no angle change exists to capture.
FOLLOW_GAIN = 0.0006      # travel per gram per second
# Measured 2026-08-03: idle force drift over 90s, untouched, is 0g on all 12 fingers. So the
# baseline never needs re-centring — the earlier self-motion was the INTEGRATOR accumulating
# incidental brushes while the arm was being posed, not sensor drift. Hence the arming key
# below rather than a fancier filter.
FOLLOW_DEADBAND = 60.0    # grams of push before a finger reacts
FOLLOW_MAXRATE = 0.35     # travel per second cap (~3s for full range)
FOLLOW_FORCE = 400.0      # grip force while shaping (below the 500g normal)

NAMES = ["R.pinky", "R.ring", "R.mid", "R.index", "R.thumbB", "R.thumbR",
         "L.pinky", "L.ring", "L.mid", "L.index", "L.thumbB", "L.thumbR"]
THUMB_ROT = [5, 11]                 # R.thumbR, L.thumbR

# REST pose — where a finger sits when nothing is touching it, used during calibration, the
# whole recording, replay and the return-to-rest at the end. It used to be hardcoded to
# all-open, which leaves both thumbs rotated out where they stick out and foul the fingers.
# Default parks thumb_rot CLOSED on both hands and leaves everything else open; --thumb-rot
# open restores the old behaviour.
REST = [1.0] * 12
for _i in THUMB_ROT:
    REST[_i] = 0.0
DATA_DIR = Path("DataG1")
CLR = "\x1b[K"


class KeyPoller:
    def __enter__(self):
        self.fd = sys.stdin.fileno()
        self.old = termios.tcgetattr(self.fd)
        tty.setcbreak(self.fd)
        return self
    def __exit__(self, *a):
        termios.tcsetattr(self.fd, termios.TCSADRAIN, self.old)
    def poll(self):
        if select.select([sys.stdin], [], [], 0)[0]:
            return sys.stdin.read(1)
        return None


class NullPoller:
    """Stand-in for KeyPoller when stdin is not a terminal.

    Replay reads a pause key, but it is also run with stdin piped or closed (scripts, cron).
    tty.setcbreak would raise there, and a replay that refuses to start because nobody can
    press pause is worse than a replay without pause.
    """
    def __enter__(self):
        return self

    def __exit__(self, *a):
        pass

    def poll(self):
        return None


def key_poller():
    return KeyPoller() if sys.stdin.isatty() else NullPoller()


def bar(v, w=12):
    v = 0.0 if v < 0 else (1.0 if v > 1 else v)
    n = int(round(v * w))
    return "█" * n + "·" * (w - n)


def unique_path(name):
    p = Path(name if str(name).endswith(".jsonl") else str(name) + ".jsonl")
    if len(p.parts) == 1:
        DATA_DIR.mkdir(parents=True, exist_ok=True)
        p = DATA_DIR / p.name
    if not p.exists():
        return p
    i = 1
    while (c := p.with_name(f"{p.stem}({i}){p.suffix}")).exists():
        i += 1
    return c


def resolve(name):
    p = Path(name if str(name).endswith(".jsonl") else str(name) + ".jsonl")
    return p if len(p.parts) > 1 else DATA_DIR / p.name


def load_shapes(url, timeout=4):
    """Pull the dashboard's saved finger shapes so they can be fired by number key.

    The hand cannot be posed by hand — measured every way: 0.000 rotation under 233g, 0mA at
    rest, +-3g on a push, and rigid at every force threshold down to 1g while the sensor read
    611g. So the only way to get a finger pose into a recording is to COMMAND it. Pre-building
    the poses on the dashboard and firing them with one key keeps both your hands on the arm.
    """
    try:
        with urllib.request.urlopen(url.rstrip("/") + "/api/poses", timeout=timeout) as r:
            d = json.load(r)
        return [(n, d[n]) for n in sorted(d) if isinstance(d[n], list) and len(d[n]) == 12][:9]
    except Exception as e:
        print(f"   (no shapes from {url}: {e})")
        return []


def clamp(v, lo, hi):
    return lo if v < lo else (hi if v > hi else v)


# Fingers named for --free. Sending q < 0 makes the RH56 driver stop driving that joint
# (0xFFFF = release), so it goes limp and CAN be posed by hand — unlike the default, where
# every finger is actively held open and pushing on one just fights the motor.
FREE_GROUPS = {
    "none":    set(),
    "thumbrot": {5, 11},
    "thumbs":  {4, 5, 10, 11},
    "right":   set(range(0, 6)),
    "left":    set(range(6, 12)),
    "all":     set(range(12)),
}


def parse_free(spec):
    """--free is DEAD — the hardware cannot do it. Kept only to explain why.

    It was built on a comment in inspire.h claiming q<0 (0xFFFF) releases the motor so the
    finger can be posed by hand. That comment is wrong: the upstream docstring three lines
    above it says "-1: no change", i.e. the hand IGNORES that joint and keeps driving it to
    its previous target. Which is why a "freed" finger feels locked.

    There is no motor-release command on the RH56, and the drive is a non-backdrivable worm
    gear, so no force setting makes a finger hand-poseable either. Touch-to-teach is not a
    workaround — it is the only mechanism the hardware offers.
    """
    if str(spec).strip().lower() not in ("", "none"):
        raise SystemExit(
            "--free does not work and has been disabled.\n"
            "  The RH56 cannot release a finger: q<0 means 'no change' (the joint keeps its\n"
            "  previous target and stays driven), NOT 'free to move'. The drive is also a\n"
            "  non-backdrivable worm gear, so no force setting makes it hand-poseable.\n"
            "  Use touch-to-teach instead: leave --free off, and TOUCH a finger during the\n"
            "  recording — how hard you press sets how far it closes on replay.")
    out = set()
    for tok in (t.strip() for t in str(spec).split(",") if t.strip()):
        low = tok.lower()
        if low in FREE_GROUPS:
            out |= FREE_GROUPS[low]
            continue
        hit = [i for i, n in enumerate(NAMES) if n.lower() == low]
        if hit:
            out.add(hit[0]); continue
        if tok.isdigit() and 0 <= int(tok) < 12:
            out.add(int(tok)); continue
        raise SystemExit(f"--free: don't know '{tok}'. Use one of {sorted(FREE_GROUPS)}, "
                         f"a finger name like R.thumbR, or an index 0-11.")
    return frozenset(out)


class G1RecordReplay:
    def __init__(self, iface, use_arm=True, use_hand=True):
        ChannelFactoryInitialize(0, iface)
        self.use_arm, self.use_hand = use_arm, use_hand
        self.low_state = None
        self.got_state = False
        if use_arm:
            self.crc = CRC()
            self.low_cmd = unitree_hg_msg_dds__LowCmd_()
            self.arm_pub = ChannelPublisher("rt/arm_sdk", LowCmd_)
            self.arm_pub.Init()
            ChannelSubscriber("rt/lowstate", LowState_).Init(self._on_arm, 10)
        self.hq = [1.0] * 12
        self.hf = [0.0] * 12
        self.hn = 0
        # Per-hand grip force actually commanded (raised above GRIP_FORCE if a finger's
        # force-sensor zero has drifted). index 0 = right, 1 = left.
        self.hand_force = [GRIP_FORCE, GRIP_FORCE]
        self.tau_ff = [0.0] * G1_NUM_MOTOR
        if use_hand:
            ChannelSubscriber("rt/inspire/state", MotorStates_).Init(self._on_hand, 10)
            self.hand_pub = ChannelPublisher("rt/inspire/cmd", MotorCmds_)
            self.hand_pub.Init()
            self.hand_msg = MotorCmds_()
            self.hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(12)]
        time.sleep(0.5)

    # ---------------- io ----------------
    def _on_arm(self, m):
        self.low_state = m
        self.got_state = True

    def _on_hand(self, m):
        try:
            self.hq = [float(m.states[i].q) for i in range(12)]
            self.hf = [float(m.states[i].tau_est) for i in range(12)]
            self.hn += 1
        except Exception:
            pass

    def send_hand(self, vals):
        if not self.use_hand:
            return
        for i in range(12):
            self.hand_msg.cmds[i].q = float(vals[i])
            # inspire_g1 reads cmds[0].kp for the right hand and cmds[6].kp for the left.
            self.hand_msg.cmds[i].kp = self.hand_force[0 if i < 6 else 1]
        self.hand_pub.Write(self.hand_msg)

    def send_arm(self, q, kp, kd, dq=None, tau=None):
        if not self.use_arm:
            return
        self.low_cmd.motor_cmd[ENABLE_ARM_SDK_INDEX].q = 1.0
        for i in WAIST_INDICES:
            c = self.low_cmd.motor_cmd[i]
            c.mode, c.q, c.kp, c.kd = 1, q[i], WAIST_KP, WAIST_KD
            c.dq = dq[i] if dq else 0.0
            c.tau = tau[i] if tau else 0.0
        for i in ARM_INDICES:
            c = self.low_cmd.motor_cmd[i]
            c.mode, c.q, c.kp, c.kd = 1, q[i], kp, kd
            c.dq = dq[i] if dq else 0.0
            c.tau = tau[i] if tau else 0.0
        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
        self.arm_pub.Write(self.low_cmd)

    def grav_step(self, q_target, dt, joints):
        """Integrate the position error into a feedforward torque. Converges to the exact
        holding torque for the current pose — model-free, and it absorbs the hand's mass
        and any payload. Clamped per joint so it can never run away."""
        if not self.low_state:
            return self.tau_ff
        for j in joints:
            err = q_target[j] - float(self.low_state.motor_state[j].q)
            self.tau_ff[j] = clamp(self.tau_ff[j] + GRAV_KI * err * dt,
                                   -TAU_CLAMP[j], TAU_CLAMP[j])
        return self.tau_ff

    def release_arm(self):
        if not self.use_arm:
            return
        print("🔌 Handing arm control back to the internal brain...")
        self.low_cmd.motor_cmd[ENABLE_ARM_SDK_INDEX].q = 0.0
        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
        for _ in range(5):
            self.arm_pub.Write(self.low_cmd)
            time.sleep(0.02)

    def wait_arm(self):
        if not self.use_arm:
            return True
        print("Waiting for robot...", end="", flush=True)
        t0 = time.time()
        while not self.got_state:
            if time.time() - t0 > 10:
                print(" TIMEOUT — is the robot up and on this interface?")
                return False
            time.sleep(0.1)
        print(" connected.")
        return True

    # ---------------- hand helpers ----------------
    def calibrate_hand(self, thresh):
        """Open the hand, learn each finger's resting force + noise, and raise the grip
        limit past any drifted force-sensor zero so that finger can still be driven."""
        parked = ", ".join(NAMES[i] for i in range(12) if REST[i] < 0.5)
        print(f"🖐️  Hand to rest pose + calibrating touch baseline — DON'T touch (2s)..."
              + (f"\n   parked closed: {parked}" if parked else ""))
        # Calibrate in the SAME pose the hand will hold all recording. A finger's resting
        # force depends on where it is sitting, so measuring open and then holding closed
        # would give every touch a false offset.
        for _ in range(25):
            self.send_hand(REST)
            time.sleep(0.02)
        acc = [0.0] * 12
        fmn = [1e9] * 12
        fmx = [-1e9] * 12
        nc = 0
        t0 = time.time()
        while time.time() - t0 < 2.0:
            self.send_hand(REST)
            if any(abs(v) > 1e-6 for v in self.hf):
                for i in range(12):
                    acc[i] += self.hf[i]
                    fmn[i] = min(fmn[i], self.hf[i])
                    fmx[i] = max(fmx[i], self.hf[i])
                nc += 1
            time.sleep(0.05)
        if not nc:
            print("   ⚠️  no hand data — is inspire_g1 running and the adapter connected?")
            return [0.0] * 12, [thresh] * 12
        base = [acc[i] / nc for i in range(12)]
        thr = [max(thresh, (fmx[i] - fmn[i]) * 1.5 + 5) for i in range(12)]

        # A finger whose resting force already exceeds the grip limit can never be driven:
        # the RH56 stops the motor the instant measured force >= the limit, so it draws 0
        # current and looks dead. Raise that hand's limit above the offset instead.
        for hand, lo in (("RIGHT", 0), ("LEFT", 6)):
            worst = max(abs(base[i]) for i in range(lo, lo + 6))
            if worst > GRIP_FORCE - FORCE_MARGIN:
                want = min(FORCE_MAX, worst + FORCE_MARGIN)
                self.hand_force[0 if lo == 0 else 1] = want
                drifted = [f"{NAMES[i]}({base[i]:.0f}g)" for i in range(lo, lo + 6)
                           if abs(base[i]) > GRIP_FORCE - FORCE_MARGIN]
                print(f"   ⚠️  {hand} hand force-sensor zero has DRIFTED: " + ", ".join(drifted))
                print(f"      those fingers would read as 'dead' (0 current) at the {GRIP_FORCE:.0f}g "
                      f"limit — raising this hand's limit to {want:.0f}g so they still drive.")
                if want < worst:
                    print(f"      ⚠️  offset {worst:.0f}g exceeds the {FORCE_MAX:.0f}g max — "
                          f"re-zero the force sensor (hand unloaded) to recover it.")
        return base, thr

    def open_hand(self, secs=1.2):
        """Drive every finger fully open and hold it there briefly so the command lands.

        Recording already holds the fingers open, but a REPLAY ends on whatever the last
        frame commanded — usually a grip — and go_home only moves the arm. So a take used to
        finish with the hand still clenched around whatever it was holding. Paired with
        go_home: release first, then bring the arm back."""
        if not self.use_hand:
            return
        if all(abs(self.hq[i] - REST[i]) < 0.1 for i in range(12)):
            return                       # already at rest — stay quiet
        print("🖐️  hands to rest pose...")
        t0 = time.time()
        while time.time() - t0 < secs:
            self.send_hand(REST)
            time.sleep(0.03)
        # Report anything that did not reach its rest position — a finger stuck part-closed is
        # worth knowing about before the arm starts moving again.
        stuck = [NAMES[i] for i in range(12) if abs(self.hq[i] - REST[i]) > 0.15]
        if stuck:
            print(f"   ⚠️  did not reach rest: {', '.join(stuck)} — check grip force / obstruction.")
        else:
            print("✅ hands at rest.")

    def remap(self, frames, thr, min_close, hold, auto_scale, span, hz, freed=frozenset()):
        """Re-derive finger positions from the recorded Δbase so the DEGREE of each touch
        survives; each finger is normalised to its own hardest touch.

        Freed fingers are skipped entirely — their recorded 'h' is the real measured position
        you posed by hand, so there is nothing to infer."""
        if not frames or "d" not in frames[0]:
            return frames, [0.0] * 12, [0] * 12
        pk = [0.0] * 12
        ntouch = [0] * 12
        for f in frames:
            for i in range(12):
                pk[i] = max(pk[i], abs(f["d"][i]))
        # Count distinct touch events so we can tell whether auto-scaling is meaningful.
        active = [False] * 12
        for f in frames:
            for i in range(12):
                if abs(f["d"][i]) > thr[i] and not active[i]:
                    ntouch[i] += 1
                    active[i] = True
                elif abs(f["d"][i]) < thr[i] * 0.5:
                    active[i] = False
        for f in frames:
            for i in range(12):
                if i in freed:
                    continue          # already holds the true posed position
                amt = abs(f["d"][i])
                if amt <= thr[i]:
                    c = 0.0
                else:
                    # Auto-scale is only meaningful with >=2 touches: with a single touch
                    # that touch IS the peak, so it would always map to 100% no matter how
                    # light it was. Fall back to the absolute --span scale in that case.
                    use_auto = auto_scale and ntouch[i] >= 2 and pk[i] > thr[i] + 5
                    top = pk[i] if use_auto else span
                    top = max(top, thr[i] + 5)
                    c = min_close + (1.0 - min_close) * min(1.0, (amt - thr[i]) / (top - thr[i]))
                # A touch closes the finger FROM ITS REST POSE, not always from fully open.
                # rest 1.0 -> h = 1-c, exactly as before. rest 0.0 (thumb_rot parked closed)
                # -> h stays 0.0, so it holds its parked pose instead of springing open.
                f["h"][i] = REST[i] * (1.0 - c)
        w = int(hold * hz)
        for i in range(12):
            if i in freed:
                continue              # no touch-hold smoothing on a hand-posed trajectory
            cl = [1.0 - f["h"][i] for f in frames]
            run, cnt = 0.0, 0
            for k in range(len(frames)):
                if cl[k] > 0:
                    run, cnt = max(run, cl[k]), w
                elif cnt > 0:
                    cnt -= 1
                else:
                    run = 0.0
                frames[k]["h"][i] = 1.0 - max(cl[k], run if cnt > 0 else 0.0)
        return frames, pk, ntouch

    # ---------------- record ----------------
    def record(self, seconds, thresh, span, min_close, hold, auto_scale, freed=frozenset(),
               pos_mode=False, ext_mode=False, follow_mode=False, shapes=()):
        if not self.wait_arm():
            return None
        base = thr = None
        if self.use_hand:
            base, thr = self.calibrate_hand(thresh)
        init_q = ([float(self.low_state.motor_state[i].q) for i in range(G1_NUM_MOTOR)]
                  if self.use_arm else [0.0] * G1_NUM_MOTOR)

        print("\n" + "=" * 58)
        print("🟢 RECORDING")
        if self.use_arm:
            print(f"   0-{SAFE_START:.0f}s: arms LOCKED, then LIMP — move them by hand")
        if self.use_hand:
            if pos_mode:
                print("   fingers: ANGLE mode — only a finger you DRIVE is recorded.")
                print("            Touching the hand records NOTHING.")
                if follow_mode:
                    print("            FORCE-FOLLOW: press  f  to ARM, then push a fingertip")
                    print("            to shape it. Press f again to disarm.")
                    print("            While disarmed (the default) contact is fully ignored,")
                    print("            so posing the arm never moves a finger.")
                elif ext_mode:
                    print("            EXTERNAL driver: this recorder sends NO hand commands.")
                    print("            Move the 12 sliders on the dashboard — :8088 — and the")
                    print("            angle each finger reaches is what gets recorded.")
            else:
                held = [i for i in range(12) if i not in freed]
                if held:
                    print("   fingers: TOUCH one to set how far it closes on replay")
                if freed:
                    print(f"   FREED (move by hand, position recorded): "
                          f"{', '.join(NAMES[i] for i in sorted(freed))}")
            if shapes:
                print("   SHAPES — press the number to apply that finger pose:")
                for n, (nm, _) in enumerate(shapes, 1):
                    print(f"     {n}  {nm}")
            print("   keys: o/c both · [ ] right · ; ' left")
        print("=" * 58 + "\n")

        if follow_mode:
            self.hand_force = [FOLLOW_FORCE, FOLLOW_FORCE]
        frames, dt, t0 = [], 1.0 / RECORD_HZ, time.time()
        # Start from the REST pose, not all-open. In position mode this IS what gets sent to
        # the hand, so initialising it to [1.0]*12 un-parked both thumb_rots the instant
        # recording began — and the mode then dutifully recorded that as an intentional move.
        hand_key = list(REST)
        ftarget = list(REST)      # force-follow target, starts at rest
        follow_on = False         # armed with F. Off by default so incidental
                                  # contact while posing the arm moves nothing.
        touches = [0] * 12
        touching = [False] * 12
        limp = False
        try:
            with KeyPoller() as kp:
                while (t := time.time() - t0) < seconds:
                    loop = time.time()
                    k = kp.poll()
                    if k and self.use_hand:
                        kl = k.lower()
                        if k.isdigit() and k != "0" and shapes and int(k) <= len(shapes):
                            nm, vals = shapes[int(k) - 1]
                            hand_key = [float(v) for v in vals]
                            ftarget = list(hand_key)
                            print(f"\n✋ shape {k}: {nm}\n")
                        elif kl == "f" and follow_mode:
                            follow_on = not follow_on
                            print(f"\n{'🖐️  FOLLOW ARMED — push fingers to shape them' if follow_on else '🔒 follow disarmed — contact ignored'}\n")
                        elif kl == "o": hand_key = list(REST)   # "open" = back to rest pose
                        elif kl == "c": hand_key = [0.0] * 12
                        elif kl == "[":
                            for i in range(6): hand_key[i] = 1.0
                        elif kl == "]":
                            for i in range(6): hand_key[i] = 0.0
                        elif kl == ";":
                            for i in range(6, 12): hand_key[i] = 1.0
                        elif kl == "'":
                            for i in range(6, 12): hand_key[i] = 0.0

                    devs = [0.0] * 12
                    if self.use_hand:
                        for i in range(12):
                            if i in freed:
                                continue          # limp: its force is you moving it, not a touch
                            devs[i] = self.hf[i] - base[i]
                            if abs(devs[i]) > thr[i]:
                                if not touching[i]:
                                    touches[i] += 1
                                    touching[i] = True
                            elif abs(devs[i]) < thr[i] * 0.5:
                                touching[i] = False
                        if follow_mode and not follow_on:
                            self.send_hand(ftarget)     # hold, ignore all contact
                        elif follow_mode:
                            # Admittance: push harder -> move faster, capped. Below the
                            # deadband nothing moves, so incidental contact leaves no trace.
                            for k in range(12):
                                dv = self.hf[k] - base[k]
                                # ADAPTIVE BASELINE: this hand's force zero drifts, and an
                                # integrator fed a biased signal walks the finger away by
                                # itself. Re-centre while idle, freeze during a real push.
                                if abs(dv) <= FOLLOW_DEADBAND:
                                    continue
                                dv -= FOLLOW_DEADBAND if dv > 0 else -FOLLOW_DEADBAND
                                stp = -FOLLOW_GAIN * dv * dt
                                stp = clamp(stp, -FOLLOW_MAXRATE * dt, FOLLOW_MAXRATE * dt)
                                ftarget[k] = clamp(ftarget[k] + stp, 0.0, 1.0)
                            self.send_hand(ftarget)
                        elif ext_mode:
                            # EXTERNAL: publish NOTHING. The recorder streams rt/inspire/cmd at
                            # 30Hz, which would out-shout the dashboard's occasional writes and
                            # drag every finger back. Staying silent hands the hand over.
                            pass
                        elif pos_mode:
                            # POSITION mode: the keys drive the hand for real, and what gets
                            # recorded is the MEASURED angle. Note send_hand(hand_key) — the
                            # keys used to update a variable that was never sent, so pressing
                            # o/c during a take did nothing to the physical hand.
                            self.send_hand(hand_key)
                        else:
                            # TOUCH mode: fingers hold the REST pose all through the recording
                            # (thumb_rot parked closed by default, everything else open).
                            self.send_hand(REST)

                    sample = {"t": round(t, 4)}
                    if self.use_arm:
                        sample["q"] = [float(self.low_state.motor_state[i].q)
                                       for i in range(G1_NUM_MOTOR)]
                    if self.use_hand:
                        if pos_mode:
                            # Record the angle the finger ACTUALLY reached. A finger only
                            # changes angle when it is driven, so leaning on an object records
                            # nothing — which is the whole point of this mode.
                            sample["h"] = [round(float(self.hq[i]), 4) for i in range(12)]
                        else:
                            sample["h"] = list(hand_key)   # replaced by remap() below
                            # A freed finger's TRUE measured position is the recording — no
                            # touch remapping, this is real pose-by-hand teaching.
                            for i in freed:
                                sample["h"][i] = round(float(self.hq[i]), 4)
                        sample["d"] = [round(x, 1) for x in devs]
                    frames.append(sample)

                    if self.use_arm:
                        if t < SAFE_START:
                            self.send_arm(init_q, HOLD_KP, HOLD_KD)
                            print(f"\r💪 HOLDING {SAFE_START - t:.1f}s ", end="", flush=True)
                        else:
                            if not limp:
                                print("\n🔓 ARMS LIMP — move them now!\n")
                                limp = True
                            cq = list(init_q)
                            for i in ARM_INDICES:
                                cq[i] = 0.0
                            self.send_arm(cq, TEACH_KP, TEACH_KD)
                    if self.use_hand and int(t * 4) % 4 == 0:
                        live = " ".join(f"{NAMES[i].split('.')[1][:3]}{'*' if abs(devs[i])>thr[i] else ''}"
                                        for i in range(12)
                                        if i not in freed and abs(devs[i]) > thr[i] * 0.6)
                        # Freed fingers show their LIVE position so you can see them follow.
                        pose = " ".join(f"{NAMES[i].split('.')[1][:3]}={self.hq[i]:.2f}"
                                        for i in sorted(freed))
                        line = (f"touching: {live or '-'}" if not freed
                                else f"free[{pose}]  touching: {live or '-'}")
                        print(f"\r  t={t:4.1f}s  {line:58s}", end="", flush=True)
                    time.sleep(max(0, dt - (time.time() - loop)))
        except KeyboardInterrupt:
            print("\n⛔ stopped early.")

        span_t = frames[-1]["t"] if frames else 0
        hz = (len(frames) / span_t) if span_t > 0 else RECORD_HZ
        print(f"\n\n✅ recorded {len(frames)} frames ({span_t:.1f}s, {hz:.1f} Hz)")

        self.open_hand()   # always finish a take with the fingers open

        if self.use_arm:
            self.report_body_drift(frames)

        if self.use_hand and pos_mode:
            # Nothing to infer — 'h' already holds the measured angle. Report only fingers
            # whose DEGREE actually changed; anything you merely leaned on stayed put and is
            # therefore absent, which is the entire reason for this mode.
            print("   captured by ANGLE CHANGE (contact alone records nothing):")
            if follow_mode:
                print("   (force-follow: only fingers you actually PUSHED into a new pose)")
            moved = False
            for i in range(12):
                hs = [f["h"][i] for f in frames]
                rng = max(hs) - min(hs)
                if rng > 0.05:
                    moved = True
                    print(f"     {NAMES[i]:9s} moved {min(hs):.2f} → {max(hs):.2f}  "
                          f"(range {rng:.2f}) → replayed exactly as recorded")
            if not moved:
                print("     (no finger changed angle — drive them with the keys during the take:")
                print("      o/c = both open/close · [ ] = right · ; ' = left)")
        elif self.use_hand:
            frames, pk, ntouch = self.remap(frames, thr, min_close, hold, auto_scale, span, hz,
                                            freed=freed)
            if freed:
                print("   posed by hand (freed):")
                for i in sorted(freed):
                    hs = [f["h"][i] for f in frames]
                    rng = max(hs) - min(hs)
                    if rng < 0.03:
                        print(f"     {NAMES[i]:9s} range {rng:.3f}  ⚠️  barely moved — it may not be "
                              f"back-drivable by hand; use touch instead (drop it from --free)")
                    else:
                        print(f"     {NAMES[i]:9s} range {rng:.3f}  ({min(hs):.2f}..{max(hs):.2f}) "
                              f"→ replayed as recorded")
            tot = sum(touches)
            if tot:
                print(f"   {tot} touch(es):")
                for i in range(12):
                    if touches[i]:
                        mx = max(1.0 - f["h"][i] for f in frames)
                        how = "own peak" if (auto_scale and ntouch[i] >= 2) else f"--span {span:.0f}g"
                        print(f"     {NAMES[i]:9s} peakΔ {pk[i]:5.0f}g  {touches[i]} touch(es)"
                              f"  → closes {mx*100:3.0f}% on replay   (scale: {how})")
            elif not freed:
                # Do not just say "lower it" — say BY HOW MUCH, from what this take actually
                # saw. Guessing the threshold blind is how takes get wasted.
                peaks = [max(abs(f["d"][i]) for f in frames) for i in range(12)]
                best = max(peaks)
                who = NAMES[peaks.index(best)]
                print(f"   (no finger touches captured at --thresh {thresh:.0f}g)")
                if best < 3:
                    print("    nothing touched the hand at all during this take.")
                else:
                    sug = max(8, int(best * 0.5 / 5) * 5)
                    print(f"    hardest press seen: {who} at {best:.0f}g")
                    print(f"    -> try  --thresh {sug}   (half your hardest press)")
                    strong = sorted(((p, NAMES[i]) for i, p in enumerate(peaks) if p > 3),
                                    reverse=True)[:5]
                    print("    per finger: " + "  ".join(f"{n.split('.')[1][:3]}{p:.0f}g"
                                                          for p, n in strong))
        return frames, init_q, hz

    def report_body_drift(self, frames):
        """The balance controller leans the whole robot while you pose the arm. Replay can
        reproduce the waist but NOT the legs, so a take with big leg drift will not land the
        hand where you put it however good the arm tracking is. Say so up front."""
        import math
        if len(frames) < 2 or "q" not in frames[0]:
            return
        worst_leg, worst_leg_j = 0.0, None
        for j in LEG_INDICES:
            qs = [f["q"][j] for f in frames]
            r = math.degrees(max(qs) - min(qs))
            if r > worst_leg:
                worst_leg, worst_leg_j = r, j
        waist = max(math.degrees(max(f["q"][j] for f in frames) - min(f["q"][j] for f in frames))
                    for j in WAIST_INDICES)
        if waist > 1.5:
            print(f"   ↻ torso moved {waist:.1f}° during the take — replayed (was discarded before).")
        if worst_leg > 2.5:
            print(f"   ⚠️  the robot LEANED during this take (leg joint {worst_leg_j} moved "
                  f"{worst_leg:.1f}°). Legs can't be replayed safely, so the hand may land a few cm off.")
            print("      → for repeatable placement, brace/steady the robot while you pose the arm.")

    # ---------------- replay ----------------
    def replay(self, frames, init_q=None, speed=1.0, home=None, hz=RECORD_HZ,
               gravity=True, waist_mode="recorded"):
        if not frames:
            print("   (nothing to replay)")
            return
        has_arm = self.use_arm and "q" in frames[0]
        has_hand = self.use_hand and "h" in frames[0]
        print(f"▶️  REPLAY — {'arm + hand' if has_arm and has_hand else 'arm' if has_arm else 'hand'}"
              f"  ({len(frames)} frames @ {hz:.0f} Hz"
              f"{', gravity ff ON' if (has_arm and gravity) else ''}"
              f"{', waist=' + waist_mode if has_arm else ''})")
        if has_arm and not self.wait_arm():
            return
        if has_arm and init_q is None:
            init_q = [float(self.low_state.motor_state[i].q) for i in range(G1_NUM_MOTOR)]
            # The waist reference must come from the RECORDING, not from wherever the robot
            # happens to be standing now — otherwise previewing a take and replaying the same
            # file later pin the torso to two different poses and place the hand differently.
            if waist_mode == "lock":
                for j in WAIST_INDICES:
                    init_q[j] = frames[0]["q"][j]

        dt_out = 1.0 / max(hz, 1.0)
        self.tau_ff = [0.0] * G1_NUM_MOTOR
        ctrl = (WAIST_INDICES + ARM_INDICES) if has_arm else []

        dqs = self._velocities(frames) if has_arm else None

        def target_at(tgt):
            """Linearly interpolate between recorded frames — nearest-previous held a stale
            target for up to a full frame, which shows up as lag on fast moves."""
            i = 0
            lo, hi = 0, len(frames) - 1
            while lo <= hi:
                mid = (lo + hi) // 2
                if frames[mid]["t"] <= tgt:
                    i, lo = mid, mid + 1
                else:
                    hi = mid - 1
            if i >= len(frames) - 1:
                return frames[-1], (dqs[-1] if dqs else None), 1.0, len(frames) - 1
            t0_, t1_ = frames[i]["t"], frames[i + 1]["t"]
            a = 0.0 if t1_ <= t0_ else clamp((tgt - t0_) / (t1_ - t0_), 0.0, 1.0)
            return frames[i], (dqs[i] if dqs else None), a, i

        def blend_q(i, a):
            f0, f1 = frames[i], frames[min(i + 1, len(frames) - 1)]
            q = [0.0] * G1_NUM_MOTOR
            for j in range(G1_NUM_MOTOR):
                q[j] = (1 - a) * f0["q"][j] + a * f1["q"][j]
            if waist_mode == "lock":
                for j in WAIST_INDICES:
                    q[j] = init_q[j]
            return q

        # ease into the first frame (gravity ff settles here so playback starts on-target)
        if has_arm:
            cur = [float(self.low_state.motor_state[i].q) for i in range(G1_NUM_MOTOR)]
            first = frames[0]["q"]
            for k in range(int(2.0 * hz)):
                a = k / (2.0 * hz)
                q = list(cur)
                for j in WAIST_INDICES:
                    q[j] = (1 - a) * cur[j] + a * (init_q[j] if waist_mode == "lock" else first[j])
                for j in ARM_INDICES:
                    q[j] = (1 - a) * cur[j] + a * first[j]
                tau = self.grav_step(q, dt_out, ctrl) if gravity else None
                self.send_arm(q, PLAY_KP, PLAY_KD, tau=tau)
                if has_hand:
                    self.send_hand(frames[0]["h"])
                time.sleep(dt_out)

        err_sum = [0.0] * G1_NUM_MOTOR
        err_max = [0.0] * G1_NUM_MOTOR
        n_err = 0
        t0 = time.time()
        # PAUSE (p or space): freeze the playback clock but KEEP publishing the frame the
        # replay is sitting on. Stopping the stream instead would drop the arm — rt/arm_sdk
        # needs a continuous command — so a pause holds the pose rather than releasing it.
        paused, pause_at = False, 0.0
        print("   keys: p = pause/resume")
        try:
            with key_poller() as kp:
                while True:
                    k = kp.poll()
                    if k and (k.lower() == "p" or k == " "):
                        paused = not paused
                        if paused:
                            pause_at = time.time()
                        else:
                            t0 += time.time() - pause_at   # the pause costs no playback time
                        print("\n%s\n" % ("⏸  PAUSED — holding this pose (p to resume)"
                                          if paused else "▶️  resumed"), flush=True)
                    tgt = ((pause_at if paused else time.time()) - t0) * speed
                    f, dqf, a, i = target_at(tgt)
                    if has_arm:
                        q = blend_q(i, a)
                        tau = self.grav_step(q, dt_out, ctrl) if gravity else None
                        dq = None
                        if dqf:
                            dq = [0.0] * G1_NUM_MOTOR
                            for j in ctrl:
                                dq[j] = dqf[j] * speed
                        self.send_arm(q, PLAY_KP, PLAY_KD, dq=dq, tau=tau)
                        if self.low_state:
                            n_err += 1
                            for j in ARM_INDICES:
                                e = abs(q[j] - float(self.low_state.motor_state[j].q))
                                err_sum[j] += e
                                err_max[j] = max(err_max[j], e)
                    if has_hand:
                        hf = f["h"]
                        self.send_hand(hf)
                        print(f"\r  t={f['t']:5.1f}s  " +
                              " ".join(f"{NAMES[j].split('.')[1][:3]}:{1-hf[j]:.0%}"
                                       for j in range(12) if hf[j] < 0.95)[:70] + CLR,
                              end="", flush=True)
                    if frames[-1]["t"] < tgt:
                        break
                    time.sleep(dt_out)
                print("\n✅ replay done.")
        except KeyboardInterrupt:
            print("\n⛔ replay stopped.")

        if has_arm and n_err:
            self.report_tracking(err_sum, err_max, n_err, gravity)
        # Release the grip BEFORE the arm travels home, so anything held is let go where it
        # was picked up rather than carried back and dropped.
        if has_hand:
            self.open_hand()
        if has_arm and home:
            self.go_home(init_q, home, hz=hz, gravity=gravity)

    def _velocities(self, frames):
        """Central-difference the recorded trajectory. Sent as dq so kd stops fighting the
        motion — without it the damping term alone adds ~1° of lag on a fast move."""
        n = len(frames)
        out = []
        for k in range(n):
            a, b = max(0, k - 1), min(n - 1, k + 1)
            dt = frames[b]["t"] - frames[a]["t"]
            if dt <= 0:
                out.append([0.0] * G1_NUM_MOTOR)
                continue
            out.append([(frames[b]["q"][j] - frames[a]["q"][j]) / dt for j in range(G1_NUM_MOTOR)])
        return out

    def report_tracking(self, err_sum, err_max, n, gravity):
        import math
        rows = [(j, math.degrees(err_sum[j] / n), math.degrees(err_max[j])) for j in ARM_INDICES]
        rows.sort(key=lambda r: -r[1])
        avg = sum(r[1] for r in rows) / len(rows)
        print(f"📐 tracking error (gravity ff {'ON' if gravity else 'OFF'}): "
              f"mean {avg:.2f}°  worst joints:")
        for j, m, mx in rows[:4]:
            print(f"     motor {j:2d}   mean {m:5.2f}°   peak {mx:5.2f}°")
        if avg > 3.0:
            print("     ⚠️  still high — raise --grav-ki, or the arm is hitting a limit/obstruction.")

    def go_home(self, init_q, home_file, hz=RECORD_HZ, gravity=True):
        hq = load_home(home_file)
        if not hq:
            return
        print(f"🏡 returning arms to home ({home_file})...")
        dt_out = 1.0 / max(hz, 1.0)
        end = [float(self.low_state.motor_state[i].q) for i in range(G1_NUM_MOTOR)]
        steps = int(3.0 * hz)
        ctrl = WAIST_INDICES + ARM_INDICES
        for k in range(steps):
            a = k / steps
            q = list(end)
            for j in WAIST_INDICES:
                q[j] = init_q[j]
            for j in ARM_INDICES:
                q[j] = (1 - a) * end[j] + a * hq[j]
            # PLAY gains, not the softer hold gains — home used to settle even lower than
            # replay did because it was driven at kp=60.
            tau = self.grav_step(q, dt_out, ctrl) if gravity else None
            self.send_arm(q, PLAY_KP, PLAY_KD, tau=tau)
            time.sleep(dt_out)
        print("✅ home.")


def load_home(name):
    p = resolve(name)
    try:
        last = None
        with open(p) as f:
            for line in f:
                d = json.loads(line)
                if "q" in d and len(d["q"]) == G1_NUM_MOTOR:
                    last = d["q"]
        if last:
            print(f"✅ home pose from {p}")
            return last
    except FileNotFoundError:
        print(f"⚠️  home file {p} not found — skipping home return")
    return None


def save(frames, path, use_arm, use_hand, hz):
    meta = {"hz": hz, "arm": use_arm, "hand": use_hand}
    if use_arm:
        meta["motors"] = G1_NUM_MOTOR
    if use_hand:
        meta["hand_motors"] = HAND_MOTORS
    with open(path, "w") as f:
        f.write(json.dumps({"meta": meta}) + "\n")
        for fr in frames:
            f.write(json.dumps(fr) + "\n")
    sz = os.path.getsize(path)
    print(f"💾 saved {len(frames)} frames to {path}  ({sz/1024:.0f} KB)")


def load(name):
    p = resolve(name)
    frames = []
    with open(p) as f:
        meta = json.loads(f.readline()).get("meta", {})
        for line in f:
            d = json.loads(line)
            if "q" in d or "h" in d:
                frames.append(d)
    # Honour the file's own rate. Every recording in DataG1/ predating this was 60 Hz while
    # RECORD_HZ was hardcoded to 30, so half of every trajectory was being skipped.
    hz = float(meta.get("hz") or 0)
    if hz <= 0 and len(frames) > 1 and frames[-1]["t"] > 0:
        hz = len(frames) / frames[-1]["t"]
    if hz <= 0:
        hz = RECORD_HZ
    print(f"loaded {len(frames)} frames from {p}  ({hz:.0f} Hz)")
    return frames, meta, hz


def ask(q):
    try:
        return input(q).strip().lower() in ("y", "yes", "")
    except KeyboardInterrupt:
        return False


def main():
    global GRAV_KI
    ap = argparse.ArgumentParser(description="G1 arm + hand record / replay")
    ap.add_argument("iface", nargs="?", default="enp3s0")
    ap.add_argument("action", choices=["record", "replay"])
    ap.add_argument("--output", default="take")
    ap.add_argument("--input", default=None, help="replay: file in DataG1/")
    ap.add_argument("--seconds", type=float, default=20.0)
    ap.add_argument("--speed", type=float, default=1.0)
    ap.add_argument("--home", default="arm_home.jsonl")
    ap.add_argument("--no-arm", action="store_true")
    ap.add_argument("--no-hand", action="store_true")
    ap.add_argument("--thresh", type=float, default=12.0, help="finger touch threshold (g)")
    # Defaults are DIRECT: closure tracks the press one-to-one, with nothing added.
    #   span 120   = grams of press that means fully closed (measured: your presses run
    #                41-110g, so 120 puts a firm press near full and a light one near 30%)
    #   min-close 0= no floor. A light press closes a little, not 35% minimum.
    #   hold 0     = no latch. Release and it opens again, exactly when you released.
    #   auto-scale off = an absolute scale, so 41g and 110g do NOT both become 100%.
    # The old behaviour is still reachable: --span 45 --min-close 0.35 --hold 0.8 --auto-scale
    ap.add_argument("--span", type=float, default=120.0,
                    help="grams of press that means FULLY closed (default 120)")
    ap.add_argument("--min-close", type=float, default=0.0,
                    help="floor closure for any touch (default 0 = proportional)")
    ap.add_argument("--hold", type=float, default=0.0,
                    help="seconds a closure is held after you release (default 0 = follow exactly)")
    ap.add_argument("--auto-scale", action="store_true",
                    help="normalise each finger to its OWN hardest press (makes every finger "
                         "reach 100%%; off by default because it destroys how hard you pressed)")
    ap.add_argument("--no-auto-scale", action="store_true", help=argparse.SUPPRESS)
    ap.add_argument("--no-gravity", action="store_true",
                    help="disable gravity feedforward (arm will sag below the recorded pose)")
    ap.add_argument("--grav-ki", type=float, default=GRAV_KI,
                    help="gravity feedforward learning rate")
    ap.add_argument("--waist", choices=["recorded", "lock"], default="recorded",
                    help="recorded: replay the torso you recorded (accurate placement). "
                         "lock: pin the torso to the start pose (old behaviour)")
    ap.add_argument("--free", default="none",
                    help=argparse.SUPPRESS)   # disabled: the RH56 cannot release a finger
    ap.add_argument("--fingers", choices=["touch", "position", "external", "follow"],
                    default="touch",
                    help="touch: press a finger to teach how far it closes (any contact counts, "
                         "including leaning on something). position: record the finger's ACTUAL "
                         "angle, driven with the o/c/[ ]/; ' keys — contact alone records "
                         "nothing. external: same, but the recorder sends NO hand commands so "
                         "the dashboard sliders (:8088) drive all 12 fingers individually")
    ap.add_argument("--shapes", default="http://10.255.254.88:8088",
                    help="dashboard URL to load saved finger shapes from; keys 1-9 apply them "
                         "during a take. Use --shapes none to disable")
    ap.add_argument("--thumb-rot", choices=["closed", "open"], default="closed",
                    help="resting position of thumb_rot on BOTH hands, held during record, "
                         "replay and the return to rest (default: closed / tucked in)")
    a = ap.parse_args()
    freed = parse_free(a.free)
    shape_list = [] if a.shapes.lower() == "none" else load_shapes(a.shapes)

    for _i in THUMB_ROT:                        # applies everywhere REST is used
        REST[_i] = 0.0 if a.thumb_rot == "closed" else 1.0

    GRAV_KI = a.grav_ki
    gravity = not a.no_gravity

    rr = G1RecordReplay(a.iface, use_arm=not a.no_arm, use_hand=not a.no_hand)

    if a.action == "replay":
        name = a.input or a.output
        frames, meta, hz = load(name)
        rr.replay(frames, speed=a.speed, home=a.home, hz=hz,
                  gravity=gravity, waist_mode=a.waist)
        rr.release_arm()
        return

    out = rr.record(a.seconds, a.thresh, a.span, a.min_close, a.hold, a.auto_scale,
                    freed=freed, pos_mode=(a.fingers in ("position", "external", "follow")),
                    shapes=shape_list,
                    ext_mode=(a.fingers == "external"),
                    follow_mode=(a.fingers == "follow"))
    if not out:
        return
    frames, init_q, hz = out
    if not a.no_arm:
        rr.go_home(init_q, a.home, hz=hz, gravity=gravity)
    while ask("▶️  Preview this recording before saving? [y/n]: "):
        rr.replay(frames, init_q=init_q, speed=a.speed, home=a.home, hz=hz,
                  gravity=gravity, waist_mode=a.waist)
    rr.open_hand()      # safety net: whichever path we exited by, leave the hand open
    rr.release_arm()
    if ask("💾 Save recording? [y/n]: "):
        save(frames, unique_path(a.output), not a.no_arm, not a.no_hand, hz)
    else:
        print("🗑️  not saved.")
    print("done.")


if __name__ == "__main__":
    main()
