"""Deterministic dual-depth clearance preflight for planned Lite6 motion.

The result produced here is a supplemental, non-safety-rated application gate.
It can veto a trajectory but it cannot authorize motion, enable motors, or
replace guarding, the controller safety chain, an emergency stop, or witnessed
site commissioning.  Unknown or conflicting depth evidence always blocks.
"""

from __future__ import annotations

import copy
import hashlib
import hmac
import json
import math
import os
import threading
import time
from collections import deque
from dataclasses import dataclass
from pathlib import Path
from typing import Any, Callable, Deque, Mapping, Optional, Sequence, Tuple, Union

import numpy as np

from main.digital_twin import (
    DIGITAL_TWIN_BLOCKERS,
    Lite6VisualizationKinematics,
)

from .cameras import AlignedDepthPacket, CameraFrame, CameraRole
from .eye_in_hand import (
    CameraIntrinsics,
    EyeInHandCalibration,
    RigidTransform,
    TimedToolPose,
)


class ClearanceError(ValueError):
    """Raised when depth evidence or configuration violates its contract."""


_HEX_64 = frozenset("0123456789abcdef")
_MAX_CALIBRATION_BYTES = 1024 * 1024
_MAX_TRAJECTORY_FRAMES = 4096
_MAX_TWIN_PATH_POINTS = 32
_MAX_ENVELOPE_SAMPLES = 200_000
_MAX_DEPTH_PIXELS_PER_CAMERA = 4_194_304
_MAX_FOOTPRINT_PIXEL_EVALUATIONS = 20_000_000
_DEPTH_SCAN_CHUNK_ROWS = 32
_BLOCKER_ORDER = {value: index for index, value in enumerate(DIGITAL_TWIN_BLOCKERS)}


def _sha256(value: Any) -> str:
    text = str(value or "")
    if len(text) != 64 or any(character not in _HEX_64 for character in text):
        raise ClearanceError("calibration SHA-256 is invalid")
    return text


def _finite(value: Any, name: str, minimum: float, maximum: float) -> float:
    if isinstance(value, bool):
        raise ClearanceError("{} must be a finite number".format(name))
    try:
        number = float(value)
    except (TypeError, ValueError, OverflowError) as exc:
        raise ClearanceError("{} must be a finite number".format(name)) from exc
    if not math.isfinite(number) or not minimum <= number <= maximum:
        raise ClearanceError("{} is outside its allowed range".format(name))
    return number


def _joint_vector(value: Any) -> Tuple[float, ...]:
    if not isinstance(value, (tuple, list)) or len(value) != 6:
        raise ClearanceError("trajectory joints must contain exactly six values")
    return tuple(_finite(item, "trajectory joint", -1_000.0, 1_000.0) for item in value)


def _ordered_blockers(values: Sequence[str]) -> Tuple[str, ...]:
    allowed = {value for value in values if value in _BLOCKER_ORDER}
    return tuple(sorted(allowed, key=_BLOCKER_ORDER.__getitem__))


@dataclass(frozen=True)
class StaticDepthCalibration:
    """Approved static workspace-camera transform into the xArm base frame."""

    schema_version: int
    created_at_s: float
    camera_source_id: str
    intrinsics: CameraIntrinsics
    base_T_camera: RigidTransform
    quality_approved: bool

    def __post_init__(self) -> None:
        if self.schema_version != 1:
            raise ClearanceError("unsupported static depth calibration schema")
        created = _finite(
            self.created_at_s,
            "calibration creation timestamp",
            0.0,
            100_000_000_000.0,
        )
        object.__setattr__(self, "created_at_s", created)
        source = str(self.camera_source_id or "")
        if not source or len(source) > 255:
            raise ClearanceError("static camera source ID is invalid")
        object.__setattr__(self, "camera_source_id", source)
        if not isinstance(self.intrinsics, CameraIntrinsics):
            raise ClearanceError("static camera intrinsics are invalid")
        if not isinstance(self.base_T_camera, RigidTransform):
            raise ClearanceError("static camera transform is invalid")
        if self.quality_approved is not True:
            raise ClearanceError("static depth calibration is not quality approved")

    def to_dict(self) -> Mapping[str, Any]:
        return {
            "schema_version": 1,
            "created_at_s": self.created_at_s,
            "camera_source_id": self.camera_source_id,
            "intrinsics": self.intrinsics.to_dict(),
            "base_T_camera": self.base_T_camera.to_list(),
            "quality_approved": True,
        }

    @classmethod
    def from_dict(cls, value: Mapping[str, Any]) -> "StaticDepthCalibration":
        expected = {
            "schema_version",
            "created_at_s",
            "camera_source_id",
            "intrinsics",
            "base_T_camera",
            "quality_approved",
        }
        if set(value) != expected:
            raise ClearanceError("static calibration fields do not match schema")
        return cls(
            schema_version=value["schema_version"],
            created_at_s=value["created_at_s"],
            camera_source_id=value["camera_source_id"],
            intrinsics=CameraIntrinsics.from_dict(value["intrinsics"]),
            base_T_camera=RigidTransform.from_array(value["base_T_camera"]),
            quality_approved=value["quality_approved"],
        )


class StaticDepthCalibrationStore:
    """Load an exact regular JSON artifact with a mandatory pinned digest."""

    def load(
        self,
        path: Union[str, os.PathLike],
        *,
        expected_sha256: str,
    ) -> StaticDepthCalibration:
        digest = _sha256(expected_sha256)
        source = Path(path)
        if source.is_symlink() or not source.is_file():
            raise ClearanceError(
                "static calibration path must be a regular, non-symlink file"
            )
        try:
            payload = source.read_bytes()
        except OSError as exc:
            raise ClearanceError("static calibration could not be read") from exc
        if not payload or len(payload) > _MAX_CALIBRATION_BYTES:
            raise ClearanceError("static calibration size is invalid")
        actual = hashlib.sha256(payload).hexdigest()
        if not hmac.compare_digest(actual, digest):
            raise ClearanceError("static calibration SHA-256 does not match")
        try:
            value = json.loads(payload.decode("utf-8"))
        except (UnicodeError, json.JSONDecodeError) as exc:
            raise ClearanceError("static calibration is not valid JSON") from exc
        if not isinstance(value, Mapping):
            raise ClearanceError("static calibration must contain a JSON object")
        return StaticDepthCalibration.from_dict(value)


