"""
threat_detector.py — Multi-source threat detection for emergency behavior (Issue #169).

Detectors
─────────
  ObstacleDetector  : Forward-sector minimum range < stop thresholds at speed.
                      Inputs: min_range_m (pre-filtered from LaserScan forward
                      sector), cmd_speed_ms.

  FallDetector      : Extreme pitch/roll from IMU, or depth floor-drop ahead.
                      Inputs: pitch_rad, roll_rad, floor_drop_m (depth-derived;
                      0.0 if depth unavailable).

  StuckDetector     : Commanded speed vs actual speed mismatch sustained for
                      stuck_timeout_s.  Tracks elapsed time with dt argument.

  BumpDetector      : IMU acceleration jerk (|Δ|a||/dt) above threshold.
                      MAJOR at jerk_threshold_ms3, CRITICAL at
                      critical_jerk_threshold_ms3.

ThreatLevel
───────────
  CLEAR    : no threat; normal operation
  MINOR    : advisory; log/beep only
  MAJOR    : stop and execute recovery
  CRITICAL : full shutdown + MQTT escalation

Pure module — no ROS2 dependency.
"""

from __future__ import annotations

import math
import time
from dataclasses import dataclass
from enum import Enum
from typing import Optional


# ── Enumerations ──────────────────────────────────────────────────────────────

class ThreatLevel(Enum):
    CLEAR    = 0
    MINOR    = 1
    MAJOR    = 2
    CRITICAL = 3


class ThreatType(Enum):
    NONE                = "NONE"
    OBSTACLE_PROXIMITY  = "OBSTACLE_PROXIMITY"
    FALL_RISK           = "FALL_RISK"
    WHEEL_STUCK         = "WHEEL_STUCK"
    BUMP                = "BUMP"


# ── ThreatEvent ───────────────────────────────────────────────────────────────

@dataclass
class ThreatEvent:
    """Snapshot of a single detected threat."""
    threat_type:  ThreatType  = ThreatType.NONE
    level:        ThreatLevel = ThreatLevel.CLEAR
    value:        float       = 0.0    # triggering metric
    detail:       str         = ""
    timestamp_s:  float       = 0.0

    @staticmethod
    def clear(timestamp_s: float = 0.0) -> "ThreatEvent":
        return ThreatEvent(timestamp_s=timestamp_s)


_CLEAR = ThreatEvent()


# ── ObstacleDetector ─────────────────────────────────────────────────────────

class ObstacleDetector:
    """
    Obstacle proximity threat from forward-sector minimum range.

    Parameters
    ----------
    stop_distance_m      : range below which MAJOR is raised (default 0.30 m)
    critical_distance_m  : range below which CRITICAL is raised (default 0.10 m)
    min_speed_ms         : only active above this commanded speed (default 0.05 m/s)
    """

    def __init__(
        self,
        stop_distance_m:     float = 0.30,
        critical_distance_m: float = 0.10,
        min_speed_ms:        float = 0.05,
    ):
        self._stop     = max(1e-3, stop_distance_m)
        self._critical = max(1e-3, min(self._stop, critical_distance_m))
        self._min_spd  = abs(min_speed_ms)

    def update(
        self,
        min_range_m:   float,
        cmd_speed_ms:  float,
        timestamp_s:   float = 0.0,
    ) -> ThreatEvent:
        """
        Parameters
        ----------
        min_range_m   : minimum obstacle range in forward sector (m)
        cmd_speed_ms  : signed commanded forward speed (m/s)
        """
        if abs(cmd_speed_ms) < self._min_spd:
            return ThreatEvent.clear(timestamp_s)

        if min_range_m <= self._critical:
            return ThreatEvent(
                threat_type=ThreatType.OBSTACLE_PROXIMITY,
                level=ThreatLevel.CRITICAL,
                value=min_range_m,
                detail=f"Obstacle {min_range_m:.2f} m (critical zone)",
                timestamp_s=timestamp_s,
            )
        if min_range_m <= self._stop:
            return ThreatEvent(
                threat_type=ThreatType.OBSTACLE_PROXIMITY,
                level=ThreatLevel.MAJOR,
                value=min_range_m,
                detail=f"Obstacle {min_range_m:.2f} m ahead",
                timestamp_s=timestamp_s,
            )
        return ThreatEvent.clear(timestamp_s)


