"""Replay-only virtual-corridor threat assessment for Mission Core M4.6.

The provider consumes the canonical object map and a source-bound replay pose.
It never reads semantic class or detector identity when calculating geometry,
motion, corridor intersection, closest approach or TTC.
"""

from __future__ import annotations

import hashlib
import json
import math
from collections.abc import Iterable
from dataclasses import dataclass
from pathlib import Path
from typing import Final, Protocol

from .contracts import (
    CorridorIntersection,
    LocalObstacleMap,
    MotionState,
    QualificationState,
    TemporalObstacle,
    TemporalState,
    ThreatAssessment,
    ThreatDecision,
)
from .geometry_math import quaternion_xyzw_to_rotation_matrix

REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v3"
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v3"
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v3.json"
LEGACY_REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2"
LEGACY_REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2"

type Vector3 = tuple[float, float, float]
type Matrix3 = tuple[Vector3, Vector3, Vector3]


class ReplayThreatError(ValueError):
    """A virtual rig, source pose or threat input is ambiguous or unsafe."""


@dataclass(frozen=True, slots=True)
class ReplayBodyFrame:
    frame_id: str
    origin_map_xyz_m: Vector3
    basis_map_from_body: Matrix3
    sensor_height_m: float
    surface_slope_deg: float
    forward_source: str
    camera_forward_alignment_deg: float

    def __post_init__(self) -> None:
        if not self.frame_id:
            raise ReplayThreatError("replay body frame id is empty")
        if (
            len(self.origin_map_xyz_m) != 3
            or len(self.basis_map_from_body) != 3
            or any(len(row) != 3 for row in self.basis_map_from_body)
            or not all(
                math.isfinite(value)
                for value in (
                    *self.origin_map_xyz_m,
                    *(value for row in self.basis_map_from_body for value in row),
                    self.sensor_height_m,
                    self.surface_slope_deg,
                    self.camera_forward_alignment_deg,
                )
            )
        ):
            raise ReplayThreatError("replay body frame is not finite")
        columns: Matrix3 = (
            (
                self.basis_map_from_body[0][0],
                self.basis_map_from_body[1][0],
                self.basis_map_from_body[2][0],
            ),
            (
                self.basis_map_from_body[0][1],
                self.basis_map_from_body[1][1],
                self.basis_map_from_body[2][1],
            ),
            (
                self.basis_map_from_body[0][2],
                self.basis_map_from_body[1][2],
                self.basis_map_from_body[2][2],
            ),
        )
        if (
            not self.forward_source
            or self.sensor_height_m <= 0.0
            or self.surface_slope_deg < 0.0
            or self.camera_forward_alignment_deg < 0.0
            or any(abs(_dot(column, column) - 1.0) > 1e-6 for column in columns)
            or any(
                abs(_dot(columns[first], columns[second])) > 1e-6
                for first, second in ((0, 1), (0, 2), (1, 2))
            )
            or _dot(_cross(columns[0], columns[1]), columns[2]) < 1.0 - 1e-6
        ):
            raise ReplayThreatError("replay body frame basis is invalid")

    def map_point_to_body(
        self,
        point_map_xyz_m: Vector3,
    ) -> Vector3:
        delta = _vector3(
            point_map_xyz_m[index] - self.origin_map_xyz_m[index] for index in range(3)
        )
        return self.map_vector_to_body(delta)

    def map_vector_to_body(
        self,
        vector_map_xyz_m: Vector3,
    ) -> Vector3:
        values = tuple(
            float(
                sum(
                    vector_map_xyz_m[row] * self.basis_map_from_body[row][column]
                    for row in range(3)
                )
            )
            for column in range(3)
        )
        return values[0], values[1], values[2]


class ReplayBodyFrameResolver(Protocol):
    def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None: ...