@dataclass(frozen=True)
class ConservativeGeometryProfile:
    """Pinned, reviewed conservative geometry used only to veto a plan.

    The six radii apply to the six consecutive segments in the seven-point
    Lite6 visualization model.  The TCP is additionally treated as a sphere
    with ``tool_capsule_radius_mm``.  This profile is deliberately separate
    from the visualization model: a missing, unreviewed, or model-mismatched
    profile must never produce a clear result.
    """

    schema_version: int
    model_sha256: str
    link_capsule_radii_mm: Tuple[float, ...]
    tool_capsule_radius_mm: float
    maximum_joint_step_radians: float
    envelope_sample_spacing_mm: float
    quality_approved: bool

    def __post_init__(self) -> None:
        if self.schema_version != 1:
            raise ClearanceError("unsupported conservative geometry schema")
        object.__setattr__(self, "model_sha256", _sha256(self.model_sha256))
        try:
            radii = tuple(self.link_capsule_radii_mm)
        except TypeError as exc:
            raise ClearanceError("link capsule radii must contain six values") from exc
        if len(radii) != 6:
            raise ClearanceError("link capsule radii must contain six values")
        object.__setattr__(
            self,
            "link_capsule_radii_mm",
            tuple(
                _finite(value, "link capsule radius", 0.001, 1_000.0) for value in radii
            ),
        )
        object.__setattr__(
            self,
            "tool_capsule_radius_mm",
            _finite(self.tool_capsule_radius_mm, "tool capsule radius", 0.001, 1_000.0),
        )
        object.__setattr__(
            self,
            "maximum_joint_step_radians",
            _finite(
                self.maximum_joint_step_radians,
                "maximum joint step",
                0.0001,
                math.radians(10.0),
            ),
        )
        object.__setattr__(
            self,
            "envelope_sample_spacing_mm",
            _finite(
                self.envelope_sample_spacing_mm,
                "envelope sample spacing",
                0.1,
                100.0,
            ),
        )
        if self.quality_approved is not True:
            raise ClearanceError("conservative geometry is not quality approved")

    def to_dict(self) -> Mapping[str, Any]:
        return {
            "schema_version": 1,
            "model_sha256": self.model_sha256,
            "link_capsule_radii_mm": list(self.link_capsule_radii_mm),
            "tool_capsule_radius_mm": self.tool_capsule_radius_mm,
            "maximum_joint_step_radians": self.maximum_joint_step_radians,
            "envelope_sample_spacing_mm": self.envelope_sample_spacing_mm,
            "quality_approved": True,
        }

    @classmethod
    def from_dict(cls, value: Mapping[str, Any]) -> "ConservativeGeometryProfile":
        expected = {
            "schema_version",
            "model_sha256",
            "link_capsule_radii_mm",
            "tool_capsule_radius_mm",
            "maximum_joint_step_radians",
            "envelope_sample_spacing_mm",
            "quality_approved",
        }
        if set(value) != expected:
            raise ClearanceError("conservative geometry fields do not match schema")
        return cls(
            schema_version=value["schema_version"],
            model_sha256=value["model_sha256"],
            link_capsule_radii_mm=tuple(value["link_capsule_radii_mm"]),
            tool_capsule_radius_mm=value["tool_capsule_radius_mm"],
            maximum_joint_step_radians=value["maximum_joint_step_radians"],
            envelope_sample_spacing_mm=value["envelope_sample_spacing_mm"],
            quality_approved=value["quality_approved"],
        )


class ConservativeGeometryProfileStore:
    """Load an exact reviewed geometry artifact with a mandatory digest."""

    def load(
        self,
        path: Union[str, os.PathLike],
        *,
        expected_sha256: str,
    ) -> ConservativeGeometryProfile:
        digest = _sha256(expected_sha256)
        source = Path(path)
        if source.is_symlink() or not source.is_file():
            raise ClearanceError(
                "geometry profile path must be a regular, non-symlink file"
            )
        try:
            payload = source.read_bytes()
        except OSError as exc:
            raise ClearanceError("geometry profile could not be read") from exc
        if not payload or len(payload) > _MAX_CALIBRATION_BYTES:
            raise ClearanceError("geometry profile size is invalid")
        actual = hashlib.sha256(payload).hexdigest()
        if not hmac.compare_digest(actual, digest):
            raise ClearanceError("geometry profile SHA-256 does not match")
        try:
            value = json.loads(payload.decode("utf-8"))
        except (UnicodeError, json.JSONDecodeError) as exc:
            raise ClearanceError("geometry profile is not valid JSON") from exc
        if not isinstance(value, Mapping):
            raise ClearanceError("geometry profile must contain a JSON object")
        return ConservativeGeometryProfile.from_dict(value)


class DepthPacketBuffer:
    """Bounded, sequence-addressable owner of copied DepthAI depth arrays."""

    def __init__(
        self,
        *,
        source_id: str,
        role: CameraRole,
        image_width: int,
        image_height: int,
        maximum_frames: int = 8,
    ) -> None:
        self.source_id = str(source_id)
        self.role = CameraRole(role)
        self.image_width = int(image_width)
        self.image_height = int(image_height)
        if not self.source_id or len(self.source_id) > 255:
            raise ClearanceError("depth buffer source ID is invalid")
        if self.image_width < 2 or self.image_height < 2:
            raise ClearanceError("depth buffer dimensions must be at least 2x2")
        if not 1 <= int(maximum_frames) <= 64:
            raise ClearanceError("depth buffer size must be between 1 and 64")
        self._frames: Deque[AlignedDepthPacket] = deque(maxlen=int(maximum_frames))
        self._lock = threading.Lock()

    def add(self, packet: AlignedDepthPacket) -> None:
        if not isinstance(packet, AlignedDepthPacket):
            raise ClearanceError("depth buffer accepts AlignedDepthPacket values")
        if packet.source_id != self.source_id or packet.role is not self.role:
            raise ClearanceError("depth packet camera identity does not match buffer")
        array = np.asarray(packet.depth_mm)
        if array.shape != (self.image_height, self.image_width):
            raise ClearanceError("depth packet dimensions do not match calibration")
        if array.dtype.kind not in ("u", "i", "f"):
            raise ClearanceError("depth packet must contain numeric values")
        owned = AlignedDepthPacket(
            source_id=packet.source_id,
            role=packet.role,
            sequence=packet.sequence,
            captured_at_s=packet.captured_at_s,
            received_monotonic_s=packet.received_monotonic_s,
            depth_mm=np.array(array, copy=True),
        )
        with self._lock:
            if self._frames and owned.sequence <= self._frames[-1].sequence:
                raise ClearanceError(
                    "depth packet sequences must be strictly increasing"
                )
            self._frames.append(owned)

    def resolve(
        self,
        frame: CameraFrame,
        *,
        now_monotonic_s: float,
        max_age_s: float,
    ) -> AlignedDepthPacket:
        if not isinstance(frame, CameraFrame):
            raise ClearanceError("depth resolution requires a CameraFrame")
        if frame.source_id != self.source_id or frame.role is not self.role:
            raise ClearanceError("depth frame camera identity does not match buffer")
        sequence = frame.metadata.get("depth_sequence")
        if isinstance(sequence, bool) or not isinstance(sequence, int) or sequence < 0:
            raise ClearanceError("depth_sequence_mismatch")
        now = _finite(now_monotonic_s, "monotonic time", 0.0, 100_000_000_000.0)
        maximum_age = _finite(max_age_s, "maximum depth age", 0.001, 60.0)
        with self._lock:
            packet = next(
                (
                    value
                    for value in reversed(self._frames)
                    if value.sequence == sequence
                ),
                None,
            )
            if packet is None:
                raise ClearanceError("depth_sequence_mismatch")
            result = AlignedDepthPacket(
                source_id=packet.source_id,
                role=packet.role,
                sequence=packet.sequence,
                captured_at_s=packet.captured_at_s,
                received_monotonic_s=packet.received_monotonic_s,
                depth_mm=np.array(packet.depth_mm, copy=True),
            )
        if abs(result.captured_at_s - frame.captured_at_s) > 0.001:
            raise ClearanceError("depth_sequence_mismatch")
        age = now - result.received_monotonic_s
        if not 0.0 <= age <= maximum_age:
            raise ClearanceError("dual_depth_stale")
        return result

    def __len__(self) -> int:
        with self._lock:
            return len(self._frames)


