from __future__ import annotations

import json
import struct

import numpy as np
import pytest

from k1link.compute.live_perception import (
    LIVE_INGRESS_WIRE_SCHEMA,
    LatestWinsQueue,
    LivePerceptionIngress,
    WorldStateProjector,
    classify_health,
    decode_live_perception_result,
    encode_live_perception_result,
)


def test_live_ingress_keeps_modalities_separately_bounded_and_ordered() -> None:
    ingress = LivePerceptionIngress()
    ingress.open_consumer("worker-1")
    ingress.begin_session("session-1")

    for sequence in range(5):
        assert ingress.publish(
            modality="camera-frame",
            source_id="sensor.camera.right",
            source_sequence=sequence,
            captured_at_epoch_ns=100 + sequence,
            received_monotonic_ns=200 + sequence,
            payload=f"camera-{sequence}".encode(),
        )
    for sequence in range(3):
        assert ingress.publish(
            modality="lidar",
            source_id="lixel/application/report/lio_pcl",
            source_sequence=sequence,
            captured_at_epoch_ns=300 + sequence,
            received_monotonic_ns=400 + sequence,
            payload=f"lidar-{sequence}".encode(),
        )
    for sequence in range(20):
        assert ingress.publish(
            modality="pose",
            source_id="lixel/application/report/lio_pose",
            source_sequence=sequence,
            captured_at_epoch_ns=500 + sequence,
            received_monotonic_ns=600 + sequence,
            payload=f"pose-{sequence}".encode(),
        )

    snapshot = ingress.snapshot()
    assert snapshot["queues"]["camera-frame"]["depth"] == 2
    assert snapshot["queues"]["camera-frame"]["dropped_overflow"] == 3
    assert snapshot["queues"]["lidar"]["depth"] == 2
    assert snapshot["queues"]["lidar"]["dropped_overflow"] == 1
    assert snapshot["queues"]["pose"]["depth"] == 16
    assert snapshot["queues"]["pose"]["dropped_overflow"] == 4

    events = []
    while (event := ingress.take_next("worker-1", timeout=0)) is not None:
        events.append(event)
    assert events[0].modality == "control"
    assert [event.ingress_sequence for event in events] == sorted(
        event.ingress_sequence for event in events
    )
    assert [event.source_sequence for event in events if event.modality == "camera-frame"] == [
        3,
        4,
    ]
    assert [event.source_sequence for event in events if event.modality == "lidar"] == [1, 2]
    assert [event.source_sequence for event in events if event.modality == "pose"] == list(
        range(4, 20)
    )


def test_live_ingress_wire_is_self_delimiting_and_explicitly_non_authoritative() -> None:
    ingress = LivePerceptionIngress()
    ingress.open_consumer("worker-1")
    ingress.begin_session("session-1")
    assert ingress.publish(
        modality="camera-init",
        source_id="sensor.camera.right",
        source_sequence=0,
        captured_at_epoch_ns=123,
        received_monotonic_ns=456,
        payload=b"init",
    )
    ingress.take_next("worker-1", timeout=0)
    event = ingress.take_next("worker-1", timeout=0)
    assert event is not None

    encoded = event.wire_bytes()
    header_bytes = struct.unpack("!I", encoded[:4])[0]
    header = json.loads(encoded[4 : 4 + header_bytes])
    assert header["schema_version"] == LIVE_INGRESS_WIRE_SCHEMA
    assert header["payload_bytes"] == 4
    assert header["commands_enabled"] is False
    assert header["navigation_or_safety_accepted"] is False
    assert encoded[4 + header_bytes :] == b"init"