class RecordedReplayBodyFrameResolver:
    """Derive a ground-level virtual body frame from verified replay evidence."""

    def __init__(self, store: object, *, profile: VirtualBodyFrameProfile) -> None:
        method = getattr(store, "replay_body_frame_inputs", None)
        if not callable(method):
            raise ReplayThreatError("recorded geometry store lacks body-frame evidence")
        available = getattr(store, "available_frame_indices", None)
        if not callable(available):
            raise ReplayThreatError("recorded geometry store lacks availability evidence")
        self._inputs_for_frame = method
        self._available_frame_indices = available
        self.profile = profile
        self._cache: dict[str, tuple[ReplayBodyFrame | None, str]] = {}

    def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None:
        return self._resolve(frame_id)[0]

    def qualified_frame_indices(self) -> tuple[int, ...]:
        return tuple(
            index
            for index in self._available_frame_indices()
            if self.body_frame_for_frame(f"frame-{index:06d}") is not None
        )

    def qualification_summary(self) -> dict[str, object]:
        indices = self._available_frame_indices()
        for index in indices:
            self._resolve(f"frame-{index:06d}")
        reasons: dict[str, int] = {}
        frames: list[ReplayBodyFrame] = []
        for frame, reason in self._cache.values():
            reasons[reason] = reasons.get(reason, 0) + 1
            if frame is not None:
                frames.append(frame)
        alignments = sorted(item.camera_forward_alignment_deg for item in frames)
        return {
            "available": len(indices),
            "qualified": len(frames),
            "rejected": len(indices) - len(frames),
            "reason_counts": dict(sorted(reasons.items())),
            "camera_forward_alignment_deg": {
                "maximum": max(alignments) if alignments else None,
                "p95": _percentile(alignments, 0.95),
            },
            "origin": self.profile.origin,
            "up": self.profile.up,
            "forward": self.profile.forward,
        }

    def _resolve(self, frame_id: str) -> tuple[ReplayBodyFrame | None, str]:
        cached = self._cache.get(frame_id)
        if cached is not None:
            return cached
        inputs = self._inputs_for_frame(
            frame_id,
            trajectory_half_window_frames=self.profile.trajectory_half_window_frames,
        )
        if inputs is None:
            return self._store(frame_id, None, "source-or-surface-unavailable")
        position = _vector3(float(value) for value in inputs.sensor_position_map)
        plane = tuple(float(value) for value in inputs.ground_plane_coefficients_map)
        normal_norm = math.sqrt(sum(value * value for value in plane[:3]))
        if normal_norm < 1e-9:
            return self._store(frame_id, None, "ground-normal-invalid")
        ground_normal = _vector3(value / normal_norm for value in plane[:3])
        if ground_normal[2] < 0.0:
            ground_normal = _vector3(-value for value in ground_normal)
            plane = tuple(-value for value in plane)
        sensor_height = _dot(position, ground_normal) + plane[3] / normal_norm
        if (
            abs(sensor_height - inputs.sensor_height_m) > 0.05
            or abs(sensor_height - self.profile.nominal_sensor_height_m)
            > self.profile.maximum_sensor_height_deviation_m
        ):
            return self._store(frame_id, None, "sensor-height-unqualified")
        if inputs.surface_slope_deg > self.profile.maximum_surface_slope_deg:
            return self._store(frame_id, None, "surface-slope-unqualified")
        # The collision corridor lives in a gravity-stable base_footprint frame.
        # Local terrain locates that footprint vertically but must not rotate the
        # SLAM world when a handheld or mounted sensor rolls and pitches.
        up = (0.0, 0.0, 1.0)
        vertical_denominator = _dot(ground_normal, up)
        if vertical_denominator < 1e-6:
            return self._store(frame_id, None, "ground-projection-invalid")
        vertical_height = sensor_height / vertical_denominator
        rotation = quaternion_xyzw_to_rotation_matrix(inputs.sensor_orientation_map_from_lidar_xyzw)
        calibration = inputs.t_camera_from_lidar
        camera_forward_lidar = _vector3(float(calibration[2, index]) for index in range(3))
        camera_forward_map = _vector3(
            float(sum(rotation[row, column] * camera_forward_lidar[column] for column in range(3)))
            for row in range(3)
        )
        camera_forward = _normalize(_reject(camera_forward_map, up))
        if camera_forward is None:
            return self._store(frame_id, None, "camera-forward-invalid")
        route = _vector3(
            float(
                inputs.trajectory_end_position_map[index]
                - inputs.trajectory_start_position_map[index]
            )
            for index in range(3)
        )
        route_on_ground = _reject(route, up)
        if (
            math.sqrt(_dot(route_on_ground, route_on_ground))
            >= self.profile.minimum_trajectory_displacement_m
        ):
            forward = _normalize(route_on_ground)
            assert forward is not None
            forward_source = "smoothed-trajectory-tangent"
            alignment = _angle_degrees(forward, camera_forward)
            if alignment > self.profile.maximum_camera_route_misalignment_deg:
                return self._store(frame_id, None, "camera-route-misaligned")
        else:
            forward = camera_forward
            forward_source = "calibrated-camera-forward-fallback"
            alignment = 0.0
        left = _normalize(_cross(up, forward))
        if left is None:
            return self._store(frame_id, None, "body-left-invalid")
        forward = _normalize(_cross(left, up))
        assert forward is not None
        origin = _vector3(position[index] - vertical_height * up[index] for index in range(3))
        basis: Matrix3 = (
            (forward[0], left[0], up[0]),
            (forward[1], left[1], up[1]),
            (forward[2], left[2], up[2]),
        )
        frame = ReplayBodyFrame(
            frame_id=frame_id,
            origin_map_xyz_m=origin,
            basis_map_from_body=basis,
            sensor_height_m=sensor_height,
            surface_slope_deg=float(inputs.surface_slope_deg),
            forward_source=forward_source,
            camera_forward_alignment_deg=alignment,
        )
        return self._store(frame_id, frame, "qualified")

    def _store(
        self,
        frame_id: str,
        frame: ReplayBodyFrame | None,
        reason: str,
    ) -> tuple[ReplayBodyFrame | None, str]:
        value = (frame, reason)
        self._cache[frame_id] = value
        return value


