"""Bridge between Saqr PPE detection and the Unitree G1 robot.

Default behavior on the robot: do NOTHING until the operator presses
**R2+X** on the G1 wireless remote. R2+X starts saqr as a subprocess,
R2+Y stops it. While saqr is running the bridge parses its event stream and:

  * UNSAFE  -> announce missing PPE via the G1 onboard TtsMaker (English,
              speaker_id=2) AND run the 'reject' arm action (id=13).
  * SAFE    -> announce "Safe to enter. Have a good day." No arm motion.
  * PARTIAL -> nothing.

See docs/DEPLOY.md for wireless-remote workflow and systemd deploy notes.

Saqr event line format (from saqr.core.events.emit_event):
    ID 0001 | NEW | UNSAFE | wearing: ... | missing: ... | unknown: ...
    ID 0001 | STATUS_CHANGE | SAFE  | wearing: ... | missing: ... | unknown: ...
"""
from __future__ import annotations

import argparse
import os
import re
import signal
import subprocess
import sys
import threading
import time
from typing import Dict, Optional

from saqr.core.paths import PROJECT_ROOT
from saqr.robot.robot_controller import RobotController

DANGER_STATUS = "UNSAFE"
SAFE_STATUS   = "SAFE"

# speaker_id=2 was confirmed English on current G1 firmware via
# Project/Sanad/voice_example.py mode 6. speaker_id=0 is Chinese.
TTS_SPEAKER_ID = 2

TTS_TEXT_SAFE           = "Safe to enter. Have a good day."
TTS_UNSAFE_WITH_MISSING = (
    "Please stop. Wear your proper safety equipment. You are missing {items}."
)
TTS_UNSAFE_GENERIC      = "Please stop. Wear your proper safety equipment."
TTS_BRIDGE_DEACTIVATED  = "Saqr deactivated."
TTS_BRIDGE_READY        = "Saqr is running. Press R2 plus X to start."
TTS_BRIDGE_NO_CAMERA    = (
    "Camera not connected. Please plug in the camera and try again."
)

QUICK_FAIL_WINDOW_S = 8.0

# ID NNNN | EVENT_TYPE | STATUS | wearing: ... | missing: ... | unknown: ...
EVENT_RE = re.compile(
    r"^ID\s+(?P<id>\d+)\s*\|\s*"
    r"(?P<event>NEW|STATUS_CHANGE)\s*\|\s*"
    r"(?P<status>SAFE|PARTIAL|UNSAFE)\s*\|\s*"
    r"wearing:\s*(?P<wearing>[^|]*?)\s*\|\s*"
    r"missing:\s*(?P<missing>[^|]*?)\s*\|\s*"
    r"unknown:\s*(?P<unknown>.*?)\s*$"
)


def _parse_list_field(s: str) -> list:
    s = (s or "").strip()
    if not s or s.lower() == "none":
        return []
    return [x.strip() for x in s.split(",") if x.strip()]


def _human_join(items: list) -> str:
    if not items:
        return ""
    if len(items) == 1:
        return items[0]
    if len(items) == 2:
        return f"{items[0]} and {items[1]}"
    return ", ".join(items[:-1]) + f", and {items[-1]}"


def build_unsafe_tts(missing: list) -> str:
    if not missing:
        return TTS_UNSAFE_GENERIC
    return TTS_UNSAFE_WITH_MISSING.format(items=_human_join(missing))


def build_saqr_cmd(saqr_extra_args: list) -> list:
    """Invoke the saqr CLI via ``python -m`` so it picks up the package layout."""
    return [sys.executable, "-u", "-m", "saqr.apps.saqr_cli", *saqr_extra_args]


def split_argv(argv):
    if "--" in argv:
        idx = argv.index("--")
        return argv[:idx], argv[idx + 1:]
    return argv, []


