"""Standalone bounded LiDAR fusion primitives for LAB E10.

The worker deliberately receives a content-addressed replay pack instead of a
K1 session or device authority.  This module contains only geometry and world
state projection; it cannot discover, start, stop, or configure a scanner.
"""

from __future__ import annotations

import hashlib
import json
import math
import statistics
from collections import defaultdict, deque
from collections.abc import Mapping, Sequence
from dataclasses import dataclass, replace
from pathlib import Path
from typing import Any

import numpy as np
import numpy.typing as npt

PACK_SCHEMA = "missioncore.e10-lidar-replay-pack/v1"
WORLD_STATE_SCHEMA = "missioncore.live-perception-world-state/v1"
IMAGE_WIDTH = 800
IMAGE_HEIGHT = 600

FloatArray = npt.NDArray[np.float64]


def canonical_json(value: object) -> bytes:
    return json.dumps(
        value,
        ensure_ascii=False,
        sort_keys=True,
        separators=(",", ":"),
        allow_nan=False,
    ).encode()


def sha256(path: Path) -> str:
    digest = hashlib.sha256()
    with path.open("rb") as stream:
        while chunk := stream.read(1024 * 1024):
            digest.update(chunk)
    return digest.hexdigest()


@dataclass(frozen=True, slots=True)
class ProjectionProfile:
    width: int
    height: int
    intrinsic_fx_fy_cx_cy: tuple[float, float, float, float]
    distortion_kb4: tuple[float, float, float, float]
    t_camera_from_lidar: FloatArray


@dataclass(frozen=True, slots=True)
class Cuboid:
    center_map: tuple[float, float, float]
    half_size: tuple[float, float, float]
    quaternion_xyzw: tuple[float, float, float, float]


@dataclass(frozen=True, slots=True)
class CompletedCuboid:
    cuboid: Cuboid
    completion_fraction: float
    ground_z_map: float
    orientation_source: str
    temporal_status: str
    support_coverage_fraction: float


@dataclass(slots=True)
class _CuboidTrackState:
    center: FloatArray
    size: FloatArray
    yaw: float
    last_seconds: float
    hits: int


@dataclass(frozen=True, slots=True)
class TrackFusion:
    track_id: int
    label: str
    association_group: str
    score: float
    bbox_xyxy: tuple[float, float, float, float]
    candidate_points: int
    semantic_points: int
    clustered_points: int
    distance_p10_m: float | None
    distance_median_m: float | None
    distance_smoothed_m: float | None
    status: str
    cuboid: Cuboid | None
    source_indices: npt.NDArray[np.int64]
    pre_ground_clustered_points: int = 0
    ground_rejected_points: int = 0
    support_ground_z_map: float | None = None
    geometry: str = "none"
    observed_cuboid: Cuboid | None = None
    completion_fraction: float | None = None
    ground_z_map: float | None = None
    orientation_source: str | None = None
    temporal_status: str | None = None
    support_coverage_fraction: float | None = None


class LidarReplayPack:
    """Validated immutable LiDAR/pose replay input for one camera selection."""

    def __init__(self, root: Path, *, expected_job_id: str) -> None:
        self.root = root.resolve(strict=True)
        manifest_path = self.root / "manifest.json"
        arrays_path = self.root / "lidar-pack.npz"
        manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
        identity = manifest.get("identity") if isinstance(manifest, dict) else None
        identity_sha256 = manifest.get("identity_sha256") if isinstance(manifest, dict) else None
        if (
            not isinstance(manifest, dict)
            or manifest.get("schema_version") != PACK_SCHEMA
            or not isinstance(identity, dict)
            or identity.get("schema_version") != PACK_SCHEMA
            or identity.get("job_id") != expected_job_id
            or not isinstance(identity_sha256, str)
            or hashlib.sha256(canonical_json(identity)).hexdigest() != identity_sha256
            or self.root.name != f"e10-lidar-pack-{identity_sha256}"
            or manifest.get("pack_id") != self.root.name
            or manifest.get("artifact", {}).get("path") != arrays_path.name
            or manifest.get("artifact", {}).get("sha256") != sha256(arrays_path)
            or manifest.get("artifact", {}).get("byte_length") != arrays_path.stat().st_size
        ):
            raise RuntimeError("LAB E10 LiDAR replay pack identity is invalid")
        self.manifest = manifest
        self.identity = identity
        self.pack_id = self.root.name
        self.arrays_path = arrays_path
        self.arrays = np.load(arrays_path, allow_pickle=False)
        required = {
            "frame_indices",
            "source_frame_indices",
            "session_seconds",
            "sample_available",
            "cloud_offsets",
            "cloud_points_map",
            "pose_positions_map",
            "pose_quaternions_map_from_lidar",
            "lidar_camera_delta_ms",
            "pose_point_delta_ms",
            "intrinsic_fx_fy_cx_cy",
            "distortion_kb4",
            "t_camera_from_lidar",
        }
        if set(self.arrays.files) != required:
            raise RuntimeError("LAB E10 LiDAR replay pack array set changed")
        self.frame_indices = self.arrays["frame_indices"]
        self.source_frame_indices = self.arrays["source_frame_indices"]
        self.session_seconds = self.arrays["session_seconds"]
        self.sample_available = self.arrays["sample_available"]
        self.cloud_offsets = self.arrays["cloud_offsets"]
        self.cloud_points = self.arrays["cloud_points_map"]
        self.positions = self.arrays["pose_positions_map"]
        self.quaternions = self.arrays["pose_quaternions_map_from_lidar"]
        self.lidar_camera_delta_ms = self.arrays["lidar_camera_delta_ms"]
        self.pose_point_delta_ms = self.arrays["pose_point_delta_ms"]
        count = int(identity.get("frame_count", -1))
        if (
            count < 2
            or self.frame_indices.dtype != np.int64
            or not np.array_equal(self.frame_indices, np.arange(count, dtype=np.int64))
            or self.source_frame_indices.shape != (count,)
            or self.session_seconds.shape != (count,)
            or self.sample_available.dtype != np.bool_
            or self.sample_available.shape != (count,)
            or self.cloud_offsets.shape != (count + 1,)
            or self.cloud_points.ndim != 2
            or self.cloud_points.shape[1:] != (3,)
            or self.positions.shape != (count, 3)
            or self.quaternions.shape != (count, 4)
            or self.lidar_camera_delta_ms.shape != (count,)
            or self.pose_point_delta_ms.shape != (count,)
            or int(self.cloud_offsets[0]) != 0
            or int(self.cloud_offsets[-1]) != int(self.cloud_points.shape[0])
            or np.any(np.diff(self.cloud_offsets) < 0)
            or not np.all(np.diff(self.session_seconds) > 0)
        ):
            raise RuntimeError("LAB E10 LiDAR replay pack shapes are invalid")
        self.profile = ProjectionProfile(
            width=int(identity["projection"]["width"]),
            height=int(identity["projection"]["height"]),
            intrinsic_fx_fy_cx_cy=tuple(
                float(value) for value in self.arrays["intrinsic_fx_fy_cx_cy"]
            ),
            distortion_kb4=tuple(float(value) for value in self.arrays["distortion_kb4"]),
            t_camera_from_lidar=np.asarray(self.arrays["t_camera_from_lidar"], dtype=np.float64),
        )
        if (
            self.profile.width != IMAGE_WIDTH
            or self.profile.height != IMAGE_HEIGHT
            or self.profile.t_camera_from_lidar.shape != (4, 4)
        ):
            raise RuntimeError("LAB E10 projection profile is incompatible")

    @property
    def frame_count(self) -> int:
        return int(self.frame_indices.shape[0])

    def close(self) -> None:
        self.arrays.close()

    def frame(self, index: int) -> tuple[FloatArray, tuple[float, ...], tuple[float, ...]] | None:
        if not 0 <= index < self.frame_count:
            raise IndexError(index)
        if not bool(self.sample_available[index]):
            return None
        start = int(self.cloud_offsets[index])
        end = int(self.cloud_offsets[index + 1])
        position = tuple(float(value) for value in self.positions[index])
        quaternion = tuple(float(value) for value in self.quaternions[index])
        points = np.asarray(self.cloud_points[start:end], dtype=np.float64)
        if not np.isfinite(points).all() or not all(
            math.isfinite(value) for value in position + quaternion
        ):
            raise RuntimeError("LAB E10 LiDAR replay frame contains non-finite geometry")
        return points, position, quaternion