@dataclass(frozen=True)
class ClearanceDecision:
    """One immutable preflight outcome; ``clear`` can only remove permission."""

    clear: bool
    evidence: Mapping[str, Any]
    blockers: Tuple[str, ...] = ()

    def __post_init__(self) -> None:
        blockers = _ordered_blockers(self.blockers)
        object.__setattr__(self, "blockers", blockers)
        object.__setattr__(self, "clear", self.clear is True and not blockers)
        object.__setattr__(self, "evidence", copy.deepcopy(dict(self.evidence)))

    def to_twin_evidence(self) -> Mapping[str, Any]:
        return copy.deepcopy(dict(self.evidence))


class DualDepthClearancePreflight:
    """Fuse two calibrated depth views and conservatively inspect a plan."""

    def __init__(
        self,
        *,
        pair_provider: Callable[[], Mapping[CameraRole, CameraFrame]],
        tool_depth_buffer: DepthPacketBuffer,
        workspace_depth_buffer: DepthPacketBuffer,
        tool_calibration: EyeInHandCalibration,
        workspace_calibration: StaticDepthCalibration,
        tool_calibration_sha256: str,
        workspace_calibration_sha256: str,
        geometry_profile: ConservativeGeometryProfile,
        geometry_profile_sha256: str,
        tool_pose_provider: Callable[[float], TimedToolPose],
        required_clearance_mm: float,
        evidence_sink: Optional[Callable[[Mapping[str, Any]], None]] = None,
        kinematics: Optional[Lite6VisualizationKinematics] = None,
        max_frame_age_s: float = 0.30,
        max_pair_skew_s: float = 0.05,
        max_tool_pose_skew_s: float = 0.03,
        min_depth_mm: float = 30.0,
        max_depth_mm: float = 3000.0,
        min_coverage_ratio: float = 0.80,
        max_camera_disagreement_mm: float = 250.0,
        sample_stride: int = 8,
        maximum_points_per_camera: int = 4096,
        clock: Callable[[], float] = time.time,
        monotonic: Callable[[], float] = time.monotonic,
    ) -> None:
        if not callable(pair_provider) or not callable(tool_pose_provider):
            raise ClearanceError("depth preflight providers must be callable")
        if (
            not isinstance(tool_calibration, EyeInHandCalibration)
            or not tool_calibration.ready
        ):
            raise ClearanceError("tool depth calibration is not approved")
        if not isinstance(workspace_calibration, StaticDepthCalibration):
            raise ClearanceError("workspace depth calibration is invalid")
        if not isinstance(geometry_profile, ConservativeGeometryProfile):
            raise ClearanceError("conservative geometry profile is invalid")
        if tool_calibration.camera_source_id != tool_depth_buffer.source_id:
            raise ClearanceError("tool calibration source does not match depth buffer")
        if workspace_calibration.camera_source_id != workspace_depth_buffer.source_id:
            raise ClearanceError(
                "workspace calibration source does not match depth buffer"
            )
        if tool_depth_buffer.role is not CameraRole.TOOL:
            raise ClearanceError("tool depth buffer has the wrong role")
        if workspace_depth_buffer.role is not CameraRole.WORKSPACE:
            raise ClearanceError("workspace depth buffer has the wrong role")
        self.tool_calibration_sha256 = _sha256(tool_calibration_sha256)
        self.workspace_calibration_sha256 = _sha256(workspace_calibration_sha256)
        self.geometry_profile_sha256 = _sha256(geometry_profile_sha256)
        self.required_clearance_mm = _finite(
            required_clearance_mm, "required clearance", 0.001, 10_000.0
        )
        self.max_frame_age_s = _finite(max_frame_age_s, "maximum frame age", 0.001, 5.0)
        self.max_pair_skew_s = _finite(
            max_pair_skew_s, "maximum camera skew", 0.0001, 1.0
        )
        self.max_tool_pose_skew_s = _finite(
            max_tool_pose_skew_s, "maximum tool-pose skew", 0.0001, 1.0
        )
        self.min_depth_mm = _finite(min_depth_mm, "minimum depth", 1.0, 10_000.0)
        self.max_depth_mm = _finite(max_depth_mm, "maximum depth", 1.0, 20_000.0)
        if self.min_depth_mm >= self.max_depth_mm:
            raise ClearanceError("minimum depth must be below maximum depth")
        self.min_coverage_ratio = _finite(
            min_coverage_ratio, "minimum coverage", 0.01, 1.0
        )
        self.max_camera_disagreement_mm = _finite(
            max_camera_disagreement_mm,
            "maximum camera disagreement",
            0.001,
            10_000.0,
        )
        if not 1 <= int(sample_stride) <= 128:
            raise ClearanceError("sample stride must be between 1 and 128")
        if not 16 <= int(maximum_points_per_camera) <= _MAX_DEPTH_PIXELS_PER_CAMERA:
            raise ClearanceError("maximum point count is invalid")
        self.sample_stride = int(sample_stride)
        self.maximum_points_per_camera = int(maximum_points_per_camera)
        self.pair_provider = pair_provider
        self.tool_depth_buffer = tool_depth_buffer
        self.workspace_depth_buffer = workspace_depth_buffer
        self.tool_calibration = tool_calibration
        self.workspace_calibration = workspace_calibration
        self.tool_pose_provider = tool_pose_provider
        self.evidence_sink = evidence_sink
        self.kinematics = kinematics or Lite6VisualizationKinematics()
        if geometry_profile.model_sha256 != self.kinematics.model_sha256:
            raise ClearanceError("geometry profile does not match Lite6 model")
        self.geometry_profile = geometry_profile
        self._kinematic_reach_bound_mm = self._chain_reach_bound_mm()
        minimum_inflated_radius = (
            min(
                *self.geometry_profile.link_capsule_radii_mm,
                self.geometry_profile.tool_capsule_radius_mm,
            )
            + self.required_clearance_mm
        )
        # Every interpolated pose is close enough to the next one that a
        # continuous moving centerline is covered by the enlarged footprint
        # samples produced by _envelope_samples().
        self._maximum_motion_center_step_mm = minimum_inflated_radius / 2.0
        self._temporal_cover_mm = self._maximum_motion_center_step_mm / 2.0
        self.clock = clock
        self.monotonic = monotonic

    def _chain_reach_bound_mm(self) -> float:
        """Return a configuration-independent Lite6 point-motion bound.

        For a serial revolute chain, a point's speed from one joint is bounded
        by its distance to that joint axis.  The sum of all fixed link-origin
        translation lengths bounds that distance for every downstream point.
        Multiplying it by the sum of absolute joint changes therefore bounds
        every centerline point's path length over one interpolation interval.
        """

        joints = getattr(self.kinematics, "_JOINTS", ())
        try:
            reach = sum(
                float(np.linalg.norm(np.asarray(origin_xyz, dtype=float)))
                for origin_xyz, _origin_rpy, _axis in joints
            )
        except Exception as exc:
            raise ClearanceError("Lite6 kinematic reach bound is unavailable") from exc
        if len(joints) != 6 or not math.isfinite(reach) or reach <= 0.0:
            raise ClearanceError("Lite6 kinematic reach bound is unavailable")
        return reach

    def __call__(
        self,
        plan: Any,
        *,
        stop_requested: Optional[Callable[[], bool]] = None,
    ) -> ClearanceDecision:
        return self.evaluate(plan, stop_requested=stop_requested)

    def evaluate(
        self,
        plan: Any,
        *,
        stop_requested: Optional[Callable[[], bool]] = None,
    ) -> ClearanceDecision:
        blockers = []
        trajectory, plan_value = self._trajectory(
            plan,
            stop_requested=stop_requested,
        )
        if trajectory is None:
            blockers.append("trajectory_unavailable")
            return self._decision(plan_value, blockers=blockers)
        if stop_requested is not None and self._stopped(stop_requested):
            blockers.append("trajectory_unavailable")
            return self._decision(plan_value, blockers=blockers)

        try:
            pair = self.pair_provider()
        except Exception:
            blockers.extend(("tool_depth_unavailable", "workspace_depth_unavailable"))
            return self._decision(plan_value, blockers=blockers)
        frames = self._pair(pair, blockers)
        if frames is None:
            return self._decision(plan_value, blockers=blockers)
        tool_frame, workspace_frame = frames

        try:
            now_wall = float(self.clock())
            now_monotonic = float(self.monotonic())
        except Exception:
            blockers.append("dual_depth_stale")
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
            )
        if not math.isfinite(now_wall) or not math.isfinite(now_monotonic):
            blockers.append("dual_depth_stale")
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
            )
        ages = {
            "tool": now_wall - tool_frame.captured_at_s,
            "workspace": now_wall - workspace_frame.captured_at_s,
        }
        if any(
            not math.isfinite(age) or not 0.0 <= age <= self.max_frame_age_s
            for age in ages.values()
        ):
            blockers.append("dual_depth_stale")
        pair_skew = abs(tool_frame.captured_at_s - workspace_frame.captured_at_s)
        if pair_skew > self.max_pair_skew_s:
            blockers.append("dual_depth_skew")

        packets = {}
        for role, frame, buffer in (
            ("tool", tool_frame, self.tool_depth_buffer),
            ("workspace", workspace_frame, self.workspace_depth_buffer),
        ):
            try:
                packets[role] = buffer.resolve(
                    frame,
                    now_monotonic_s=now_monotonic,
                    max_age_s=self.max_frame_age_s,
                )
            except ClearanceError as exc:
                code = str(exc)
                blockers.append(
                    code
                    if code in {"dual_depth_stale", "depth_sequence_mismatch"}
                    else "{}_depth_unavailable".format(role)
                )
        if set(packets) != {"tool", "workspace"}:
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
                ages=ages,
                pair_skew=pair_skew,
            )

        try:
            tool_pose = self.tool_pose_provider(tool_frame.captured_at_s)
        except Exception:
            tool_pose = None
        if not isinstance(tool_pose, TimedToolPose) or (
            abs(tool_pose.timestamp_s - tool_frame.captured_at_s)
            > self.max_tool_pose_skew_s
        ):
            blockers.append("tool_pose_skew")
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
                ages=ages,
                pair_skew=pair_skew,
            )

        tool_transform = tool_pose.base_T_tool.compose(
            self.tool_calibration.tool_T_camera
        )
        if stop_requested is not None and self._stopped(stop_requested):
            blockers.append("trajectory_unavailable")
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
                ages=ages,
                pair_skew=pair_skew,
            )
        try:
            swept_coverage, swept_obstacle, camera_disagreement = (
                self._swept_envelope_visibility(
                    trajectory,
                    packets["tool"].depth_mm,
                    packets["workspace"].depth_mm,
                    tool_transform,
                    stop_requested=stop_requested,
                )
            )
        except ClearanceError as exc:
            blockers.append(
                "trajectory_unavailable"
                if str(exc) == "trajectory_unavailable"
                else "unknown_swept_volume"
            )
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
                ages=ages,
                pair_skew=pair_skew,
            )
        except Exception:
            blockers.append("unknown_swept_volume")
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
                ages=ages,
                pair_skew=pair_skew,
            )
        coverage = swept_coverage
        if swept_coverage < 1.0 - 1e-12:
            blockers.append("unknown_swept_volume")

        if swept_obstacle:
            blockers.append("clearance_below_limit")
        if camera_disagreement:
            blockers.append("camera_disagreement")
        if stop_requested is not None and self._stopped(stop_requested):
            blockers.append("trajectory_unavailable")

        # The footprint pass is a conservative certificate, not a distance
        # estimator.  When every inflated footprint is known free it proves
        # the configured lower bound exactly.  Unknown or intersecting pixels
        # instead report zero and already carry a blocking code.
        certified_clearance = (
            self.required_clearance_mm
            if swept_coverage >= 1.0 - 1e-12 and not swept_obstacle
            else 0.0
        )
        current_clearance = certified_clearance
        planned_clearance = certified_clearance

        try:
            scene_digest = self._scene_digest(
                packets,
                plan_value["plan_sha256"],
                pair_skew,
            )
        except Exception:
            blockers.append("unknown_swept_volume")
            return self._decision(
                plan_value,
                blockers=blockers,
                frames=frames,
                ages=ages,
                pair_skew=pair_skew,
            )
        blockers = list(_ordered_blockers(blockers))
        clear = not blockers
        evidence = self._evidence(
            plan=plan_value,
            blockers=blockers,
            frames=frames,
            ages=ages,
            pair_skew=pair_skew,
            coverage=coverage,
            scene_sha256=scene_digest,
            nearest_obstacle_mm=None,
            current_clearance_mm=current_clearance,
            planned_min_clearance_mm=planned_clearance,
            clear=clear,
        )
        return self._publish(ClearanceDecision(clear, evidence, tuple(blockers)))

    @staticmethod
    def _stopped(callback: Callable[[], bool]) -> bool:
        try:
            return callback() is True
        except Exception:
            return True

    def _trajectory(
        self,
        plan: Any,
        *,
        stop_requested: Optional[Callable[[], bool]] = None,
    ) -> Tuple[Optional[Tuple[Tuple[float, ...], ...]], dict]:
        candidate = getattr(plan, "joint_trajectory", plan)
        frames = getattr(candidate, "frames", None)
        if frames is None and isinstance(candidate, (tuple, list)):
            frames = candidate
        try:
            frame_values = tuple(frames)
        except TypeError:
            frame_values = ()
        if (
            not frame_values
            or len(frame_values) > _MAX_TRAJECTORY_FRAMES
            or (stop_requested is not None and self._stopped(stop_requested))
        ):
            return None, {
                "state": "unavailable",
                "plan_sha256": "",
                "waypoint_count": 0,
                "tcp_path_mm": [],
            }
        joints = []
        try:
            for frame in frame_values:
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                value = getattr(frame, "joints", frame)
                joints.append(_joint_vector(value))
            original_link_paths_values = []
            for value in joints:
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                original_link_paths_values.append(self.kinematics.link_points_mm(value))
            original_link_paths = tuple(original_link_paths_values)
        except Exception:
            return None, {
                "state": "failed",
                "plan_sha256": "",
                "waypoint_count": 0,
                "tcp_path_mm": [],
            }
        expanded_joints = [joints[0]]
        maximum_step = self.geometry_profile.maximum_joint_step_radians
        for start, end in zip(joints[:-1], joints[1:]):
            if stop_requested is not None and self._stopped(stop_requested):
                return None, {
                    "state": "unavailable",
                    "plan_sha256": "",
                    "waypoint_count": 0,
                    "tcp_path_mm": [],
                }
            deltas = tuple(after - before for before, after in zip(start, end))
            largest_delta = max(abs(delta) for delta in deltas)
            joint_steps = int(math.ceil(largest_delta / maximum_step))
            center_path_bound_mm = self._kinematic_reach_bound_mm * sum(
                abs(delta) for delta in deltas
            )
            motion_steps = int(
                math.ceil(center_path_bound_mm / self._maximum_motion_center_step_mm)
            )
            steps = max(1, joint_steps, motion_steps)
            if len(expanded_joints) + steps > _MAX_TRAJECTORY_FRAMES:
                return None, {
                    "state": "failed",
                    "plan_sha256": "",
                    "waypoint_count": 0,
                    "tcp_path_mm": [],
                }
            for step in range(1, steps + 1):
                if stop_requested is not None and self._stopped(stop_requested):
                    return None, {
                        "state": "unavailable",
                        "plan_sha256": "",
                        "waypoint_count": 0,
                        "tcp_path_mm": [],
                    }
                fraction = float(step) / float(steps)
                expanded_joints.append(
                    tuple(
                        before + (after - before) * fraction
                        for before, after in zip(start, end)
                    )
                )
        try:
            link_path_values = []
            for value in expanded_joints:
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                link_path_values.append(self.kinematics.link_points_mm(value))
            link_paths = tuple(link_path_values)
        except Exception:
            return None, {
                "state": "failed",
                "plan_sha256": "",
                "waypoint_count": 0,
                "tcp_path_mm": [],
            }
        plan_digest = hashlib.sha256(
            json.dumps(
                {"model_sha256": self.kinematics.model_sha256, "joints": joints},
                allow_nan=False,
                separators=(",", ":"),
                sort_keys=True,
            ).encode("utf-8")
        ).hexdigest()
        indices = self._display_indices(len(original_link_paths))
        plan_value = {
            "state": "planned",
            "plan_sha256": plan_digest,
            "waypoint_count": len(original_link_paths),
            "tcp_path_mm": [list(original_link_paths[index][-1]) for index in indices],
            # Internal-only binding consumed by _evidence and never copied
            # into the public plan object.
            "_start_joint_angles_rad": list(joints[0]),
        }
        return link_paths, plan_value

    @staticmethod
    def _display_indices(count: int) -> Tuple[int, ...]:
        if count <= _MAX_TWIN_PATH_POINTS:
            return tuple(range(count))
        values = np.linspace(0, count - 1, _MAX_TWIN_PATH_POINTS)
        return tuple(int(round(value)) for value in values)

    def _pair(
        self,
        pair: Any,
        blockers: list,
    ) -> Optional[Tuple[CameraFrame, CameraFrame]]:
        if not isinstance(pair, Mapping):
            blockers.extend(("tool_depth_unavailable", "workspace_depth_unavailable"))
            return None
        tool = pair.get(CameraRole.TOOL)
        workspace = pair.get(CameraRole.WORKSPACE)
        if not isinstance(tool, CameraFrame):
            blockers.append("tool_depth_unavailable")
        if not isinstance(workspace, CameraFrame):
            blockers.append("workspace_depth_unavailable")
        if not isinstance(tool, CameraFrame) or not isinstance(workspace, CameraFrame):
            return None
        if tool.role is not CameraRole.TOOL or (
            tool.source_id != self.tool_calibration.camera_source_id
        ):
            blockers.append("tool_depth_unavailable")
        if workspace.role is not CameraRole.WORKSPACE or (
            workspace.source_id != self.workspace_calibration.camera_source_id
        ):
            blockers.append("workspace_depth_unavailable")
        return None if blockers else (tool, workspace)

    def _point_cloud(
        self,
        depth_mm: Any,
        intrinsics: CameraIntrinsics,
        base_T_camera: RigidTransform,
    ) -> Tuple[np.ndarray, float]:
        depth = np.asarray(depth_mm, dtype=float)
        if depth.shape != (intrinsics.height, intrinsics.width):
            raise ClearanceError("depth dimensions do not match calibration")
        sampled = depth[:: self.sample_stride, :: self.sample_stride]
        rows, columns = np.indices(sampled.shape)
        pixels_x = columns.reshape(-1) * self.sample_stride
        pixels_y = rows.reshape(-1) * self.sample_stride
        values = sampled.reshape(-1)
        valid = (
            np.isfinite(values)
            & (values >= self.min_depth_mm)
            & (values <= self.max_depth_mm)
        )
        coverage = float(np.count_nonzero(valid)) / float(max(1, values.size))
        if not np.any(valid):
            raise ClearanceError("depth has no valid samples")
        pixels_x = pixels_x[valid].astype(float)
        pixels_y = pixels_y[valid].astype(float)
        values = values[valid].astype(float)
        if values.size > self.maximum_points_per_camera:
            selected = np.linspace(
                0, values.size - 1, self.maximum_points_per_camera
            ).astype(int)
            pixels_x = pixels_x[selected]
            pixels_y = pixels_y[selected]
            values = values[selected]
        if any(abs(value) > 1e-15 for value in intrinsics.distortion):
            try:
                import cv2
            except ImportError as exc:  # pragma: no cover - deployment dependency
                raise ClearanceError(
                    "OpenCV is required for distorted intrinsics"
                ) from exc
            pixels = np.stack((pixels_x, pixels_y), axis=1).reshape(-1, 1, 2)
            normalized = cv2.undistortPoints(
                pixels,
                intrinsics.camera_matrix,
                np.asarray(intrinsics.distortion, dtype=float),
            ).reshape(-1, 2)
            normalized_x, normalized_y = normalized[:, 0], normalized[:, 1]
        else:
            normalized_x = (pixels_x - intrinsics.cx) / intrinsics.fx
            normalized_y = (pixels_y - intrinsics.cy) / intrinsics.fy
        camera_points = np.stack(
            (normalized_x * values, normalized_y * values, values), axis=1
        )
        base_points = camera_points.dot(base_T_camera.rotation.T)
        base_points += base_T_camera.translation
        if not np.all(np.isfinite(base_points)):
            raise ClearanceError("transformed depth cloud is invalid")
        return base_points, coverage

    def _minimum_link_distance(
        self,
        points: np.ndarray,
        trajectory: Sequence[Sequence[Sequence[float]]],
    ) -> float:
        minimum = float("inf")
        for link_points in trajectory:
            links = np.asarray(link_points, dtype=float)
            for start, end, radius in zip(
                links[:-1],
                links[1:],
                self.geometry_profile.link_capsule_radii_mm,
            ):
                segment = end - start
                denominator = float(np.dot(segment, segment))
                if denominator <= 1e-12:
                    distances = np.linalg.norm(points - start, axis=1)
                else:
                    projection = np.clip(
                        (points - start).dot(segment) / denominator,
                        0.0,
                        1.0,
                    )
                    closest = start + projection[:, None] * segment
                    distances = np.linalg.norm(points - closest, axis=1)
                minimum = min(minimum, float(np.min(distances)) - radius)
            tool_distances = np.linalg.norm(points - links[-1], axis=1)
            minimum = min(
                minimum,
                float(np.min(tool_distances))
                - self.geometry_profile.tool_capsule_radius_mm,
            )
        if not math.isfinite(minimum):
            raise ClearanceError("trajectory clearance could not be calculated")
        return max(0.0, minimum)

    def _swept_envelope_visibility(
        self,
        trajectory: Sequence[Sequence[Sequence[float]]],
        tool_depth_mm: Any,
        workspace_depth_mm: Any,
        base_T_tool_camera: RigidTransform,
        *,
        stop_requested: Optional[Callable[[], bool]] = None,
    ) -> Tuple[float, bool, bool]:
        """Require every sampled inflated capsule envelope to be observed.

        A camera can prove one envelope sample free only when the complete
        conservative projected footprint has valid depth behind that sample.
        A foreground surface is treated as an occlusion (unknown), while a
        surface intersecting the inflated envelope is an obstacle.  Either
        camera may prove a sample free, but both calibrated cameras must
        contribute evidence somewhere along the sweep.
        """

        samples = self._envelope_samples(
            trajectory,
            stop_requested=stop_requested,
        )
        if not samples:
            raise ClearanceError("trajectory envelope has no samples")
        views = (
            (
                "tool",
                np.asarray(tool_depth_mm, dtype=float),
                self.tool_calibration.intrinsics,
                base_T_tool_camera,
            ),
            (
                "workspace",
                np.asarray(workspace_depth_mm, dtype=float),
                self.workspace_calibration.intrinsics,
                self.workspace_calibration.base_T_camera,
            ),
        )
        known = 0
        obstacle = False
        disagreement = False
        contributors = set()
        pixel_budget = [_MAX_FOOTPRINT_PIXEL_EVALUATIONS]
        for sample_index, (center, radius) in enumerate(samples):
            if stop_requested is not None and self._stopped(stop_requested):
                raise ClearanceError("trajectory_unavailable")
            sample_free = False
            sample_obstacle = False
            sample_states = set()
            for role, depth, intrinsics, base_T_camera in views:
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                state = self._view_envelope_state(
                    center,
                    radius,
                    depth,
                    intrinsics,
                    base_T_camera,
                    stop_requested=stop_requested,
                    pixel_budget=pixel_budget,
                )
                sample_states.add(state)
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                if state == "obstacle":
                    sample_obstacle = True
                elif state == "free":
                    sample_free = True
                    contributors.add(role)
            if {"free", "obstacle"}.issubset(sample_states):
                disagreement = True
            if sample_obstacle:
                obstacle = True
            elif sample_free:
                known += 1
        coverage = float(known) / float(len(samples))
        if contributors != {"tool", "workspace"}:
            coverage = min(
                coverage,
                float(max(0, len(samples) - 1)) / float(len(samples)),
            )
        return coverage, obstacle, disagreement

    def _envelope_samples(
        self,
        trajectory: Sequence[Sequence[Sequence[float]]],
        *,
        stop_requested: Optional[Callable[[], bool]] = None,
    ) -> Tuple[Tuple[np.ndarray, float], ...]:
        spacing = self.geometry_profile.envelope_sample_spacing_mm
        result = []
        for link_points in trajectory:
            if stop_requested is not None and self._stopped(stop_requested):
                raise ClearanceError("trajectory_unavailable")
            links = np.asarray(link_points, dtype=float)
            if links.shape != (7, 3) or not np.all(np.isfinite(links)):
                raise ClearanceError("trajectory link geometry is invalid")
            for start, end, radius in zip(
                links[:-1],
                links[1:],
                self.geometry_profile.link_capsule_radii_mm,
            ):
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                length = float(np.linalg.norm(end - start))
                inflated_radius = radius + self.required_clearance_mm
                center_spacing = min(spacing, inflated_radius / 2.0)
                intervals = max(1, int(math.ceil(length / center_spacing)))
                actual_spacing = length / float(intervals)
                spatial_cover = actual_spacing / 2.0
                sampled_radius = radius + spatial_cover + self._temporal_cover_mm
                for index in range(intervals + 1):
                    if stop_requested is not None and self._stopped(stop_requested):
                        raise ClearanceError("trajectory_unavailable")
                    fraction = float(index) / float(intervals)
                    result.append((start + (end - start) * fraction, sampled_radius))
                    if len(result) > _MAX_ENVELOPE_SAMPLES:
                        raise ClearanceError("trajectory envelope is too large")
            result.append(
                (
                    links[-1],
                    self.geometry_profile.tool_capsule_radius_mm
                    + self._temporal_cover_mm,
                )
            )
            if len(result) > _MAX_ENVELOPE_SAMPLES:
                raise ClearanceError("trajectory envelope is too large")
        return tuple(result)

    def _view_envelope_state(
        self,
        center_base_mm: np.ndarray,
        geometry_radius_mm: float,
        depth_mm: np.ndarray,
        intrinsics: CameraIntrinsics,
        base_T_camera: RigidTransform,
        *,
        stop_requested: Optional[Callable[[], bool]] = None,
        pixel_budget: Optional[list] = None,
    ) -> str:
        if depth_mm.shape != (intrinsics.height, intrinsics.width):
            return "unknown"
        if any(abs(value) > 1e-15 for value in intrinsics.distortion):
            # The aligned-depth deployment artifact must describe an already
            # rectified pinhole image.  An unmodelled distorted footprint is
            # not accepted as known space.
            return "unknown"
        center_camera = np.asarray(
            base_T_camera.inverse().transform_point(center_base_mm),
            dtype=float,
        )
        if center_camera.shape != (3,) or not np.all(np.isfinite(center_camera)):
            return "unknown"
        x, y, z = (float(value) for value in center_camera)
        inflated = float(geometry_radius_mm) + self.required_clearance_mm
        near = z - inflated
        far = z + inflated
        if near < self.min_depth_mm or far > self.max_depth_mm:
            return "unknown"
        # Project all near/far corners of the camera-axis-aligned cube that
        # contains the sphere.  The former center +/- fx*R/(z-R) shortcut
        # missed the |x| and |y| perspective terms for off-axis geometry.
        projected_x = tuple(
            intrinsics.fx * candidate_x / candidate_z + intrinsics.cx
            for candidate_x in (x - inflated, x + inflated)
            for candidate_z in (near, far)
        )
        projected_y = tuple(
            intrinsics.fy * candidate_y / candidate_z + intrinsics.cy
            for candidate_y in (y - inflated, y + inflated)
            for candidate_z in (near, far)
        )
        left = int(math.floor(min(projected_x)))
        right = int(math.ceil(max(projected_x)))
        top = int(math.floor(min(projected_y)))
        bottom = int(math.ceil(max(projected_y)))
        if (
            left < 0
            or top < 0
            or right >= intrinsics.width
            or bottom >= intrinsics.height
        ):
            return "unknown"
        pixel_count = (bottom - top + 1) * (right - left + 1)
        if pixel_count <= 0:
            return "unknown"
        if pixel_budget is not None:
            if (
                len(pixel_budget) != 1
                or isinstance(pixel_budget[0], bool)
                or not isinstance(pixel_budget[0], int)
                or pixel_budget[0] < pixel_count
            ):
                raise ClearanceError("footprint pixel budget exceeded")
            pixel_budget[0] -= pixel_count

        obstacle = False
        foreground = False
        for row in range(top, bottom + 1, _DEPTH_SCAN_CHUNK_ROWS):
            if stop_requested is not None and self._stopped(stop_requested):
                raise ClearanceError("trajectory_unavailable")
            region = depth_mm[
                row : min(row + _DEPTH_SCAN_CHUNK_ROWS, bottom + 1),
                left : right + 1,
            ]
            valid = (
                np.isfinite(region)
                & (region >= self.min_depth_mm)
                & (region <= self.max_depth_mm)
            )
            if not np.all(valid):
                if stop_requested is not None and self._stopped(stop_requested):
                    raise ClearanceError("trajectory_unavailable")
                return "unknown"
            obstacle = obstacle or bool(np.any((region >= near) & (region <= far)))
            foreground = foreground or bool(np.any(region < near))
            if stop_requested is not None and self._stopped(stop_requested):
                raise ClearanceError("trajectory_unavailable")
        if obstacle:
            return "obstacle"
        if foreground:
            return "unknown"
        return "free"

    def _scene_digest(
        self,
        packets: Mapping[str, AlignedDepthPacket],
        plan_sha256: str,
        pair_skew: float,
    ) -> str:
        digest = hashlib.sha256()
        digest.update(
            json.dumps(
                {
                    "model_sha256": self.kinematics.model_sha256,
                    "plan_sha256": plan_sha256,
                    "tool_calibration_sha256": self.tool_calibration_sha256,
                    "workspace_calibration_sha256": (self.workspace_calibration_sha256),
                    "geometry_profile_sha256": self.geometry_profile_sha256,
                    "tool_sequence": packets["tool"].sequence,
                    "workspace_sequence": packets["workspace"].sequence,
                    "pair_skew_seconds": pair_skew,
                },
                allow_nan=False,
                separators=(",", ":"),
                sort_keys=True,
            ).encode("utf-8")
        )
        for role in ("tool", "workspace"):
            array = np.ascontiguousarray(packets[role].depth_mm)
            digest.update(str(array.dtype).encode("ascii"))
            digest.update(str(array.shape).encode("ascii"))
            digest.update(array.tobytes())
        return digest.hexdigest()

    def _decision(
        self,
        plan: Mapping[str, Any],
        *,
        blockers: Sequence[str],
        frames: Optional[Tuple[CameraFrame, CameraFrame]] = None,
        ages: Optional[Mapping[str, float]] = None,
        pair_skew: Optional[float] = None,
    ) -> ClearanceDecision:
        evidence = self._evidence(
            plan=plan,
            blockers=blockers,
            frames=frames,
            ages=ages,
            pair_skew=pair_skew,
        )
        return self._publish(ClearanceDecision(False, evidence, tuple(blockers)))

    def _evidence(
        self,
        *,
        plan: Mapping[str, Any],
        blockers: Sequence[str],
        frames: Optional[Tuple[CameraFrame, CameraFrame]] = None,
        ages: Optional[Mapping[str, float]] = None,
        pair_skew: Optional[float] = None,
        coverage: float = 0.0,
        scene_sha256: str = "",
        nearest_obstacle_mm: Optional[float] = None,
        current_clearance_mm: Optional[float] = None,
        planned_min_clearance_mm: Optional[float] = None,
        clear: bool = False,
    ) -> Mapping[str, Any]:
        ordered = _ordered_blockers(tuple(blockers))
        ages = ages or {}
        cameras = {}
        frame_values = {
            "tool": frames[0] if frames else None,
            "workspace": frames[1] if frames else None,
        }
        digests = {
            "tool": self.tool_calibration_sha256,
            "workspace": self.workspace_calibration_sha256,
        }
        buffers = {
            "tool": self.tool_depth_buffer,
            "workspace": self.workspace_depth_buffer,
        }
        for role in ("tool", "workspace"):
            frame = frame_values[role]
            age = ages.get(role)
            cameras[role] = {
                "source_id": buffers[role].source_id,
                "frame_sequence": int(frame.sequence) if frame is not None else 0,
                "age_seconds": (
                    float(age)
                    if isinstance(age, (int, float))
                    and not isinstance(age, bool)
                    and math.isfinite(float(age))
                    and float(age) >= 0.0
                    else None
                ),
                "depth_ready": bool(
                    frame is not None
                    and "{}_depth_unavailable".format(role) not in ordered
                    and "depth_sequence_mismatch" not in ordered
                    and "dual_depth_stale" not in ordered
                ),
                "calibration_sha256": digests[role],
            }
        trajectory_valid = bool(plan.get("plan_sha256")) and (
            "trajectory_unavailable" not in ordered
        )
        pair_synchronized = bool(
            pair_skew is not None
            and "dual_depth_skew" not in ordered
            and "dual_depth_stale" not in ordered
            and "depth_sequence_mismatch" not in ordered
        )
        coverage_verified = bool(
            coverage >= 1.0 - 1e-12 and "unknown_swept_volume" not in ordered
        )
        clearance_verified = bool(
            clear
            and coverage_verified
            and "camera_disagreement" not in ordered
            and "clearance_below_limit" not in ordered
        )
        plan_value = {
            "state": plan.get("state", "failed"),
            "plan_sha256": plan.get("plan_sha256", ""),
            "waypoint_count": plan.get("waypoint_count", 0),
            "tcp_path_mm": plan.get("tcp_path_mm", []),
        }
        if trajectory_valid:
            plan_value["state"] = "validated" if clearance_verified else "planned"
        try:
            start_joints = list(_joint_vector(plan.get("_start_joint_angles_rad")))
        except Exception:
            start_joints = []
        try:
            evaluated_at = float(self.monotonic())
        except Exception:
            evaluated_at = -1.0
        if not math.isfinite(evaluated_at) or evaluated_at < 0.0:
            evaluated_at = -1.0
        return {
            # Internal producer binding.  DigitalTwinSnapshotProvider removes
            # it from the wire contract and accepts positive gates only after
            # WorkcellServices adds the exact cached pose sequence.
            "_binding": {
                "schema_version": 1,
                "evaluated_at_monotonic_s": evaluated_at,
                "pose_sequence": 0,
                "joint_angles_rad": start_joints,
                "model_sha256": self.kinematics.model_sha256,
                "tool_calibration_sha256": self.tool_calibration_sha256,
                "workspace_calibration_sha256": self.workspace_calibration_sha256,
                "geometry_profile_sha256": self.geometry_profile_sha256,
                "tool_frame_sequence": cameras["tool"]["frame_sequence"],
                "workspace_frame_sequence": cameras["workspace"]["frame_sequence"],
                "tool_camera_generation": self._frame_generation(frame_values["tool"]),
                "workspace_camera_generation": self._frame_generation(
                    frame_values["workspace"]
                ),
                "tool_camera_mxid": self._frame_mxid(frame_values["tool"]),
                "workspace_camera_mxid": self._frame_mxid(frame_values["workspace"]),
            },
            "cameras": cameras,
            "scene": {
                "scene_sha256": scene_sha256,
                "pair_skew_seconds": pair_skew,
                "coverage_ratio": max(0.0, min(float(coverage), 1.0)),
                "nearest_obstacle_mm": nearest_obstacle_mm,
                "current_clearance_mm": current_clearance_mm,
                "planned_min_clearance_mm": planned_min_clearance_mm,
                "required_clearance_mm": self.required_clearance_mm,
                "swept_volume_clear": clearance_verified,
            },
            "plan": plan_value,
            "gates": {
                "pair_synchronized": pair_synchronized,
                "calibrations_verified": True,
                "coverage_verified": coverage_verified,
                "trajectory_valid": trajectory_valid,
                "clearance_verified": clearance_verified,
                "blockers": list(ordered),
            },
        }

    @staticmethod
    def _frame_generation(frame: Optional[CameraFrame]) -> int:
        if frame is None:
            return 0
        value = frame.metadata.get("stream_generation")
        if isinstance(value, bool) or not isinstance(value, int) or value < 0:
            return 0
        return min(value, 10_000_000_000)

    @staticmethod
    def _frame_mxid(frame: Optional[CameraFrame]) -> str:
        if frame is None:
            return ""
        value = frame.metadata.get("camera_mxid")
        if not isinstance(value, str) or len(value) > 255:
            return ""
        return value

    def _publish(self, decision: ClearanceDecision) -> ClearanceDecision:
        if self.evidence_sink is not None:
            try:
                self.evidence_sink(decision.to_twin_evidence())
            except Exception:
                # Rendering evidence is optional; failure must never turn a
                # blocked result into a clear result or affect STOP.
                pass
        return decision


__all__ = [
    "ClearanceDecision",
    "ClearanceError",
    "ConservativeGeometryProfile",
    "ConservativeGeometryProfileStore",
    "DepthPacketBuffer",
    "DualDepthClearancePreflight",
    "StaticDepthCalibration",
    "StaticDepthCalibrationStore",
]