class Bridge:
    """Owns the saqr subprocess lifecycle and the event-stream parser."""

    def __init__(self, robot: RobotController, cooldown_s: float,
                 release_after_s: float, saqr_args: list,
                 env: Dict[str, str], cwd: str):
        self.robot = robot
        self.cooldown_s = cooldown_s
        self.release_after_s = release_after_s
        self.saqr_args = saqr_args
        self.env = env
        self.cwd = cwd

        self.last_status: Dict[int, str] = {}
        self.last_trigger_t: Dict[tuple, float] = {}
        self._state_lock = threading.Lock()

        self.proc: Optional[subprocess.Popen] = None
        self.reader_thread: Optional[threading.Thread] = None
        self._proc_lock = threading.Lock()
        self._proc_start_t: float = 0.0

    def is_running(self) -> bool:
        with self._proc_lock:
            return self.proc is not None and self.proc.poll() is None

    def start_saqr(self):
        with self._proc_lock:
            if self.proc is not None and self.proc.poll() is None:
                print("[BRIDGE] start ignored — saqr already running", flush=True)
                return

            cmd = build_saqr_cmd(self.saqr_args)
            print(f"[BRIDGE] starting saqr: {' '.join(cmd)}", flush=True)
            self.proc = subprocess.Popen(
                cmd,
                cwd=self.cwd,
                stdout=subprocess.PIPE,
                stderr=subprocess.STDOUT,
                bufsize=1,
                text=True,
                env=self.env,
            )
            self._proc_start_t = time.time()

            with self._state_lock:
                self.last_status.clear()
                self.last_trigger_t.clear()

            self.reader_thread = threading.Thread(
                target=self._read_stdout, args=(self.proc,), daemon=True,
            )
            self.reader_thread.start()

    def stop_saqr(self):
        with self._proc_lock:
            proc = self.proc
            if proc is None or proc.poll() is not None:
                print("[BRIDGE] stop ignored — saqr not running", flush=True)
                self.proc = None
                return
            print("[BRIDGE] stopping saqr (SIGINT)", flush=True)
            try:
                proc.send_signal(signal.SIGINT)
            except Exception:
                pass

        try:
            proc.wait(timeout=3.0)
        except subprocess.TimeoutExpired:
            print("[BRIDGE] saqr did not exit in 3s, sending SIGTERM", flush=True)
            try:
                proc.terminate()
                proc.wait(timeout=2.0)
            except subprocess.TimeoutExpired:
                print("[BRIDGE] saqr unresponsive, sending SIGKILL", flush=True)
                proc.kill()
                proc.wait()

        if self.reader_thread is not None:
            self.reader_thread.join(timeout=2.0)

        with self._proc_lock:
            self.proc = None
            self.reader_thread = None

        self.robot.speak(TTS_BRIDGE_DEACTIVATED)

    def _read_stdout(self, proc: subprocess.Popen):
        start_t = self._proc_start_t
        try:
            assert proc.stdout is not None
            for line in proc.stdout:
                self.handle_line(line)
        except Exception as e:
            print(f"[BRIDGE][ERR] reader thread: {e}", flush=True)
        rc = proc.wait()
        lifetime = time.time() - start_t if start_t > 0 else 0.0
        print(f"[BRIDGE] saqr exited rc={rc} (lifetime={lifetime:.1f}s)",
              flush=True)

        if rc not in (0, -2) and 0 < lifetime < QUICK_FAIL_WINDOW_S:
            try:
                self.robot.speak(TTS_BRIDGE_NO_CAMERA)
            except Exception as e:
                print(f"[BRIDGE][ERR] no-camera tts failed: {e}", flush=True)

    def handle_line(self, line: str):
        line = line.rstrip()
        if not line:
            return
        print(line, flush=True)

        m = EVENT_RE.match(line)
        if not m:
            return

        track_id = int(m.group("id"))
        status = m.group("status")
        missing = _parse_list_field(m.group("missing"))

        with self._state_lock:
            prev = self.last_status.get(track_id)
            self.last_status[track_id] = status

            if status not in (DANGER_STATUS, SAFE_STATUS):
                return

            if prev == status:
                return

            now = time.time()
            last_t = self.last_trigger_t.get((track_id, status), 0.0)
            if (now - last_t) < self.cooldown_s:
                return
            self.last_trigger_t[(track_id, status)] = now

        try:
            if status == DANGER_STATUS:
                self.robot.speak(build_unsafe_tts(missing))
                self.robot.reject(release_after=self.release_after_s)
            else:
                self.robot.speak(TTS_TEXT_SAFE)
        except Exception as e:
            print(f"[BRIDGE][ERR] robot action failed: {e}", flush=True)