def quaternion_to_rotation(orientation_xyzw: Sequence[float]) -> FloatArray:
    quaternion = np.asarray(orientation_xyzw, dtype=np.float64)
    norm = float(np.linalg.norm(quaternion))
    if quaternion.shape != (4,) or not math.isfinite(norm) or norm < 1e-9:
        raise RuntimeError("LAB E10 pose quaternion is invalid")
    x, y, z, w = quaternion / norm
    return np.asarray(
        [
            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
        ],
        dtype=np.float64,
    )


def map_points_to_lidar(
    points_map: FloatArray,
    position_map: Sequence[float],
    orientation_map_from_lidar: Sequence[float],
) -> FloatArray:
    return (points_map - np.asarray(position_map, dtype=np.float64)) @ quaternion_to_rotation(
        orientation_map_from_lidar
    )


def project_points(
    points_map: FloatArray,
    position_map: Sequence[float],
    orientation_map_from_lidar: Sequence[float],
    profile: ProjectionProfile,
) -> tuple[FloatArray, FloatArray, npt.NDArray[np.int64], FloatArray]:
    points_lidar = map_points_to_lidar(points_map, position_map, orientation_map_from_lidar)
    transform = profile.t_camera_from_lidar
    points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3]
    front = points_camera[:, 2] > 1e-6
    source = np.flatnonzero(front)
    camera = points_camera[front]
    if camera.size == 0:
        return (
            np.empty((0, 2), dtype=np.float64),
            np.empty((0,), dtype=np.float64),
            np.empty((0,), dtype=np.int64),
            points_lidar,
        )
    x, y, z = camera.T
    radial = np.hypot(x, y)
    theta = np.arctan2(radial, z)
    theta2 = theta * theta
    k1, k2, k3, k4 = profile.distortion_kb4
    distorted = theta * (1 + k1 * theta2 + k2 * theta2**2 + k3 * theta2**3 + k4 * theta2**4)
    scale = np.divide(distorted, radial, out=np.zeros_like(distorted), where=radial > 1e-12)
    fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
    u = fx * x * scale + cx
    v = fy * y * scale + cy
    keep = (
        np.isfinite(u)
        & np.isfinite(v)
        & (u >= 0)
        & (u < profile.width)
        & (v >= 0)
        & (v < profile.height)
    )
    pixels = np.column_stack((u[keep], v[keep]))
    depths = z[keep]
    indices = source[keep].astype(np.int64, copy=False)
    if depths.size:
        px = np.rint(pixels[:, 0]).astype(np.int64)
        py = np.rint(pixels[:, 1]).astype(np.int64)
        keys = py * profile.width + px
        order = np.lexsort((depths, keys))
        ordered_keys = keys[order]
        first = np.concatenate(([True], ordered_keys[1:] != ordered_keys[:-1]))
        visible = np.sort(order[first])
        pixels, depths, indices = pixels[visible], depths[visible], indices[visible]
    return pixels, depths, indices, points_lidar


def _box_iou(one: Sequence[float], two: Sequence[float]) -> float:
    x1, y1 = max(one[0], two[0]), max(one[1], two[1])
    x2, y2 = min(one[2], two[2]), min(one[3], two[3])
    intersection = max(0.0, x2 - x1) * max(0.0, y2 - y1)
    union = max(0.0, one[2] - one[0]) * max(0.0, one[3] - one[1])
    union += max(0.0, two[2] - two[0]) * max(0.0, two[3] - two[1]) - intersection
    return 0.0 if union <= 0 else intersection / union


def _depth_cluster(
    indices: npt.NDArray[np.int64], depths: FloatArray, profile: Mapping[str, Any]
) -> npt.NDArray[np.int64]:
    if indices.size < 2:
        return indices
    ordered = indices[np.argsort(depths[indices])]
    groups: list[npt.NDArray[np.int64]] = []
    start = 0
    for offset, gap in enumerate(np.diff(depths[ordered]), start=1):
        threshold = max(
            float(profile["depth_cluster_minimum_gap_m"]),
            float(profile["depth_cluster_gap_fraction"]) * float(depths[ordered[offset - 1]]),
        )
        if float(gap) > threshold:
            groups.append(ordered[start:offset])
            start = offset
    groups.append(ordered[start:])
    return min(groups, key=lambda group: (-int(group.size), float(np.median(depths[group]))))


