#!/usr/bin/env python3
"""Read-only readiness window for original RL; never claims a command writer."""
from __future__ import annotations

import argparse
from pathlib import Path
import sys
import time

ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
    sys.path.insert(0, str(ROOT))

from runtime.h2_shm_bridge import H2SharedMemoryBridge, SHM_NAME
from scripts.inspect_shm_state import validate_snapshot
from scripts.inspect_mocap_pipeline import EXPECTED_PACKET_LEN, InspectionFailure, validate_planner_packet


class ReadinessWindow:
    def __init__(self, interface, origin):
        self.interface, self.origin = interface, origin
        self.reset()

    def reset(self):
        self.since = None
        self.last = None
        self.counts = [0, 0, 0]
        self.command_id = None

    def observe(self, state, now):
        result = validate_snapshot(state, require_planner=True, require_disarmed=True,
                                   expected_interface=self.interface, now_ns=int(now*1e9))
        if not result.passed:
            raise InspectionFailure("; ".join(result.failures))
        # H2BR reserves 32 doubles; planner-v3 occupies the first 27.
        packet = validate_planner_packet(state.tt_data[:EXPECTED_PACKET_LEN], now_monotonic_s=now,
            expected_origin_m=self.origin, require_pose_alignment=False,
            require_ball_source_freshness=False)
        stamps = (state.state_stamp_ns, state.tt_stamp_ns, packet.robot_source_stamp)
        if self.last is not None:
            if any(current < old for current, old in zip(stamps, self.last)):
                raise InspectionFailure("startup source timestamp moved backwards")
            if packet.command_id < self.command_id:
                raise InspectionFailure("startup planner command id moved backwards")
        if self.since is None:
            self.since = now
        for i, stamp in enumerate(stamps):
            if self.last is None or stamp > self.last[i]:
                self.counts[i] += 1
        self.last, self.command_id = stamps, packet.command_id
        return now-self.since >= 0.2-1e-9 and min(self.counts) >= 3


def wait_ready(interface, origin, *, timeout_s=10.0, clock=time.monotonic,
               sleep=time.sleep, factory=H2SharedMemoryBridge):
    started = clock()
    deadline = started + timeout_s
    window = ReadinessWindow(interface, origin)
    bridge = None
    reason = "waiting for H2BR"
    last_report = -float("inf")
    print(f"[STARTUP] communication readiness begin timeout_s={timeout_s:g}", flush=True)
    try:
        while clock() < deadline:
            try:
                if bridge is None:
                    bridge = factory(SHM_NAME)
                state = bridge.read_state()
                now = clock()
                if window.observe(state, now):
                    print(f"[STARTUP] communication ready elapsed_s={now-started:.3f} "
                          f"unique_state/planner/robot={window.counts}", flush=True)
                    return
                reason = "waiting for 0.2s of healthy updates (at least 3 per source)"
            except (FileNotFoundError, InspectionFailure) as error:
                window.reset()
                reason = str(error)
            now = clock()
            if now-last_report >= 1.0:
                print(f"[STARTUP] communication not ready: {reason}", flush=True)
                last_report = now
            sleep(min(0.02, max(0.0, deadline-clock())))
        raise RuntimeError(f"communication readiness timed out after {timeout_s:g}s: {reason}")
    finally:
        if bridge is not None:
            bridge.close()


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--network-interface", required=True)
    parser.add_argument("--origin", type=float, nargs=3, default=(-1.87, 0.0, -0.76))
    args = parser.parse_args()
    try:
        wait_ready(args.network_interface, args.origin)
    except (OSError, RuntimeError, ValueError) as error:
        print(f"[STARTUP ERROR] {error}", flush=True)
        return 1
    return 0


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