# ── FallDetector ──────────────────────────────────────────────────────────────

class FallDetector:
    """
    Fall / tipping risk from IMU pitch/roll and optional depth floor-drop.

    Parameters
    ----------
    minor_tilt_rad    : |pitch| or |roll| above which MINOR fires (default 0.20 rad)
    major_tilt_rad    : above which MAJOR fires (default 0.35 rad)
    critical_tilt_rad : above which CRITICAL fires (default 0.52 rad ≈ 30°)
    floor_drop_m      : depth discontinuity (m) triggering MAJOR (default 0.15 m)
    """

    def __init__(
        self,
        minor_tilt_rad:    float = 0.20,
        major_tilt_rad:    float = 0.35,
        critical_tilt_rad: float = 0.52,
        floor_drop_m:      float = 0.15,
    ):
        self._minor    = float(minor_tilt_rad)
        self._major    = float(major_tilt_rad)
        self._critical = float(critical_tilt_rad)
        self._drop     = float(floor_drop_m)

    def update(
        self,
        pitch_rad:    float,
        roll_rad:     float,
        floor_drop_m: float = 0.0,
        timestamp_s:  float = 0.0,
    ) -> ThreatEvent:
        """
        Parameters
        ----------
        pitch_rad    : forward tilt (rad); +ve = nose up
        roll_rad     : lateral tilt (rad); +ve = left side up
        floor_drop_m : depth discontinuity ahead of robot (m); 0 = not measured
        """
        tilt = max(abs(pitch_rad), abs(roll_rad))

        if tilt >= self._critical:
            return ThreatEvent(
                threat_type=ThreatType.FALL_RISK,
                level=ThreatLevel.CRITICAL,
                value=tilt,
                detail=f"Critical tilt {math.degrees(tilt):.1f}°",
                timestamp_s=timestamp_s,
            )
        if tilt >= self._major or floor_drop_m >= self._drop:
            value   = max(tilt, floor_drop_m)
            detail  = (
                f"Floor drop {floor_drop_m:.2f} m" if floor_drop_m >= self._drop
                else f"Major tilt {math.degrees(tilt):.1f}°"
            )
            return ThreatEvent(
                threat_type=ThreatType.FALL_RISK,
                level=ThreatLevel.MAJOR,
                value=value,
                detail=detail,
                timestamp_s=timestamp_s,
            )
        if tilt >= self._minor:
            return ThreatEvent(
                threat_type=ThreatType.FALL_RISK,
                level=ThreatLevel.MINOR,
                value=tilt,
                detail=f"Tilt advisory {math.degrees(tilt):.1f}°",
                timestamp_s=timestamp_s,
            )
        return ThreatEvent.clear(timestamp_s)


# ── StuckDetector ─────────────────────────────────────────────────────────────