def _spatial_cluster(
    indices: npt.NDArray[np.int64], points_lidar: FloatArray, radius: float
) -> npt.NDArray[np.int64]:
    if indices.size < 2:
        return indices
    points = points_lidar[indices]
    adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius * radius
    unseen = set(range(indices.size))
    groups: list[list[int]] = []
    while unseen:
        seed = unseen.pop()
        group = [seed]
        pending = [seed]
        while pending:
            current = pending.pop()
            for neighbor in [value for value in tuple(unseen) if adjacent[current, value]]:
                unseen.remove(neighbor)
                pending.append(neighbor)
                group.append(neighbor)
        groups.append(group)
    selected = min(
        groups,
        key=lambda group: (-len(group), float(np.median(np.linalg.norm(points[group], axis=1)))),
    )
    return indices[np.asarray(selected, dtype=np.int64)]


def _cuboid(points_map: FloatArray, profile: Mapping[str, Any], group: str) -> Cuboid | None:
    if points_map.shape[0] < 2:
        return None
    xy = points_map[:, :2]
    centered = xy - np.median(xy, axis=0)
    covariance = centered.T @ centered / max(1, centered.shape[0] - 1)
    values, vectors = np.linalg.eigh(covariance)
    axis = vectors[:, int(np.argmax(values))]
    yaw = math.atan2(float(axis[1]), float(axis[0]))
    forward = np.asarray([math.cos(yaw), math.sin(yaw)])
    side = np.asarray([-math.sin(yaw), math.cos(yaw)])
    local = np.column_stack((xy @ forward, xy @ side))
    lower = np.percentile(local, 5.0, axis=0)
    upper = np.percentile(local, 95.0, axis=0)
    lower_z, upper_z = np.percentile(points_map[:, 2], [5.0, 95.0])
    spans = np.asarray([upper[0] - lower[0], upper[1] - lower[1], upper_z - lower_z])
    maximum = np.asarray(profile["maximum_oriented_extent_m"][group], dtype=np.float64)
    if (
        not np.isfinite(spans).all()
        or np.any(spans > float(profile["maximum_cuboid_span_m"]))
        or np.any(spans > maximum)
    ):
        return None
    sizes = np.maximum(spans, float(profile["minimum_cuboid_extent_m"]))
    local_center = (lower + upper) * 0.5
    center_xy = local_center[0] * forward + local_center[1] * side
    return Cuboid(
        center_map=(
            float(center_xy[0]),
            float(center_xy[1]),
            (float(lower_z) + float(upper_z)) * 0.5,
        ),
        half_size=tuple(float(value) for value in sizes * 0.5),
        quaternion_xyzw=(0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)),
    )


def _box_yaw(cuboid: Cuboid) -> float:
    z, w = cuboid.quaternion_xyzw[2:]
    return 2.0 * math.atan2(z, w)


def _align_box_yaw(yaw: float, reference: float) -> float:
    """Align a cuboid yaw to the closest equivalent pi-periodic heading."""

    return reference + ((yaw - reference + math.pi * 0.5) % math.pi) - math.pi * 0.5


def _completion_profile(profile: Mapping[str, Any]) -> dict[str, Any]:
    required_labels = {"person", "bicycle", "motorcycle", "car", "truck", "bus"}
    classes = profile.get("classes")
    ground = profile.get("ground")
    temporal = profile.get("temporal")
    orientation = profile.get("orientation")
    if (
        profile.get("mode") != "class-prior-amodal-v1"
        or profile.get("failure_policy") not in {"reject", "visible-envelope"}
        or not isinstance(classes, Mapping)
        or set(classes) != required_labels
        or not all(isinstance(value, Mapping) for value in (ground, temporal, orientation))
    ):
        raise RuntimeError("LAB E13 cuboid completion profile is invalid")
    for label, value in classes.items():
        nominal = np.asarray(value.get("nominal_size_m"), dtype=np.float64)
        minimum = np.asarray(value.get("minimum_size_m"), dtype=np.float64)
        maximum = np.asarray(value.get("maximum_size_m"), dtype=np.float64)
        padding = np.asarray(value.get("support_padding_m"), dtype=np.float64)
        if (
            nominal.shape != (3,)
            or minimum.shape != (3,)
            or maximum.shape != (3,)
            or padding.shape != (3,)
            or not np.isfinite(np.concatenate((nominal, minimum, maximum, padding))).all()
            or np.any(minimum <= 0)
            or np.any(padding < 0)
            or np.any(nominal < minimum)
            or np.any(nominal > maximum)
        ):
            raise RuntimeError(f"LAB E13 {label} size prior is invalid")
    scalar_checks = (
        (ground, "local_radius_m", 0.5, 10.0),
        (ground, "lower_percentile", 0.0, 30.0),
        (ground, "maximum_below_support_m", 0.1, 3.0),
        (ground, "maximum_above_support_m", 0.0, 0.5),
        (ground, "fallback_below_support_m", 0.0, 1.0),
        (orientation, "minimum_anisotropy_ratio", 1.0, 20.0),
        (orientation, "face_width_switch_fraction", 0.5, 2.0),
        (temporal, "center_alpha", 0.01, 1.0),
        (temporal, "size_alpha", 0.01, 1.0),
        (temporal, "yaw_alpha", 0.01, 1.0),
        (temporal, "maximum_center_innovation_m", 0.1, 20.0),
        (temporal, "maximum_yaw_innovation_degrees", 1.0, 90.0),
        (temporal, "maximum_idle_s", 0.05, 10.0),
        (profile, "minimum_support_coverage_fraction", 0.5, 1.0),
    )
    for owner, name, lower, upper in scalar_checks:
        try:
            value = float(owner[name])
        except (KeyError, TypeError, ValueError) as exc:
            raise RuntimeError(f"LAB E13 cuboid completion field {name} is invalid") from exc
        if not lower <= value <= upper:
            raise RuntimeError(f"LAB E13 cuboid completion field {name} is invalid")
    if not 1 <= int(temporal.get("confirmation_hits", 0)) <= 20:
        raise RuntimeError("LAB E13 cuboid confirmation hit count is invalid")
    return dict(profile)


