"""One-way adapters from admitted historical primitives to product contracts. The adapters do not mutate TrackGeometry/E34 documents and never upgrade held, persistent or missing evidence into current metric occupancy. """ from __future__ import annotations import math from collections.abc import Mapping import numpy as np from k1link.compute.temporal_occupied_layer import TemporalFrameProjection from k1link.compute.track_geometry import ( TrackGeometryCurrentness, TrackGeometryEvidenceState, TrackGeometryFrame, TrackGeometryMetricBasis, TrackGeometryOwnerKind, ) from .contracts import ( EvidenceBasis, EvidenceCurrentness, GridCell, HistorySample, MetricGeometry, MotionState, ObstacleObservation, TemporalObstacle, TemporalState, validate_exclusive_point_ownership, ) class PerceptionAdapterError(ValueError): """An admitted historical value cannot be represented without inventing evidence.""" def observations_from_track_geometry( frame: TrackGeometryFrame, *, source_id: str, frame_id: str, evidence_time_ns: int, proposal_ids_by_owner: Mapping[str, tuple[str, ...]] | None = None, ) -> tuple[ObstacleObservation, ...]: """Adapt TrackGeometry v1 while preserving exact point ownership and uncertainty.""" proposals = proposal_ids_by_owner or {} observations: list[ObstacleObservation] = [] for geometry in frame.geometries: source_indices = frame.point_slab.owned_source_indices(geometry.owner_key) source_point_ids = tuple(int(value) for value in source_indices.tolist()) metric_geometry: MetricGeometry | None = None currentness = _currentness(geometry.currentness) occupied_support = False if geometry.metric_basis is TrackGeometryMetricBasis.CURRENT_POINTS: if not source_point_ids or geometry.range_m is None: raise PerceptionAdapterError( "current TrackGeometry lost its qualified point support" ) owner_index = frame.point_slab.owner_keys.index(geometry.owner_key) points = frame.point_slab.points_xyz_m[ frame.point_slab.owner_indices == owner_index ].astype(np.float64, copy=False) centroid = points.mean(axis=0) covariance = points.var(axis=0) metric_geometry = MetricGeometry( coordinate_frame=frame.binding.coordinate_frame, centroid_xyz_m=(float(centroid[0]), float(centroid[1]), float(centroid[2])), range_m=float(geometry.range_m), covariance_diagonal_m2=( float(covariance[0]), float(covariance[1]), float(covariance[2]), ), ) occupied_support = True else: source_point_ids = () basis = _basis(geometry.evidence_state, geometry.owner_kind) if basis in {EvidenceBasis.CAMERA, EvidenceBasis.CONFLICT}: metric_geometry = None source_point_ids = () occupied_support = False if currentness is not EvidenceCurrentness.CURRENT: metric_geometry = None source_point_ids = () occupied_support = False reason_codes = list(geometry.reason_codes) if geometry.currentness is TrackGeometryCurrentness.PERSISTENT: reason_codes.append("persistent-model-not-product-authority") observation = ObstacleObservation( observation_id=f"{frame_id}:{geometry.owner_key}", occupancy_key=f"{frame_id}:{geometry.owner_key}", source_id=source_id, frame_id=frame_id, evidence_time_ns=evidence_time_ns, basis=basis, currentness=currentness, occupied_support=occupied_support, source_point_ids=source_point_ids, metric_geometry=metric_geometry, proposal_ids=proposals.get(geometry.owner_key, ()), semantic_hint=geometry.semantic_label, reason_codes=tuple(dict.fromkeys(reason_codes)), ) if basis is EvidenceBasis.CAMERA and not observation.proposal_ids: raise PerceptionAdapterError( "camera-only geometry requires its source proposal binding" ) observations.append(observation) result = tuple(observations) validate_exclusive_point_ownership(result) return result def temporal_obstacles_from_e34_projection( projection: TemporalFrameProjection, *, coordinate_frame: str, ttl_ns: int, ) -> tuple[TemporalObstacle, ...]: """Adapt current/held/expired E34 components without persistent identity claims.""" document = projection.document results: list[TemporalObstacle] = [] for collection_name, state in ( ("current", TemporalState.CURRENT), ("held", TemporalState.HELD), ("expired", TemporalState.EXPIRED), ): collection = document.get(collection_name) if not isinstance(collection, list): raise PerceptionAdapterError(f"E34 projection {collection_name} must be an array") for value in collection: component = _object(value, f"E34 {collection_name} component") temporal_id = _integer(component.get("temporal_id"), "E34 temporal id") age_seconds = _number( component.get("last_observed_age_seconds"), "E34 component age", ) age_ns = max(0, int(round(age_seconds * 1_000_000_000))) history = _history(component.get("history_tail")) cells = _cells(projection, component) if state is not TemporalState.EXPIRED else () centroid = ( None if not cells else _vector3(component.get("centroid_map_xyz_m"), "E34 centroid") ) labels = _semantic_labels(component.get("semantic_provenance")) results.append( TemporalObstacle( component_id=f"e34-component-{temporal_id}", identity_scope="ephemeral", state=state, ttl_ns=ttl_ns, last_hit_ns=max(0, history[-1].evidence_time_ns), age_ns=age_ns, association_basis=_safe_reason(component.get("association_reason")), history=history, cells=cells, coordinate_frame=coordinate_frame if cells else None, last_centroid_xyz_m=centroid, motion=MotionState.UNKNOWN, motion_confidence=0.0, motion_reason="motion-not-estimated", semantic_hint=labels[-1] if labels else None, ) ) component_ids = [item.component_id for item in results] if len(set(component_ids)) != len(component_ids): raise PerceptionAdapterError("E34 projection contains duplicate temporal ids") return tuple(results) def _basis( evidence_state: TrackGeometryEvidenceState, owner_kind: TrackGeometryOwnerKind, ) -> EvidenceBasis: if evidence_state is TrackGeometryEvidenceState.AGREE: return EvidenceBasis.FUSED if evidence_state is TrackGeometryEvidenceState.CONFLICT: return EvidenceBasis.CONFLICT if owner_kind is TrackGeometryOwnerKind.GEOMETRY_CLUSTER: return EvidenceBasis.LIDAR return EvidenceBasis.CAMERA def _currentness(value: TrackGeometryCurrentness) -> EvidenceCurrentness: if value is TrackGeometryCurrentness.CURRENT: return EvidenceCurrentness.CURRENT if value is TrackGeometryCurrentness.HELD: return EvidenceCurrentness.HELD return EvidenceCurrentness.STALE def _history(value: object) -> tuple[HistorySample, ...]: if not isinstance(value, list) or not value: raise PerceptionAdapterError("E34 component history must be nonempty") history: list[HistorySample] = [] for item in value[-32:]: document = _object(item, "E34 history sample") frame_index = _integer(document.get("frame_index"), "E34 history frame") session_seconds = _number(document.get("session_seconds"), "E34 history time") history.append( HistorySample( frame_id=f"e34-frame-{frame_index}", evidence_time_ns=max(0, int(round(session_seconds * 1_000_000_000))), centroid_xyz_m=_vector3( document.get("centroid_map_xyz_m"), "E34 history centroid", ), ) ) return tuple(history) def _cells( projection: TemporalFrameProjection, component: dict[str, object], ) -> tuple[GridCell, ...]: start = _integer(component.get("cell_row_start"), "E34 cell start") count = _integer(component.get("cell_row_count"), "E34 cell count") if start < 0 or count < 0 or start + count > int(projection.cell_rows.shape[0]): raise PerceptionAdapterError("E34 component cell range is invalid") rows = projection.cell_rows[start : start + count] return tuple(GridCell(int(row[0]), int(row[1]), int(row[2])) for row in rows) def _semantic_labels(value: object) -> tuple[str, ...]: document = _object(value, "E34 semantic provenance") labels = document.get("labels") if not isinstance(labels, list): raise PerceptionAdapterError("E34 semantic labels must be an array") return tuple(_safe_reason(label) for label in labels) def _safe_reason(value: object) -> str: if not isinstance(value, str) or not value: raise PerceptionAdapterError("E34 reason must be a nonempty string") return value 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 PerceptionAdapterError(f"{label} must be an object") return value def _integer(value: object, label: str) -> int: if not isinstance(value, int) or isinstance(value, bool): raise PerceptionAdapterError(f"{label} must be an integer") return value def _number(value: object, label: str) -> float: if ( not isinstance(value, (int, float)) or isinstance(value, bool) or not math.isfinite(float(value)) ): raise PerceptionAdapterError(f"{label} must be finite") return float(value) def _vector3(value: object, label: str) -> tuple[float, float, float]: if not isinstance(value, list) or len(value) != 3: raise PerceptionAdapterError(f"{label} must contain three values") return (_number(value[0], label), _number(value[1], label), _number(value[2], label))