"""Bounded latest-wins primitives for derived live perception."""

from __future__ import annotations

import json
import math
import statistics
import struct
import threading
import time
import zlib
from collections import deque
from collections.abc import Mapping, Sequence
from dataclasses import dataclass
from hashlib import sha256
from typing import Any, Final, Literal

import numpy as np

from k1link.data_plane import DecodedPointCloudView, DecodedPoseView

WORLD_STATE_SCHEMA = "missioncore.live-perception-world-state/v1"
TELEMETRY_SCHEMA = "missioncore.live-perception-telemetry/v1"

HealthState = Literal["healthy", "degraded", "stale", "unavailable"]
LiveIngressModality = Literal[
    "control",
    "camera-init",
    "camera-frame",
    "lidar",
    "pose",
]

LIVE_INGRESS_SCHEMA: Final = "missioncore.live-perception-ingress/v1"
LIVE_INGRESS_WIRE_SCHEMA: Final = "missioncore.live-perception-wire/v1"
LIVE_RESULT_WIRE_SCHEMA: Final = "missioncore.live-perception-result-wire/v1"
LIVE_RESULT_MAGIC: Final = b"MCPR"
LIVE_RESULT_MAX_HEADER_BYTES: Final = 256 * 1024
LIVE_RESULT_MAX_PAYLOAD_BYTES: Final = 2 * 1024 * 1024


@dataclass(frozen=True, slots=True)
class LivePerceptionResultFrame:
    frame_index: int
    source_frame_index: int
    session_seconds: float
    captured_at_epoch_ns: int
    image_jpeg: bytes
    segmentation_mask: np.ndarray[Any, np.dtype[np.uint8]] | None
    objects: tuple[dict[str, Any], ...]
    delivery: dict[str, Any]


def encode_live_perception_result(
    *,
    frame_index: int,
    source_frame_index: int,
    session_seconds: float,
    captured_at_epoch_ns: int,
    image_jpeg: bytes,
    segmentation_mask: np.ndarray[Any, Any] | None,
    objects: Sequence[Mapping[str, Any]],
    delivery: Mapping[str, Any],
) -> bytes:
    """Encode one bounded, non-authoritative worker-to-viewer result frame."""

    if (
        frame_index < 0
        or source_frame_index < 0
        or captured_at_epoch_ns < 0
        or not math.isfinite(session_seconds)
        or session_seconds < 0
        or not 4 <= len(image_jpeg) <= 1024 * 1024
        or not image_jpeg.startswith(b"\xff\xd8")
        or not image_jpeg.endswith(b"\xff\xd9")
    ):
        raise ValueError("live perception result identity or image is invalid")
    normalized_objects = tuple(_normalize_live_result_object(value) for value in objects)
    if len(normalized_objects) > 128:
        raise ValueError("live perception result object count exceeds the bound")
    mask_payload = b""
    mask_shape: list[int] | None = None
    if segmentation_mask is not None:
        mask = np.asarray(segmentation_mask, dtype=np.uint8)
        if mask.shape != (600, 800):
            raise ValueError("live perception segmentation shape is invalid")
        mask_payload = zlib.compress(mask.tobytes(order="C"), level=1)
        mask_shape = [600, 800]
    payload = bytes(image_jpeg) + mask_payload
    if len(payload) > LIVE_RESULT_MAX_PAYLOAD_BYTES:
        raise ValueError("live perception result payload exceeds the bound")
    header = {
        "schema_version": LIVE_RESULT_WIRE_SCHEMA,
        "frame_index": frame_index,
        "source_frame_index": source_frame_index,
        "session_seconds": session_seconds,
        "captured_at_epoch_ns": captured_at_epoch_ns,
        "image": {"codec": "jpeg", "byte_length": len(image_jpeg)},
        "segmentation": (
            None
            if mask_shape is None
            else {
                "codec": "zlib-uint8-c1",
                "shape": mask_shape,
                "byte_length": len(mask_payload),
            }
        ),
        "objects": normalized_objects,
        "delivery": dict(delivery),
        "payload_bytes": len(payload),
        "payload_sha256": sha256(payload).hexdigest(),
        "authority": "shadow-diagnostic-only",
        "commands_enabled": False,
        "navigation_or_safety_accepted": False,
    }
    encoded_header = json.dumps(
        header,
        sort_keys=True,
        separators=(",", ":"),
        allow_nan=False,
    ).encode("utf-8")
    if len(encoded_header) > LIVE_RESULT_MAX_HEADER_BYTES:
        raise ValueError("live perception result header exceeds the bound")
    return LIVE_RESULT_MAGIC + struct.pack("!I", len(encoded_header)) + encoded_header + payload