def _object_support_ground_filter_profile(
    profile: Mapping[str, Any],
) -> dict[str, Any]:
    groups = {"person", "bicycle", "motorcycle", "vehicle"}
    minimum_height = profile.get("minimum_height_above_ground_m")
    maximum_height = profile.get("maximum_height_above_ground_m")
    if (
        profile.get("mode") != "local-ground-relative-object-support-v1"
        or not isinstance(minimum_height, Mapping)
        or set(minimum_height) != groups
        or not isinstance(maximum_height, Mapping)
        or set(maximum_height) != groups
    ):
        raise RuntimeError("object-support ground filter profile is invalid")
    scalar_checks = (
        ("local_radius_m", 0.5, 10.0),
        ("lower_percentile", 0.0, 30.0),
        ("maximum_below_support_m", 0.1, 3.0),
        ("maximum_above_support_m", 0.0, 0.5),
        ("fallback_below_support_m", 0.0, 1.0),
    )
    for name, lower, upper in scalar_checks:
        try:
            value = float(profile[name])
        except (KeyError, TypeError, ValueError) as exc:
            raise RuntimeError(f"object-support ground filter field {name} is invalid") from exc
        if not lower <= value <= upper:
            raise RuntimeError(f"object-support ground filter field {name} is invalid")
    for group in groups:
        try:
            lower = float(minimum_height[group])
            upper = float(maximum_height[group])
        except (KeyError, TypeError, ValueError) as exc:
            raise RuntimeError(
                f"object-support ground filter height for {group} is invalid"
            ) from exc
        if not 0.0 <= lower < upper <= 6.0:
            raise RuntimeError(f"object-support ground filter height for {group} is invalid")
    return dict(profile)


def _local_ground_z(
    *,
    all_points_map: FloatArray,
    center_xy: FloatArray,
    support_lower_z: float,
    profile: Mapping[str, Any],
) -> float:
    radius = float(profile["local_radius_m"])
    if all_points_map.size:
        distances = np.linalg.norm(all_points_map[:, :2] - center_xy, axis=1)
        candidates = all_points_map[distances <= radius, 2]
    else:
        candidates = np.empty(0, dtype=np.float64)
    estimate = (
        float(np.percentile(candidates, float(profile["lower_percentile"])))
        if candidates.size
        else math.nan
    )
    if (
        not math.isfinite(estimate)
        or estimate < support_lower_z - float(profile["maximum_below_support_m"])
        or estimate > support_lower_z + float(profile["maximum_above_support_m"])
    ):
        estimate = support_lower_z - float(profile["fallback_below_support_m"])
    return estimate


def _filter_object_support_by_ground(
    indices: npt.NDArray[np.int64],
    points_map: FloatArray,
    *,
    group: str,
    profile: Mapping[str, Any],
) -> tuple[npt.NDArray[np.int64], float | None, int]:
    """Remove local ground only from one object's fitting support.

    `points_map` is never modified or reduced. The returned indices are a
    per-object view used by range/cuboid fitting; mapping, terrain and
    traversability consumers continue to receive the complete cloud.
    """

    if indices.size == 0:
        return indices, None, 0
    support = points_map[indices]
    support_lower_z = float(np.percentile(support[:, 2], 5.0))
    ground_z = _local_ground_z(
        all_points_map=points_map,
        center_xy=np.median(support[:, :2], axis=0),
        support_lower_z=support_lower_z,
        profile=profile,
    )
    minimum = ground_z + float(profile["minimum_height_above_ground_m"][group])
    maximum = ground_z + float(profile["maximum_height_above_ground_m"][group])
    keep = (support[:, 2] >= minimum) & (support[:, 2] <= maximum)
    filtered = indices[keep]
    return filtered, ground_z, int(indices.size - filtered.size)


def _support_overlap_fraction(
    one: npt.NDArray[np.int64],
    two: npt.NDArray[np.int64],
) -> float:
    if one.size == 0 or two.size == 0:
        return 0.0
    shared = np.intersect1d(one, two, assume_unique=False).size
    return float(shared / min(one.size, two.size))


def _suppress_duplicate_support_fusions(
    fusions: Sequence[TrackFusion],
    *,
    overlap_threshold: float,
) -> tuple[TrackFusion, ...]:
    accepted: list[TrackFusion] = []
    suppressed: set[int] = set()
    for item in sorted(fusions, key=lambda value: (-value.score, value.track_id)):
        if item.cuboid is None:
            continue
        if any(
            other.association_group == item.association_group
            and _support_overlap_fraction(other.source_indices, item.source_indices)
            >= overlap_threshold
            for other in accepted
        ):
            suppressed.add(item.track_id)
        else:
            accepted.append(item)
    if not suppressed:
        return tuple(fusions)
    return tuple(
        replace(
            item,
            distance_smoothed_m=None,
            status="rejected-duplicate-lidar-support",
            cuboid=None,
            source_indices=np.empty(0, dtype=np.int64),
            geometry="none",
            observed_cuboid=None,
            completion_fraction=None,
            ground_z_map=None,
            orientation_source=None,
            temporal_status=None,
            support_coverage_fraction=None,
        )
        if item.track_id in suppressed
        else item
        for item in fusions
    )


