#!/usr/bin/env python3
"""
g1_camera_pose_replay_v2.py  —  Enhanced

Original v2 features:
  • Skip rows with invalid/missing 29-joint data
  • Skip rows where tracked=false
  • Lock to first valid track_id by default
  • Clamp sudden arm jumps between accepted frames
  • Hold last good pose when frames are skipped
  • Preflight summary before moving the robot

Enhancements in this version:
  • Joint limit enforcement  — loads joint.json limits, clamps out-of-range values
                               and reports violation count in preflight
  • Frame interpolation      — linearly interpolates between source frames at 60 Hz,
                               eliminating step-and-hold jitter at low recording rates
  • Live progress bar        — shows elapsed / total time and frame counter during playback
  • Loop mode  (--loop)      — replay the file repeatedly until q is pressed
  • Dry-run    (--dry-run)   — full preflight and validation without touching the robot
  • Fix duplicate timestamps — skips frames with t <= last accepted t (was t < last_t)
  • Progress feedback        — shows % completion during start / home interpolation phases
  • Cleaner preflight report — adds duration, playback ETA, limit violation counts
"""

from __future__ import annotations

import argparse
from dataclasses import dataclass, field
import json
import math
from pathlib import Path
import select
import sys
import termios
import time
import tty

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

from camera_recorder_config import load_config
from g1_camera_pose_mapper import load_home_pose


# ---------------------------------------------------------------------------
# Constants
# ---------------------------------------------------------------------------

G1_NUM_MOTOR         = 29
ENABLE_ARM_SDK_INDEX = 29
DATA_DIR             = Path("DataG1")
REPLAY_HZ            = 60.0
ARM_START            = 15
ARM_END              = 29

KP_HIGH  = 300.0
KD_HIGH  = 3.0
KP_LOW   = 80.0
KD_LOW   = 3.0
KP_WRIST = 40.0
KD_WRIST = 1.5

WEAK_MOTORS  = {4, 10, 15, 16, 17, 18, 22, 23, 24, 25}
WRIST_MOTORS = {19, 20, 21, 26, 27, 28}


# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------

def resolve_input_path(in_path: str) -> str:
    path = Path(in_path)
    if not path.suffix:
        path = path.with_suffix(".jsonl")
    if len(path.parts) == 1:
        return str(DATA_DIR / path.name)
    return str(path)


def clamp(value: float, low: float, high: float) -> float:
    return max(low, min(high, float(value)))


def lerp(a: float, b: float, t: float) -> float:
    return (1.0 - t) * float(a) + t * float(b)


def interp_q(a: list[float], b: list[float], t: float) -> list[float]:
    """Linearly interpolate between two full joint position vectors."""
    return [lerp(a[i], b[i], t) for i in range(len(a))]


def arm_delta(a: list[float], b: list[float]) -> float:
    return max(abs(float(a[i]) - float(b[i])) for i in range(ARM_START, ARM_END))


def clamp_arm_step(
    target_q: list[float], prev_q: list[float], max_delta: float
) -> tuple[list[float], bool]:
    clamped_q   = list(target_q)
    was_clamped = False
    for i in range(ARM_START, ARM_END):
        delta = float(target_q[i]) - float(prev_q[i])
        if abs(delta) > max_delta:
            clamped_q[i] = float(prev_q[i]) + math.copysign(max_delta, delta)
            was_clamped  = True
    return clamped_q, was_clamped


def apply_joint_limits(
    q: list[float], limits: dict[int, tuple[float, float]]
) -> tuple[list[float], int]:
    """Clamp q to physical joint limits. Returns (clamped_q, number_of_violations)."""
    out        = list(q)
    violations = 0
    for idx, (lo, hi) in limits.items():
        val     = float(out[idx])
        clamped = clamp(val, lo, hi)
        if clamped != val:
            violations += 1
            out[idx]    = clamped
    return out, violations