@dataclass(frozen=True, slots=True)
class VirtualBodyFrameProfile:
    schema_version: str
    origin: str
    up: str
    forward: str
    trajectory_half_window_frames: int
    minimum_trajectory_displacement_m: float
    maximum_camera_route_misalignment_deg: float
    maximum_sensor_height_deviation_m: float
    maximum_surface_slope_deg: float
    nominal_sensor_height_m: float


@dataclass(frozen=True, slots=True)
class VirtualRigProfile:
    profile_id: str
    body_length_m: float
    body_width_m: float
    lidar_reference: str
    nominal_sensor_height_m: float


@dataclass(frozen=True, slots=True)
class VirtualCorridorProfile:
    profile_id: str
    forward_length_m: float
    rear_margin_m: float
    lateral_clearance_m: float
    prediction_horizon_seconds: float
    occupied_voxel_size_m: float
    minimum_motion_span_seconds: float


@dataclass(frozen=True, slots=True)
class ReplayThreatProfile:
    profile_id: str
    provider_id: str
    source_id: str
    session_id: str
    temporal_result_id: str
    temporal_frames_sha256: str
    geometry_result_id: str
    geometry_frames_sha256: str
    detector_result_id: str
    detector_frames_sha256: str
    source_pack_id: str
    source_pack_sha256: str
    calibration_id: str
    calibration_content_sha256: str
    body_frame: VirtualBodyFrameProfile
    rig: VirtualRigProfile
    corridor: VirtualCorridorProfile
    profile_sha256: str