class CuboidCompletionTracker:
    """Complete visible LiDAR surfaces into provenance-marked, smoothed cuboids.

    Completion is deliberately class-prior based.  The returned box is useful for
    perception visualization and planning experiments, but it is never represented
    as fully measured geometry or ground truth.
    """

    def __init__(self, profile: Mapping[str, Any]) -> None:
        self.profile = _completion_profile(profile)
        self.states: dict[tuple[int, str], _CuboidTrackState] = {}
        self.last_failure_reason: str | None = None

    def complete(
        self,
        *,
        track_id: int,
        label: str,
        support_points_map: FloatArray,
        all_points_map: FloatArray,
        sensor_position_map: Sequence[float],
        session_seconds: float,
        observed_cuboid: Cuboid,
    ) -> CompletedCuboid | None:
        self.last_failure_reason = None
        if support_points_map.shape[0] < 2 or support_points_map.shape[1:] != (3,):
            self.last_failure_reason = "insufficient-support"
            return None
        prior = self.profile["classes"].get(label)
        if not isinstance(prior, Mapping):
            self.last_failure_reason = "missing-class-prior"
            return None
        now = float(session_seconds)
        sensor = np.asarray(sensor_position_map, dtype=np.float64)
        if sensor.shape != (3,) or not np.isfinite(sensor).all() or not math.isfinite(now):
            self.last_failure_reason = "invalid-pose-or-time"
            return None
        self._prune(now)
        key = (int(track_id), label)
        previous = self.states.get(key)
        nominal = np.asarray(prior["nominal_size_m"], dtype=np.float64)
        observed_yaw = _box_yaw(observed_cuboid)
        orientation_source = "support-pca"
        centered = support_points_map[:, :2] - np.median(support_points_map[:, :2], axis=0)
        covariance = centered.T @ centered / max(1, centered.shape[0] - 1)
        values = np.linalg.eigvalsh(covariance)
        anisotropy = float(values[-1] / max(values[0], 1e-9))
        anisotropy_minimum = float(self.profile["orientation"]["minimum_anisotropy_ratio"])
        if anisotropy >= anisotropy_minimum and nominal[0] >= nominal[1] * 1.4:
            principal = np.asarray([math.cos(observed_yaw), math.sin(observed_yaw)])
            projected = support_points_map[:, :2] @ principal
            principal_span = float(np.percentile(projected, 95.0) - np.percentile(projected, 5.0))
            if principal_span <= nominal[1] * float(
                self.profile["orientation"]["face_width_switch_fraction"]
            ):
                observed_yaw += math.pi * 0.5
                orientation_source = "support-face-normal"
        if previous is not None:
            observed_yaw = _align_box_yaw(observed_yaw, previous.yaw)
            if anisotropy < anisotropy_minimum:
                observed_yaw = previous.yaw
                orientation_source = "track-history"
            else:
                alternative = _align_box_yaw(observed_yaw + math.pi * 0.5, previous.yaw)
                if abs(alternative - previous.yaw) < abs(observed_yaw - previous.yaw):
                    observed_yaw = alternative
                    orientation_source = "track-history-axis-disambiguation"
        elif anisotropy < anisotropy_minimum:
            bearing = math.atan2(
                float(np.median(support_points_map[:, 1]) - sensor[1]),
                float(np.median(support_points_map[:, 0]) - sensor[0]),
            )
            observed_yaw = bearing
            orientation_source = "sensor-bearing-fallback"

        forward = np.asarray([math.cos(observed_yaw), math.sin(observed_yaw)])
        side = np.asarray([-math.sin(observed_yaw), math.cos(observed_yaw)])
        local = np.column_stack(
            (support_points_map[:, :2] @ forward, support_points_map[:, :2] @ side)
        )
        lower_xy = np.percentile(local, 5.0, axis=0)
        upper_xy = np.percentile(local, 95.0, axis=0)
        lower_z, upper_z = np.percentile(support_points_map[:, 2], [5.0, 95.0])
        ground_z = self._ground_z(
            all_points_map=all_points_map,
            center_xy=np.median(support_points_map[:, :2], axis=0),
            support_lower_z=float(lower_z),
        )

        minimum = np.asarray(prior["minimum_size_m"], dtype=np.float64)
        maximum = np.asarray(prior["maximum_size_m"], dtype=np.float64)
        padding = np.asarray(prior["support_padding_m"], dtype=np.float64)
        observed_size = np.asarray(
            [upper_xy[0] - lower_xy[0], upper_xy[1] - lower_xy[1], upper_z - lower_z],
            dtype=np.float64,
        )
        required_size = np.asarray(
            [
                observed_size[0] + 2.0 * padding[0],
                observed_size[1] + 2.0 * padding[1],
                max(0.0, float(upper_z) - ground_z) + padding[2],
            ],
            dtype=np.float64,
        )
        size = np.clip(np.maximum.reduce((nominal, minimum, required_size)), minimum, maximum)
        if np.any(required_size > maximum + 1e-9) or not np.isfinite(size).all():
            self.last_failure_reason = "required-size-exceeds-class-bound"
            return None

        sensor_local = np.asarray([sensor[:2] @ forward, sensor[:2] @ side], dtype=np.float64)
        center_local = np.asarray(
            [
                self._amodal_axis_center(
                    float(lower_xy[axis]),
                    float(upper_xy[axis]),
                    float(size[axis]),
                    float(sensor_local[axis]),
                )
                for axis in range(2)
            ],
            dtype=np.float64,
        )
        center_xy = center_local[0] * forward + center_local[1] * side
        center = np.asarray([center_xy[0], center_xy[1], ground_z + size[2] * 0.5])
        yaw = observed_yaw
        candidate_center = center.copy()
        candidate_size = size.copy()
        candidate_yaw = yaw
        temporal_status = "tentative"
        if previous is not None:
            temporal = self.profile["temporal"]
            center_innovation = float(np.linalg.norm(center - previous.center))
            yaw_delta = _align_box_yaw(yaw, previous.yaw) - previous.yaw
            maximum_yaw = math.radians(float(temporal["maximum_yaw_innovation_degrees"]))
            if (
                center_innovation > float(temporal["maximum_center_innovation_m"])
                or abs(yaw_delta) > maximum_yaw
            ):
                temporal_status = "reset-innovation"
                hits = 1
            else:
                center = previous.center + float(temporal["center_alpha"]) * (
                    center - previous.center
                )
                size = previous.size + float(temporal["size_alpha"]) * (size - previous.size)
                yaw = previous.yaw + float(temporal["yaw_alpha"]) * yaw_delta
                hits = previous.hits + 1
                temporal_status = (
                    "confirmed" if hits >= int(temporal["confirmation_hits"]) else "tentative"
                )
        else:
            hits = 1

        coverage = self._coverage(support_points_map, center, size, yaw)
        minimum_coverage = float(self.profile["minimum_support_coverage_fraction"])
        if coverage < minimum_coverage and previous is not None:
            # A smoothed box must never drift away from its current measured support.
            center = candidate_center
            size = candidate_size
            yaw = candidate_yaw
            coverage = self._coverage(support_points_map, center, size, yaw)
            temporal_status = "reset-support-coverage"
            hits = 1
        if coverage < minimum_coverage:
            self.last_failure_reason = "support-coverage-below-threshold"
            return None
        self.states[key] = _CuboidTrackState(
            center=center.copy(),
            size=size.copy(),
            yaw=float(yaw),
            last_seconds=now,
            hits=hits,
        )
        observed_volume = float(np.prod(np.maximum(observed_size, 0.0)))
        completed_volume = float(np.prod(size))
        completion_fraction = 1.0 - min(1.0, observed_volume / completed_volume)
        return CompletedCuboid(
            cuboid=Cuboid(
                center_map=tuple(float(value) for value in center),
                half_size=tuple(float(value) for value in size * 0.5),
                quaternion_xyzw=(
                    0.0,
                    0.0,
                    math.sin(yaw * 0.5),
                    math.cos(yaw * 0.5),
                ),
            ),
            completion_fraction=completion_fraction,
            ground_z_map=ground_z,
            orientation_source=orientation_source,
            temporal_status=temporal_status,
            support_coverage_fraction=coverage,
        )

    def _ground_z(
        self,
        *,
        all_points_map: FloatArray,
        center_xy: FloatArray,
        support_lower_z: float,
    ) -> float:
        return _local_ground_z(
            all_points_map=all_points_map,
            center_xy=center_xy,
            support_lower_z=support_lower_z,
            profile=self.profile["ground"],
        )

    @staticmethod
    def _amodal_axis_center(lower: float, upper: float, size: float, sensor: float) -> float:
        half = size * 0.5
        allowed_lower = upper - half
        allowed_upper = lower + half
        if sensor < lower:
            return allowed_upper
        if sensor > upper:
            return allowed_lower
        return min(allowed_upper, max(allowed_lower, (lower + upper) * 0.5))

    @staticmethod
    def _coverage(points: FloatArray, center: FloatArray, size: FloatArray, yaw: float) -> float:
        forward = np.asarray([math.cos(yaw), math.sin(yaw)])
        side = np.asarray([-math.sin(yaw), math.cos(yaw)])
        relative = points[:, :2] - center[:2]
        local = np.column_stack((relative @ forward, relative @ side))
        horizontal = np.all(np.abs(local) <= size[:2] * 0.5 + 1e-6, axis=1)
        vertical = np.abs(points[:, 2] - center[2]) <= size[2] * 0.5 + 1e-6
        return float(np.mean(horizontal & vertical))

    def _prune(self, now: float) -> None:
        maximum_idle = float(self.profile["temporal"]["maximum_idle_s"])
        stale = [
            key for key, state in self.states.items() if now - state.last_seconds > maximum_idle
        ]
        for key in stale:
            del self.states[key]