def decode_live_perception_result(encoded: bytes) -> LivePerceptionResultFrame:
    """Validate and decode one result frame before it reaches the Rerun bridge."""

    if len(encoded) < 10 or not encoded.startswith(LIVE_RESULT_MAGIC):
        raise ValueError("live perception result frame is truncated")
    header_length = struct.unpack("!I", encoded[4:8])[0]
    if not 2 <= header_length <= LIVE_RESULT_MAX_HEADER_BYTES:
        raise ValueError("live perception result header length is invalid")
    boundary = 8 + header_length
    if boundary > len(encoded):
        raise ValueError("live perception result header is truncated")
    try:
        header = json.loads(encoded[8:boundary])
    except (UnicodeDecodeError, json.JSONDecodeError) as exc:
        raise ValueError("live perception result header is invalid") from exc
    payload = encoded[boundary:]
    if (
        not isinstance(header, dict)
        or header.get("schema_version") != LIVE_RESULT_WIRE_SCHEMA
        or header.get("authority") != "shadow-diagnostic-only"
        or header.get("commands_enabled") is not False
        or header.get("navigation_or_safety_accepted") is not False
        or header.get("payload_bytes") != len(payload)
        or len(payload) > LIVE_RESULT_MAX_PAYLOAD_BYTES
        or header.get("payload_sha256") != sha256(payload).hexdigest()
    ):
        raise ValueError("live perception result contract is invalid")
    image = header.get("image")
    segmentation = header.get("segmentation")
    objects = header.get("objects")
    delivery = header.get("delivery")
    if (
        not isinstance(image, dict)
        or image.get("codec") != "jpeg"
        or not isinstance(image.get("byte_length"), int)
        or not isinstance(objects, list)
        or len(objects) > 128
        or not isinstance(delivery, dict)
    ):
        raise ValueError("live perception result content descriptor is invalid")
    image_length = image["byte_length"]
    if not 4 <= image_length <= min(len(payload), 1024 * 1024):
        raise ValueError("live perception result image length is invalid")
    image_jpeg = payload[:image_length]
    if not image_jpeg.startswith(b"\xff\xd8") or not image_jpeg.endswith(b"\xff\xd9"):
        raise ValueError("live perception result JPEG is invalid")
    mask: np.ndarray[Any, np.dtype[np.uint8]] | None = None
    if segmentation is None:
        if len(payload) != image_length:
            raise ValueError("live perception result has an undescribed payload tail")
    else:
        if (
            not isinstance(segmentation, dict)
            or segmentation.get("codec") != "zlib-uint8-c1"
            or segmentation.get("shape") != [600, 800]
            or not isinstance(segmentation.get("byte_length"), int)
            or segmentation["byte_length"] != len(payload) - image_length
        ):
            raise ValueError("live perception segmentation descriptor is invalid")
        try:
            raw_mask = zlib.decompress(payload[image_length:])
        except zlib.error as exc:
            raise ValueError("live perception segmentation payload is invalid") from exc
        if len(raw_mask) != 600 * 800:
            raise ValueError("live perception segmentation byte length is invalid")
        mask = np.frombuffer(raw_mask, dtype=np.uint8).reshape((600, 800)).copy()
    normalized_objects = tuple(_normalize_live_result_object(value) for value in objects)
    frame_index = header.get("frame_index")
    source_frame_index = header.get("source_frame_index")
    session_seconds = header.get("session_seconds")
    captured_at_epoch_ns = header.get("captured_at_epoch_ns")
    if (
        not isinstance(frame_index, int)
        or isinstance(frame_index, bool)
        or frame_index < 0
        or not isinstance(source_frame_index, int)
        or isinstance(source_frame_index, bool)
        or source_frame_index < 0
        or not isinstance(captured_at_epoch_ns, int)
        or isinstance(captured_at_epoch_ns, bool)
        or captured_at_epoch_ns < 0
        or not isinstance(session_seconds, (int, float))
        or isinstance(session_seconds, bool)
        or not math.isfinite(float(session_seconds))
        or float(session_seconds) < 0
    ):
        raise ValueError("live perception result time identity is invalid")
    return LivePerceptionResultFrame(
        frame_index=frame_index,
        source_frame_index=source_frame_index,
        session_seconds=float(session_seconds),
        captured_at_epoch_ns=captured_at_epoch_ns,
        image_jpeg=image_jpeg,
        segmentation_mask=mask,
        objects=normalized_objects,
        delivery=dict(delivery),
    )


