from __future__ import annotations

import threading
import time

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