def fuse_tracks(
    *,
    tracks: list[dict[str, Any]],
    semantic_map: npt.NDArray[np.uint8],
    pixels: FloatArray,
    depths: FloatArray,
    source_indices: npt.NDArray[np.int64],
    points_map: FloatArray,
    points_lidar: FloatArray,
    association: Mapping[str, Any],
    distance_history: dict[int, deque[float]],
    completion_tracker: CuboidCompletionTracker | None = None,
    sensor_position_map: Sequence[float] | None = None,
    session_seconds: float | None = None,
) -> tuple[TrackFusion, ...]:
    vehicle_labels = set(str(value) for value in association["vehicle_labels"])
    ground_filter = association.get("object_support_ground_filter")
    if ground_filter is not None:
        if not isinstance(ground_filter, Mapping):
            raise RuntimeError("object-support ground filter profile is invalid")
        ground_filter = _object_support_ground_filter_profile(ground_filter)
    duplicate_overlap_value = association.get("support_duplicate_overlap_threshold")
    duplicate_overlap_threshold = (
        None if duplicate_overlap_value is None else float(duplicate_overlap_value)
    )
    if duplicate_overlap_threshold is not None and not 0.0 < duplicate_overlap_threshold <= 1.0:
        raise RuntimeError("support duplicate overlap threshold is invalid")
    accepted_tracks: list[dict[str, Any]] = []
    for track in sorted(tracks, key=lambda value: float(value["score"]), reverse=True):
        label = str(track["label"])
        box = tuple(float(value) for value in track["bbox_xyxy"])
        if label in vehicle_labels and any(
            str(other["label"]) in vehicle_labels
            and _box_iou(box, other["bbox_xyxy"]) > float(association["group_nms_iou_threshold"])
            for other in accepted_tracks
        ):
            continue
        accepted_tracks.append(track)
    accepted_tracks.sort(key=lambda value: int(value["track_id"]))
    pixel_x = np.clip(np.rint(pixels[:, 0]).astype(np.int64), 0, IMAGE_WIDTH - 1)
    pixel_y = np.clip(np.rint(pixels[:, 1]).astype(np.int64), 0, IMAGE_HEIGHT - 1)
    sampled_semantic = semantic_map[pixel_y, pixel_x]
    result: list[TrackFusion] = []
    for track in accepted_tracks:
        label = str(track["label"])
        group = "vehicle" if label in vehicle_labels else label
        if group not in association["semantic_ids"]:
            continue
        track_id = int(track["track_id"])
        x1, y1, x2, y2 = (float(value) for value in track["bbox_xyxy"])
        width, height = x2 - x1, y2 - y1
        inset = association["box_inset"]
        inside = (
            (pixels[:, 0] >= x1 + width * float(inset["horizontal_fraction"]))
            & (pixels[:, 0] < x2 - width * float(inset["horizontal_fraction"]))
            & (pixels[:, 1] >= y1 + height * float(inset["top_fraction"]))
            & (pixels[:, 1] < y2 - height * float(inset["bottom_fraction"]))
        )
        candidates = np.flatnonzero(inside).astype(np.int64)
        allowed = np.asarray(association["semantic_ids"][group], dtype=np.uint8)
        compatible = candidates[np.isin(sampled_semantic[candidates], allowed)]
        clustered = _depth_cluster(compatible, depths, association)
        selected_before_ground = _spatial_cluster(
            source_indices[clustered],
            points_lidar,
            float(association["spatial_cluster_radius_m"][group]),
        )
        selected = selected_before_ground
        support_ground_z = None
        ground_rejected_points = 0
        if ground_filter is not None:
            selected, support_ground_z, ground_rejected_points = _filter_object_support_by_ground(
                selected_before_ground,
                points_map,
                group=group,
                profile=ground_filter,
            )
        ranges = np.linalg.norm(points_lidar[selected], axis=1)
        p10 = None if ranges.size == 0 else float(np.percentile(ranges, 10))
        median = None if ranges.size == 0 else float(np.median(ranges))
        minimum = int(association["minimum_support_points"][group])
        cuboid = None
        observed_cuboid = None
        geometry = "none"
        completion_fraction = None
        ground_z_map = None
        orientation_source = None
        temporal_status = None
        support_coverage_fraction = None
        if compatible.size == 0:
            status = "rejected-no-semantic-lidar-support"
        elif (
            selected_before_ground.size >= minimum
            and selected.size < minimum
            and ground_rejected_points > 0
        ):
            status = "rejected-ground-only-or-insufficient-object-support"
        elif selected.size < minimum:
            status = f"rejected-fewer-than-{minimum}-clustered-points"
        else:
            cuboid = _cuboid(points_map[selected], association, group)
            status = (
                "accepted-point-supported-oriented-p05-p95"
                if cuboid
                else "rejected-implausible-cuboid"
            )
            if cuboid is not None:
                observed_cuboid = cuboid
                geometry = "point-supported-visible-surface-envelope"
        smoothed = None
        if cuboid is not None and median is not None:
            history = distance_history[track_id]
            if history:
                reference = float(np.median(np.asarray(history, dtype=np.float64)))
                maximum = max(
                    float(association["maximum_distance_innovation_m"]),
                    reference * float(association["maximum_distance_innovation_fraction"]),
                )
                if abs(median - reference) > maximum:
                    cuboid = None
                    status = "rejected-distance-innovation"
            if cuboid is not None:
                history.append(median)
                smoothed = float(np.median(np.asarray(history, dtype=np.float64)))
        if cuboid is None:
            observed_cuboid = None
            geometry = "none"
        elif completion_tracker is not None:
            if sensor_position_map is None or session_seconds is None:
                raise RuntimeError("LAB E13 completion requires sensor pose and session time")
            completion = completion_tracker.complete(
                track_id=track_id,
                label=label,
                support_points_map=points_map[selected],
                all_points_map=points_map,
                sensor_position_map=sensor_position_map,
                session_seconds=session_seconds,
                observed_cuboid=observed_cuboid,
            )
            if completion is None:
                reason = completion_tracker.last_failure_reason or "unknown"
                if completion_tracker.profile["failure_policy"] == "reject":
                    cuboid = None
                    geometry = "none"
                    status = f"rejected-amodal-completion-{reason}"
                else:
                    status = f"accepted-visible-envelope-completion-{reason}"
            else:
                cuboid = completion.cuboid
                status = "accepted-class-prior-amodal-v1"
                geometry = "class-prior-completed-from-visible-lidar-support"
                completion_fraction = completion.completion_fraction
                ground_z_map = completion.ground_z_map
                orientation_source = completion.orientation_source
                temporal_status = completion.temporal_status
                support_coverage_fraction = completion.support_coverage_fraction
        result.append(
            TrackFusion(
                track_id=track_id,
                label=label,
                association_group=group,
                score=float(track["score"]),
                bbox_xyxy=(x1, y1, x2, y2),
                candidate_points=int(candidates.size),
                semantic_points=int(compatible.size),
                clustered_points=int(selected.size),
                distance_p10_m=p10,
                distance_median_m=median,
                distance_smoothed_m=smoothed,
                status=status,
                cuboid=cuboid,
                source_indices=selected,
                pre_ground_clustered_points=int(selected_before_ground.size),
                ground_rejected_points=ground_rejected_points,
                support_ground_z_map=support_ground_z,
                geometry=geometry,
                observed_cuboid=observed_cuboid,
                completion_fraction=completion_fraction,
                ground_z_map=ground_z_map,
                orientation_source=orientation_source,
                temporal_status=temporal_status,
                support_coverage_fraction=support_coverage_fraction,
            )
        )
    if duplicate_overlap_threshold is None:
        return tuple(result)
    return _suppress_duplicate_support_fusions(
        result, overlap_threshold=duplicate_overlap_threshold
    )


