"""
emergency_fsm.py — Master emergency FSM integrating all detectors (Issue #169).

States
──────
  NOMINAL    : normal operation; minor alerts pass through; major/critical → STOPPING.
  STOPPING   : commanding zero velocity until robot speed drops below stopped_ms.
               Critical threats skip RECOVERING → ESCALATED immediately.
  RECOVERING : RecoverySequencer executing reverse+turn sequence.
               Success → NOMINAL; gave-up → ESCALATED.
  ESCALATED  : full stop; critical alert emitted once; stays until acknowledge.

Alert actions produced by state
────────────────────────────────
  NOMINAL    : emit MINOR alert (beep only); no velocity override.
  STOPPING   : suppress nav, publish zero; emit MAJOR alert once.
  RECOVERING : suppress nav, publish recovery cmds; no new alerts.
  ESCALATED  : suppress nav, publish zero; emit CRITICAL alert once per entry.

Pure module — no ROS2 dependency.
"""

from __future__ import annotations

from dataclasses import dataclass, field
from enum import Enum
from typing import Optional

from saltybot_emergency.alert_manager import Alert, AlertLevel, AlertManager
from saltybot_emergency.recovery_sequencer import RecoveryInputs, RecoverySequencer
from saltybot_emergency.threat_detector import ThreatEvent, ThreatLevel


# ── States ────────────────────────────────────────────────────────────────────

class EmergencyState(Enum):
    NOMINAL    = "NOMINAL"
    STOPPING   = "STOPPING"
    RECOVERING = "RECOVERING"
    ESCALATED  = "ESCALATED"


# ── I/O ───────────────────────────────────────────────────────────────────────

@dataclass
class EmergencyInputs:
    threat:         ThreatEvent  # highest-severity threat this tick
    robot_speed_ms: float = 0.0  # actual speed from odometry (m/s)
    acknowledge:    bool  = False # operator cleared the escalation


@dataclass
class EmergencyOutputs:
    state:        EmergencyState    = EmergencyState.NOMINAL
    cmd_override: bool              = False   # True = emergency owns cmd_vel
    cmd_linear:   float             = 0.0
    cmd_angular:  float             = 0.0
    alert:        Optional[Alert]   = None
    e_stop:       bool              = False   # assert /saltybot/e_stop
    state_changed: bool             = False
    recovery_progress: float        = 0.0
    recovery_retry_count: int       = 0


# ── EmergencyFSM ──────────────────────────────────────────────────────────────