class DualEvidenceReplayThreatProvider:
    """Assess metric and nonmetric evidence without choosing a primary sensor."""

    provider_id: str = REPLAY_THREAT_PROVIDER_ID

    def __init__(
        self,
        *,
        body_frame_resolver: ReplayBodyFrameResolver,
        profile: ReplayThreatProfile,
    ) -> None:
        if profile.provider_id not in {
            self.provider_id,
            LEGACY_REPLAY_THREAT_PROVIDER_ID,
        }:
            raise ReplayThreatError("threat provider identity changed")
        self.body_frame_resolver = body_frame_resolver
        self.profile = profile

    def assess(self, obstacle_map: LocalObstacleMap) -> tuple[ThreatAssessment, ...]:
        if (
            obstacle_map.source_id != self.profile.source_id
            or obstacle_map.session_id != self.profile.session_id
        ):
            raise ReplayThreatError("obstacle map escaped the threat profile")
        body_frame = self.body_frame_resolver.body_frame_for_frame(obstacle_map.frame_id)
        assessments = [
            self._metric_or_stale(obstacle_map.frame_id, obstacle, body_frame)
            for obstacle in (*obstacle_map.occupied, *obstacle_map.unknown)
        ]
        assessments.extend(
            self._camera_only(obstacle_map.frame_id, proposal.proposal_id)
            for proposal in obstacle_map.camera_uncertainty
        )
        return tuple(assessments)

    def _metric_or_stale(
        self,
        frame_id: str,
        obstacle: TemporalObstacle,
        body_frame: ReplayBodyFrame | None,
    ) -> ThreatAssessment:
        if obstacle.state not in {TemporalState.CURRENT, TemporalState.RETAINED}:
            return self._unknown(
                frame_id,
                obstacle.component_id,
                ("stale-evidence", f"temporal-state-{obstacle.state.value}"),
            )
        if body_frame is None or obstacle.last_centroid_xyz_m is None or not obstacle.cells:
            return self._unknown(
                frame_id,
                obstacle.component_id,
                ("current-pose-or-metric-geometry-unavailable",),
            )

        centroid_body = body_frame.map_point_to_body(obstacle.last_centroid_xyz_m)
        cells_body = tuple(
            body_frame.map_point_to_body(
                (
                    (cell.x + 0.5) * self.profile.corridor.occupied_voxel_size_m,
                    (cell.y + 0.5) * self.profile.corridor.occupied_voxel_size_m,
                    (cell.z + 0.5) * self.profile.corridor.occupied_voxel_size_m,
                )
            )
            for cell in obstacle.cells
        )
        velocity_body = self._relative_velocity_body(obstacle, body_frame)
        corridor_entry = _first_corridor_entry_seconds(
            cells_body,
            velocity_body,
            rig=self.profile.rig,
            corridor=self.profile.corridor,
        )
        current_intersection = _intersects_corridor_now(
            cells_body,
            rig=self.profile.rig,
            corridor=self.profile.corridor,
        )
        retained = obstacle.state is TemporalState.RETAINED
        motion_complete = (
            not retained
            and obstacle.motion is not MotionState.UNKNOWN
            and velocity_body is not None
        )
        if current_intersection or (motion_complete and corridor_entry is not None):
            intersection = CorridorIntersection.INTERSECTS
            decision = ThreatDecision.THREAT
            reasons = [
                (
                    "retained-corridor-intersection"
                    if retained
                    else "current-corridor-intersection"
                )
                if current_intersection
                else "predicted-corridor-intersection",
                "metric-lidar-geometry",
            ]
        elif motion_complete:
            intersection = CorridorIntersection.CLEAR
            decision = ThreatDecision.NOT_THREAT
            reasons = ["predicted-corridor-clear", "metric-lidar-geometry"]
        elif retained:
            intersection = CorridorIntersection.UNKNOWN
            decision = ThreatDecision.UNKNOWN
            reasons = [
                "retained-map-occupancy-outside-current-corridor",
                "metric-lidar-geometry",
                "absence-of-republication-is-not-free",
            ]
        else:
            intersection = CorridorIntersection.UNKNOWN
            decision = ThreatDecision.UNKNOWN
            reasons = ["motion-incomplete", "metric-lidar-geometry"]

        closest = _closest_body_clearance_m(
            cells_body,
            velocity_body,
            rig=self.profile.rig,
            horizon_seconds=self.profile.corridor.prediction_horizon_seconds,
        )
        ttc = _first_body_entry_seconds(
            cells_body,
            velocity_body,
            rig=self.profile.rig,
            voxel_size_m=self.profile.corridor.occupied_voxel_size_m,
            horizon_seconds=self.profile.corridor.prediction_horizon_seconds,
        )
        relative_speed = _closing_speed_mps(centroid_body, velocity_body)
        if retained:
            reasons.append("motion-retained-map-increment-no-current-motion")
        elif velocity_body is None:
            reasons.append(f"motion-{obstacle.motion_reason}")
        else:
            reasons.append(f"motion-{obstacle.motion.value}")
        if obstacle.semantic_hint is None:
            reasons.append("geometry-only-evidence")
        else:
            reasons.append("camera-lidar-associated-evidence")
        return ThreatAssessment(
            assessment_id=_assessment_id(frame_id, obstacle.component_id),
            component_id=obstacle.component_id,
            rig_profile_id=self.profile.rig.profile_id,
            corridor_profile_id=self.profile.corridor.profile_id,
            qualification=QualificationState.QUALIFIED,
            relative_speed_mps=relative_speed,
            closest_approach_m=closest,
            ttc_seconds=ttc,
            corridor_intersection=intersection,
            decision=decision,
            reason_codes=tuple(reasons),
        )

    def _relative_velocity_body(
        self,
        obstacle: TemporalObstacle,
        current_body_frame: ReplayBodyFrame,
    ) -> tuple[float, float] | None:
        if len(obstacle.history) < 2:
            return None
        first = obstacle.history[0]
        last = obstacle.history[-1]
        span_seconds = (last.evidence_time_ns - first.evidence_time_ns) / 1_000_000_000
        if span_seconds < self.profile.corridor.minimum_motion_span_seconds:
            return None
        first_body_frame = self.body_frame_resolver.body_frame_for_frame(first.frame_id)
        last_body_frame = self.body_frame_resolver.body_frame_for_frame(last.frame_id)
        if (
            first_body_frame is None
            or last_body_frame is None
            or last.frame_id != current_body_frame.frame_id
        ):
            return None
        obstacle_delta = _vector3(
            last.centroid_xyz_m[index] - first.centroid_xyz_m[index] for index in range(3)
        )
        rig_delta = _vector3(
            last_body_frame.origin_map_xyz_m[index] - first_body_frame.origin_map_xyz_m[index]
            for index in range(3)
        )
        relative_map = _vector3(
            (obstacle_delta[index] - rig_delta[index]) / span_seconds for index in range(3)
        )
        body = current_body_frame.map_vector_to_body(relative_map)
        return body[0], body[1]

    def _camera_only(self, frame_id: str, proposal_id: str) -> ThreatAssessment:
        return self._unknown(
            frame_id,
            proposal_id,
            ("camera-only-no-metric-geometry", "absence-of-lidar-is-not-safe"),
        )

    def _unknown(
        self,
        frame_id: str,
        component_id: str,
        reasons: tuple[str, ...],
    ) -> ThreatAssessment:
        return ThreatAssessment(
            assessment_id=_assessment_id(frame_id, component_id),
            component_id=component_id,
            rig_profile_id=self.profile.rig.profile_id,
            corridor_profile_id=self.profile.corridor.profile_id,
            qualification=QualificationState.UNQUALIFIED,
            relative_speed_mps=None,
            closest_approach_m=None,
            ttc_seconds=None,
            corridor_intersection=CorridorIntersection.UNKNOWN,
            decision=ThreatDecision.UNKNOWN,
            reason_codes=reasons,
        )