def load_joint_limits_from_config(config: dict) -> dict[int, tuple[float, float]]:
    """Load joint limits from the joint.json path referenced in config."""
    joint_config_path = config.get("paths", {}).get("joint_config", "joint.json")
    p = Path(joint_config_path)
    if not p.is_absolute():
        p = Path(__file__).parent / p
    try:
        with open(p, "r", encoding="utf-8") as fh:
            jdata = json.load(fh)
        return {
            int(k): (float(v[0]), float(v[1]))
            for k, v in jdata.get("joint_limits", {}).items()
        }
    except Exception as exc:
        print(f"⚠️  Could not load joint limits from {p}: {exc}")
        return {}


# ---------------------------------------------------------------------------
# Data classes
# ---------------------------------------------------------------------------

@dataclass
class PlaybackFrame:
    t:                float
    q:                list[float]
    track_id:         int | None = None
    source_line:      int        = 0
    tracked:          bool       = True
    clamped:          bool       = False
    limit_violations: int        = 0


@dataclass
class PreflightReport:
    meta:                   dict         = field(default_factory=dict)
    raw_rows:               int          = 0
    accepted_frames:        int          = 0
    skipped_bad_q:          int          = 0
    skipped_untracked:      int          = 0
    skipped_track_switch:   int          = 0
    skipped_non_monotonic:  int          = 0
    skipped_non_finite:     int          = 0
    clamped_frames:         int          = 0
    limit_violation_frames: int          = 0
    total_limit_violations: int          = 0
    locked_track_id:        int | None   = None
    track_ids_seen:         set[int]     = field(default_factory=set)
    max_raw_arm_delta:      float        = 0.0
    max_clean_arm_delta:    float        = 0.0
    max_gap_seconds:        float        = 0.0
    long_gap_count:         int          = 0
    first_time:             float | None = None
    last_time:              float | None = None


# ---------------------------------------------------------------------------
# Home pose loader
# ---------------------------------------------------------------------------

def load_home_pose_for_replay(home_path: str) -> list[float]:
    path = resolve_input_path(home_path)
    try:
        q = load_home_pose(path, G1_NUM_MOTOR)
        print(f"✅ Loaded home pose from {path}")
        return q
    except FileNotFoundError:
        print(f"⚠️  Home file not found: {path}")
    except ValueError:
        print(f"⚠️  {path} contained no valid 29-joint pose.")
    print("⚠️  Using default home (all zeros).")
    return [0.0] * G1_NUM_MOTOR


# ---------------------------------------------------------------------------
# Dataset sanitization
# ---------------------------------------------------------------------------