def _normalize_live_result_object(value: Mapping[str, Any]) -> dict[str, Any]:
    if not isinstance(value, Mapping):
        raise ValueError("live perception result object is invalid")
    track_id = value.get("track_id")
    label = value.get("label")
    score = value.get("score")
    bbox = value.get("bbox_xyxy")
    if (
        not isinstance(track_id, int)
        or isinstance(track_id, bool)
        or track_id < 0
        or not isinstance(label, str)
        or not 1 <= len(label) <= 64
        or not isinstance(score, (int, float))
        or isinstance(score, bool)
        or not math.isfinite(float(score))
        or not isinstance(bbox, Sequence)
        or isinstance(bbox, (str, bytes))
        or len(bbox) != 4
    ):
        raise ValueError("live perception result object identity is invalid")
    bbox_values = [float(item) for item in bbox]
    if not all(math.isfinite(item) for item in bbox_values):
        raise ValueError("live perception result 2D box is invalid")
    normalized: dict[str, Any] = {
        "track_id": track_id,
        "label": label,
        "score": float(score),
        "bbox_xyxy": bbox_values,
    }
    distance = value.get("distance_smoothed_m", value.get("distance_median_m"))
    if distance is not None:
        if (
            not isinstance(distance, (int, float))
            or isinstance(distance, bool)
            or not math.isfinite(float(distance))
            or float(distance) < 0
        ):
            raise ValueError("live perception result distance is invalid")
        normalized["distance_m"] = float(distance)
    else:
        normalized["distance_m"] = None
    cuboid_fields = (
        ("cuboid_center_map", 3),
        ("cuboid_half_size", 3),
        ("cuboid_quaternion_xyzw", 4),
    )
    present = [value.get(name) is not None for name, _ in cuboid_fields]
    if any(present) and not all(present):
        raise ValueError("live perception result cuboid is incomplete")
    for name, length in cuboid_fields:
        candidate = value.get(name)
        if candidate is None:
            normalized[name] = None
            continue
        if (
            not isinstance(candidate, Sequence)
            or isinstance(candidate, (str, bytes))
            or len(candidate) != length
        ):
            raise ValueError("live perception result cuboid geometry is invalid")
        values = [float(item) for item in candidate]
        if not all(math.isfinite(item) for item in values):
            raise ValueError("live perception result cuboid contains non-finite values")
        normalized[name] = values
    return normalized


@dataclass(frozen=True, slots=True)
class LiveIngressEvent:
    """One raw-first, derived-only event admitted to the shadow transport."""

    ingress_sequence: int
    session_id: str
    modality: LiveIngressModality
    source_id: str
    source_sequence: int
    captured_at_epoch_ns: int
    received_monotonic_ns: int
    payload: bytes

    def wire_bytes(self) -> bytes:
        header = json.dumps(
            {
                "schema_version": LIVE_INGRESS_WIRE_SCHEMA,
                "ingress_sequence": self.ingress_sequence,
                "session_id": self.session_id,
                "modality": self.modality,
                "source_id": self.source_id,
                "source_sequence": self.source_sequence,
                "captured_at_epoch_ns": self.captured_at_epoch_ns,
                "received_monotonic_ns": self.received_monotonic_ns,
                "payload_bytes": len(self.payload),
                "payload_sha256": sha256(self.payload).hexdigest(),
                "authority": "shadow-diagnostic-only",
                "commands_enabled": False,
                "navigation_or_safety_accepted": False,
            },
            ensure_ascii=True,
            separators=(",", ":"),
            sort_keys=True,
        ).encode("utf-8")
        return struct.pack("!I", len(header)) + header + self.payload


@dataclass(frozen=True, slots=True)
class LiveIngressQueueSnapshot:
    capacity: int
    depth: int
    maximum_depth: int
    published: int
    consumed: int
    dropped_overflow: int
    rejected_oversize: int


