"""LIDAR Feature Extractor — Surface characteristics from RPLIDAR."""

from __future__ import annotations
import numpy as np
from typing import Optional
from collections import deque


class LidarExtractor:
    """Extract terrain reflectance features from RPLIDAR scans."""

    def __init__(
        self,
        ground_angle_front: float = 10.0,
        ground_angle_rear: float = 170.0,
        reflectance_window: int = 20,
    ):
        self._front_angle = ground_angle_front
        self._rear_angle = ground_angle_rear
        self._refl_window = deque(maxlen=reflectance_window)

    def extract_reflectance(self, ranges: np.ndarray, intensities: np.ndarray) -> Optional[float]:
        """Extract mean reflectance from ground-hitting rays."""
        if len(ranges) == 0 or len(intensities) == 0:
            return None

        intensities_norm = intensities.astype(np.float32)
        if np.max(intensities_norm) > 1.5:
            intensities_norm /= 255.0
        else:
            intensities_norm = np.clip(intensities_norm, 0.0, 1.0)

        valid_mask = (ranges > 0.2) & (ranges < 5.0)
        valid_intensities = intensities_norm[valid_mask]

        if len(valid_intensities) < 5:
            return None

        mean_reflectance = float(np.mean(valid_intensities))
        self._refl_window.append(mean_reflectance)

        if self._refl_window:
            return float(np.mean(list(self._refl_window)))
        return mean_reflectance

    def extract_range_variance(self, ranges: np.ndarray) -> Optional[float]:
        """Extract surface variance from range measurements."""
        if len(ranges) == 0:
            return None

        valid_ranges = ranges[(ranges > 0.2) & (ranges < 5.0)]

        if len(valid_ranges) < 5:
            return None

        range_variance = float(np.var(valid_ranges))
        normalized = min(1.0, range_variance / 0.2)
        return float(normalized)
