from __future__ import annotations

import json
import struct
from pathlib import Path

import lz4.block
import pytest
from fastapi import APIRouter
from fastapi.routing import APIRoute

from k1link.compute import (
    K1_LIDAR_PACK_V2_PROFILE,
    LidarPipelineStage,
    LidarReadiness,
    LidarReplayError,
    LidarReplayPackV2,
    assess_lidar_profile,
    build_lidar_replay_pack_v2,
    verify_lidar_replay_equivalence,
)
from k1link.device_plugins.xgrids_k1.mqtt.capture import FRAME_HEADER, RAW_MAGIC
from k1link.web.lidar_api import build_lidar_router


def _varint(value: int) -> bytes:
    encoded = bytearray()
    while value > 0x7F:
        encoded.append((value & 0x7F) | 0x80)
        value >>= 7
    encoded.append(value)
    return bytes(encoded)


def _key(number: int, wire_type: int) -> bytes:
    return _varint((number << 3) | wire_type)


def _uint(number: int, value: int) -> bytes:
    return _key(number, 0) + _varint(value)


def _sint(number: int, value: int) -> bytes:
    zigzag = (value << 1) ^ (value >> 63)
    return _uint(number, zigzag & 0xFFFFFFFFFFFFFFFF)


def _bytes(number: int, value: bytes) -> bytes:
    return _key(number, 2) + _varint(len(value)) + value


def _fixed32(number: int, value: float) -> bytes:
    return _key(number, 5) + struct.pack("<f", value)


def _fixed64(number: int, value: float) -> bytes:
    return _key(number, 1) + struct.pack("<d", value)


def _header(sequence: int, stamp: int, scaler: int = 1000) -> bytes:
    return b"".join(
        (
            _uint(1, sequence),
            _sint(2, stamp),
            _sint(3, scaler),
            _bytes(4, b"device-redacted"),
            _bytes(5, b"session-redacted"),
        )
    )


def _pcl_payload(sequence: int, stamp: int, first_rgbi: int) -> bytes:
    point_1 = (
        _sint(1, 1000 * sequence)
        + _sint(2, -2000)
        + _sint(3, 500)
        + _uint(4, first_rgbi)
    )
    point_2 = (
        _sint(1, -250)
        + _sint(2, sequence)
        + _sint(3, 4000)
        + _uint(4, 0xAABBCC09)
    )
    report = (
        _bytes(1, _header(sequence, stamp))
        + _bytes(2, point_1)
        + _bytes(2, point_2)
    )
    compressed = lz4.block.compress(report, store_size=False)
    return _uint(3, len(report)) + _bytes(4, compressed)


def _pose_payload(sequence: int, stamp: int) -> bytes:
    position = (
        _fixed64(1, float(sequence))
        + _fixed64(2, -2.5)
        + _fixed64(3, 3.75)
    )
    orientation = (
        _fixed64(1, 0.0)
        + _fixed64(2, 0.0)
        + _fixed64(3, 0.0)
        + _fixed64(4, 1.0)
    )
    pose = _bytes(1, position) + _bytes(2, orientation)
    stamped = _sint(1, stamp + 1) + _bytes(2, pose)
    return (
        _bytes(1, _header(sequence, stamp))
        + _bytes(2, stamped)
        + _fixed32(3, 12.5)
        + _fixed32(4, 0.001)
    )


def _capture(tmp_path: Path) -> Path:
    session = tmp_path / "session-001"
    root = session / "captures" / "mqtt_live"
    root.mkdir(parents=True)
    messages = [
        ("lixel/application/report/lio_pcl", _pcl_payload(10, 1_000, 0x11223344)),
        ("lixel/application/report/lio_pose", _pose_payload(20, 1_010)),
        ("lixel/application/report/lio_pcl", _pcl_payload(11, 2_000, 0x55667788)),
        ("lixel/application/report/lio_pose", _pose_payload(21, 2_010)),
    ]
    raw = bytearray(RAW_MAGIC)
    metadata: list[str] = []
    for sequence, (topic, payload) in enumerate(messages, start=1):
        topic_bytes = topic.encode()
        raw.extend(FRAME_HEADER.pack(len(topic_bytes), len(payload)))
        raw.extend(topic_bytes)
        raw.extend(payload)
        metadata.append(
            json.dumps(
                {
                    "schema_version": 1,
                    "record_type": "message",
                    "sequence": sequence,
                    "received_at_epoch_ns": 1_000_000_000 + sequence * 10_000_000,
                    "received_monotonic_ns": 5_000_000_000 + sequence * 10_000_000,
                },
                separators=(",", ":"),
            )
        )
    capture = root / "mqtt.raw.k1mqtt"
    capture.write_bytes(raw)
    (root / "mqtt.metadata.jsonl").write_text(
        "\n".join(metadata) + "\n",
        encoding="utf-8",
    )
    (root / "mqtt.timeline.origin.json").write_text(
        json.dumps(
            {
                "schema_version": 1,
                "started_at_epoch_ns": 1_000_000_000,
                "started_monotonic_ns": 5_000_000_000,
            }
        ),
        encoding="utf-8",
    )
    return capture