def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
    if not path.is_file() or path.is_symlink():
        raise ReplayThreatError("replay threat profile is not a regular file")
    raw = path.read_bytes()
    try:
        document = _object(json.loads(raw), "replay threat profile")
    except json.JSONDecodeError as exc:
        raise ReplayThreatError("replay threat profile JSON is invalid") from exc
    _exact_keys(
        document,
        {
            "schema_version",
            "profile_id",
            "provider_id",
            "source",
            "calibration",
            "body_frame",
            "virtual_rig",
            "corridor",
            "policy",
            "authority",
        },
        "replay threat profile",
    )
    identity = (document["schema_version"], document["provider_id"])
    if identity not in {
        (REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID),
        (LEGACY_REPLAY_THREAT_PROFILE_SCHEMA, LEGACY_REPLAY_THREAT_PROVIDER_ID),
    }:
        raise ReplayThreatError("replay threat profile identity is incompatible")
    is_v3 = identity == (REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID)
    source = _object(document["source"], "threat source")
    calibration = _object(document["calibration"], "threat calibration")
    body_frame = _object(document["body_frame"], "virtual body frame")
    rig = _object(document["virtual_rig"], "virtual rig")
    corridor = _object(document["corridor"], "virtual corridor")
    policy = _object(document["policy"], "threat policy")
    authority = _object(document["authority"], "threat authority")
    _exact_keys(
        source,
        {
            "source_id",
            "session_id",
            "temporal_result_id",
            "temporal_frames_sha256",
            "geometry_result_id",
            "geometry_frames_sha256",
            "detector_result_id",
            "detector_frames_sha256",
            "source_pack_id",
            "source_pack_sha256",
        },
        "threat source",
    )
    _exact_keys(
        calibration,
        {"calibration_id", "content_identity_sha256", "usage"},
        "threat calibration",
    )
    _exact_keys(
        body_frame,
        {
            "schema_version",
            "origin",
            "up",
            "forward",
            "trajectory_half_window_frames",
            "minimum_trajectory_displacement_m",
            "maximum_camera_route_misalignment_deg",
            "maximum_sensor_height_deviation_m",
            "maximum_surface_slope_deg",
        },
        "virtual body frame",
    )
    _exact_keys(
        rig,
        {
            "profile_id",
            "body_length_m",
            "body_width_m",
            "lidar_reference",
            "nominal_sensor_height_m",
            "physical_mount_claimed",
        },
        "virtual rig",
    )
    _exact_keys(
        corridor,
        {
            "profile_id",
            "forward_length_m",
            "rear_margin_m",
            "lateral_clearance_m",
            "prediction_horizon_seconds",
            "occupied_voxel_size_m",
            "minimum_motion_span_seconds",
        },
        "virtual corridor",
    )
    policy_keys = {
            "camera_only_decision",
            "held_or_stale_decision",
            "semantic_class_used",
            "detector_identity_used",
            "absence_of_points_means_free",
            "geometry_only_is_eligible",
    }
    if is_v3:
        policy_keys.add("retained_map_intersection_decision")
    _exact_keys(policy, policy_keys, "threat policy")
    _exact_keys(
        authority,
        {
            "mode",
            "physical_live",
            "physical_collision_accepted",
            "commands_enabled",
            "actuation_allowed",
            "navigation_or_safety_accepted",
        },
        "threat authority",
    )
    if (
        calibration.get("usage") != "projection-and-forward-axis-binding"
        or body_frame.get("schema_version") != "missioncore.replay-body-frame-profile/v1"
        or body_frame.get("origin") != "local-surface-vertical-projection"
        or body_frame.get("up") != "vendor-slam-map-gravity-axis"
        or body_frame.get("forward") != "smoothed-slam-trajectory-validated-by-camera-axis"
        or rig.get("physical_mount_claimed") is not False
        or policy != {
            "camera_only_decision": "unknown",
            "held_or_stale_decision": "unknown",
            "semantic_class_used": False,
            "detector_identity_used": False,
            "absence_of_points_means_free": False,
            "geometry_only_is_eligible": True,
            **(
                {"retained_map_intersection_decision": "threat"}
                if is_v3
                else {}
            ),
        }
        or authority
        != {
            "mode": "replay-simulated",
            "physical_live": False,
            "physical_collision_accepted": False,
            "commands_enabled": False,
            "actuation_allowed": False,
            "navigation_or_safety_accepted": False,
        }
    ):
        raise ReplayThreatError("replay threat policy or authority is unsafe")
    virtual_rig = VirtualRigProfile(
        profile_id=_string(rig, "profile_id"),
        body_length_m=_positive_number(rig, "body_length_m"),
        body_width_m=_positive_number(rig, "body_width_m"),
        lidar_reference=_string(rig, "lidar_reference"),
        nominal_sensor_height_m=_positive_number(rig, "nominal_sensor_height_m"),
    )
    body_frame_profile = VirtualBodyFrameProfile(
        schema_version=_string(body_frame, "schema_version"),
        origin=_string(body_frame, "origin"),
        up=_string(body_frame, "up"),
        forward=_string(body_frame, "forward"),
        trajectory_half_window_frames=_positive_integer(
            body_frame, "trajectory_half_window_frames"
        ),
        minimum_trajectory_displacement_m=_positive_number(
            body_frame, "minimum_trajectory_displacement_m"
        ),
        maximum_camera_route_misalignment_deg=_positive_number(
            body_frame, "maximum_camera_route_misalignment_deg"
        ),
        maximum_sensor_height_deviation_m=_positive_number(
            body_frame, "maximum_sensor_height_deviation_m"
        ),
        maximum_surface_slope_deg=_positive_number(body_frame, "maximum_surface_slope_deg"),
        nominal_sensor_height_m=virtual_rig.nominal_sensor_height_m,
    )
    virtual_corridor = VirtualCorridorProfile(
        profile_id=_string(corridor, "profile_id"),
        forward_length_m=_positive_number(corridor, "forward_length_m"),
        rear_margin_m=_nonnegative_number(corridor, "rear_margin_m"),
        lateral_clearance_m=_nonnegative_number(corridor, "lateral_clearance_m"),
        prediction_horizon_seconds=_positive_number(corridor, "prediction_horizon_seconds"),
        occupied_voxel_size_m=_positive_number(corridor, "occupied_voxel_size_m"),
        minimum_motion_span_seconds=_positive_number(corridor, "minimum_motion_span_seconds"),
    )
    if virtual_rig.lidar_reference != "virtual-body-center":
        raise ReplayThreatError("virtual LiDAR reference is unsupported")
    for value in (
        source.get("temporal_frames_sha256"),
        source.get("geometry_frames_sha256"),
        source.get("detector_frames_sha256"),
        source.get("source_pack_sha256"),
        calibration.get("content_identity_sha256"),
    ):
        _sha256(value, "threat evidence digest")
    return ReplayThreatProfile(
        profile_id=_string(document, "profile_id"),
        provider_id=_string(document, "provider_id"),
        source_id=_string(source, "source_id"),
        session_id=_string(source, "session_id"),
        temporal_result_id=_string(source, "temporal_result_id"),
        temporal_frames_sha256=_string(source, "temporal_frames_sha256"),
        geometry_result_id=_string(source, "geometry_result_id"),
        geometry_frames_sha256=_string(source, "geometry_frames_sha256"),
        detector_result_id=_string(source, "detector_result_id"),
        detector_frames_sha256=_string(source, "detector_frames_sha256"),
        source_pack_id=_string(source, "source_pack_id"),
        source_pack_sha256=_string(source, "source_pack_sha256"),
        calibration_id=_string(calibration, "calibration_id"),
        calibration_content_sha256=_string(calibration, "content_identity_sha256"),
        body_frame=body_frame_profile,
        rig=virtual_rig,
        corridor=virtual_corridor,
        profile_sha256=hashlib.sha256(raw).hexdigest(),
    )