@dataclass(slots=True)
class _LiveIngressQueue:
    capacity: int
    items: deque[LiveIngressEvent]
    maximum_depth: int = 0
    published: int = 0
    consumed: int = 0
    dropped_overflow: int = 0
    rejected_oversize: int = 0


class LivePerceptionIngress:
    """Exclusive, bounded fan-out from committed K1 evidence to one AI worker.

    The ingress is deliberately not an acquisition source and has no command
    surface. Camera and MQTT producers call it only after their raw evidence
    commit has completed. Separate modality queues prevent camera bursts from
    evicting pose or LiDAR observations.
    """

    _CAPACITIES: Final[dict[LiveIngressModality, int]] = {
        "control": 4,
        "camera-init": 1,
        "camera-frame": 2,
        "lidar": 2,
        "pose": 16,
    }
    _MAX_PAYLOAD_BYTES: Final[dict[LiveIngressModality, int]] = {
        "control": 16 * 1024,
        "camera-init": 1024 * 1024,
        "camera-frame": 1024 * 1024,
        "lidar": 2 * 1024 * 1024,
        "pose": 2 * 1024 * 1024,
    }

    def __init__(self) -> None:
        self._condition = threading.Condition()
        self._queues = {
            modality: _LiveIngressQueue(capacity, deque())
            for modality, capacity in self._CAPACITIES.items()
        }
        self._ingress_sequence = 0
        self._session_id: str | None = None
        self._active = False
        self._closed = False
        self._consumer_id: str | None = None

    def begin_session(self, session_id: str) -> None:
        if not session_id or len(session_id) > 160:
            raise ValueError("live perception session id is invalid")
        with self._condition:
            if self._closed:
                raise RuntimeError("live perception ingress is closed")
            if self._active:
                if self._session_id == session_id:
                    return
                raise RuntimeError("another live perception session is active")
            self._session_id = session_id
            self._active = True
            self._publish_locked(
                modality="control",
                source_id="mission-core",
                source_sequence=0,
                captured_at_epoch_ns=time.time_ns(),
                received_monotonic_ns=time.monotonic_ns(),
                payload=b'{"event":"session-start"}',
            )

    def end_session(self, session_id: str) -> None:
        with self._condition:
            if not self._active or self._session_id != session_id:
                return
            self._publish_locked(
                modality="control",
                source_id="mission-core",
                source_sequence=0,
                captured_at_epoch_ns=time.time_ns(),
                received_monotonic_ns=time.monotonic_ns(),
                payload=b'{"event":"session-end"}',
            )
            self._active = False

    def publish(
        self,
        *,
        modality: LiveIngressModality,
        source_id: str,
        source_sequence: int,
        captured_at_epoch_ns: int,
        received_monotonic_ns: int,
        payload: bytes,
    ) -> bool:
        if modality == "control":
            raise ValueError("control events are owned by the ingress lifecycle")
        if not source_id or source_sequence < 0:
            raise ValueError("live perception source identity is invalid")
        if captured_at_epoch_ns < 0 or received_monotonic_ns < 0:
            raise ValueError("live perception timestamps must be non-negative")
        with self._condition:
            if self._closed or not self._active:
                return False
            return self._publish_locked(
                modality=modality,
                source_id=source_id,
                source_sequence=source_sequence,
                captured_at_epoch_ns=captured_at_epoch_ns,
                received_monotonic_ns=received_monotonic_ns,
                payload=payload,
            )

    def open_consumer(self, consumer_id: str) -> None:
        if not consumer_id or len(consumer_id) > 128:
            raise ValueError("live perception consumer id is invalid")
        with self._condition:
            if self._closed:
                raise RuntimeError("live perception ingress is closed")
            if self._consumer_id is not None and self._consumer_id != consumer_id:
                raise RuntimeError("live perception ingress already has a consumer")
            self._consumer_id = consumer_id

    def close_consumer(self, consumer_id: str) -> None:
        with self._condition:
            if self._consumer_id == consumer_id:
                self._consumer_id = None
                self._condition.notify_all()

    def take_next(
        self,
        consumer_id: str,
        *,
        timeout: float | None = None,
    ) -> LiveIngressEvent | None:
        with self._condition:
            if self._consumer_id != consumer_id:
                raise RuntimeError("live perception consumer lease is not active")
            ready = self._condition.wait_for(
                lambda: any(queue.items for queue in self._queues.values()) or self._closed,
                timeout=timeout,
            )
            if not ready:
                return None
            candidates = [
                (queue.items[0].ingress_sequence, modality, queue)
                for modality, queue in self._queues.items()
                if queue.items
            ]
            if not candidates:
                return None
            _, _, selected = min(candidates, key=lambda item: item[0])
            selected.consumed += 1
            return selected.items.popleft()

    def close(self) -> None:
        with self._condition:
            self._closed = True
            self._active = False
            self._condition.notify_all()

    def snapshot(self) -> dict[str, Any]:
        with self._condition:
            return {
                "schema_version": LIVE_INGRESS_SCHEMA,
                "mode": "shadow-diagnostic-only",
                "active": self._active,
                "session_id": self._session_id,
                "consumer_connected": self._consumer_id is not None,
                "commands_enabled": False,
                "navigation_or_safety_accepted": False,
                "closed": self._closed,
                "queues": {
                    modality: {
                        "capacity": queue.capacity,
                        "depth": len(queue.items),
                        "maximum_depth": queue.maximum_depth,
                        "published": queue.published,
                        "consumed": queue.consumed,
                        "dropped_overflow": queue.dropped_overflow,
                        "rejected_oversize": queue.rejected_oversize,
                    }
                    for modality, queue in self._queues.items()
                },
            }

    def _publish_locked(
        self,
        *,
        modality: LiveIngressModality,
        source_id: str,
        source_sequence: int,
        captured_at_epoch_ns: int,
        received_monotonic_ns: int,
        payload: bytes,
    ) -> bool:
        queue = self._queues[modality]
        if len(payload) > self._MAX_PAYLOAD_BYTES[modality]:
            queue.rejected_oversize += 1
            return False
        session_id = self._session_id
        if session_id is None:
            return False
        self._ingress_sequence += 1
        event = LiveIngressEvent(
            ingress_sequence=self._ingress_sequence,
            session_id=session_id,
            modality=modality,
            source_id=source_id,
            source_sequence=source_sequence,
            captured_at_epoch_ns=captured_at_epoch_ns,
            received_monotonic_ns=received_monotonic_ns,
            payload=bytes(payload),
        )
        if len(queue.items) == queue.capacity:
            queue.items.popleft()
            queue.dropped_overflow += 1
        queue.items.append(event)
        queue.published += 1
        queue.maximum_depth = max(queue.maximum_depth, len(queue.items))
        self._condition.notify_all()
        return True


