from __future__ import annotations

import pytest

from k1link.compute.rig_geometry import (
    RIG_GEOMETRY_SCHEMA,
    RigGeometryError,
    parse_rig_geometry,
)


def _unbound() -> dict[str, object]:
    return {
        "schema_version": RIG_GEOMETRY_SCHEMA,
        "profile_id": "portable-k1/unbound-v1",
        "rig_kind": "portable",
        "qualification": {
            "state": "unbound",
            "reason": "no measured mount",
            "evidence_sha256": [],
        },
        "coordinate_frames": {
            "lidar_frame": "k1-lidar",
            "body_frame": None,
            "body_from_lidar": None,
        },
        "vehicle_body": None,
        "authority": {
            "commands_enabled": False,
            "navigation_or_safety_accepted": False,
        },
    }


def test_unbound_portable_rig_fails_closed_without_inventing_geometry() -> None:
    geometry = parse_rig_geometry(_unbound())

    assert geometry.metric_body_geometry_available is False
    assert geometry.collision_geometry_qualified is False
    assert geometry.collision_contract()["state"] == "unavailable"
    assert geometry.collision_contract()["recent_collision_publishable"] is False


def test_unbound_rig_rejects_hidden_physical_values() -> None:
    value = _unbound()
    value["coordinate_frames"] = {
        "lidar_frame": "k1-lidar",
        "body_frame": "vehicle-body",
        "body_from_lidar": None,
    }

    with pytest.raises(RigGeometryError, match="must not contain physical values"):
        parse_rig_geometry(value)


def test_measured_mount_does_not_grant_collision_authority() -> None:
    value = _unbound()
    value.update(
        {
            "profile_id": "measured-rig/v1",
            "rig_kind": "vehicle-mounted",
            "qualification": {
                "state": "measured",
                "reason": "bench measurement only",
                "evidence_sha256": ["a" * 64],
                "uncertainty": {
                    "translation_1sigma_m": 0.01,
                    "rotation_1sigma_deg": 0.2,
                    "body_dimension_1sigma_m": 0.01,
                },
            },
            "coordinate_frames": {
                "lidar_frame": "k1-lidar",
                "body_frame": "vehicle-body",
                "body_from_lidar": {
                    "from_frame": "k1-lidar",
                    "to_frame": "vehicle-body",
                    "translation_m": [0.0, 0.0, 1.0],
                    "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
                },
            },
            "vehicle_body": {
                "frame": "vehicle-body",
                "shape": "axis-aligned-box",
                "minimum_xyz_m": [-1.0, -0.5, -0.2],
                "maximum_xyz_m": [1.0, 0.5, 1.2],
            },
        }
    )

    geometry = parse_rig_geometry(value)

    assert geometry.metric_body_geometry_available is True
    assert geometry.collision_geometry_qualified is False
    assert geometry.collision_contract()["state"] == "unavailable"