class EmergencyFSM:
    """
    Master emergency FSM.

    Owns an AlertManager and a RecoverySequencer; coordinates them each tick.

    Parameters
    ----------
    stopped_ms              : speed below which robot is considered stopped (m/s)
    major_count_threshold   : MAJOR events within window before escalation
    escalation_window_s     : sliding window for escalation (s)
    suppression_s           : alert de-bounce period (s)
    reverse_speed_ms        : reverse speed during recovery (m/s)
    reverse_distance_m      : reverse travel per cycle (m)
    angular_speed_rads      : turn speed during recovery (rad/s)
    turn_angle_rad          : turn per cycle (rad)
    retry_timeout_s         : time in RETRYING before next cycle (s)
    clear_hold_s            : clear duration required to declare success (s)
    max_retries             : recovery cycles before GAVE_UP
    """

    def __init__(
        self,
        stopped_ms:            float = 0.03,
        major_count_threshold: int   = 3,
        escalation_window_s:   float = 10.0,
        suppression_s:         float = 1.0,
        reverse_speed_ms:      float = -0.15,
        reverse_distance_m:    float = 0.30,
        angular_speed_rads:    float = 0.60,
        turn_angle_rad:        float = 1.5708,
        retry_timeout_s:       float = 3.0,
        clear_hold_s:          float = 0.5,
        max_retries:           int   = 3,
    ):
        self._stopped_ms = max(0.0, stopped_ms)
        self._alert_mgr  = AlertManager(
            major_count_threshold=major_count_threshold,
            escalation_window_s=escalation_window_s,
            suppression_s=suppression_s,
        )
        self._recovery = RecoverySequencer(
            reverse_speed_ms=reverse_speed_ms,
            reverse_distance_m=reverse_distance_m,
            angular_speed_rads=angular_speed_rads,
            turn_angle_rad=turn_angle_rad,
            retry_timeout_s=retry_timeout_s,
            clear_hold_s=clear_hold_s,
            max_retries=max_retries,
        )
        self._state            = EmergencyState.NOMINAL
        self._critical_pending = False   # STOPPING → ESCALATED (not RECOVERING)
        self._escalation_alerted = False # CRITICAL alert emitted once per ESCALATED entry

    # ── Public API ────────────────────────────────────────────────────────────

    @property
    def state(self) -> EmergencyState:
        return self._state

    def reset(self) -> None:
        self._state              = EmergencyState.NOMINAL
        self._critical_pending   = False
        self._escalation_alerted = False
        self._alert_mgr.reset()
        self._recovery.reset()

    def tick(self, inputs: EmergencyInputs) -> EmergencyOutputs:
        prev = self._state
        out  = self._step(inputs)
        out.state         = self._state
        out.state_changed = (self._state != prev)
        return out

    # ── Step ─────────────────────────────────────────────────────────────────

    def _step(self, inp: EmergencyInputs) -> EmergencyOutputs:
        out = EmergencyOutputs(state=self._state)

        # Run alert manager for this threat
        alert = self._alert_mgr.update(inp.threat)

        # ── NOMINAL ───────────────────────────────────────────────────────────
        if self._state == EmergencyState.NOMINAL:
            if inp.threat.level == ThreatLevel.CRITICAL:
                self._state            = EmergencyState.STOPPING
                self._critical_pending = True
                out.alert              = alert
                out.cmd_override       = True   # start overriding on entry tick
            elif inp.threat.level == ThreatLevel.MAJOR:
                self._state            = EmergencyState.STOPPING
                self._critical_pending = False
                out.alert              = alert
                out.cmd_override       = True   # start overriding on entry tick
            elif inp.threat.level == ThreatLevel.MINOR:
                # Advisory only — no override
                out.alert = alert

        # ── STOPPING ──────────────────────────────────────────────────────────
        elif self._state == EmergencyState.STOPPING:
            out.cmd_override = True
            out.cmd_linear   = 0.0
            out.cmd_angular  = 0.0
            # Upgrade to critical if new critical arrives
            if inp.threat.level == ThreatLevel.CRITICAL:
                self._critical_pending = True
            if abs(inp.robot_speed_ms) <= self._stopped_ms:
                if self._critical_pending:
                    self._state              = EmergencyState.ESCALATED
                    self._escalation_alerted = False
                else:
                    self._state = EmergencyState.RECOVERING
                    self._recovery.reset()
                    self._recovery.tick(RecoveryInputs(trigger=True, dt=0.0))

        # ── RECOVERING ────────────────────────────────────────────────────────
        elif self._state == EmergencyState.RECOVERING:
            threat_cleared = inp.threat.level == ThreatLevel.CLEAR
            rec_inp = RecoveryInputs(
                trigger=False,
                threat_cleared=threat_cleared,
                dt=0.02,   # nominal dt; node should pass actual dt
            )
            rec_out = self._recovery.tick(rec_inp)

            out.cmd_override          = True
            out.cmd_linear            = rec_out.cmd_linear
            out.cmd_angular           = rec_out.cmd_angular
            out.recovery_progress     = rec_out.progress
            out.recovery_retry_count  = rec_out.retry_count

            if rec_out.gave_up:
                self._state              = EmergencyState.ESCALATED
                self._escalation_alerted = False
            elif rec_out.state.value == "IDLE" and not inp.trigger if hasattr(inp, "trigger") else True:
                # RecoverySequencer returned to IDLE = success
                from saltybot_emergency.recovery_sequencer import RecoveryState
                if self._recovery.state == RecoveryState.IDLE and not rec_out.gave_up:
                    self._state = EmergencyState.NOMINAL
                    self._recovery.reset()

        # ── ESCALATED ─────────────────────────────────────────────────────────
        elif self._state == EmergencyState.ESCALATED:
            out.cmd_override = True
            out.cmd_linear   = 0.0
            out.cmd_angular  = 0.0
            out.e_stop       = True
            if not self._escalation_alerted:
                # Force a CRITICAL alert regardless of suppression
                from saltybot_emergency.alert_manager import Alert, AlertLevel
                out.alert = Alert(
                    level=AlertLevel.CRITICAL,
                    source=inp.threat.threat_type.value,
                    message=f"[CRITICAL] ESCALATED: {inp.threat.detail or 'Recovery gave up'}",
                    timestamp_s=inp.threat.timestamp_s,
                )
                self._escalation_alerted = True
            if inp.acknowledge:
                self._state              = EmergencyState.NOMINAL
                self._critical_pending   = False
                self._escalation_alerted = False
                out.e_stop               = False
                self._alert_mgr.reset()
                self._recovery.reset()

        return out
