from __future__ import annotations

from pathlib import Path

import pytest

from k1link.device_plugins.xgrids_k1.calibration_schema import (
    FactoryCalibrationSchemaError,
    parse_k1_factory_calibration,
)

FIXTURE_ROOT = Path(__file__).parent / "fixtures" / "k1" / "calibration"


def _documents() -> tuple[bytes, bytes]:
    return (
        (FIXTURE_ROOT / "camera.yaml").read_bytes(),
        (FIXTURE_ROOT / "extrinsic_camera_lidar.yaml").read_bytes(),
    )


def test_factory_calibration_normalizes_main_camera_mapping_and_transforms() -> None:
    calibration = parse_k1_factory_calibration(*_documents())
    profile = calibration.normalized_profile()
    streams = profile["stream_bindings"]
    assert isinstance(streams, dict)

    left = streams["sensor.camera.left"]
    right = streams["sensor.camera.right"]
    assert isinstance(left, dict)
    assert isinstance(right, dict)
    assert left["calibration_slot"] == "camera_0"
    assert right["calibration_slot"] == "camera_1"
    assert left["rtsp_path"] == "/live/chn_left_main"
    assert right["rtsp_path"] == "/live/chn_right_main"
    assert left["native_resolution"] == [4000, 3000]
    assert left["admitted_resolution"] == [800, 600]
    assert left["admitted_intrinsic_fx_fy_cx_cy"] == pytest.approx(
        [193.6, 193.6, 400.0, 300.0]
    )
    assert right["admitted_intrinsic_fx_fy_cx_cy"] == pytest.approx(
        [194.0, 194.2, 396.0, 302.0]
    )
    assert profile["transform_notation"] == "T_destination_from_source"

    right_transform = calibration.t_camera_from_lidar("camera_1")
    assert right_transform[0] == pytest.approx((-1.0, 0.0, 0.0, 0.007))
    assert right_transform[1] == pytest.approx((0.0, 0.0, -1.0, -0.0948))
    assert right_transform[2] == pytest.approx((0.0, -1.0, 0.0, -0.0328))


@pytest.mark.parametrize(
    ("mutation", "message"),
    [
        (
            lambda data: data.replace(
                b"calibrated: true\n",
                b"calibrated: true\ncalibrated: true\n",
                1,
            ),
            "safe valid YAML",
        ),
        (
            lambda data: data.replace(
                b"camera_pose: [1, 0, 0, 0,",
                b"camera_pose: &pose [1, 0, 0, 0,",
                1,
            ),
            "anchors",
        ),
        (
            lambda data: data.replace(b"intrinsic: [968,", b"intrinsic: [.nan,", 1),
            "finite",
        ),
        (
            lambda data: data.replace(b"image_width: 4000", b"image_width: 3999", 1),
            "resolution",
        ),
        (
            lambda data: data.replace(
                b"camera_model: kb4",
                b"camera_model: kb4\n  unexpected: true",
                1,
            ),
            "unexpected key set",
        ),
    ],
)
def test_factory_calibration_rejects_unsafe_or_nonprofile_camera_yaml(
    mutation: object,
    message: str,
) -> None:
    camera, extrinsic = _documents()
    mutate = mutation
    assert callable(mutate)
    with pytest.raises(FactoryCalibrationSchemaError, match=message):
        parse_k1_factory_calibration(mutate(camera), extrinsic)


def test_factory_calibration_rejects_version_or_rigid_transform_mismatch() -> None:
    camera, extrinsic = _documents()
    mismatched = extrinsic.replace(b"V2.2.0_alpha", b"V2.2.1_alpha")
    with pytest.raises(FactoryCalibrationSchemaError, match="versions do not match"):
        parse_k1_factory_calibration(camera, mismatched)

    invalid_rotation = extrinsic.replace(
        b"transform: [1, 0, 0,",
        b"transform: [2, 0, 0,",
    )
    with pytest.raises(FactoryCalibrationSchemaError, match="not orthonormal"):
        parse_k1_factory_calibration(camera, invalid_rotation)