from __future__ import annotations

import threading
import time

import pytest

from k1link.compute import (
    K1LocalSurfacePoseBinder,
    K1LocalSurfaceShadowCoordinator,
)
from k1link.compute.live_perception import LiveSensorSynchronizer
from k1link.data_plane import ConsumerFrameContext, DecodedPointCloudView, DecodedPoseView


def _context(sequence: int, epoch_ns: int) -> ConsumerFrameContext:
    return ConsumerFrameContext(
        sequence=sequence,
        captured_at_epoch_ns=epoch_ns,
        received_monotonic_ns=epoch_ns,
        processing_started_monotonic_ns=epoch_ns,
        encoded_size_bytes=1,
        live=True,
    )


def _points(sequence: int, epoch_ns: int) -> DecodedPointCloudView:
    return DecodedPointCloudView(
        context=_context(sequence, epoch_ns),
        frame_id="map",
        positions_xyz=((1.0, 2.0, 3.0),),
        intensities=b"\x01",
    )


def _pose(sequence: int, epoch_ns: int) -> DecodedPoseView:
    return DecodedPoseView(
        context=_context(sequence, epoch_ns),
        frame_id="map",
        child_frame_id="sensor",
        position_xyz=(0.0, 0.0, 0.0),
        orientation_xyzw=(0.0, 0.0, 0.0, 1.0),
    )


def test_live_sensor_synchronizer_selects_nearest_bounded_pair() -> None:
    synchronizer = LiveSensorSynchronizer(
        maximum_lidar_camera_delta_ms=100,
        maximum_pose_point_delta_ms=100,
    )
    synchronizer.publish_point_cloud(_points(1, 1_000_000_000))
    synchronizer.publish_point_cloud(_points(2, 1_090_000_000))
    synchronizer.publish_pose(_pose(1, 1_020_000_000))
    synchronizer.publish_pose(_pose(2, 1_095_000_000))

    binding = synchronizer.bind_camera(1_080_000_000)

    assert binding.state == "fused-ready"
    assert binding.point_cloud is not None
    assert binding.point_cloud.context.sequence == 2
    assert binding.pose is not None
    assert binding.pose.context.sequence == 2
    assert binding.lidar_camera_delta_ms == 10.0
    assert binding.pose_point_delta_ms == 5.0


def test_live_sensor_synchronizer_reports_each_fail_closed_boundary() -> None:
    synchronizer = LiveSensorSynchronizer(
        maximum_lidar_camera_delta_ms=20,
        maximum_pose_point_delta_ms=10,
    )
    assert synchronizer.bind_camera(1_000_000_000).state == "lidar-unavailable"

    synchronizer.publish_point_cloud(_points(1, 900_000_000))
    assert synchronizer.bind_camera(1_000_000_000).state == "lidar-camera-delta-exceeded"

    synchronizer.publish_point_cloud(_points(2, 1_000_000_000))
    assert synchronizer.bind_camera(1_000_000_000).state == "pose-unavailable"

    synchronizer.publish_pose(_pose(1, 1_100_000_000))
    assert synchronizer.bind_camera(1_000_000_000).state == "pose-point-delta-exceeded"


def test_live_sensor_synchronizer_waits_only_for_bounded_inflight_pair() -> None:
    synchronizer = LiveSensorSynchronizer(
        maximum_lidar_camera_delta_ms=100,
        maximum_pose_point_delta_ms=100,
    )

    def publish() -> None:
        time.sleep(0.02)
        synchronizer.publish_point_cloud(_points(1, 1_000_000_000))
        synchronizer.publish_pose(_pose(1, 1_000_000_000))

    thread = threading.Thread(target=publish)
    thread.start()
    started = time.monotonic()
    binding = synchronizer.bind_camera(1_000_000_000, wait_seconds=0.2)
    elapsed = time.monotonic() - started
    thread.join()

    assert binding.state == "fused-ready"
    assert 0.01 <= elapsed < 0.15


