#!/usr/bin/env python3
"""Robot arm BLE proximity wave helper.

Modes:
  --scan                 print nearby BLE devices from robot arm MCP scan_ble
  --check-and-wave       if TARGET_MAC is seen above RSSI threshold, wave once per day

Configure with env vars:
  ROBOT_ARM_MCP_URL       default http://192.168.68.140/api/mcp
  ROBOT_ARM_PHONE_MAC     BLE MAC/address to watch, e.g. aa:bb:cc:dd:ee:ff
  ROBOT_ARM_RSSI_MIN      default -65 (higher/less-negative means closer)
  ROBOT_ARM_STATE_FILE    default ~/.hermes/state/robot_arm_proximity_wave.json
"""
import argparse
import datetime as dt
import json
import os
import subprocess
import sys
import time
from pathlib import Path

URL = os.environ.get("ROBOT_ARM_MCP_URL", "http://192.168.68.140/api/mcp")
STATE_FILE = Path(os.environ.get("ROBOT_ARM_STATE_FILE", str(Path.home() / ".hermes/state/robot_arm_proximity_wave.json"))).expanduser()
TARGET_MAC = os.environ.get("ROBOT_ARM_PHONE_MAC", "").lower().strip()
RSSI_MIN = int(os.environ.get("ROBOT_ARM_RSSI_MIN", "-65"))


def mcp_call(tool, arguments=None, timeout=15):
    payload = {
        "jsonrpc": "2.0",
        "id": int(time.time() * 1000) % 1000000,
        "method": "tools/call",
        "params": {"name": tool, "arguments": arguments or {}},
    }
    proc = subprocess.run(
        ["curl", "-sS", "--max-time", str(timeout), "-H", "Content-Type: application/json", "-d", json.dumps(payload), URL],
        capture_output=True,
        text=True,
        timeout=timeout + 3,
    )
    if proc.returncode != 0:
        raise RuntimeError(proc.stderr.strip() or f"curl exit {proc.returncode}")
    data = json.loads(proc.stdout)
    if "error" in data:
        raise RuntimeError(json.dumps(data["error"]))
    content = data["result"]["content"][0]
    return content.get("text", "")


def scan_ble(duration_ms=8000):
    text = mcp_call("scan_ble", {"duration_ms": duration_ms}, timeout=max(12, duration_ms // 1000 + 6))
    devices = json.loads(text)
    return {k.lower(): v for k, v in devices.items()}


def wave():
    # Correct friendly wave for this arm: bigger all-joint motion at double speed.
    # Home is base=70, shoulder=40, elbow=20, pitch=90, roll=90, gripper=90.
    poses = [
        {"base": 70, "shoulder": 55, "elbow": 45, "pitch": 105, "roll": 90, "gripper": 120, "duration": 0.40},
        {"base": 45, "shoulder": 65, "elbow": 55, "pitch": 70, "roll": 45, "gripper": 135, "duration": 0.33},
        {"base": 105, "shoulder": 35, "elbow": 30, "pitch": 125, "roll": 145, "gripper": 95, "duration": 0.33},
        {"base": 45, "shoulder": 65, "elbow": 55, "pitch": 70, "roll": 45, "gripper": 135, "duration": 0.33},
        {"base": 105, "shoulder": 35, "elbow": 30, "pitch": 125, "roll": 145, "gripper": 95, "duration": 0.33},
        {"base": 70, "shoulder": 60, "elbow": 50, "pitch": 90, "roll": 30, "gripper": 150, "duration": 0.28},
        {"base": 70, "shoulder": 60, "elbow": 50, "pitch": 90, "roll": 150, "gripper": 80, "duration": 0.28},
        {"base": 70, "shoulder": 55, "elbow": 40, "pitch": 95, "roll": 90, "gripper": 120, "duration": 0.28},
        {"base": 70, "shoulder": 40, "elbow": 20, "pitch": 90, "roll": 90, "gripper": 90, "duration": 0.50},
    ]
    for pose in poses:
        mcp_call("move_all_joints", pose, timeout=8)
        time.sleep(max(0.06, pose["duration"] * 0.35))


def load_state():
    if not STATE_FILE.exists():
        return {}
    try:
        return json.loads(STATE_FILE.read_text())
    except Exception:
        return {}


def save_state(state):
    STATE_FILE.parent.mkdir(parents=True, exist_ok=True)
    STATE_FILE.write_text(json.dumps(state, indent=2, sort_keys=True))


def today_key():
    return dt.datetime.now().strftime("%Y-%m-%d")


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--scan", action="store_true")
    ap.add_argument("--check-and-wave", action="store_true")
    ap.add_argument("--duration-ms", type=int, default=8000)
    args = ap.parse_args()

    if args.scan:
        devices = scan_ble(args.duration_ms)
        rows = sorted(devices.items(), key=lambda kv: kv[1].get("rssi", -999), reverse=True)
        print(json.dumps({mac: info for mac, info in rows}, indent=2))
        return 0

    if args.check_and_wave:
        if not TARGET_MAC:
            print("ROBOT_ARM_PHONE_MAC is not set; scan first and choose a target address.")
            return 2
        devices = scan_ble(args.duration_ms)
        hit = devices.get(TARGET_MAC)
        if not hit:
            print(f"target {TARGET_MAC} not seen")
            return 0
        rssi = int(hit.get("rssi", -999))
        print(f"target {TARGET_MAC} seen with RSSI {rssi}")
        if rssi < RSSI_MIN:
            print(f"RSSI below threshold {RSSI_MIN}; not waving")
            return 0
        state = load_state()
        today = today_key()
        if state.get("last_wave_date") == today:
            print(f"already waved today ({today}); not waving")
            return 0
        wave()
        state.update({"last_wave_date": today, "last_seen_rssi": rssi, "last_seen_at": dt.datetime.now().isoformat(timespec="seconds")})
        save_state(state)
        print("waved and saved once-per-day state")
        return 0

    ap.print_help()
    return 1


if __name__ == "__main__":
    raise SystemExit(main())