def fusion_document(item: TrackFusion) -> dict[str, Any]:
    return {
        "track_id": item.track_id,
        "label": item.label,
        "association_group": item.association_group,
        "score": item.score,
        "bbox_xyxy": list(item.bbox_xyxy),
        "candidate_projected_points": item.candidate_points,
        "semantic_compatible_points": item.semantic_points,
        "clustered_points": item.clustered_points,
        "pre_ground_clustered_points": item.pre_ground_clustered_points,
        "ground_rejected_points": item.ground_rejected_points,
        "support_ground_z_map": item.support_ground_z_map,
        "distance_p10_m": item.distance_p10_m,
        "distance_median_m": item.distance_median_m,
        "distance_smoothed_m": item.distance_smoothed_m,
        "cuboid_status": item.status,
        "cuboid_center_map": None if item.cuboid is None else list(item.cuboid.center_map),
        "cuboid_half_size": None if item.cuboid is None else list(item.cuboid.half_size),
        "cuboid_quaternion_xyzw": None
        if item.cuboid is None
        else list(item.cuboid.quaternion_xyzw),
        "geometry": item.geometry,
        "observed_cuboid_center_map": None
        if item.observed_cuboid is None
        else list(item.observed_cuboid.center_map),
        "observed_cuboid_half_size": None
        if item.observed_cuboid is None
        else list(item.observed_cuboid.half_size),
        "observed_cuboid_quaternion_xyzw": None
        if item.observed_cuboid is None
        else list(item.observed_cuboid.quaternion_xyzw),
        "completion_fraction": item.completion_fraction,
        "ground_z_map": item.ground_z_map,
        "orientation_source": item.orientation_source,
        "temporal_status": item.temporal_status,
        "support_coverage_fraction": item.support_coverage_fraction,
    }


