from __future__ import annotations

import re
import threading
from collections import deque
from dataclasses import dataclass
from enum import StrEnum
from typing import Any, Final

import numpy as np
import numpy.typing as npt

from k1link.data_plane import DecodedPointCloudView, DecodedPoseView
from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
    CalibratedProjectionError,
    map_points_to_lidar,
)

LIDAR_EVIDENCE_PROFILE_SCHEMA: Final = "missioncore.lidar-evidence-profile/v1"
LIDAR_READINESS_SCHEMA: Final = "missioncore.lidar-readiness/v1"
_IDENTIFIER = re.compile(r"^[A-Za-z0-9][A-Za-z0-9._:/-]{0,159}$")

Float32Array = npt.NDArray[np.float32]


class LidarContractError(ValueError):
    """A LiDAR evidence profile or conversion violates the admitted contract."""


class LidarRepresentation(StrEnum):
    SENSOR_SCAN = "sensor-scan"
    VENDOR_MAP_INCREMENT = "vendor-map-increment"
    ACCUMULATED_MAP = "accumulated-map"


class LidarCoordinateSpace(StrEnum):
    SENSOR = "sensor"
    MAP = "map"


class LidarTimeBasis(StrEnum):
    SENSOR = "sensor"
    HOST_ARRIVAL = "host-arrival"
    CAMERA_BOUND_HOST_ARRIVAL = "camera-bound-host-arrival"


class LidarPointField(StrEnum):
    XYZ = "xyz"
    INTENSITY = "intensity"
    RING = "ring"
    RELATIVE_TIME = "relative-time"


class LidarPoseStatus(StrEnum):
    NONE = "none"
    BEST_EFFORT = "best-effort"
    SENSOR_SYNCHRONIZED = "sensor-synchronized"


class LidarPipelineStage(StrEnum):
    SCANNER_QUALITY = "scanner-quality"
    GROUND_SEGMENTATION = "ground-segmentation"
    LIDAR_3D_DETECTION = "lidar-3d-detection"
    NVIDIA_NVBLOX = "nvidia-nvblox"
    LIDAR_ODOMETRY = "lidar-odometry"
    LIDAR_INERTIAL_SLAM = "lidar-inertial-slam"
    CAMERA_LIDAR_FUSION = "camera-lidar-fusion"


class LidarReadiness(StrEnum):
    READY = "ready"
    DEGRADED = "degraded"
    BLOCKED = "blocked"


