"""
terrain_adapter.py — Speed / PID / bias adaptation from terrain state (Issue #142).

Provides:
  TerrainState    — typed snapshot of current terrain conditions
  TerrainAdapter  — computes speed_scale, pid_scales, motor_bias
  TerrainHistory  — rolling deque of TerrainState, JSON export for route planning

Speed scale model
─────────────────
  speed_scale = roughness_scale × incline_scale × slip_scale

  roughness_scale = 1.0 − 0.6 × roughness       (0.4 at max roughness)
  incline_scale   = 1.0 − 0.5 × |incline| / max_incline   (0.5 at max incline)
  slip_scale      = 1.0 − 0.8 × slip_ratio       (0.2 at full slip)

  Result clamped to [min_speed_scale, 1.0].

PID scale model (grip-based aggressiveness)
──────────────────────────────────────────
  kp_scale = 0.70 + 0.30 × grip   (0.70 at no grip,  1.00 at full grip)
  ki_scale = 0.50 + 0.50 × grip   (0.50 at no grip,  1.00 at full grip)
  kd_scale = 0.80 + 0.20 × grip   (0.80 at no grip,  1.00 at full grip)

  On slippery surfaces, reduced P/I prevents oscillation from un-predicted
  wheel slip.  D is kept relatively high to damp any vibration.

Motor bias
──────────
  See incline_detector.motor_bias() — terrain_adapter re-exports it.

Pure module — no ROS2 dependency.
"""

from __future__ import annotations

import math
import time
from collections import deque
from dataclasses import dataclass, field
from typing import Optional


# ── TerrainState ─────────────────────────────────────────────────────────────

@dataclass
class TerrainState:
    surface_type:  str   = "unknown"   # "smooth"|"tile"|"carpet"|"grass"|"gravel"|"unknown"
    roughness:     float = 0.0         # 0.0–1.0
    incline_rad:   float = 0.0         # rad; +ve = uphill
    grip:          float = 0.6         # 0.0–1.0
    slip_ratio:    float = 0.0         # 0.0–1.0
    speed_scale:   float = 1.0         # recommended speed multiplier
    timestamp_s:   float = field(default_factory=time.monotonic)


# ── TerrainAdapter ────────────────────────────────────────────────────────────

class TerrainAdapter:
    """
    Computes adaptive outputs (speed scale, PID scales, motor bias) from a
    TerrainState.

    Parameters
    ----------
    max_incline_rad  : incline at which incline_scale reaches minimum
    min_speed_scale  : floor for speed_scale (never zero)
    """

    def __init__(
        self,
        max_incline_rad: float = 0.52,   # ≈ 30°
        min_speed_scale: float = 0.15,
    ):
        self._max_incline = max(0.01, max_incline_rad)
        self._min_scale = max(0.05, min_speed_scale)

    # ── Speed scale ───────────────────────────────────────────────────────────

    def speed_scale(self, state: TerrainState) -> float:
        """
        Combined speed-limit multiplier.  Returns value in [min_speed_scale, 1.0].
        """
        r_scale = 1.0 - 0.6 * state.roughness
        i_scale = 1.0 - 0.5 * min(1.0, abs(state.incline_rad) / self._max_incline)
        s_scale = 1.0 - 0.8 * state.slip_ratio

        combined = r_scale * i_scale * s_scale
        return max(self._min_scale, min(1.0, combined))

    # ── PID scales ────────────────────────────────────────────────────────────

    def pid_scales(self, state: TerrainState) -> dict[str, float]:
        """
        Gain multipliers keyed "kp", "ki", "kd".  All in (0, 1].
        Multiply these against the base PID gains from adaptive_pid_node.
        """
        grip = max(0.0, min(1.0, state.grip))
        return {
            "kp": 0.70 + 0.30 * grip,
            "ki": 0.50 + 0.50 * grip,
            "kd": 0.80 + 0.20 * grip,
        }

    # ── Motor bias ────────────────────────────────────────────────────────────

    @staticmethod
    def motor_bias(state: TerrainState, k_bias: float = 1.0) -> float:
        """
        Forward motor bias (m/s) for incline compensation.
        Positive = push forward on uphill.
        """
        return k_bias * math.sin(state.incline_rad)

    # ── Full state update ─────────────────────────────────────────────────────

    def apply(self, state: TerrainState) -> TerrainState:
        """Return a copy of state with speed_scale populated."""
        return TerrainState(
            surface_type=state.surface_type,
            roughness=state.roughness,
            incline_rad=state.incline_rad,
            grip=state.grip,
            slip_ratio=state.slip_ratio,
            speed_scale=self.speed_scale(state),
            timestamp_s=state.timestamp_s,
        )


# ── TerrainHistory ────────────────────────────────────────────────────────────

class TerrainHistory:
    """
    Rolling log of TerrainState snapshots for route planning.

    Tracks surface-type change events and exports them as a JSON-serialisable
    list.  The full sample buffer is kept for statistical queries.

    Parameters
    ----------
    max_entries : maximum samples kept (oldest discarded)
    """

    def __init__(self, max_entries: int = 200):
        self._entries: deque = deque(maxlen=max(1, max_entries))

    @property
    def entries(self) -> list[TerrainState]:
        return list(self._entries)

    @property
    def current(self) -> Optional[TerrainState]:
        return self._entries[-1] if self._entries else None

    def add(self, state: TerrainState) -> bool:
        """
        Append a state snapshot.

        Returns True if the surface type changed from the previous entry
        (useful for callers that want to log only type-change events).
        """
        changed = (
            not self._entries
            or self._entries[-1].surface_type != state.surface_type
        )
        self._entries.append(state)
        return changed

    def to_json_log(self) -> list[dict]:
        """
        Return a list of surface-type change events (deduped by type).
        Suitable for route planning or structured logging.

        Each entry:
            {"surface": str, "roughness": float, "incline_rad": float,
             "grip": float, "slip_ratio": float, "timestamp_s": float}
        """
        events: list[dict] = []
        prev_type: Optional[str] = None
        for s in self._entries:
            if s.surface_type != prev_type:
                events.append({
                    "surface":     s.surface_type,
                    "roughness":   round(s.roughness, 3),
                    "incline_rad": round(s.incline_rad, 4),
                    "grip":        round(s.grip, 3),
                    "slip_ratio":  round(s.slip_ratio, 3),
                    "speed_scale": round(s.speed_scale, 3),
                    "timestamp_s": round(s.timestamp_s, 2),
                })
                prev_type = s.surface_type
        return events

    def clear(self) -> None:
        self._entries.clear()