@dataclass(frozen=True, slots=True)
class QueueSnapshot:
    capacity: int
    depth: int
    maximum_depth: int
    published: int
    consumed: int
    dropped_overflow: int
    dropped_superseded: int
    closed: bool

    @property
    def dropped_total(self) -> int:
        return self.dropped_overflow + self.dropped_superseded


class LatestWinsQueue[T]:
    """A bounded derived-data queue that never lets old preview work accumulate."""

    def __init__(self, capacity: int) -> None:
        if capacity < 1:
            raise ValueError("latest-wins queue capacity must be positive")
        self._capacity = capacity
        self._items: deque[T] = deque()
        self._condition = threading.Condition()
        self._maximum_depth = 0
        self._published = 0
        self._consumed = 0
        self._dropped_overflow = 0
        self._dropped_superseded = 0
        self._closed = False

    def publish(self, item: T) -> None:
        with self._condition:
            if self._closed:
                raise RuntimeError("cannot publish to a closed latest-wins queue")
            if len(self._items) == self._capacity:
                self._items.popleft()
                self._dropped_overflow += 1
            self._items.append(item)
            self._published += 1
            self._maximum_depth = max(self._maximum_depth, len(self._items))
            self._condition.notify()

    def take_next(self, timeout: float | None = None) -> T | None:
        with self._condition:
            ready = self._condition.wait_for(
                lambda: bool(self._items) or self._closed,
                timeout=timeout,
            )
            if not ready or not self._items:
                return None
            item = self._items.popleft()
            self._consumed += 1
            return item

    def close(self) -> None:
        with self._condition:
            self._closed = True
            self._condition.notify_all()

    def snapshot(self) -> QueueSnapshot:
        with self._condition:
            return QueueSnapshot(
                capacity=self._capacity,
                depth=len(self._items),
                maximum_depth=self._maximum_depth,
                published=self._published,
                consumed=self._consumed,
                dropped_overflow=self._dropped_overflow,
                dropped_superseded=self._dropped_superseded,
                closed=self._closed,
            )