def sanitize_frames(
    filename:         str,
    *,
    max_arm_delta:    float,
    gap_warn_seconds: float,
    lock_first_track: bool,
    joint_limits:     dict[int, tuple[float, float]],
) -> tuple[list[PlaybackFrame], PreflightReport]:
    report          = PreflightReport()
    frames:          list[PlaybackFrame] = []
    last_accepted_t: float | None        = None
    last_good:       PlaybackFrame | None = None

    with open(filename, "r", encoding="utf-8") as fh:
        for line_num, raw_line in enumerate(fh, start=1):
            line = raw_line.strip()
            if not line:
                continue
            try:
                data = json.loads(line)
            except json.JSONDecodeError:
                continue

            # metadata line
            if "meta" in data and "q" not in data:
                report.meta = data.get("meta", {})
                continue

            report.raw_rows += 1

            # ── q validation ──────────────────────────────────────────────
            q = data.get("q")
            if not isinstance(q, list) or len(q) != G1_NUM_MOTOR:
                report.skipped_bad_q += 1
                continue
            try:
                qf = [float(v) for v in q]
            except (TypeError, ValueError):
                report.skipped_bad_q += 1
                continue
            if any(not math.isfinite(v) for v in qf):
                report.skipped_non_finite += 1
                continue

            # ── strict monotonic timestamp (skips equal too — fixes duplicates) ──
            t = float(data.get("t", report.raw_rows / REPLAY_HZ))
            if last_accepted_t is not None and t <= last_accepted_t:
                report.skipped_non_monotonic += 1
                continue

            # ── tracking ──────────────────────────────────────────────────
            track_id_raw = data.get("track_id")
            track_id     = int(track_id_raw) if isinstance(track_id_raw, int) else None
            if track_id is not None:
                report.track_ids_seen.add(track_id)

            tracked = bool(data.get("tracked", True))
            if not tracked:
                report.skipped_untracked += 1
                continue

            if lock_first_track and track_id is not None:
                if report.locked_track_id is None:
                    report.locked_track_id = track_id
                elif track_id != report.locked_track_id:
                    report.skipped_track_switch += 1
                    continue

            # ── joint limit enforcement ───────────────────────────────────
            q_limited, violations = apply_joint_limits(qf, joint_limits)
            if violations > 0:
                report.limit_violation_frames += 1
                report.total_limit_violations += violations

            # ── arm jump clamping ─────────────────────────────────────────
            q_clean     = q_limited
            was_clamped = False
            if last_good is not None:
                raw_delta            = arm_delta(q_limited, last_good.q)
                report.max_raw_arm_delta = max(report.max_raw_arm_delta, raw_delta)
                q_clean, was_clamped = clamp_arm_step(q_limited, last_good.q, max_arm_delta)
                report.max_clean_arm_delta = max(
                    report.max_clean_arm_delta, arm_delta(q_clean, last_good.q)
                )
                gap                  = t - last_good.t
                report.max_gap_seconds = max(report.max_gap_seconds, gap)
                if gap_warn_seconds > 0 and gap > gap_warn_seconds:
                    report.long_gap_count += 1

            if was_clamped:
                report.clamped_frames += 1

            if report.first_time is None:
                report.first_time = t
            report.last_time  = t
            last_accepted_t   = t

            frame = PlaybackFrame(
                t=t,
                q=q_clean,
                track_id=track_id,
                source_line=line_num,
                tracked=tracked,
                clamped=was_clamped,
                limit_violations=violations,
            )
            frames.append(frame)
            last_good = frame

    report.accepted_frames = len(frames)
    return frames, report


# ---------------------------------------------------------------------------
# Preflight report
# ---------------------------------------------------------------------------

def print_preflight(filename: str, report: PreflightReport, args) -> None:
    duration     = (
        (report.last_time - report.first_time)
        if report.first_time is not None and report.last_time is not None
        else 0.0
    )
    playback_eta = duration / max(args.speed, 1e-6)

    print("\n" + "=" * 66)
    print("📋  CAMERA REPLAY PREFLIGHT")
    print(f"   File             : {filename}")
    if report.meta:
        print(f"   Format           : {report.meta.get('format', 'unknown')}")
        print(f"   Model            : {report.meta.get('model', 'unknown')}")
        print(f"   Recorded Hz      : {report.meta.get('hz', 'unknown')}")
    print(f"   Duration         : {duration:.2f} s  ({report.accepted_frames} frames)")
    print(f"   Playback ETA     : ~{playback_eta:.1f} s  at {args.speed:.2f}×")
    print("─" * 66)
    print(f"   Raw rows         : {report.raw_rows}")
    print(f"   Accepted         : {report.accepted_frames}")
    print(f"   Skipped bad q    : {report.skipped_bad_q}")
    print(f"   Skipped non-finite: {report.skipped_non_finite}")
    print(f"   Skipped untracked: {report.skipped_untracked}")
    print(f"   Skipped track swap: {report.skipped_track_switch}")
    print(f"   Skipped bad time : {report.skipped_non_monotonic}")
    print(f"   Clamped frames   : {report.clamped_frames}  (max delta = {args.max_arm_delta:.3f} rad)")
    if report.total_limit_violations:
        print(
            f"   Limit violations : {report.limit_violation_frames} frames, "
            f"{report.total_limit_violations} joints total  ← clamped to safe range"
        )
    else:
        print("   Limit violations : none  ✅")
    print("─" * 66)
    print(f"   Locked track id  : {report.locked_track_id}")
    print(f"   Track ids seen   : {sorted(report.track_ids_seen) if report.track_ids_seen else 'none'}")
    print(f"   Max raw arm jump : {report.max_raw_arm_delta:.4f} rad")
    print(f"   Max clean arm jump: {report.max_clean_arm_delta:.4f} rad")
    print(f"   Max frame gap    : {report.max_gap_seconds:.4f} s")
    print(f"   Gap warnings     : {report.long_gap_count}  (threshold = {args.gap_warn_seconds:.3f} s)")
    if args.abort_gap_seconds <= 0:
        print("   Gap abort        : DISABLED  (pass --abort-gap-seconds to enable)")
    else:
        print(f"   Gap abort        : {args.abort_gap_seconds:.3f} s")
    print("=" * 66 + "\n")