@dataclass(frozen=True, slots=True)
class LidarEvidenceProfile:
    """Describe what one LiDAR source actually provides before model selection."""

    profile_id: str
    representation: LidarRepresentation
    coordinate_space: LidarCoordinateSpace
    coordinate_frame: str
    frame_time_basis: LidarTimeBasis
    point_fields: tuple[LidarPointField, ...]
    pose_status: LidarPoseStatus
    scan_geometry_known: bool
    imu_samples_available: bool
    lidar_imu_extrinsic_available: bool
    camera_extrinsic_available: bool
    commands_enabled: bool = False
    navigation_or_safety_accepted: bool = False

    def __post_init__(self) -> None:
        _safe_identifier(self.profile_id, "LiDAR profile id")
        _safe_identifier(self.coordinate_frame, "LiDAR coordinate frame")
        if not self.point_fields or LidarPointField.XYZ not in self.point_fields:
            raise LidarContractError("LiDAR evidence must contain xyz")
        if len(self.point_fields) != len(set(self.point_fields)):
            raise LidarContractError("LiDAR point fields must be unique")
        if (
            self.coordinate_space is LidarCoordinateSpace.MAP
            and self.pose_status is LidarPoseStatus.NONE
        ):
            raise LidarContractError("map-frame LiDAR evidence requires a sensor pose")
        if self.lidar_imu_extrinsic_available and not self.imu_samples_available:
            raise LidarContractError("LiDAR/IMU extrinsic has no admitted IMU samples")
        if self.commands_enabled or self.navigation_or_safety_accepted:
            raise LidarContractError("v1 LiDAR evidence is diagnostic-only")

    def to_dict(self) -> dict[str, object]:
        return {
            "schema_version": LIDAR_EVIDENCE_PROFILE_SCHEMA,
            "profile_id": self.profile_id,
            "representation": self.representation.value,
            "coordinates": {
                "space": self.coordinate_space.value,
                "frame": self.coordinate_frame,
            },
            "time": {
                "frame_basis": self.frame_time_basis.value,
                "point_relative_time": LidarPointField.RELATIVE_TIME in self.point_fields,
            },
            "point_fields": [field.value for field in self.point_fields],
            "pose_status": self.pose_status.value,
            "scan_geometry_known": self.scan_geometry_known,
            "imu": {
                "samples_available": self.imu_samples_available,
                "lidar_extrinsic_available": self.lidar_imu_extrinsic_available,
            },
            "camera_extrinsic_available": self.camera_extrinsic_available,
            "authority": {
                "commands_enabled": self.commands_enabled,
                "navigation_or_safety_accepted": self.navigation_or_safety_accepted,
            },
        }

    @classmethod
    def from_dict(cls, value: object) -> LidarEvidenceProfile:
        document = _object(value, "LiDAR evidence profile")
        _exact_keys(
            document,
            {
                "schema_version",
                "profile_id",
                "representation",
                "coordinates",
                "time",
                "point_fields",
                "pose_status",
                "scan_geometry_known",
                "imu",
                "camera_extrinsic_available",
                "authority",
            },
            "LiDAR evidence profile",
        )
        if document.get("schema_version") != LIDAR_EVIDENCE_PROFILE_SCHEMA:
            raise LidarContractError("LiDAR evidence profile schema is incompatible")
        coordinates = _object(document.get("coordinates"), "LiDAR coordinates")
        time = _object(document.get("time"), "LiDAR time")
        imu = _object(document.get("imu"), "LiDAR IMU evidence")
        authority = _object(document.get("authority"), "LiDAR authority")
        _exact_keys(coordinates, {"space", "frame"}, "LiDAR coordinates")
        _exact_keys(time, {"frame_basis", "point_relative_time"}, "LiDAR time")
        _exact_keys(
            imu,
            {"samples_available", "lidar_extrinsic_available"},
            "LiDAR IMU evidence",
        )
        _exact_keys(
            authority,
            {"commands_enabled", "navigation_or_safety_accepted"},
            "LiDAR authority",
        )
        point_fields = _array(document, "point_fields")
        try:
            parsed_fields = tuple(
                LidarPointField(_string_value(field, "LiDAR point field"))
                for field in point_fields
            )
            profile = cls(
                profile_id=_string(document, "profile_id"),
                representation=LidarRepresentation(
                    _string(document, "representation")
                ),
                coordinate_space=LidarCoordinateSpace(_string(coordinates, "space")),
                coordinate_frame=_string(coordinates, "frame"),
                frame_time_basis=LidarTimeBasis(_string(time, "frame_basis")),
                point_fields=parsed_fields,
                pose_status=LidarPoseStatus(_string(document, "pose_status")),
                scan_geometry_known=_bool(document, "scan_geometry_known"),
                imu_samples_available=_bool(imu, "samples_available"),
                lidar_imu_extrinsic_available=_bool(imu, "lidar_extrinsic_available"),
                camera_extrinsic_available=_bool(
                    document,
                    "camera_extrinsic_available",
                ),
                commands_enabled=_bool(authority, "commands_enabled"),
                navigation_or_safety_accepted=_bool(
                    authority,
                    "navigation_or_safety_accepted",
                ),
            )
        except ValueError as exc:
            raise LidarContractError("LiDAR evidence profile enum is unknown") from exc
        if (
            time.get("point_relative_time")
            is not (LidarPointField.RELATIVE_TIME in parsed_fields)
        ):
            raise LidarContractError("LiDAR point-time declarations disagree")
        return profile


