NODEDC_MISSION_CORE/tests/test_sensor_representation.py

from __future__ import annotations

from dataclasses import replace

import pytest

from k1link.compute.lidar_contract import K1_LIVE_LIDAR_PROFILE
from k1link.compute.sensor_representation import (
    K1_LIO_PCL_CAPABILITIES,
    SensorAlgorithmRequirements,
    SensorCapability,
    SensorRepresentationCapabilities,
    SensorRepresentationContractError,
    SensorRepresentationKind,
    SensorSourceCurrentness,
    assess_sensor_algorithm,
    require_sensor_algorithm_admission,
)


def _projective_mapper_requirements() -> SensorAlgorithmRequirements:
    return SensorAlgorithmRequirements(
        algorithm_id="lidar-projective-mapper/v1",
        accepted_representations=(SensorRepresentationKind.NATIVE_SENSOR_SCAN,),
        required_capabilities=frozenset(
            {
                SensorCapability.METRIC_XYZ,
                SensorCapability.NATIVE_RAY_MODEL,
                SensorCapability.SENSOR_ORIGIN_PER_POINT,
                SensorCapability.RAY_CLEARING_VALID,
                SensorCapability.FREE_SPACE_EVIDENCE_VALID,
            }
        ),
    )


def test_k1_lio_pcl_capabilities_round_trip_without_free_space_or_authority() -> None:
    document = K1_LIO_PCL_CAPABILITIES.to_dict()
    restored = SensorRepresentationCapabilities.from_dict(document)

    assert restored == K1_LIO_PCL_CAPABILITIES
    assert restored.source_profile_id == K1_LIVE_LIDAR_PROFILE.profile_id
    assert document["representation_kind"] == "registered-map-increment"
    assert document["source_currentness"] == "frame-increment"
    assert document["semantics"] == {
        "absence_of_endpoints_means_free": False,
        "unknown_remains_unknown": True,
    }
    assert document["authority"] == {
        "compatibility_only": True,
        "commands_enabled": False,
        "navigation_or_safety_accepted": False,
    }
    assert not restored.supports(SensorCapability.NATIVE_RAY_MODEL)
    assert not restored.supports(SensorCapability.RAY_CLEARING_VALID)
    assert not restored.supports(SensorCapability.FREE_SPACE_EVIDENCE_VALID)


def test_k1_lio_pcl_rejects_projective_free_space_mapper() -> None:
    requirements = _projective_mapper_requirements()

    admission = assess_sensor_algorithm(K1_LIO_PCL_CAPABILITIES, requirements)

    assert admission.admitted is False
    assert admission.reasons == (
        "representation-not-accepted:registered-map-increment",
        "missing-capability:free_space_evidence_valid",
        "missing-capability:native_ray_model",
        "missing-capability:ray_clearing_valid",
        "missing-capability:sensor_origin_per_point",
    )
    with pytest.raises(
        SensorRepresentationContractError,
        match="lidar-projective-mapper/v1 rejected",
    ):
        require_sensor_algorithm_admission(K1_LIO_PCL_CAPABILITIES, requirements)


def test_k1_lio_pcl_admits_endpoint_marking_without_granting_authority() -> None:
    requirements = SensorAlgorithmRequirements(
        algorithm_id="endpoint-occupied-marking/v1",
        accepted_representations=(
            SensorRepresentationKind.REGISTERED_MAP_INCREMENT,
        ),
        required_capabilities=frozenset(
            {
                SensorCapability.METRIC_XYZ,
                SensorCapability.MAP_REGISTERED,
            }
        ),
    )

    admission = require_sensor_algorithm_admission(
        K1_LIO_PCL_CAPABILITIES,
        requirements,
    )

    assert admission.admitted is True
    assert admission.reasons == ()
    assert admission.to_dict()["authority"] == {
        "compatibility_only": True,
        "commands_enabled": False,
        "navigation_or_safety_accepted": False,
    }


def test_complete_native_scan_admits_projective_mapper() -> None:
    profile = SensorRepresentationCapabilities(
        profile_id="synthetic-native-lidar-capabilities/v1",
        source_profile_id="synthetic-native-lidar/v1",
        representation_kind=SensorRepresentationKind.NATIVE_SENSOR_SCAN,
        coordinate_frame="lidar",
        source_currentness=SensorSourceCurrentness.CURRENT_OBSERVATION,
        capabilities=frozenset(
            {
                SensorCapability.METRIC_XYZ,
                SensorCapability.METRIC_INTENSITY,
                SensorCapability.SENSOR_POSE_AVAILABLE,
                SensorCapability.PER_POINT_TIME,
                SensorCapability.RING_OR_CHANNEL,
                SensorCapability.SEPARATE_IMU,
                SensorCapability.SHARED_HARDWARE_CLOCK,
                SensorCapability.NATIVE_RAY_MODEL,
                SensorCapability.SENSOR_ORIGIN_PER_POINT,
                SensorCapability.RAY_CLEARING_VALID,
                SensorCapability.MOTION_COMPENSATION_VALID,
                SensorCapability.FREE_SPACE_EVIDENCE_VALID,
            }
        ),
    )

    assert require_sensor_algorithm_admission(
        profile,
        _projective_mapper_requirements(),
    ).admitted


def test_contract_rejects_invented_free_space_and_authority() -> None:
    with pytest.raises(
        SensorRepresentationContractError,
        match="free-space evidence requires admitted ray clearing",
    ):
        replace(
            K1_LIO_PCL_CAPABILITIES,
            capabilities=K1_LIO_PCL_CAPABILITIES.capabilities
            | {SensorCapability.FREE_SPACE_EVIDENCE_VALID},
        )

    document = K1_LIO_PCL_CAPABILITIES.to_dict()
    authority = document["authority"]
    assert isinstance(authority, dict)
    authority["navigation_or_safety_accepted"] = True
    with pytest.raises(
        SensorRepresentationContractError,
        match="cannot grant authority",
    ):
        SensorRepresentationCapabilities.from_dict(document)


def test_algorithm_requirements_are_strict_and_round_trip() -> None:
    requirements = _projective_mapper_requirements()
    restored = SensorAlgorithmRequirements.from_dict(requirements.to_dict())

    assert restored == requirements
    document = requirements.to_dict()
    document["on_capability_mismatch"] = "degrade"
    with pytest.raises(
        SensorRepresentationContractError,
        match="mismatch must reject",
    ):
        SensorAlgorithmRequirements.from_dict(document)