def clearance(points_lidar: FloatArray, profile: Mapping[str, Any]) -> dict[str, Any]:
    sectors = int(profile["sector_count"])
    empty = {
        "schema": "diagnostic-polar-obstacle-clearance/v1",
        "state": "unavailable",
        "frame": "k1-lidar",
        "front_m": None,
        "observed_sector_fraction": 0.0,
        "sector_ranges_m": [None] * sectors,
    }
    if points_lidar.size == 0:
        return empty
    ground = float(np.percentile(points_lidar[:, 2], float(profile["ground_percentile"])))
    ranges = np.linalg.norm(points_lidar[:, :2], axis=1)
    keep = (
        (ranges >= float(profile["minimum_range_m"]))
        & (ranges <= float(profile["maximum_range_m"]))
        & (points_lidar[:, 2] >= ground + float(profile["minimum_height_above_ground_m"]))
        & (points_lidar[:, 2] <= ground + float(profile["maximum_height_above_ground_m"]))
    )
    points, ranges = points_lidar[keep], ranges[keep]
    if points.size == 0:
        return {**empty, "ground_z_estimate_m": ground}
    angles = np.arctan2(points[:, 1], points[:, 0])
    indices = np.floor((angles + math.pi) / (2 * math.pi) * sectors).astype(int)
    indices = np.clip(indices, 0, sectors - 1)
    values = np.full(sectors, np.inf, dtype=np.float64)
    np.minimum.at(values, indices, ranges)
    observed = np.isfinite(values)
    front = ranges[np.abs(angles) <= math.radians(float(profile["front_half_angle_degrees"]))]
    return {
        "schema": empty["schema"],
        "state": "available",
        "frame": "k1-lidar",
        "front_m": None if front.size == 0 else float(np.min(front)),
        "observed_sector_fraction": float(np.mean(observed)),
        "sector_ranges_m": [None if not math.isfinite(value) else float(value) for value in values],
        "ground_z_estimate_m": ground,
    }


class WorldStateProjector:
    def __init__(self, history_limit_s: float) -> None:
        self.history_limit_s = history_limit_s
        self.history: dict[int, deque[tuple[float, tuple[float, float, float]]]] = {}

    def project(
        self,
        *,
        frame_index: int,
        source_frame_index: int,
        session_seconds: float,
        fusion_state: str,
        fusions: Sequence[TrackFusion],
        points_lidar: FloatArray,
        clearance_state: Mapping[str, Any],
        delivery: Mapping[str, Any],
    ) -> dict[str, Any]:
        objects: list[dict[str, Any]] = []
        for item in fusions:
            if item.cuboid is None or item.distance_smoothed_m is None:
                continue
            center = item.cuboid.center_map
            velocity, velocity_status, residual = self._velocity(
                item.track_id, session_seconds, center
            )
            lidar_center = None
            if item.source_indices.size:
                lidar_center = np.median(points_lidar[item.source_indices], axis=0)
            objects.append(
                {
                    "track_id": item.track_id,
                    "class": item.association_group,
                    "detector_label": item.label,
                    "confidence": item.score,
                    "position_map_m": list(center),
                    "position_lidar_m": None if lidar_center is None else lidar_center.tolist(),
                    "orientation_map_xyzw": list(item.cuboid.quaternion_xyzw),
                    "size_m": [2 * value for value in item.cuboid.half_size],
                    "range_m": item.distance_smoothed_m,
                    "velocity_map_mps": None if velocity is None else list(velocity),
                    "speed_mps": None
                    if velocity is None
                    else math.sqrt(sum(value * value for value in velocity)),
                    "velocity_status": velocity_status,
                    "velocity_residual_m": residual,
                    "support_points": item.clustered_points,
                    "geometry": item.geometry,
                    "completion_fraction": item.completion_fraction,
                    "ground_z_map": item.ground_z_map,
                    "orientation_source": item.orientation_source,
                    "temporal_status": item.temporal_status,
                    "support_coverage_fraction": item.support_coverage_fraction,
                }
            )
        self._prune(session_seconds)
        return {
            "schema_version": WORLD_STATE_SCHEMA,
            "frame_index": frame_index,
            "source_frame_index": source_frame_index,
            "session_seconds": session_seconds,
            "coordinate_frames": {
                "world": "k1-map",
                "sensor_relative": "k1-lidar",
                "vehicle_body": "unavailable-no-rig-to-vehicle-transform",
            },
            "fusion_state": fusion_state,
            "delivery": dict(delivery),
            "objects": objects,
            "object_count": len(objects),
            "clearance": dict(clearance_state),
        }

    def _velocity(
        self, track_id: int, now: float, center: tuple[float, float, float]
    ) -> tuple[tuple[float, float, float] | None, str, float | None]:
        history = self.history.setdefault(track_id, deque(maxlen=32))
        history.append((now, center))
        while history and history[0][0] < now - self.history_limit_s:
            history.popleft()
        if len(history) < 4 or history[-1][0] - history[0][0] < 0.4:
            return None, "unavailable-insufficient-history", None
        slopes = []
        values = list(history)
        for left, (left_time, left_center) in enumerate(values):
            for right_time, right_center in values[left + 1 :]:
                delta = right_time - left_time
                if delta >= 0.2:
                    slopes.append(
                        tuple(
                            (right_center[index] - left_center[index]) / delta for index in range(3)
                        )
                    )
        if not slopes:
            return None, "unavailable-insufficient-baseline", None
        velocity = tuple(statistics.median(value[index] for value in slopes) for index in range(3))
        latest_time, latest_center = values[-1]
        residuals = []
        for observed_time, observed in values:
            predicted = tuple(
                latest_center[index] - velocity[index] * (latest_time - observed_time)
                for index in range(3)
            )
            residuals.append(
                math.sqrt(sum((observed[index] - predicted[index]) ** 2 for index in range(3)))
            )
        residual = statistics.median(residuals)
        speed = math.sqrt(sum(value * value for value in velocity))
        if speed > 20:
            return None, "rejected-speed-bound", residual
        if residual > 0.75:
            return None, "rejected-position-residual", residual
        return velocity, "diagnostic-robust-history", residual

    def _prune(self, now: float) -> None:
        expired = [
            key
            for key, value in self.history.items()
            if not value or value[-1][0] < now - self.history_limit_s
        ]
        for key in expired:
            del self.history[key]


def distance_history(length: int) -> dict[int, deque[float]]:
    return defaultdict(lambda: deque(maxlen=length))