@dataclass(frozen=True, slots=True)
class LidarStageAssessment:
    stage: LidarPipelineStage
    readiness: LidarReadiness
    reasons: tuple[str, ...]

    def to_dict(self) -> dict[str, object]:
        return {
            "stage": self.stage.value,
            "readiness": self.readiness.value,
            "reasons": list(self.reasons),
        }


class LidarQualityMonitor:
    """Bounded scanner telemetry over decoded point frames.

    The monitor reports observed distributions and field coverage. It does not
    turn those measurements into navigation or safety acceptance.
    """

    def __init__(
        self,
        profile: LidarEvidenceProfile,
        *,
        frame_sample_capacity: int = 512,
        point_sample_capacity: int = 32_768,
        points_sampled_per_frame: int = 256,
    ) -> None:
        if (
            not 2 <= frame_sample_capacity <= 16_384
            or not 256 <= point_sample_capacity <= 1_048_576
            or not 1 <= points_sampled_per_frame <= 4096
        ):
            raise LidarContractError("LiDAR quality monitor bounds are invalid")
        self.profile = profile
        self._frame_points: deque[int] = deque(maxlen=frame_sample_capacity)
        self._frame_intervals_ms: deque[float] = deque(maxlen=frame_sample_capacity)
        self._range_samples_m: deque[float] = deque(maxlen=point_sample_capacity)
        self._intensity_samples: deque[float] = deque(maxlen=point_sample_capacity)
        self._points_sampled_per_frame = points_sampled_per_frame
        self._frames = 0
        self._points = 0
        self._intensity_frames = 0
        self._range_frames = 0
        self._range_unavailable_frames = 0
        self._nonincreasing_frame_times = 0
        self._last_frame_time_ns: int | None = None
        self._lock = threading.Lock()

    def observe(
        self,
        point_cloud: DecodedPointCloudView,
        *,
        pose: DecodedPoseView | None = None,
    ) -> None:
        if point_cloud.frame_id != self.profile.coordinate_frame:
            raise LidarContractError("LiDAR quality frame differs from its evidence profile")
        points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
        sample_indices = _uniform_sample_indices(
            point_cloud.point_count,
            self._points_sampled_per_frame,
        )
        sampled_ranges: npt.NDArray[np.float64] | None = None
        if self.profile.coordinate_space is LidarCoordinateSpace.SENSOR:
            sampled_ranges = np.linalg.norm(points[sample_indices], axis=1)
        elif pose is not None:
            if pose.frame_id != point_cloud.frame_id:
                raise LidarContractError("LiDAR quality pose uses another map frame")
            try:
                points_sensor = map_points_to_lidar(
                    points[sample_indices],
                    position_map_xyz=pose.position_xyz,
                    orientation_map_from_lidar_xyzw=pose.orientation_xyzw,
                )
            except CalibratedProjectionError as exc:
                raise LidarContractError("LiDAR quality pose is invalid") from exc
            sampled_ranges = np.linalg.norm(points_sensor, axis=1)

        intensity_samples: npt.NDArray[np.float64] | None = None
        if point_cloud.intensities is not None:
            intensities = np.frombuffer(point_cloud.intensities, dtype=np.uint8)
            intensity_samples = intensities[sample_indices].astype(np.float64) / 255.0
        elif LidarPointField.INTENSITY in self.profile.point_fields:
            raise LidarContractError("LiDAR frame dropped profile-required intensity")

        frame_time_ns = point_cloud.context.captured_at_epoch_ns
        with self._lock:
            if self._last_frame_time_ns is not None:
                delta_ns = frame_time_ns - self._last_frame_time_ns
                if delta_ns <= 0:
                    self._nonincreasing_frame_times += 1
                else:
                    self._frame_intervals_ms.append(delta_ns / 1_000_000)
            self._last_frame_time_ns = frame_time_ns
            self._frames += 1
            self._points += point_cloud.point_count
            self._frame_points.append(point_cloud.point_count)
            if intensity_samples is not None:
                self._intensity_frames += 1
                self._intensity_samples.extend(float(value) for value in intensity_samples)
            if sampled_ranges is None:
                self._range_unavailable_frames += 1
            else:
                self._range_frames += 1
                self._range_samples_m.extend(float(value) for value in sampled_ranges)

    def snapshot(self) -> dict[str, object]:
        with self._lock:
            return {
                "schema_version": "missioncore.lidar-quality-report/v1",
                "profile_id": self.profile.profile_id,
                "frames_observed": self._frames,
                "points_observed": self._points,
                "intensity_frames": self._intensity_frames,
                "range_frames": self._range_frames,
                "sensor_range_unavailable_frames": self._range_unavailable_frames,
                "nonincreasing_frame_times": self._nonincreasing_frame_times,
                "sample_bounds": {
                    "frame_capacity": self._frame_points.maxlen,
                    "point_capacity": self._range_samples_m.maxlen,
                    "points_sampled_per_frame": self._points_sampled_per_frame,
                },
                "point_count_per_frame": _distribution(self._frame_points),
                "frame_interval_ms": _distribution(self._frame_intervals_ms),
                "sensor_range_m": _distribution(self._range_samples_m),
                "intensity_0_1": _distribution(self._intensity_samples),
                "authority": {
                    "commands_enabled": False,
                    "navigation_or_safety_accepted": False,
                },
            }