@dataclass(frozen=True, slots=True)
class LiveSensorBinding:
    """One bounded camera→LiDAR→pose match for live diagnostic fusion."""

    state: Literal[
        "fused-ready",
        "lidar-unavailable",
        "lidar-camera-delta-exceeded",
        "pose-unavailable",
        "pose-point-delta-exceeded",
    ]
    point_cloud: DecodedPointCloudView | None
    pose: DecodedPoseView | None
    lidar_camera_delta_ms: float | None
    pose_point_delta_ms: float | None


class LiveSensorSynchronizer:
    """Keep a small arrival-time window and bind sensors without back-pressure.

    The synchronizer deliberately uses the already-recorded host arrival clock
    carried by the shadow wire contract.  It does not claim hardware-clock
    synchronization.  A short wait budget lets a LiDAR or pose event that is
    already in flight reach the receiver while keeping the detector/world-state
    latency bounded.
    """

    def __init__(
        self,
        *,
        maximum_lidar_camera_delta_ms: float,
        maximum_pose_point_delta_ms: float,
        capacity_per_modality: int = 32,
        retention_seconds: float = 3.0,
    ) -> None:
        if (
            maximum_lidar_camera_delta_ms <= 0
            or maximum_pose_point_delta_ms <= 0
            or capacity_per_modality < 2
            or retention_seconds <= 0
        ):
            raise ValueError("live sensor synchronizer bounds are invalid")
        self._maximum_lidar_camera_delta_ns = round(
            maximum_lidar_camera_delta_ms * 1_000_000
        )
        self._maximum_pose_point_delta_ns = round(maximum_pose_point_delta_ms * 1_000_000)
        self._capacity = capacity_per_modality
        self._retention_ns = round(retention_seconds * 1_000_000_000)
        self._condition = threading.Condition()
        self._points: deque[DecodedPointCloudView] = deque()
        self._poses: deque[DecodedPoseView] = deque()
        self._published_points = 0
        self._published_poses = 0
        self._evicted_points = 0
        self._evicted_poses = 0
        self._maximum_point_depth = 0
        self._maximum_pose_depth = 0

    def publish_point_cloud(self, value: DecodedPointCloudView) -> None:
        with self._condition:
            self._points.append(value)
            self._published_points += 1
            self._evicted_points += self._prune(self._points)
            self._maximum_point_depth = max(self._maximum_point_depth, len(self._points))
            self._condition.notify_all()

    def publish_pose(self, value: DecodedPoseView) -> None:
        with self._condition:
            self._poses.append(value)
            self._published_poses += 1
            self._evicted_poses += self._prune(self._poses)
            self._maximum_pose_depth = max(self._maximum_pose_depth, len(self._poses))
            self._condition.notify_all()

    def bind_camera(
        self,
        captured_at_epoch_ns: int,
        *,
        wait_seconds: float = 0.0,
    ) -> LiveSensorBinding:
        if captured_at_epoch_ns < 0 or wait_seconds < 0 or not math.isfinite(wait_seconds):
            raise ValueError("camera synchronization input is invalid")
        deadline = time.monotonic() + wait_seconds
        with self._condition:
            while True:
                binding = self._binding(captured_at_epoch_ns)
                if binding.state == "fused-ready" or wait_seconds == 0:
                    return binding
                remaining = deadline - time.monotonic()
                if remaining <= 0:
                    return binding
                self._condition.wait(timeout=remaining)

    def snapshot(self) -> dict[str, int | float]:
        with self._condition:
            return {
                "capacity_per_modality": self._capacity,
                "retention_seconds": self._retention_ns / 1_000_000_000,
                "point_depth": len(self._points),
                "pose_depth": len(self._poses),
                "maximum_point_depth": self._maximum_point_depth,
                "maximum_pose_depth": self._maximum_pose_depth,
                "published_points": self._published_points,
                "published_poses": self._published_poses,
                "evicted_points": self._evicted_points,
                "evicted_poses": self._evicted_poses,
            }

    def _binding(self, captured_at_epoch_ns: int) -> LiveSensorBinding:
        if not self._points:
            return LiveSensorBinding("lidar-unavailable", None, None, None, None)
        point = min(
            self._points,
            key=lambda value: abs(value.context.captured_at_epoch_ns - captured_at_epoch_ns),
        )
        lidar_delta_ns = point.context.captured_at_epoch_ns - captured_at_epoch_ns
        lidar_delta_ms = lidar_delta_ns / 1_000_000
        if abs(lidar_delta_ns) > self._maximum_lidar_camera_delta_ns:
            return LiveSensorBinding(
                "lidar-camera-delta-exceeded",
                point,
                None,
                lidar_delta_ms,
                None,
            )
        if not self._poses:
            return LiveSensorBinding(
                "pose-unavailable",
                point,
                None,
                lidar_delta_ms,
                None,
            )
        pose = min(
            self._poses,
            key=lambda value: abs(
                value.context.captured_at_epoch_ns - point.context.captured_at_epoch_ns
            ),
        )
        pose_delta_ns = pose.context.captured_at_epoch_ns - point.context.captured_at_epoch_ns
        pose_delta_ms = pose_delta_ns / 1_000_000
        if abs(pose_delta_ns) > self._maximum_pose_point_delta_ns:
            return LiveSensorBinding(
                "pose-point-delta-exceeded",
                point,
                pose,
                lidar_delta_ms,
                pose_delta_ms,
            )
        return LiveSensorBinding(
            "fused-ready",
            point,
            pose,
            lidar_delta_ms,
            pose_delta_ms,
        )

    def _prune(self, values: deque[DecodedPointCloudView] | deque[DecodedPoseView]) -> int:
        removed = 0
        newest = values[-1].context.captured_at_epoch_ns
        oldest_allowed = newest - self._retention_ns
        while values and (
            len(values) > self._capacity
            or values[0].context.captured_at_epoch_ns < oldest_allowed
        ):
            values.popleft()
            removed += 1
        return removed


