"""Terrain Classifier — Multi-sensor classification (Issue #469)."""

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


@dataclass
class TerrainClassification:
    """Classification result with confidence."""
    surface_type: str
    confidence: float
    roughness: float
    reflectance: float
    speed_scale: float


class TerrainClassifier:
    """Multi-sensor terrain classifier using depth variance + reflectance."""

    _SURFACE_RULES = [
        ("smooth",  0.10, 0.70, 1.00, 1.00),
        ("carpet",  0.25, 0.60, 0.90, 0.90),
        ("grass",   0.40, 0.40, 0.70, 0.75),
        ("gravel",  1.00, 0.20, 0.60, 0.60),
    ]

    def __init__(
        self,
        depth_std_threshold: float = 0.02,
        reflectance_window_size: int = 10,
        hysteresis_count: int = 5,
        control_rate_hz: float = 10.0,
    ):
        self._depth_std_threshold = depth_std_threshold
        self._hysteresis_count = hysteresis_count
        self._depth_vars = deque(maxlen=reflectance_window_size)
        self._reflectances = deque(maxlen=reflectance_window_size)
        self._current_type = "unknown"
        self._current_confidence = 0.0
        self._candidate_type = "unknown"
        self._candidate_count = 0

    @property
    def current_surface_type(self) -> str:
        return self._current_type

    @property
    def current_confidence(self) -> float:
        return self._current_confidence

    def update(self, depth_variance: float, reflectance_mean: float) -> TerrainClassification:
        """Update classifier with sensor measurements."""
        roughness = min(1.0, depth_variance / (self._depth_std_threshold + 1e-6))
        reflectance = np.clip(reflectance_mean, 0.0, 1.0)

        self._depth_vars.append(roughness)
        self._reflectances.append(reflectance)

        avg_roughness = np.mean(list(self._depth_vars)) if self._depth_vars else 0.0
        avg_reflectance = np.mean(list(self._reflectances)) if self._reflectances else 0.5

        candidate_type, candidate_confidence, speed_scale = self._classify(
            avg_roughness, avg_reflectance
        )

        if candidate_type == self._candidate_type:
            self._candidate_count += 1
        else:
            self._candidate_type = candidate_type
            self._candidate_count = 1

        if self._candidate_count >= self._hysteresis_count:
            self._current_type = candidate_type
            self._current_confidence = candidate_confidence

        return TerrainClassification(
            surface_type=self._current_type,
            confidence=self._current_confidence,
            roughness=avg_roughness,
            reflectance=avg_reflectance,
            speed_scale=speed_scale,
        )

    def reset(self) -> None:
        """Clear buffers and reset to unknown."""
        self._depth_vars.clear()
        self._reflectances.clear()
        self._current_type = "unknown"
        self._current_confidence = 0.0
        self._candidate_type = "unknown"
        self._candidate_count = 0

    def _classify(self, roughness: float, reflectance: float) -> tuple[str, float, float]:
        """Classify terrain and compute confidence + speed scale."""
        best_match = None
        best_score = 0.0
        best_speed = 1.0

        for name, rough_max, refl_min, refl_max, speed in self._SURFACE_RULES:
            rough_penalty = max(0.0, (roughness - rough_max) / rough_max) if roughness > rough_max else 0.0
            refl_penalty = 0.0

            if reflectance < refl_min:
                refl_penalty = (refl_min - reflectance) / refl_min
            elif reflectance > refl_max:
                refl_penalty = (reflectance - refl_max) / (1.0 - refl_max)

            score = 1.0 - (rough_penalty + refl_penalty) * 0.5

            if score > best_score:
                best_score = score
                best_match = name
                best_speed = speed

        confidence = max(0.0, best_score)
        return best_match or "unknown", confidence, best_speed

    @staticmethod
    def speed_scale_for_surface(surface_type: str) -> float:
        """Get speed scale for a surface type."""
        for name, _, _, _, speed in TerrainClassifier._SURFACE_RULES:
            if name == surface_type:
                return speed
        return 0.75