def assess_lidar_profile(
    profile: LidarEvidenceProfile,
) -> tuple[LidarStageAssessment, ...]:
    """Return deterministic readiness without inferring missing sensor evidence."""

    assessments = [
        _quality_assessment(profile),
        _ground_assessment(profile),
        _detector_assessment(profile),
        _nvblox_assessment(profile),
        _odometry_assessment(profile),
        _lio_assessment(profile),
        _fusion_assessment(profile),
    ]
    return tuple(assessments)


def lidar_readiness_document(profile: LidarEvidenceProfile) -> dict[str, object]:
    return {
        "schema_version": LIDAR_READINESS_SCHEMA,
        "profile": profile.to_dict(),
        "stages": [assessment.to_dict() for assessment in assess_lidar_profile(profile)],
        "authority": {
            "commands_enabled": False,
            "navigation_or_safety_accepted": False,
        },
    }


def sensor_frame_xyzi(
    point_cloud: DecodedPointCloudView,
    pose: DecodedPoseView | None = None,
) -> Float32Array:
    """Build the finite sensor-frame XYZI tensor expected by LiDAR detectors.

    K1 `lio_pcl` positions are already in the canonical `map` frame; they are not raw
    sensor-frame sweeps. The matching `T_map_from_lidar` pose is therefore
    required to invert them. Intensity is normalized from the verified uint8
    low byte to the [0, 1] reflectance interval used by the admitted
    PointPillars baseline.
    """

    if point_cloud.intensities is None:
        raise LidarContractError("sensor-frame XYZI requires intensity")
    points = np.asarray(point_cloud.positions_xyz, dtype=np.float64).reshape((-1, 3))
    if point_cloud.frame_id == (pose.child_frame_id if pose is not None else None):
        points_sensor = points
    elif pose is not None and point_cloud.frame_id == pose.frame_id:
        try:
            points_sensor = map_points_to_lidar(
                points,
                position_map_xyz=pose.position_xyz,
                orientation_map_from_lidar_xyzw=pose.orientation_xyzw,
            )
        except CalibratedProjectionError as exc:
            raise LidarContractError("map-frame LiDAR pose is invalid") from exc
    else:
        raise LidarContractError(
            "LiDAR coordinates cannot be bound to the supplied sensor pose"
        )
    intensity = np.frombuffer(point_cloud.intensities, dtype=np.uint8).astype(np.float32)
    xyzi = np.empty((point_cloud.point_count, 4), dtype=np.float32)
    xyzi[:, :3] = points_sensor.astype(np.float32)
    xyzi[:, 3] = intensity / 255.0
    if not np.isfinite(xyzi).all():
        raise LidarContractError("sensor-frame XYZI contains non-finite values")
    return xyzi