def _assessment_id(frame_id: str, component_id: str) -> str:
    digest = hashlib.sha256(f"{frame_id}\0{component_id}".encode()).hexdigest()
    return f"threat-{digest}"


def _corridor_bounds(
    rig: VirtualRigProfile,
    corridor: VirtualCorridorProfile,
    *,
    expansion_m: float,
) -> tuple[float, float, float, float]:
    return (
        -(rig.body_length_m / 2 + corridor.rear_margin_m + expansion_m),
        rig.body_length_m / 2 + corridor.forward_length_m + expansion_m,
        -(rig.body_width_m / 2 + corridor.lateral_clearance_m + expansion_m),
        rig.body_width_m / 2 + corridor.lateral_clearance_m + expansion_m,
    )


def _body_bounds(
    rig: VirtualRigProfile,
    *,
    expansion_m: float,
) -> tuple[float, float, float, float]:
    return (
        -(rig.body_length_m / 2 + expansion_m),
        rig.body_length_m / 2 + expansion_m,
        -(rig.body_width_m / 2 + expansion_m),
        rig.body_width_m / 2 + expansion_m,
    )


def _intersects_corridor_now(
    cells_body: tuple[tuple[float, float, float], ...],
    *,
    rig: VirtualRigProfile,
    corridor: VirtualCorridorProfile,
) -> bool:
    expansion = corridor.occupied_voxel_size_m * math.sqrt(2) / 2
    bounds = _corridor_bounds(rig, corridor, expansion_m=expansion)
    return any(_inside((point[0], point[1]), bounds) for point in cells_body)