def test_live_ingress_rejects_oversize_without_affecting_other_modalities() -> None:
    ingress = LivePerceptionIngress()
    ingress.begin_session("session-1")
    assert not ingress.publish(
        modality="camera-frame",
        source_id="sensor.camera.right",
        source_sequence=1,
        captured_at_epoch_ns=1,
        received_monotonic_ns=1,
        payload=b"x" * (1024 * 1024 + 1),
    )
    assert ingress.publish(
        modality="pose",
        source_id="RealtimePath",
        source_sequence=2,
        captured_at_epoch_ns=2,
        received_monotonic_ns=2,
        payload=b"pose",
    )
    snapshot = ingress.snapshot()
    assert snapshot["queues"]["camera-frame"]["rejected_oversize"] == 1
    assert snapshot["queues"]["pose"]["published"] == 1


def test_live_ingress_allows_only_one_worker_consumer() -> None:
    ingress = LivePerceptionIngress()
    ingress.open_consumer("worker-1")
    with pytest.raises(RuntimeError, match="already has a consumer"):
        ingress.open_consumer("worker-2")
    ingress.close_consumer("worker-1")
    ingress.open_consumer("worker-2")


def test_live_result_round_trip_keeps_video_mask_boxes_and_shadow_authority() -> None:
    mask = np.zeros((600, 800), dtype=np.uint8)
    mask[100:120, 200:240] = 4
    encoded = encode_live_perception_result(
        frame_index=12,
        source_frame_index=44,
        session_seconds=1.25,
        captured_at_epoch_ns=123_000_000,
        image_jpeg=b"\xff\xd8test\xff\xd9",
        segmentation_mask=mask,
        objects=[
            {
                "track_id": 7,
                "label": "car",
                "score": 0.91,
                "bbox_xyxy": [10, 20, 110, 80],
                "cuboid_center_map": [1, 2, 0.5],
                "cuboid_half_size": [2.25, 0.925, 0.775],
                "cuboid_quaternion_xyzw": [0, 0, 0, 1],
            }
        ],
        delivery={"health": "healthy", "result_age_ms": 49.0},
    )

    frame = decode_live_perception_result(encoded)

    assert frame.frame_index == 12
    assert frame.source_frame_index == 44
    assert frame.image_jpeg == b"\xff\xd8test\xff\xd9"
    assert frame.segmentation_mask is not None
    assert np.array_equal(frame.segmentation_mask, mask)
    assert frame.objects[0]["bbox_xyxy"] == [10.0, 20.0, 110.0, 80.0]
    assert frame.objects[0]["cuboid_center_map"] == [1.0, 2.0, 0.5]
    assert frame.delivery["health"] == "healthy"


def test_live_result_rejects_tampering_and_partial_cuboid() -> None:
    with pytest.raises(ValueError, match="cuboid is incomplete"):
        encode_live_perception_result(
            frame_index=0,
            source_frame_index=0,
            session_seconds=0.0,
            captured_at_epoch_ns=1,
            image_jpeg=b"\xff\xd8x\xff\xd9",
            segmentation_mask=None,
            objects=[
                {
                    "track_id": 1,
                    "label": "car",
                    "score": 0.5,
                    "bbox_xyxy": [0, 0, 1, 1],
                    "cuboid_center_map": [0, 0, 0],
                }
            ],
            delivery={"health": "degraded"},
        )
    encoded = encode_live_perception_result(
        frame_index=0,
        source_frame_index=0,
        session_seconds=0.0,
        captured_at_epoch_ns=1,
        image_jpeg=b"\xff\xd8x\xff\xd9",
        segmentation_mask=None,
        objects=[],
        delivery={"health": "healthy"},
    )
    changed = bytearray(encoded)
    changed[-3] ^= 0x01
    with pytest.raises(ValueError, match="contract is invalid"):
        decode_live_perception_result(bytes(changed))


def test_latest_wins_queue_never_exceeds_capacity() -> None:
    queue = LatestWinsQueue[int](capacity=2)
    queue.publish(1)
    queue.publish(2)
    queue.publish(3)

    assert queue.take_next(timeout=0) == 2
    assert queue.take_next(timeout=0) == 3
    snapshot = queue.snapshot()
    assert snapshot.maximum_depth == 2
    assert snapshot.depth == 0
    assert snapshot.dropped_overflow == 1
    assert snapshot.dropped_superseded == 0
    assert snapshot.dropped_total == 1