def _quality_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    if LidarPointField.INTENSITY not in profile.point_fields:
        reasons.append("intensity-unavailable")
    if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
        reasons.append("sensor-clock-unproven")
    return _assessment(LidarPipelineStage.SCANNER_QUALITY, reasons, blocked=False)


def _ground_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    if profile.representation is not LidarRepresentation.SENSOR_SCAN:
        reasons.append("vendor-mapped-points-are-not-raw-returns")
    if profile.coordinate_space is LidarCoordinateSpace.MAP:
        reasons.append("sensor-frame-conversion-required")
    return _assessment(LidarPipelineStage.GROUND_SEGMENTATION, reasons, blocked=False)


def _detector_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    blocked = False
    if LidarPointField.INTENSITY not in profile.point_fields:
        reasons.append("admitted-pointpillars-baseline-requires-intensity")
        blocked = True
    if (
        profile.coordinate_space is LidarCoordinateSpace.MAP
        and profile.pose_status is LidarPoseStatus.NONE
    ):
        reasons.append("sensor-frame-conversion-has-no-pose")
        blocked = True
    elif profile.coordinate_space is LidarCoordinateSpace.MAP:
        reasons.append("sensor-frame-conversion-required")
    if profile.representation is not LidarRepresentation.SENSOR_SCAN:
        reasons.append("pretrained-domain-expects-sensor-scan")
    return _assessment(LidarPipelineStage.LIDAR_3D_DETECTION, reasons, blocked)


def _nvblox_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    blocked = False
    if not profile.scan_geometry_known:
        reasons.append("lidar-intrinsics-or-scan-geometry-unknown")
        blocked = True
    if profile.pose_status is LidarPoseStatus.NONE:
        reasons.append("pose-unavailable")
        blocked = True
    elif profile.pose_status is not LidarPoseStatus.SENSOR_SYNCHRONIZED:
        reasons.append("pose-is-best-effort")
    if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
        reasons.append("sensor-clock-unproven")
    return _assessment(LidarPipelineStage.NVIDIA_NVBLOX, reasons, blocked)


def _odometry_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    blocked = False
    if profile.representation is not LidarRepresentation.SENSOR_SCAN:
        reasons.append("odometry-requires-unregistered-sensor-scans")
        blocked = True
    if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
        reasons.append("sensor-clock-unproven")
    return _assessment(LidarPipelineStage.LIDAR_ODOMETRY, reasons, blocked)


def _lio_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    if profile.representation is not LidarRepresentation.SENSOR_SCAN:
        reasons.append("lio-requires-unregistered-sensor-scans")
    if LidarPointField.RELATIVE_TIME not in profile.point_fields:
        reasons.append("per-point-time-unavailable")
    if not profile.imu_samples_available:
        reasons.append("imu-samples-unavailable")
    if not profile.lidar_imu_extrinsic_available:
        reasons.append("lidar-imu-extrinsic-unavailable")
    if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
        reasons.append("sensor-clock-unproven")
    return _assessment(
        LidarPipelineStage.LIDAR_INERTIAL_SLAM,
        reasons,
        blocked=bool(reasons),
    )


def _fusion_assessment(profile: LidarEvidenceProfile) -> LidarStageAssessment:
    reasons: list[str] = []
    blocked = False
    if not profile.camera_extrinsic_available:
        reasons.append("camera-extrinsic-unavailable")
        blocked = True
    if profile.pose_status is LidarPoseStatus.NONE:
        reasons.append("pose-unavailable")
        blocked = True
    if profile.frame_time_basis is not LidarTimeBasis.SENSOR:
        reasons.append("camera-lidar-synchronization-is-best-effort")
    return _assessment(LidarPipelineStage.CAMERA_LIDAR_FUSION, reasons, blocked)


def _assessment(
    stage: LidarPipelineStage,
    reasons: list[str],
    blocked: bool,
) -> LidarStageAssessment:
    if blocked:
        readiness = LidarReadiness.BLOCKED
    elif reasons:
        readiness = LidarReadiness.DEGRADED
    else:
        readiness = LidarReadiness.READY
    return LidarStageAssessment(stage, readiness, tuple(reasons))