def test_live_sensor_synchronizer_storage_never_exceeds_capacity() -> None:
    synchronizer = LiveSensorSynchronizer(
        maximum_lidar_camera_delta_ms=100,
        maximum_pose_point_delta_ms=100,
        capacity_per_modality=3,
        retention_seconds=100,
    )
    for sequence in range(1, 8):
        synchronizer.publish_point_cloud(_points(sequence, sequence * 1_000_000))
        synchronizer.publish_pose(_pose(sequence, sequence * 1_000_000))

    snapshot = synchronizer.snapshot()
    assert snapshot["point_depth"] == 3
    assert snapshot["pose_depth"] == 3
    assert snapshot["evicted_points"] == 4
    assert snapshot["evicted_poses"] == 4


def test_local_surface_pose_binder_accepts_either_event_order() -> None:
    points_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
    assert points_first.publish_point_cloud(_points(1, 1_000_000_000)) == ()
    bound = points_first.publish_pose(_pose(1, 1_010_000_000))
    assert len(bound) == 1
    assert bound[0].point_cloud.context.sequence == 1
    assert bound[0].pose.context.sequence == 1
    assert bound[0].pose_binding_age_ms == 10.0

    pose_first = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
    assert pose_first.publish_pose(_pose(1, 2_000_000_000)) == ()
    bound = pose_first.publish_point_cloud(_points(1, 2_010_000_000))
    assert len(bound) == 1
    assert bound[0].pose_binding_age_ms == 10.0

    snapshot = pose_first.snapshot()
    assert snapshot["points"]["published"] == 1
    assert snapshot["points"]["bound"] == 1
    assert snapshot["points"]["missed"] == 0
    assert snapshot["authority"]["commands_enabled"] is False


def test_local_surface_pose_binder_is_bounded_and_accounts_for_misses() -> None:
    binder = K1LocalSurfacePoseBinder(
        maximum_pose_binding_ms=100,
        point_capacity=2,
        pose_capacity=3,
        retention_seconds=10,
    )
    for sequence in range(1, 6):
        assert (
            binder.publish_point_cloud(_points(sequence, 1_000_000_000 + sequence * 1_000_000))
            == ()
        )
    assert binder.flush() == ()
    snapshot = binder.snapshot()
    points = snapshot["points"]
    assert points["capacity"] == 2
    assert points["maximum_depth"] == 2
    assert points["published"] == 5
    assert points["bound"] == 0
    assert points["dropped_overflow"] == 3
    assert points["missed"] == 2
    assert (
        points["bound"] + points["dropped_overflow"] + points["missed"] + points["depth"]
        == points["published"]
    )


def test_local_surface_pose_binder_fails_closed_after_pose_deadline() -> None:
    binder = K1LocalSurfacePoseBinder(maximum_pose_binding_ms=100)
    assert binder.publish_point_cloud(_points(1, 1_000_000_000)) == ()
    assert binder.publish_pose(_pose(1, 1_200_000_000)) == ()
    snapshot = binder.snapshot()
    assert snapshot["points"]["missed"] == 1
    assert snapshot["points"]["depth"] == 0

    with pytest.raises(ValueError, match="sequence"):
        binder.publish_pose(_pose(1, 1_210_000_000))


def test_local_surface_shadow_coordinator_closes_exact_accounting() -> None:
    coordinator = K1LocalSurfaceShadowCoordinator(result_capacity=2)
    coordinator.begin_session("physical-k1-shadow-test")
    assert coordinator.publish_pose(_pose(1, 1_000_000_000)) == 0
    assert coordinator.publish_point_cloud(_points(1, 1_010_000_000)) == 1
    coordinator.close()

    snapshot = coordinator.snapshot()
    assert snapshot["closed"] is True
    assert snapshot["binder"]["points"]["bound"] == 1
    assert snapshot["runtime"]["queue"]["published"] == 1
    assert snapshot["runtime"]["queue"]["consumed"] == 1
    assert snapshot["runtime"]["results"]["failed"] == 0
    assert snapshot["authority"]["navigation_or_safety_accepted"] is False