def test_closed_empty_queue_returns_none() -> None:
    queue = LatestWinsQueue[str](capacity=1)
    queue.close()

    assert queue.take_next(timeout=0) is None
    assert queue.snapshot().closed is True


def test_health_distinguishes_depth_degradation_from_staleness() -> None:
    assert classify_health(
        source_available=True,
        fusion_state="fused",
        result_age_ms=25.0,
        stale_after_ms=300.0,
        unavailable_after_ms=1000.0,
    ) == ("healthy", ())
    assert classify_health(
        source_available=True,
        fusion_state="depth-unavailable-sync-gate",
        result_age_ms=25.0,
        stale_after_ms=300.0,
        unavailable_after_ms=1000.0,
    ) == ("degraded", ("depth-unavailable-sync-gate",))
    assert classify_health(
        source_available=True,
        fusion_state="fused",
        result_age_ms=350.0,
        stale_after_ms=300.0,
        unavailable_after_ms=1000.0,
    ) == ("stale", ("result-age-exceeded",))
    assert classify_health(
        source_available=False,
        fusion_state="fused",
        result_age_ms=0.0,
        stale_after_ms=300.0,
        unavailable_after_ms=1000.0,
    ) == ("unavailable", ("source-unavailable",))


def test_world_state_contains_metric_position_size_range_and_velocity() -> None:
    projector = WorldStateProjector(velocity_history_limit_s=1.0)
    accepted = {
        "track_id": 7,
        "label": "car",
        "association_group": "vehicle",
        "score": 0.8,
        "clustered_points": 12,
        "distance_smoothed_m": 5.0,
        "cuboid_status": "accepted-point-supported-oriented-p05-p95",
        "cuboid_center_map": [1.0, 2.0, 0.5],
        "cuboid_half_size": [2.0, 1.0, 0.75],
        "cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
    }
    base = {
        "frame_index": 0,
        "source_frame_index": 1000,
        "session_seconds": 10.0,
        "state": "fused",
        "objects": [
            accepted,
            {**accepted, "track_id": 9, "cuboid_status": "rejected-distance-innovation"},
        ],
    }
    first = projector.project(
        frame=base,
        lidar_positions={7: [4.0, 0.0, 0.0]},
        clearance={"front_m": 4.0},
    )
    states = [first]
    for index, elapsed in enumerate((0.2, 0.4, 0.6), start=1):
        states.append(
            projector.project(
                frame={
                    **base,
                    "frame_index": index,
                    "session_seconds": 10.0 + elapsed,
                    "objects": [
                        {
                            **accepted,
                            "cuboid_center_map": [1.0 + 2.0 * elapsed, 2.0, 0.5],
                        }
                    ],
                },
                lidar_positions={7: [3.0, 0.0, 0.0]},
                clearance={"front_m": 3.0},
            )
        )
    second = states[-1]

    assert first["object_count"] == 1
    assert first["objects"][0]["size_m"] == [4.0, 2.0, 1.5]
    assert first["objects"][0]["position_lidar_m"] == [4.0, 0.0, 0.0]
    assert first["objects"][0]["velocity_map_mps"] is None
    assert second["objects"][0]["velocity_map_mps"] == pytest.approx([2.0, 0.0, 0.0])
    assert second["objects"][0]["speed_mps"] == pytest.approx(2.0)
    assert second["objects"][0]["velocity_status"] == "diagnostic-robust-history"


def test_world_state_declares_missing_vehicle_body_transform() -> None:
    state = WorldStateProjector().project(
        frame={
            "frame_index": 0,
            "source_frame_index": 1,
            "session_seconds": 1.0,
            "state": "depth-unavailable-sync-gate",
            "objects": [],
        },
        lidar_positions={},
        clearance={"state": "unavailable"},
    )

    assert state["coordinate_frames"]["sensor_relative"] == "k1-lidar"
    assert state["coordinate_frames"]["vehicle_body"].startswith("unavailable")