def _safe_identifier(value: str, label: str) -> str:
    if not isinstance(value, str) or _IDENTIFIER.fullmatch(value) is None:
        raise LidarContractError(f"{label} is not a safe identifier")
    return value


def _uniform_sample_indices(point_count: int, maximum: int) -> npt.NDArray[np.int64]:
    if point_count <= maximum:
        return np.arange(point_count, dtype=np.int64)
    return np.linspace(0, point_count - 1, maximum, dtype=np.int64)


def _distribution(values: deque[int] | deque[float]) -> dict[str, float | int | None]:
    if not values:
        return {
            "sample_count": 0,
            "minimum": None,
            "mean": None,
            "p50": None,
            "p95": None,
            "maximum": None,
        }
    array = np.asarray(values, dtype=np.float64)
    return {
        "sample_count": int(array.size),
        "minimum": float(np.min(array)),
        "mean": float(np.mean(array)),
        "p50": float(np.percentile(array, 50)),
        "p95": float(np.percentile(array, 95)),
        "maximum": float(np.max(array)),
    }


def _object(value: object, label: str) -> dict[str, Any]:
    if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
        raise LidarContractError(f"{label} must be an object")
    return value


def _exact_keys(document: dict[str, Any], expected: set[str], label: str) -> None:
    if set(document) != expected:
        raise LidarContractError(f"{label} fields are incompatible")


def _array(document: dict[str, Any], key: str) -> list[object]:
    value = document.get(key)
    if not isinstance(value, list):
        raise LidarContractError(f"{key} must be an array")
    return value


def _string(document: dict[str, Any], key: str) -> str:
    return _string_value(document.get(key), key)


def _string_value(value: object, label: str) -> str:
    if not isinstance(value, str) or not value:
        raise LidarContractError(f"{label} must be a nonempty string")
    return value


def _bool(document: dict[str, Any], key: str) -> bool:
    value = document.get(key)
    if not isinstance(value, bool):
        raise LidarContractError(f"{key} must be a boolean")
    return value


K1_LIVE_LIDAR_PROFILE: Final = LidarEvidenceProfile(
    profile_id="xgrids-k1-live-lio-pcl/v1",
    representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
    coordinate_space=LidarCoordinateSpace.MAP,
    coordinate_frame="map",
    frame_time_basis=LidarTimeBasis.HOST_ARRIVAL,
    point_fields=(LidarPointField.XYZ, LidarPointField.INTENSITY),
    pose_status=LidarPoseStatus.BEST_EFFORT,
    scan_geometry_known=False,
    imu_samples_available=False,
    lidar_imu_extrinsic_available=False,
    camera_extrinsic_available=True,
)

K1_LAB_LIDAR_PACK_V1_PROFILE: Final = LidarEvidenceProfile(
    profile_id="xgrids-k1-e10-lidar-replay-pack/v1",
    representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
    coordinate_space=LidarCoordinateSpace.MAP,
    coordinate_frame="map",
    frame_time_basis=LidarTimeBasis.CAMERA_BOUND_HOST_ARRIVAL,
    point_fields=(LidarPointField.XYZ,),
    pose_status=LidarPoseStatus.BEST_EFFORT,
    scan_geometry_known=False,
    imu_samples_available=False,
    lidar_imu_extrinsic_available=False,
    camera_extrinsic_available=True,
)

K1_LIDAR_PACK_V2_PROFILE: Final = LidarEvidenceProfile(
    profile_id="xgrids-k1-lidar-replay-pack/v2",
    representation=LidarRepresentation.VENDOR_MAP_INCREMENT,
    coordinate_space=LidarCoordinateSpace.MAP,
    coordinate_frame="map",
    frame_time_basis=LidarTimeBasis.HOST_ARRIVAL,
    point_fields=(LidarPointField.XYZ, LidarPointField.INTENSITY),
    pose_status=LidarPoseStatus.BEST_EFFORT,
    scan_geometry_known=False,
    imu_samples_available=False,
    lidar_imu_extrinsic_available=False,
    camera_extrinsic_available=True,
)