def _endpoint(router: APIRouter, path: str) -> object:
    for route in router.routes:
        if isinstance(route, APIRoute) and route.path == path and "GET" in route.methods:
            return route.endpoint
    raise AssertionError(f"GET {path} route is missing")


def test_lidar_replay_v2_retains_fields_and_passes_equivalence(tmp_path: Path) -> None:
    capture = _capture(tmp_path)
    output = build_lidar_replay_pack_v2(capture, tmp_path / "packs")
    pack = LidarReplayPackV2(output)
    try:
        assert pack.identity["session_id"] == "session-001"
        assert pack.identity["lidar_evidence_profile"] == K1_LIDAR_PACK_V2_PROFILE.to_dict()
        assert pack.point_frame_count == 2
        assert pack.pose_frame_count == 2
        assert pack.point_count == 4

        point = pack.point_frame(0)
        assert point.capture_sequence == 1
        assert point.received_at_epoch_ns == 1_010_000_000
        assert point.received_monotonic_ns == 5_010_000_000
        assert point.header_seq == 10
        assert point.header_stamp == 1_000
        assert point.scaler == 1_000
        assert point.raw_xyz.tolist() == [[10_000, -2_000, 500], [-250, 10, 4_000]]
        assert point.xyz_map.tolist() == [[10.0, -2.0, 0.5], [-0.25, 0.01, 4.0]]
        assert point.rgbi.tolist() == [0x11223344, 0xAABBCC09]
        assert point.intensity.tolist() == [0x44, 0x09]
        assert point.decoded_view().intensities == bytes((0x44, 0x09))

        pose = pack.pose_frame(0)
        assert pose.capture_sequence == 2
        assert pose.header_seq == 20
        assert pose.pose_stamp == 1_011
        assert pose.position_map == (20.0, -2.5, 3.75)
        assert pose.decoded_view().frame_id == "map"

        assert pack.quality["field_retention"]["raw_rgbi"] is True
        assert pack.quality["field_retention"]["host_monotonic_ns"] is True
        assert pack.quality["sensor_range_m"]["sample_count"] == 0
        assert pack.quality["sensor_range_status"].startswith("unavailable-")
        assert pack.quality["pose_binding"]["coverage_fraction"] == pytest.approx(1.0)
        assert pack.equivalence["status"] == "passed"
        assert pack.equivalence["array_mismatches"] == 0
        rerun = verify_lidar_replay_equivalence(capture, pack)
        assert rerun["status"] == "passed"
    finally:
        pack.close()

    assert build_lidar_replay_pack_v2(capture, tmp_path / "packs") == output


def test_lidar_replay_v2_unblocks_detector_input_but_not_mapping() -> None:
    assessments = {
        item.stage: item for item in assess_lidar_profile(K1_LIDAR_PACK_V2_PROFILE)
    }
    assert (
        assessments[LidarPipelineStage.LIDAR_3D_DETECTION].readiness
        is LidarReadiness.DEGRADED
    )
    assert (
        assessments[LidarPipelineStage.LIDAR_INERTIAL_SLAM].readiness
        is LidarReadiness.BLOCKED
    )
    assert (
        assessments[LidarPipelineStage.NVIDIA_NVBLOX].readiness
        is LidarReadiness.BLOCKED
    )


def test_lidar_replay_v2_fails_closed_without_exact_host_timing(
    tmp_path: Path,
) -> None:
    capture = _capture(tmp_path)
    capture.with_name("mqtt.metadata.jsonl").unlink()
    with pytest.raises(LidarReplayError, match="exact host timing"):
        build_lidar_replay_pack_v2(capture, tmp_path / "packs")


def test_lidar_api_exposes_path_free_quality_and_equivalence(tmp_path: Path) -> None:
    capture = _capture(tmp_path)
    root = tmp_path / "packs"
    output = build_lidar_replay_pack_v2(capture, root)
    router = build_lidar_router(root_provider=lambda: root)

    catalog_route = _endpoint(router, "/api/v1/lidar/replay-packs")
    detail_route = _endpoint(router, "/api/v1/lidar/replay-packs/{pack_id}")
    catalog = catalog_route(limit=20)  # type: ignore[operator]
    detail = detail_route(pack_id=output.name)  # type: ignore[operator]

    assert catalog["schema_version"] == "missioncore.lidar-replay-pack-catalog/v1"
    assert catalog["valid_total"] == 1
    assert catalog["invalid_total"] == 0
    assert catalog["items"][0]["equivalence_status"] == "passed"
    assert detail["pack"]["pack_id"] == output.name
    assert detail["quality"]["field_retention"]["raw_rgbi"] is True
    assert detail["equivalence"]["status"] == "passed"
    assert detail["access"] == "read-only"
    serialized = repr({"catalog": catalog, "detail": detail})
    assert str(tmp_path) not in serialized