def classify_health(
    *,
    source_available: bool,
    fusion_state: str,
    result_age_ms: float,
    stale_after_ms: float,
    unavailable_after_ms: float,
) -> tuple[HealthState, tuple[str, ...]]:
    """Classify freshness separately from whether depth was available."""

    if stale_after_ms <= 0 or unavailable_after_ms <= stale_after_ms:
        raise ValueError("health thresholds are invalid")
    if not source_available or result_age_ms >= unavailable_after_ms:
        return "unavailable", ("source-unavailable",)
    if result_age_ms >= stale_after_ms:
        return "stale", ("result-age-exceeded",)
    if fusion_state != "fused":
        return "degraded", (fusion_state,)
    return "healthy", ()


class WorldStateProjector:
    """Project accepted E6 observations into a control-facing, timestamped state."""

    def __init__(self, *, velocity_history_limit_s: float = 1.0) -> None:
        if velocity_history_limit_s <= 0:
            raise ValueError("velocity history limit must be positive")
        self._velocity_history_limit_s = velocity_history_limit_s
        self._track_history: dict[
            int, deque[tuple[float, tuple[float, float, float]]]
        ] = {}

    def project(
        self,
        *,
        frame: Mapping[str, Any],
        lidar_positions: Mapping[int, Sequence[float]],
        clearance: Mapping[str, Any],
    ) -> dict[str, Any]:
        session_seconds = float(frame["session_seconds"])
        objects: list[dict[str, Any]] = []
        for raw in frame.get("objects", []):
            if not str(raw.get("cuboid_status", "")).startswith("accepted-"):
                continue
            track_id = int(raw["track_id"])
            center_map = _vector3(raw["cuboid_center_map"], "cuboid center")
            half_size = _vector3(raw["cuboid_half_size"], "cuboid half-size")
            quaternion = _vector4(raw["cuboid_quaternion_xyzw"], "cuboid quaternion")
            velocity, velocity_status, velocity_residual = self._velocity(
                track_id, session_seconds, center_map
            )
            speed = None
            if velocity is not None:
                speed = math.sqrt(sum(component * component for component in velocity))
            lidar = lidar_positions.get(track_id)
            objects.append(
                {
                    "track_id": track_id,
                    "class": str(raw["association_group"]),
                    "detector_label": str(raw["label"]),
                    "confidence": float(raw["score"]),
                    "position_map_m": list(center_map),
                    "position_lidar_m": (
                        None if lidar is None else [float(value) for value in lidar]
                    ),
                    "orientation_map_xyzw": list(quaternion),
                    "size_m": [2.0 * value for value in half_size],
                    "range_m": float(raw["distance_smoothed_m"]),
                    "velocity_map_mps": None if velocity is None else list(velocity),
                    "speed_mps": speed,
                    "velocity_status": velocity_status,
                    "velocity_residual_m": velocity_residual,
                    "support_points": int(raw["clustered_points"]),
                    "geometry": "point-supported-visible-surface-envelope",
                }
            )
        self._prune(session_seconds)
        return {
            "schema_version": WORLD_STATE_SCHEMA,
            "frame_index": int(frame["frame_index"]),
            "source_frame_index": int(frame["source_frame_index"]),
            "session_seconds": session_seconds,
            "coordinate_frames": {
                "world": "k1-map",
                "sensor_relative": "k1-lidar",
                "vehicle_body": "unavailable-no-rig-to-vehicle-transform",
            },
            "fusion_state": str(frame["state"]),
            "objects": objects,
            "object_count": len(objects),
            "clearance": dict(clearance),
        }

    def _velocity(
        self,
        track_id: int,
        session_seconds: float,
        center_map: tuple[float, float, float],
    ) -> tuple[tuple[float, float, float] | None, str, float | None]:
        history = self._track_history.setdefault(track_id, deque(maxlen=32))
        if history and session_seconds <= history[-1][0]:
            return None, "unavailable-nonmonotonic-time", None
        history.append((session_seconds, center_map))
        oldest = session_seconds - self._velocity_history_limit_s
        while history and history[0][0] < oldest:
            history.popleft()
        if len(history) < 4 or history[-1][0] - history[0][0] < 0.4:
            return None, "unavailable-insufficient-history", None
        slopes: list[tuple[float, float, float]] = []
        values = list(history)
        for left, (left_time, left_center) in enumerate(values):
            for right_time, right_center in values[left + 1 :]:
                delta = right_time - left_time
                if delta < 0.2:
                    continue
                slopes.append(
                    (
                        (right_center[0] - left_center[0]) / delta,
                        (right_center[1] - left_center[1]) / delta,
                        (right_center[2] - left_center[2]) / delta,
                    )
                )
        if not slopes:
            return None, "unavailable-insufficient-baseline", None
        velocity = (
            statistics.median(item[0] for item in slopes),
            statistics.median(item[1] for item in slopes),
            statistics.median(item[2] for item in slopes),
        )
        speed = math.sqrt(sum(component * component for component in velocity))
        latest_time, latest_center = values[-1]
        residuals = []
        for observed_time, observed_center in values:
            predicted = tuple(
                latest - component * (latest_time - observed_time)
                for latest, component in zip(latest_center, velocity, strict=True)
            )
            residuals.append(
                math.sqrt(
                    sum(
                        (observed - expected) ** 2
                        for observed, expected in zip(
                            observed_center, predicted, strict=True
                        )
                    )
                )
            )
        residual = statistics.median(residuals)
        if speed > 20.0:
            return None, "rejected-speed-bound", residual
        if residual > 0.75:
            return None, "rejected-position-residual", residual
        return velocity, "diagnostic-robust-history", residual

    def _prune(self, session_seconds: float) -> None:
        oldest = session_seconds - self._velocity_history_limit_s
        expired = [
            track_id
            for track_id, history in self._track_history.items()
            if not history or history[-1][0] < oldest
        ]
        for track_id in expired:
            del self._track_history[track_id]


def wait_until(deadline: float) -> float:
    """Wait for a replay deadline and return non-negative scheduling lag seconds."""

    remaining = deadline - time.perf_counter()
    if remaining > 0:
        time.sleep(remaining)
    return max(0.0, time.perf_counter() - deadline)


def _vector3(value: Sequence[Any], label: str) -> tuple[float, float, float]:
    if len(value) != 3:
        raise ValueError(f"{label} must contain three values")
    return float(value[0]), float(value[1]), float(value[2])


def _vector4(value: Sequence[Any], label: str) -> tuple[float, float, float, float]:
    if len(value) != 4:
        raise ValueError(f"{label} must contain four values")
    return float(value[0]), float(value[1]), float(value[2]), float(value[3])