def _first_corridor_entry_seconds(
    cells_body: tuple[tuple[float, float, float], ...],
    velocity_body: tuple[float, float] | None,
    *,
    rig: VirtualRigProfile,
    corridor: VirtualCorridorProfile,
) -> float | None:
    if velocity_body is None:
        return None
    expansion = corridor.occupied_voxel_size_m * math.sqrt(2) / 2
    bounds = _corridor_bounds(rig, corridor, expansion_m=expansion)
    entries = (_ray_box_entry((point[0], point[1]), velocity_body, bounds) for point in cells_body)
    valid = [
        entry
        for entry in entries
        if entry is not None and entry <= corridor.prediction_horizon_seconds
    ]
    return None if not valid else round(min(valid), 12)


def _first_body_entry_seconds(
    cells_body: tuple[tuple[float, float, float], ...],
    velocity_body: tuple[float, float] | None,
    *,
    rig: VirtualRigProfile,
    voxel_size_m: float,
    horizon_seconds: float,
) -> float | None:
    if velocity_body is None:
        return None
    expansion = voxel_size_m * math.sqrt(2) / 2
    bounds = _body_bounds(rig, expansion_m=expansion)
    valid = [
        entry
        for point in cells_body
        if (entry := _ray_box_entry((point[0], point[1]), velocity_body, bounds)) is not None
        and entry <= horizon_seconds
    ]
    return None if not valid else round(min(valid), 12)


def _ray_box_entry(
    point: tuple[float, float],
    velocity: tuple[float, float],
    bounds: tuple[float, float, float, float],
) -> float | None:
    t_min = 0.0
    t_max = math.inf
    for coordinate, speed, lower, upper in (
        (point[0], velocity[0], bounds[0], bounds[1]),
        (point[1], velocity[1], bounds[2], bounds[3]),
    ):
        if abs(speed) < 1e-12:
            if coordinate < lower or coordinate > upper:
                return None
            continue
        first = (lower - coordinate) / speed
        second = (upper - coordinate) / speed
        near, far = min(first, second), max(first, second)
        t_min = max(t_min, near)
        t_max = min(t_max, far)
        if t_min > t_max:
            return None
    return max(0.0, t_min) if t_max >= 0.0 else None


def _closest_body_clearance_m(
    cells_body: tuple[tuple[float, float, float], ...],
    velocity_body: tuple[float, float] | None,
    *,
    rig: VirtualRigProfile,
    horizon_seconds: float,
) -> float:
    bounds = _body_bounds(rig, expansion_m=0.0)
    candidates = {0.0, horizon_seconds}
    if velocity_body is not None:
        speed_squared = velocity_body[0] ** 2 + velocity_body[1] ** 2
        if speed_squared > 1e-12:
            for point in cells_body:
                candidates.add(
                    min(
                        horizon_seconds,
                        max(
                            0.0,
                            -(point[0] * velocity_body[0] + point[1] * velocity_body[1])
                            / speed_squared,
                        ),
                    )
                )
                for coordinate, speed, lower, upper in (
                    (point[0], velocity_body[0], bounds[0], bounds[1]),
                    (point[1], velocity_body[1], bounds[2], bounds[3]),
                ):
                    if abs(speed) > 1e-12:
                        candidates.add(min(horizon_seconds, max(0.0, (lower - coordinate) / speed)))
                        candidates.add(min(horizon_seconds, max(0.0, (upper - coordinate) / speed)))
    velocity = velocity_body or (0.0, 0.0)
    clearance = min(
        _point_box_distance(
            (point[0] + velocity[0] * time_s, point[1] + velocity[1] * time_s),
            bounds,
        )
        for point in cells_body
        for time_s in candidates
    )
    return round(clearance, 12)