# ---------------------------------------------------------------------------
# Keyboard poller
# ---------------------------------------------------------------------------

class KeyPoller:
    def __enter__(self):
        self.enabled     = sys.stdin.isatty()
        self.fd          = None
        self.old_settings = None
        if self.enabled:
            self.fd           = sys.stdin.fileno()
            self.old_settings = termios.tcgetattr(self.fd)
            tty.setcbreak(self.fd)
        return self

    def __exit__(self, exc_type, exc, tb):
        if self.enabled and self.fd is not None and self.old_settings is not None:
            termios.tcsetattr(self.fd, termios.TCSADRAIN, self.old_settings)

    def poll(self) -> str | None:
        if not self.enabled or self.fd is None:
            return None
        if select.select([sys.stdin], [], [], 0)[0]:
            return sys.stdin.read(1)
        return None


# ---------------------------------------------------------------------------
# Replay engine
# ---------------------------------------------------------------------------

class ReplayWithHomeV2:
    def __init__(self):
        self.low_state   = None
        self.low_cmd     = unitree_hg_msg_dds__LowCmd_()
        self.crc         = CRC()
        self.arm_pub     = ChannelPublisher("rt/arm_sdk", LowCmd_)
        self.arm_pub.Init()
        self.state_sub   = ChannelSubscriber("rt/lowstate", LowState_)
        self.state_sub.Init(self.LowStateHandler, 10)
        self.first_state = False

    def LowStateHandler(self, msg: LowState_) -> None:
        self.low_state   = msg
        self.first_state = True

    def SendFrame(self, arm_target_q: list[float], body_lock_q: list[float]) -> None:
        self.low_cmd.motor_cmd[ENABLE_ARM_SDK_INDEX].q = 1.0
        for i in range(G1_NUM_MOTOR):
            self.low_cmd.motor_cmd[i].mode = 1
            self.low_cmd.motor_cmd[i].dq   = 0.0
            self.low_cmd.motor_cmd[i].tau  = 0.0
            self.low_cmd.motor_cmd[i].q    = arm_target_q[i] if i >= ARM_START else body_lock_q[i]
            if i in WRIST_MOTORS:
                self.low_cmd.motor_cmd[i].kp = KP_WRIST
                self.low_cmd.motor_cmd[i].kd = KD_WRIST
            elif i in WEAK_MOTORS:
                self.low_cmd.motor_cmd[i].kp = KP_LOW
                self.low_cmd.motor_cmd[i].kd = KD_LOW
            else:
                self.low_cmd.motor_cmd[i].kp = KP_HIGH
                self.low_cmd.motor_cmd[i].kd = KD_HIGH
        self.low_cmd.crc = self.crc.Crc(self.low_cmd)
        self.arm_pub.Write(self.low_cmd)

    def DisableSDK(self) -> None:
        print("\n🔌 Disabling SDK...")
        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(10):
            self.arm_pub.Write(self.low_cmd)
            time.sleep(0.02)

    def _interpolate_phase(
        self,
        label:   str,
        from_q:  list[float],
        to_q:    list[float],
        body_q:  list[float],
        steps:   int,
    ) -> None:
        """Smoothly move arms from from_q → to_q over `steps` frames at REPLAY_HZ."""
        dt = 1.0 / REPLAY_HZ
        for step in range(steps):
            alpha = step / max(1, steps - 1)
            self.SendFrame(interp_q(from_q, to_q, alpha), body_q)
            if step % 15 == 0 or step == steps - 1:
                pct = int(100.0 * alpha)
                bar = ("█" * (pct // 5)).ljust(20)
                print(f"\r  {label} [{bar}] {pct:3d}%", end="", flush=True)
            time.sleep(dt)
        print()

    def _play_once(
        self,
        frames:      list[PlaybackFrame],
        body_lock_q: list[float],
        speed:       float,
        kp:          KeyPoller,
    ) -> tuple[list[float], bool]:
        """
        Play all frames once with frame interpolation and a live progress bar.
        Returns (last_played_q, quit_requested).
        """
        first_t       = frames[0].t
        final_t       = frames[-1].t - first_t
        frame_idx     = 0
        play_elapsed  = 0.0
        last_real     = time.time()
        last_played_q = list(frames[0].q)
        paused        = False
        quit_req      = False
        dt            = 1.0 / REPLAY_HZ

        while True:
            key = kp.poll()
            if key:
                k = key.lower()
                if k == "x" and not paused:
                    paused    = True
                    print("\n⏸️  PAUSED  (z = resume, q = quit)")
                elif k == "z" and paused:
                    paused    = False
                    last_real = time.time()
                    print("\n▶️  RESUMED")
                elif k == "q":
                    quit_req  = True
                    break

            now_real  = time.time()
            if paused:
                self.SendFrame(last_played_q, body_lock_q)
                time.sleep(dt)
                continue

            dt_real      = now_real - last_real
            last_real    = now_real
            play_elapsed += dt_real * speed

            # advance frame index
            while frame_idx + 1 < len(frames):
                if frames[frame_idx + 1].t - first_t <= play_elapsed:
                    frame_idx += 1
                else:
                    break

            # ── interpolate between source frames (smooth 60 Hz output) ──
            if frame_idx + 1 < len(frames):
                fa      = frames[frame_idx]
                fb      = frames[frame_idx + 1]
                seg_len = max(fb.t - fa.t, 1e-9)
                alpha   = clamp((play_elapsed - (fa.t - first_t)) / seg_len, 0.0, 1.0)
                out_q   = interp_q(fa.q, fb.q, alpha)
            else:
                out_q = list(frames[frame_idx].q)

            last_played_q = out_q
            self.SendFrame(out_q, body_lock_q)

            # ── live progress bar ─────────────────────────────────────────
            pct = clamp(play_elapsed / max(final_t, 1e-9) * 100.0, 0.0, 100.0)
            bar = ("█" * int(pct / 5)).ljust(20)
            print(
                f"\r  ▶ [{bar}] {pct:5.1f}%  "
                f"{min(play_elapsed, final_t):.1f}/{final_t:.1f}s  "
                f"frame {frame_idx + 1}/{len(frames)}  ",
                end="",
                flush=True,
            )

            if frame_idx >= len(frames) - 1 and play_elapsed >= final_t:
                break

            time.sleep(dt)

        print()
        return last_played_q, quit_req

    def Run(self, filename: str, home_filename: str, args, config: dict) -> None:
        # ── Joint limits ──────────────────────────────────────────────────
        joint_limits = load_joint_limits_from_config(config)
        if joint_limits:
            print(f"✅ Joint limits loaded for {len(joint_limits)} joints.")
        else:
            print("⚠️  No joint limits loaded — limit enforcement skipped.")

        # ── Sanitize dataset ──────────────────────────────────────────────
        print(f"📂 Loading: {filename}")
        frames, report = sanitize_frames(
            filename,
            max_arm_delta=args.max_arm_delta,
            gap_warn_seconds=args.gap_warn_seconds,
            lock_first_track=not args.allow_track_switch,
            joint_limits=joint_limits,
        )
        print_preflight(filename, report, args)

        if report.accepted_frames < 2:
            raise SystemExit("❌ Not enough valid frames after filtering. Replay aborted.")
        if args.abort_gap_seconds > 0 and report.max_gap_seconds > args.abort_gap_seconds:
            raise SystemExit(
                f"❌ Replay aborted: max gap {report.max_gap_seconds:.3f}s exceeds "
                f"--abort-gap-seconds {args.abort_gap_seconds:.3f}s"
            )

        # ── Connect to robot ──────────────────────────────────────────────
        print("Waiting for robot state...", end="", flush=True)
        while not self.first_state:
            time.sleep(0.1)
        print(" Connected!\n")

        home_q      = load_home_pose_for_replay(home_filename)
        body_lock_q = [self.low_state.motor_state[i].q for i in range(G1_NUM_MOTOR)]

        print("🔒 Body LOCKED. Arms will follow filtered camera data.")
        print("🎮 Controls:  x = PAUSE   z = RESUME   q = QUIT\n")
        if sys.stdin.isatty() and not args.no_prompt:
            input("👉 Press Enter to Begin... ")

        # ── Move to start pose ────────────────────────────────────────────
        self._interpolate_phase(
            "Moving to start pose",
            body_lock_q, frames[0].q,
            body_lock_q, args.start_steps,
        )
        last_played_q = list(frames[0].q)

        # ── Playback (with optional loop) ─────────────────────────────────
        loop_num = 0
        try:
            with KeyPoller() as kp:
                while True:
                    loop_num += 1
                    if args.loop:
                        print(f"▶️  Loop {loop_num}  (x = pause, q = quit)")
                    else:
                        print("▶️  Playing...  (x = pause, q = quit)")

                    last_played_q, quit_req = self._play_once(
                        frames, body_lock_q, args.speed, kp
                    )
                    if quit_req or not args.loop:
                        break
                    time.sleep(0.3)
        except KeyboardInterrupt:
            print("\nStopped.")

        # ── Return to home ────────────────────────────────────────────────
        print(f"\n🏡 Returning to home  ({home_filename})...")
        self._interpolate_phase(
            "Returning to home",
            last_played_q, home_q,
            body_lock_q, args.home_steps,
        )
        print("✅ Home reached.")
        self.DisableSDK()


# ---------------------------------------------------------------------------
# Dry-run path  (preflight only, no SDK)
# ---------------------------------------------------------------------------

def run_dry(filename: str, home_filename: str, args, config: dict) -> None:
    joint_limits = load_joint_limits_from_config(config)
    if joint_limits:
        print(f"✅ Joint limits loaded for {len(joint_limits)} joints.")
    else:
        print("⚠️  No joint limits loaded.")

    print(f"📂 Loading: {filename}")
    frames, report = sanitize_frames(
        filename,
        max_arm_delta=args.max_arm_delta,
        gap_warn_seconds=args.gap_warn_seconds,
        lock_first_track=not args.allow_track_switch,
        joint_limits=joint_limits,
    )
    print_preflight(filename, report, args)

    if report.accepted_frames < 2:
        raise SystemExit("❌ Not enough valid frames after filtering.")
    if args.abort_gap_seconds > 0 and report.max_gap_seconds > args.abort_gap_seconds:
        raise SystemExit(
            f"❌ Aborted: max gap {report.max_gap_seconds:.3f}s exceeds "
            f"--abort-gap-seconds {args.abort_gap_seconds:.3f}s"
        )
    print("✅ Dry-run complete. No robot connection made.")


# ---------------------------------------------------------------------------
# Entry point
# ---------------------------------------------------------------------------

def main() -> None:
    pre_parser = argparse.ArgumentParser(add_help=False)
    pre_parser.add_argument("--config", default="camera_recorder_config.json")
    pre_args, _ = pre_parser.parse_known_args()
    config, _   = load_config(pre_args.config)
    rd          = config.get("replay_defaults", {})

    parser = argparse.ArgumentParser(
        description="Camera pose replay for G1 humanoid — enhanced v2",
        formatter_class=argparse.ArgumentDefaultsHelpFormatter,
    )
    parser.add_argument("--config",   default=pre_args.config,        help="Config JSON path")
    parser.add_argument("iface_pos",  nargs="?", default=None,        help="Network interface (positional)")
    parser.add_argument("--iface",    default=None,                    help="Network interface")
    parser.add_argument(
        "--input", "--input-file", dest="input",
        default=rd.get("input"),
        help="Input JSONL recording file",
    )
    parser.add_argument(
        "--home", "--home-file", dest="home",
        default=rd.get("home", "arm_home.jsonl"),
        help="Home pose JSONL file",
    )
    parser.add_argument(
        "--speed", type=float,
        default=float(rd.get("speed", 0.35)),
        help="Replay speed multiplier",
    )
    parser.add_argument(
        "--max-arm-delta", type=float,
        default=float(rd.get("max_arm_delta", 0.18)),
        help="Max allowed arm-joint delta between source frames (rad)",
    )
    parser.add_argument(
        "--gap-warn-seconds", type=float,
        default=float(rd.get("gap_warn_seconds", 0.35)),
        help="Warn when source frame gap exceeds this (s)",
    )
    parser.add_argument(
        "--abort-gap-seconds", type=float,
        default=float(rd.get("abort_gap_seconds", 0.0)),
        help="Abort replay if frame gap exceeds this (s). 0 = disabled.",
    )
    parser.add_argument(
        "--allow-track-switch", action="store_true",
        default=bool(rd.get("allow_track_switch", False)),
        help="Do not lock to the first valid track_id",
    )
    parser.add_argument(
        "--start-steps", type=int,
        default=int(rd.get("start_steps", 60)),
        help="Frames to reach start pose (@ 60 Hz = seconds × 60)",
    )
    parser.add_argument(
        "--home-steps", type=int,
        default=int(rd.get("home_steps", 180)),
        help="Frames to return to home (@ 60 Hz = seconds × 60)",
    )
    parser.add_argument(
        "--no-prompt", action="store_true",
        default=bool(rd.get("no_prompt", False)),
        help="Skip the Enter-to-begin prompt",
    )
    parser.add_argument(
        "--loop", action="store_true",
        default=bool(rd.get("loop", False)),
        help="Loop replay until q is pressed",
    )
    parser.add_argument(
        "--dry-run", action="store_true",
        default=False,
        help="Run preflight validation only — no robot connection",
    )
    args = parser.parse_args()

    args.iface = args.iface or args.iface_pos or rd.get("iface")

    if not args.dry_run and not args.iface:
        parser.error(
            "Missing interface. Pass it positionally, via --iface, "
            "or set replay_defaults.iface in camera_recorder_config.json."
        )
    if not args.input:
        parser.error(
            "Missing input file. Pass --input or set replay_defaults.input "
            "in camera_recorder_config.json."
        )

    path = resolve_input_path(args.input)

    if args.dry_run:
        run_dry(path, args.home, args, config)
    else:
        ChannelFactoryInitialize(0, args.iface)
        ReplayWithHomeV2().Run(path, args.home, args, config)


if __name__ == "__main__":
    main()