def trigger_loop(bridge: Bridge, hub, stop_event: threading.Event,
                 poll_hz: float = 50.0):
    """Watch the wireless remote for R2+X (start) and R2+Y (stop)."""
    period = 1.0 / max(poll_hz, 1.0)
    waiting_release_x = False
    waiting_release_y = False
    print("[BRIDGE] trigger loop ready — press R2+X to start, R2+Y to stop.",
          flush=True)
    while not stop_event.is_set():
        time.sleep(period)
        if not hub.first_state:
            continue

        r2x = hub.combo_r2x()
        r2y = hub.combo_r2y()

        if waiting_release_x:
            if not r2x:
                waiting_release_x = False
        elif r2x:
            waiting_release_x = True
            print("[BRIDGE] R2+X pressed -> start saqr", flush=True)
            try:
                bridge.start_saqr()
            except Exception as e:
                print(f"[BRIDGE][ERR] start_saqr failed: {e}", flush=True)

        if waiting_release_y:
            if not r2y:
                waiting_release_y = False
        elif r2y:
            waiting_release_y = True
            print("[BRIDGE] R2+Y pressed -> stop saqr", flush=True)
            try:
                bridge.stop_saqr()
            except Exception as e:
                print(f"[BRIDGE][ERR] stop_saqr failed: {e}", flush=True)


def main():
    bridge_argv, saqr_extra = split_argv(sys.argv[1:])

    ap = argparse.ArgumentParser(
        description="Bridge Saqr PPE events to the G1 arm 'reject' action."
    )
    ap.add_argument("--iface", default=None,
                    help="DDS network interface (e.g. eth0).")
    ap.add_argument("--timeout", type=float, default=10.0)
    ap.add_argument("--cooldown", type=float, default=8.0)
    ap.add_argument("--release-after", type=float, default=2.0)
    ap.add_argument("--dry-run", action="store_true")
    ap.add_argument("--speaker-id", type=int, default=TTS_SPEAKER_ID)
    ap.add_argument("--no-trigger", action="store_true")

    ap.add_argument("--source", default=None)
    ap.add_argument("--headless", action="store_true")
    ap.add_argument("--saqr-conf", type=float, default=None)
    ap.add_argument("--imgsz", type=int, default=None)
    ap.add_argument("--device", default=None)

    args = ap.parse_args(bridge_argv)

    saqr_args: list = []
    if args.source is not None:
        saqr_args += ["--source", args.source]
    if args.headless:
        saqr_args += ["--headless"]
    if args.saqr_conf is not None:
        saqr_args += ["--conf", str(args.saqr_conf)]
    if args.imgsz is not None:
        saqr_args += ["--imgsz", str(args.imgsz)]
    if args.device is not None:
        saqr_args += ["--device", args.device]
    saqr_args += saqr_extra

    use_trigger = not args.no_trigger and not args.dry_run

    robot = RobotController(
        iface=args.iface,
        timeout=args.timeout,
        dry_run=args.dry_run,
        tts_speaker_id=args.speaker_id,
        want_lowstate=use_trigger,
    )

    env = os.environ.copy()
    env["PYTHONUNBUFFERED"] = "1"

    bridge = Bridge(
        robot=robot,
        cooldown_s=args.cooldown,
        release_after_s=args.release_after,
        saqr_args=saqr_args,
        env=env,
        cwd=str(PROJECT_ROOT),
    )
    print(f"[BRIDGE] saqr cmd template: {' '.join(build_saqr_cmd(saqr_args))}",
          flush=True)
    print(f"[BRIDGE] cwd: {PROJECT_ROOT}", flush=True)

    stop_event = threading.Event()

    def _forward_signal(signum, _frame):
        print(f"[BRIDGE] signal {signum} -> shutting down", flush=True)
        stop_event.set()

    signal.signal(signal.SIGINT, _forward_signal)
    signal.signal(signal.SIGTERM, _forward_signal)

    have_hub = use_trigger and robot.hub is not None
    if use_trigger and not have_hub:
        print("[BRIDGE][WARN] --no-trigger not set, but no LowState hub is "
              "available. Falling back to legacy auto-start mode.", flush=True)

    trigger_thread: Optional[threading.Thread] = None
    try:
        if have_hub:
            try:
                robot.speak(TTS_BRIDGE_READY)
            except Exception as e:
                print(f"[BRIDGE][WARN] startup announce failed: {e}", flush=True)

            trigger_thread = threading.Thread(
                target=trigger_loop,
                args=(bridge, robot.hub, stop_event),
                daemon=True,
            )
            trigger_thread.start()

            while not stop_event.is_set():
                time.sleep(0.2)
        else:
            bridge.start_saqr()
            while not stop_event.is_set() and bridge.is_running():
                time.sleep(0.2)
    finally:
        if bridge.is_running():
            bridge.stop_saqr()
        stop_event.set()
        if trigger_thread is not None:
            trigger_thread.join(timeout=1.0)
        try:
            robot.shutdown_tts()
        except Exception:
            pass
        print("[BRIDGE] bye.", flush=True)
        sys.exit(0)


if __name__ == "__main__":
    main()