def _point_box_distance(
    point: tuple[float, float],
    bounds: tuple[float, float, float, float],
) -> float:
    dx = max(bounds[0] - point[0], 0.0, point[0] - bounds[1])
    dy = max(bounds[2] - point[1], 0.0, point[1] - bounds[3])
    return math.hypot(dx, dy)


def _closing_speed_mps(
    centroid_body: tuple[float, float, float],
    velocity_body: tuple[float, float] | None,
) -> float | None:
    if velocity_body is None:
        return None
    distance = math.hypot(centroid_body[0], centroid_body[1])
    if distance < 1e-9:
        return round(math.hypot(*velocity_body), 12)
    return round(
        -(centroid_body[0] * velocity_body[0] + centroid_body[1] * velocity_body[1]) / distance,
        12,
    )


def _inside(
    point: tuple[float, float],
    bounds: tuple[float, float, float, float],
) -> bool:
    return bounds[0] <= point[0] <= bounds[1] and bounds[2] <= point[1] <= bounds[3]


def _object(value: object, label: str) -> dict[str, object]:
    if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
        raise ReplayThreatError(f"{label} must be an object")
    return value


def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None:
    if set(document) != expected:
        raise ReplayThreatError(f"{label} fields are incompatible")


def _string(document: dict[str, object], key: str) -> str:
    value = document.get(key)
    if not isinstance(value, str) or not value:
        raise ReplayThreatError(f"{key} must be a nonempty string")
    return value


def _positive_number(document: dict[str, object], key: str) -> float:
    value = _number(document, key)
    if value <= 0.0:
        raise ReplayThreatError(f"{key} must be positive")
    return value


def _positive_integer(document: dict[str, object], key: str) -> int:
    value = document.get(key)
    if not isinstance(value, int) or isinstance(value, bool) or value <= 0:
        raise ReplayThreatError(f"{key} must be a positive integer")
    return value


def _vector3(values: Iterable[float]) -> Vector3:
    first, second, third = values
    return float(first), float(second), float(third)


def _dot(first: Vector3, second: Vector3) -> float:
    return sum(first[index] * second[index] for index in range(3))


def _cross(
    first: Vector3,
    second: Vector3,
) -> Vector3:
    return (
        first[1] * second[2] - first[2] * second[1],
        first[2] * second[0] - first[0] * second[2],
        first[0] * second[1] - first[1] * second[0],
    )


def _normalize(
    value: Vector3,
) -> Vector3 | None:
    norm = math.sqrt(_dot(value, value))
    if norm < 1e-9:
        return None
    return value[0] / norm, value[1] / norm, value[2] / norm


def _reject(
    value: Vector3,
    normal: Vector3,
) -> Vector3:
    along = _dot(value, normal)
    return (
        value[0] - along * normal[0],
        value[1] - along * normal[1],
        value[2] - along * normal[2],
    )


def _angle_degrees(
    first: Vector3,
    second: Vector3,
) -> float:
    return math.degrees(math.acos(max(-1.0, min(1.0, _dot(first, second)))))


def _percentile(values: list[float], fraction: float) -> float | None:
    if not values:
        return None
    position = fraction * (len(values) - 1)
    lower = math.floor(position)
    upper = math.ceil(position)
    if lower == upper:
        return values[lower]
    weight = position - lower
    return values[lower] * (1.0 - weight) + values[upper] * weight


def _nonnegative_number(document: dict[str, object], key: str) -> float:
    value = _number(document, key)
    if value < 0.0:
        raise ReplayThreatError(f"{key} must be nonnegative")
    return value


def _number(document: dict[str, object], key: str) -> float:
    value = document.get(key)
    if not isinstance(value, int | float) or isinstance(value, bool) or not math.isfinite(value):
        raise ReplayThreatError(f"{key} must be a finite number")
    return float(value)


def _sha256(value: object, label: str) -> None:
    if (
        not isinstance(value, str)
        or len(value) != 64
        or any(character not in "0123456789abcdef" for character in value)
    ):
        raise ReplayThreatError(f"{label} is invalid")


__all__ = [
    "DEFAULT_REPLAY_THREAT_PROFILE_PATH",
    "DualEvidenceReplayThreatProvider",
    "REPLAY_THREAT_PROFILE_SCHEMA",
    "REPLAY_THREAT_PROVIDER_ID",
    "RecordedReplayBodyFrameResolver",
    "ReplayBodyFrame",
    "ReplayBodyFrameResolver",
    "ReplayThreatError",
    "ReplayThreatProfile",
    "VirtualBodyFrameProfile",
    "VirtualCorridorProfile",
    "VirtualRigProfile",
    "load_replay_threat_profile",
]