#!/usr/bin/env python3
"""
Live view + logger of the Inspire hand's 12 joints (rt/inspire/state).

  conda activate g1_env
  cd ~/Robotics_workspace/yslootahtech/G1/G1_Lootah/Manual_Recorder
  python3 hand_monitor.py enp3s0                 # live bars + auto CSV log
  python3 hand_monitor.py enp3s0 --hz 30         # faster refresh/log
  python3 hand_monitor.py enp3s0 --log run1.csv  # choose log file
  python3 hand_monitor.py enp3s0 --no-log        # view only, no file

Needs inspire_g1 running on the robot (the Docker container).
q in [0,1]:  1.00 = open,  0.00 = closed.  Ctrl+C to stop.
Idx 0-5 = right hand, 6-11 = left; per hand: pinky,ring,middle,index,thumb_bend,thumb_rot.
"""
import sys, time, argparse, csv
from datetime import datetime
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorStates_

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"]

CLR = "\x1b[K"   # clear to end of line (avoids redraw artifacts)


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


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("iface", nargs="?", default="enp3s0", help="network interface (default enp3s0)")
    ap.add_argument("--hz", type=float, default=20.0, help="refresh + log rate (default 20)")
    ap.add_argument("--log", default=None, help="CSV log path (default hand_log_<timestamp>.csv)")
    ap.add_argument("--no-log", action="store_true", help="view only, do not write a log")
    args = ap.parse_args()

    ChannelFactoryInitialize(0, args.iface)

    st = {"q": [0.0] * 12, "n": 0}

    def on_state(msg):
        try:
            st["q"] = [float(msg.states[i].q) for i in range(12)]
            st["n"] += 1
        except Exception:
            pass

    sub = ChannelSubscriber("rt/inspire/state", MotorStates_)
    sub.Init(on_state, 10)

    writer = logf = log_path = None
    if not args.no_log:
        log_path = args.log or f"hand_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        logf = open(log_path, "w", newline="")
        writer = csv.writer(logf)
        writer.writerow(["t_epoch", "iso"] + NAMES)

    dt = 1.0 / args.hz
    rows = 0
    print("\x1b[2J", end="")  # clear once
    try:
        while True:
            now = time.time()
            q = st["q"]
            live_r = any(abs(x) > 1e-3 for x in q[:6])
            live_l = any(abs(x) > 1e-3 for x in q[6:])

            lines = ["\x1b[H"]  # cursor home; redraw in place
            lines.append(f"Inspire hand — live joints   iface={args.iface}   msgs={st['n']:>6d}   "
                         f"{datetime.now().strftime('%H:%M:%S')}   (Ctrl+C to stop){CLR}")
            lines.append(f"q: 1.00 open ░ 0.00 closed" + (f"   log: {log_path}" if log_path else "   (no log)") + CLR)
            lines.append(CLR)
            for i, nm in enumerate(NAMES):
                if i == 6:
                    lines.append(CLR)  # blank line between hands
                lines.append(f"  {nm:9s} [{bar(q[i])}] {q[i]:5.2f}{CLR}")
            lines.append(CLR)
            lines.append(f"  RIGHT: {'● live' if live_r else '○ FLAT 0 (adapter dropped?)'}     "
                         f"LEFT: {'● live' if live_l else '○ FLAT 0 (adapter dropped?)'}{CLR}")
            if log_path:
                lines.append(f"  logged rows: {rows}{CLR}")
            print("\n".join(lines), end="", flush=True)

            if writer:
                writer.writerow([f"{now:.3f}", datetime.now().isoformat()] + [f"{x:.4f}" for x in q])
                rows += 1

            time.sleep(dt)
    except KeyboardInterrupt:
        pass
    finally:
        if logf:
            logf.close()
        print(f"\n\nstopped." + (f"  Saved {rows} rows to {log_path}" if log_path else ""))


if __name__ == "__main__":
    main()