from __future__ import annotations import json import threading from collections.abc import Iterator from dataclasses import replace from pathlib import Path from threading import Event import numpy as np import pytest from k1link.compute.pipeline_telemetry import PipelineTelemetryIdentity from k1link.perception.baseline import BASELINE_PROFILE_ID from k1link.perception.contracts import ( BoundingRegion2D, ClockBasis, CorridorIntersection, EvidenceBasis, EvidenceCurrentness, GridCell, HistorySample, LocalObstacleMap, MetricGeometry, ModalityOutcome, ModalityStatus, MotionState, ObjectProposal2D, ObstacleObservation, QualificationState, SourceEnvelope, TemporalObstacle, TemporalState, ThreatAssessment, ThreatDecision, TimestampBundle, ) from k1link.perception.graph import ( GraphRunResult, GraphState, ReferencePerceptionGraphV1, TerminalOutcomeType, ) from k1link.perception.providers import ( GraphAuthority, ProviderPin, ProviderRole, QueuePolicy, ReferencePerceptionGraphConfig, SourcePacket, ) from k1link.perception.recorded_source import ( DecodedRecordedSource, RecordedRavnoves00Source, RecordedSourceError, ReplayPacing, ) def _status(available: bool = True) -> ModalityStatus: return ModalityStatus( available=available, outcome=ModalityOutcome.AVAILABLE if available else ModalityOutcome.UNAVAILABLE, reason="test-available" if available else "test-unavailable", ) def _packet( sequence: int, *, image: bool = True, lidar: bool = True, source_age_ns: int = 0, ) -> SourcePacket: envelope = SourceEnvelope( source_id="RAVNOVES00", session_id="20260720T065719Z_viewer_live", frame_id=f"frame-{sequence:06d}", sequence=sequence, timestamps=TimestampBundle( utc_ns=1_786_000_000_000_000_000 + sequence, monotonic_ns=1_000 + sequence, source_ns=35_000_000_000 + sequence * 100_000_000, clock_basis=ClockBasis.RECORDED_HOST, ), source_age_ns=source_age_ns, binding_reason="test-recorded-source", calibration_id="camera-1-kb4-test", representation_id="registered-map-increment-v1", image=_status(image), registered_point_increment=_status(lidar), pose=_status(lidar), ) return SourcePacket( envelope=envelope, image_payload=("image", sequence) if image else None, registered_point_increment_payload=("points", sequence) if lidar else None, pose_payload=("pose", sequence) if lidar else None, ) class _Source: provider_id = "test-source/v1" def __init__(self, packets: tuple[SourcePacket, ...]) -> None: self.values = packets def packets(self, stop_event: Event) -> Iterator[SourcePacket]: for packet in self.values: if stop_event.is_set(): return yield packet class _Detector: provider_id = "test-detector/v1" def detect(self, packet: SourcePacket) -> tuple[ObjectProposal2D, ...]: sequence = packet.envelope.sequence return ( ObjectProposal2D( proposal_id=f"proposal-{sequence}", source_id=packet.envelope.source_id, frame_id=packet.envelope.frame_id, region=BoundingRegion2D(1.0, 2.0, 20.0, 30.0), objectness=0.9, provider_id=self.provider_id, model_id="objectness-test/v1", preprocess_id="kb4-test/v1", ), ) class _Geometry: provider_id = "test-geometry/v1" def associate( self, packet: SourcePacket, proposals: tuple[ObjectProposal2D, ...], ) -> tuple[ObstacleObservation, ...]: sequence = packet.envelope.sequence if not packet.envelope.registered_point_increment.available: return ( ObstacleObservation( observation_id=f"camera-observation-{sequence}", occupancy_key=f"camera-uncertainty-{sequence}", source_id=packet.envelope.source_id, frame_id=packet.envelope.frame_id, evidence_time_ns=packet.envelope.timestamps.source_ns, basis=EvidenceBasis.CAMERA, currentness=EvidenceCurrentness.CURRENT, occupied_support=False, source_point_ids=(), metric_geometry=None, proposal_ids=(proposals[0].proposal_id,), semantic_hint=None, reason_codes=("camera-only",), ), ) return ( ObstacleObservation( observation_id=f"observation-{sequence}", occupancy_key=f"occupancy-{sequence}", source_id=packet.envelope.source_id, frame_id=packet.envelope.frame_id, evidence_time_ns=packet.envelope.timestamps.source_ns, basis=EvidenceBasis.FUSED, currentness=EvidenceCurrentness.CURRENT, occupied_support=True, source_point_ids=(sequence,), metric_geometry=MetricGeometry( coordinate_frame="map", centroid_xyz_m=(4.0, 1.0, 0.5), range_m=4.15, covariance_diagonal_m2=(0.04, 0.04, 0.09), ), proposal_ids=(proposals[0].proposal_id,), semantic_hint=None, reason_codes=("current-qualified-points",), ), ) class _Temporal: provider_id = "test-temporal/v1" def update( self, packet: SourcePacket, observations: tuple[ObstacleObservation, ...], ) -> tuple[TemporalObstacle, ...]: if not packet.envelope.registered_point_increment.available: return () sequence = packet.envelope.sequence return ( TemporalObstacle( component_id=f"component-{sequence}", identity_scope="ephemeral", state=TemporalState.CURRENT, ttl_ns=750_000_000, last_hit_ns=packet.envelope.timestamps.source_ns, age_ns=0, association_basis="current-spatial-support", history=( HistorySample( frame_id=packet.envelope.frame_id, evidence_time_ns=packet.envelope.timestamps.source_ns, centroid_xyz_m=(4.0, 1.0, 0.5), ), ), cells=(GridCell(sequence, 0, 0),), coordinate_frame="map", last_centroid_xyz_m=(4.0, 1.0, 0.5), motion=MotionState.UNKNOWN, motion_confidence=0.0, motion_reason="insufficient-history", ), ) class _Motion: provider_id = "test-motion/v1" def estimate( self, packet: SourcePacket, obstacles: tuple[TemporalObstacle, ...], ) -> tuple[TemporalObstacle, ...]: return obstacles class _Threat: provider_id = "test-threat/v1" def assess(self, obstacle_map: LocalObstacleMap) -> tuple[ThreatAssessment, ...]: occupied = obstacle_map.occupied return tuple( ThreatAssessment( assessment_id=f"assessment-{item.component_id}", component_id=item.component_id, rig_profile_id="ravnoves00-virtual-rig/v1", corridor_profile_id="ravnoves00-virtual-corridor/v1", qualification=QualificationState.QUALIFIED, relative_speed_mps=0.0, closest_approach_m=4.0, ttc_seconds=None, corridor_intersection=CorridorIntersection.CLEAR, decision=ThreatDecision.NOT_THREAT, reason_codes=("qualified-corridor-clear",), ) for item in occupied ) def _config( capacity: int = 8, terminal_timeout_ns: int = 500_000_000, ) -> ReferencePerceptionGraphConfig: ids = { ProviderRole.SOURCE: _Source.provider_id, ProviderRole.DETECTOR: _Detector.provider_id, ProviderRole.GEOMETRY: _Geometry.provider_id, ProviderRole.TEMPORAL: _Temporal.provider_id, ProviderRole.MOTION: _Motion.provider_id, ProviderRole.THREAT: _Threat.provider_id, } return ReferencePerceptionGraphConfig( graph_id="reference-perception-graph/v1", source_profile_id=BASELINE_PROFILE_ID, providers=tuple( ProviderPin(role, provider_id, "v1", "test-revision", "a" * 64) for role, provider_id in ids.items() ), queues=tuple( QueuePolicy(stage, capacity, 80_000_000, terminal_timeout_ns) for stage in ("detector", "geometry", "temporal", "threat") ), authority=GraphAuthority(), ) class _MemoryTelemetry: def __init__(self) -> None: self.records: list[dict[str, object]] = [] self._lock = threading.Lock() def publish(self, topic: str, payload: bytes) -> None: with self._lock: self.records.append({"topic": topic, "payload": json.loads(payload)}) def _graph( source: object, *, detector: object | None = None, capacity: int = 8, terminal_timeout_ns: int = 500_000_000, telemetry: _MemoryTelemetry | None = None, ) -> ReferencePerceptionGraphV1: identity = ( PipelineTelemetryIdentity( contour_id="worker-006", agent_id="mission-core-worker", node_id="DESKTOP-OPJ8J04", lab_id="M4", run_id="m4-unit-run", source_id="RAVNOVES00", source_package_id="ravnoves00-recorded-source/v1", method_id="reference-perception-graph/v1", ) if telemetry is not None else None ) return ReferencePerceptionGraphV1( config=_config(capacity, terminal_timeout_ns), source=source, detector=detector or _Detector(), geometry=_Geometry(), temporal=_Temporal(), motion=_Motion(), threat=_Threat(), telemetry_identity=identity, telemetry_sink=telemetry, clock_ns=lambda: 10_000, ) def test_reference_graph_closes_accounting_telemetry_and_deterministic_digest() -> None: telemetry = _MemoryTelemetry() graph = _graph(_Source((_packet(0), _packet(1))), telemetry=telemetry) first = graph.run() second = graph.run() assert first.state is GraphState.STOPPED assert first.admitted_count == 2 assert [item.outcome for item in first.terminal_outcomes] == [ TerminalOutcomeType.DELIVERED, TerminalOutcomeType.DELIVERED, ] assert all( delivery.obstacle_map.accounting.source_envelopes == 1 for delivery in first.deliveries ) assert all(delivery.obstacle_map.free_space_claimed is False for delivery in first.deliveries) assert first.canonical_payload_sha256 == second.canonical_payload_sha256 assert graph.worker_threads_alive == 0 stage_ids = { record["payload"].get("stage_id") for record in telemetry.records if isinstance(record["payload"], dict) } assert {"detector", "geometry", "temporal", "threat"}.issubset(stage_ids) def test_reference_graph_marks_unavailable_stale_and_provider_failure() -> None: class FailingDetector(_Detector): def detect(self, packet: SourcePacket) -> tuple[ObjectProposal2D, ...]: if packet.envelope.sequence == 2: raise RuntimeError("deliberate provider failure") return super().detect(packet) FailingDetector.provider_id = _Detector.provider_id packets = ( _packet(0, image=False), _packet(1, source_age_ns=80_000_001), _packet(2), _packet(3), ) result = _graph(_Source(packets), detector=FailingDetector()).run() assert [item.outcome for item in result.terminal_outcomes] == [ TerminalOutcomeType.UNAVAILABLE, TerminalOutcomeType.STALE, TerminalOutcomeType.FAILED, TerminalOutcomeType.DELIVERED, ] assert len(result.deliveries) == 1 assert result.deliveries[0].sequence == 3 def test_bounded_detector_queue_has_explicit_latest_wins_supersession() -> None: detector_started = Event() source_emitted = Event() release_detector = Event() class BlockingDetector(_Detector): def detect(self, packet: SourcePacket) -> tuple[ObjectProposal2D, ...]: if packet.envelope.sequence == 0: detector_started.set() assert release_detector.wait(2) return super().detect(packet) BlockingDetector.provider_id = _Detector.provider_id class CoordinatedSource(_Source): def packets(self, stop_event: Event) -> Iterator[SourcePacket]: yield self.values[0] assert detector_started.wait(2) yield from self.values[1:] source_emitted.set() source = CoordinatedSource(tuple(_packet(index) for index in range(4))) graph = _graph( source, detector=BlockingDetector(), capacity=1, terminal_timeout_ns=1_000_000_000, ) holder: list[GraphRunResult] = [] runner = threading.Thread(target=lambda: holder.append(graph.run())) runner.start() assert source_emitted.wait(2) release_detector.set() runner.join(3) assert not runner.is_alive() outcomes = {item.sequence: item.outcome for item in holder[0].terminal_outcomes} assert outcomes[1] is TerminalOutcomeType.SUPERSEDED assert outcomes[2] is TerminalOutcomeType.SUPERSEDED assert outcomes[3] is TerminalOutcomeType.DELIVERED assert set(outcomes) == {0, 1, 2, 3} assert graph.worker_threads_alive == 0 def test_cancelled_graph_closes_threads_and_can_restart() -> None: source_waiting = Event() class RestartableSource(_Source): block = True def packets(self, stop_event: Event) -> Iterator[SourcePacket]: yield self.values[0] if self.block: source_waiting.set() stop_event.wait(2) source = RestartableSource((_packet(0),)) graph = _graph(source) holder: list[GraphRunResult] = [] runner = threading.Thread(target=lambda: holder.append(graph.run())) runner.start() assert source_waiting.wait(2) graph.cancel() runner.join(3) assert holder[0].state is GraphState.CANCELLED assert graph.worker_threads_alive == 0 source.block = False restarted = graph.run() assert restarted.state is GraphState.STOPPED assert restarted.terminal_outcomes[0].outcome is TerminalOutcomeType.DELIVERED assert graph.worker_threads_alive == 0 def _write_recorded_fixture(root: Path, *, mismatched: bool = False) -> tuple[Path, Path]: camera_path = root / "camera.jsonl" timeline_path = root / "source-pack.npz" camera_rows = [ { "schema_version": "missioncore.camera-recording-index/v1", "kind": "media", "sequence": index + 1, "host_epoch_ns": 100 + index, "host_monotonic_ns": 200 + index, "sha256": f"{index + 1:064x}", } for index in range(2) ] camera_path.write_text("\n".join(json.dumps(row) for row in camera_rows) + "\n", "utf-8") np.savez( timeline_path, frame_indices=np.asarray([0, 2 if mismatched else 1], dtype=np.int64), source_frame_indices=np.asarray([0, 1], dtype=np.int64), session_seconds=np.asarray([10.0, 10.1], dtype=np.float64), sample_available=np.asarray([True, False], dtype=np.bool_), ) return camera_path, timeline_path def test_recorded_source_reuses_one_timeline_for_1x_and_uncapped(tmp_path: Path) -> None: camera_path, timeline_path = _write_recorded_fixture(tmp_path) now = [1_000_000_000] waits: list[float] = [] def wait(stop_event: Event, seconds: float) -> bool: waits.append(seconds) now[0] += round(seconds * 1_000_000_000) return stop_event.is_set() source = RecordedRavnoves00Source( camera_index_path=camera_path, source_pack_path=timeline_path, pacing=ReplayPacing.ONE_X, expected_frame_count=2, expected_source_pack_sha256=None, clock_ns=lambda: now[0], wait=wait, ) one_x = list(source.packets(Event())) uncapped = list( RecordedRavnoves00Source( camera_index_path=camera_path, source_pack_path=timeline_path, pacing=ReplayPacing.UNCAPPED, expected_frame_count=2, expected_source_pack_sha256=None, wait=lambda _event, _seconds: pytest.fail("uncapped replay waited"), ).packets(Event()) ) assert [packet.envelope.to_dict() for packet in one_x] == [ packet.envelope.to_dict() for packet in uncapped ] assert waits == pytest.approx([0.1]) assert one_x[1].envelope.registered_point_increment.available is False assert one_x[1].registered_point_increment_payload is None def test_recorded_source_rejects_timeline_mismatch(tmp_path: Path) -> None: camera_path, timeline_path = _write_recorded_fixture(tmp_path, mismatched=True) source = RecordedRavnoves00Source( camera_index_path=camera_path, source_pack_path=timeline_path, expected_frame_count=2, expected_source_pack_sha256=None, ) with pytest.raises(RecordedSourceError, match="timeline frame index"): list(source.packets(Event())) def test_decoded_recorded_source_attaches_images_without_detector_logic(tmp_path: Path) -> None: camera_path, timeline_path = _write_recorded_fixture(tmp_path) source = RecordedRavnoves00Source( camera_index_path=camera_path, source_pack_path=timeline_path, expected_frame_count=2, expected_source_pack_sha256=None, ) class Decoder: def frames(self, stop_event: Event) -> Iterator[np.ndarray]: for value in (3, 7): if stop_event.is_set(): return yield np.full((600, 800, 3), value, dtype=np.uint8) packets = list(DecodedRecordedSource(source=source, decoder=Decoder()).packets(Event())) assert len(packets) == 2 assert isinstance(packets[0].image_payload, np.ndarray) assert int(packets[0].image_payload[0, 0, 0]) == 3 assert int(packets[1].image_payload[0, 0, 0]) == 7 def test_camera_only_path_never_invents_metric_occupancy_or_free_space() -> None: result = _graph(_Source((_packet(0, lidar=False),))).run() delivery = result.deliveries[0] assert delivery.obstacle_map.occupied == () assert delivery.obstacle_map.unknown == () assert delivery.obstacle_map.free_space_claimed is False assert len(delivery.obstacle_map.camera_uncertainty) == 1 def test_graph_config_rejects_unbounded_or_missing_stage_policy() -> None: config = _config() with pytest.raises(ValueError, match="each reference stage"): replace(config, queues=config.queues[:-1])