class StuckDetector:
    """
    Wheel stall / stuck detection from cmd_vel vs odometry mismatch.

    Accumulates stuck time while |cmd| > min_cmd_ms AND |actual| < moving_ms.
    Resets when motion resumes or commanded speed drops below min_cmd_ms.

    Parameters
    ----------
    stuck_timeout_s  : accumulated stuck time before MAJOR fires (default 3.0 s)
    min_cmd_ms       : minimum commanded speed to consider stalling (0.05 m/s)
    moving_threshold_ms : actual speed above which robot is considered moving
    """

    def __init__(
        self,
        stuck_timeout_s:     float = 3.0,
        min_cmd_ms:          float = 0.05,
        moving_threshold_ms: float = 0.05,
    ):
        self._timeout   = max(0.1, stuck_timeout_s)
        self._min_cmd   = abs(min_cmd_ms)
        self._moving    = abs(moving_threshold_ms)
        self._stuck_time: float = 0.0

    @property
    def stuck_time(self) -> float:
        """Accumulated stuck duration (s)."""
        return self._stuck_time

    def update(
        self,
        cmd_speed_ms:    float,
        actual_speed_ms: float,
        dt:              float,
        timestamp_s:     float = 0.0,
    ) -> ThreatEvent:
        """
        Parameters
        ----------
        cmd_speed_ms    : commanded forward speed (m/s)
        actual_speed_ms : measured forward speed from odometry (m/s)
        dt              : elapsed time since last call (s)
        """
        commanding = abs(cmd_speed_ms) >= self._min_cmd
        moving     = abs(actual_speed_ms) >= self._moving

        if not commanding or moving:
            self._stuck_time = 0.0
            return ThreatEvent.clear(timestamp_s)

        self._stuck_time += max(0.0, dt)

        if self._stuck_time >= self._timeout:
            return ThreatEvent(
                threat_type=ThreatType.WHEEL_STUCK,
                level=ThreatLevel.MAJOR,
                value=self._stuck_time,
                detail=f"Wheels stuck for {self._stuck_time:.1f} s",
                timestamp_s=timestamp_s,
            )
        return ThreatEvent.clear(timestamp_s)

    def reset(self) -> None:
        self._stuck_time = 0.0


# ── BumpDetector ─────────────────────────────────────────────────────────────

class BumpDetector:
    """
    Collision / bump detection via IMU acceleration jerk.

    Jerk = |Δ|a|| / dt   where |a| = sqrt(ax²+ay²+az²) − g  (gravity removed)

    Parameters
    ----------
    jerk_threshold_ms3          : MAJOR at jerk above this (m/s³, default 8.0)
    critical_jerk_threshold_ms3 : CRITICAL at jerk above this (m/s³, default 25.0)
    gravity_ms2                 : gravity magnitude to subtract (default 9.81)
    """

    def __init__(
        self,
        jerk_threshold_ms3:          float = 8.0,
        critical_jerk_threshold_ms3: float = 25.0,
        gravity_ms2:                 float = 9.81,
    ):
        self._jerk_major    = float(jerk_threshold_ms3)
        self._jerk_critical = float(critical_jerk_threshold_ms3)
        self._gravity       = float(gravity_ms2)
        self._prev_dyn_mag: Optional[float] = None   # previous |a_dynamic|

    def update(
        self,
        ax: float,
        ay: float,
        az: float,
        dt: float,
        timestamp_s: float = 0.0,
    ) -> ThreatEvent:
        """
        Parameters
        ----------
        ax, ay, az : IMU linear acceleration (m/s²)
        dt         : elapsed time since last call (s)
        """
        raw_mag = math.sqrt(ax * ax + ay * ay + az * az)
        dyn_mag = abs(raw_mag - self._gravity)   # remove gravity component

        if self._prev_dyn_mag is None or dt <= 0.0:
            self._prev_dyn_mag = dyn_mag
            return ThreatEvent.clear(timestamp_s)

        jerk = abs(dyn_mag - self._prev_dyn_mag) / dt
        self._prev_dyn_mag = dyn_mag

        if jerk >= self._jerk_critical:
            return ThreatEvent(
                threat_type=ThreatType.BUMP,
                level=ThreatLevel.CRITICAL,
                value=jerk,
                detail=f"Critical impact: jerk {jerk:.1f} m/s³",
                timestamp_s=timestamp_s,
            )
        if jerk >= self._jerk_major:
            return ThreatEvent(
                threat_type=ThreatType.BUMP,
                level=ThreatLevel.MAJOR,
                value=jerk,
                detail=f"Bump detected: jerk {jerk:.1f} m/s³",
                timestamp_s=timestamp_s,
            )
        return ThreatEvent.clear(timestamp_s)

    def reset(self) -> None:
        self._prev_dyn_mag = None


# ── Utility: pick highest-severity threat ─────────────────────────────────────

def highest_threat(events: list[ThreatEvent]) -> ThreatEvent:
    """Return the ThreatEvent with the highest ThreatLevel from a list."""
    if not events:
        return _CLEAR
    return max(events, key=lambda e: e.level.value)
