"""
recovery_sequencer.py — Reverse + turn recovery FSM for emergency behavior (Issue #169).

Sequence
────────
  IDLE → REVERSING → TURNING → RETRYING → (IDLE on success)
                                         → (REVERSING on re-threat, retry loop)
                                         → (GAVE_UP after max_retries)

  REVERSING : command reverse at reverse_speed_ms until reverse_distance_m covered.
  TURNING   : command angular_speed_rads until turn_angle_rad covered (90°).
  RETRYING  : zero velocity; wait up to retry_timeout_s for threat to clear.
              If threat clears within clear_hold_s → back to IDLE (success).
              If timeout without clearance → start another REVERSING cycle.
              If retry_count >= max_retries → GAVE_UP.

Pure module — no ROS2 dependency.
"""

from __future__ import annotations

from dataclasses import dataclass
from enum import Enum


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

class RecoveryState(Enum):
    IDLE      = "IDLE"
    REVERSING = "REVERSING"
    TURNING   = "TURNING"
    RETRYING  = "RETRYING"
    GAVE_UP   = "GAVE_UP"


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

@dataclass
class RecoveryInputs:
    trigger:        bool  = False   # True to start (or restart) recovery
    threat_cleared: bool  = False   # True when all threats are CLEAR
    dt:             float = 0.02    # time step (s)


@dataclass
class RecoveryOutputs:
    state:         RecoveryState = RecoveryState.IDLE
    cmd_linear:    float         = 0.0   # m/s
    cmd_angular:   float         = 0.0   # rad/s
    progress:      float         = 0.0   # [0, 1] completion of current phase
    retry_count:   int           = 0
    gave_up:       bool          = False
    state_changed: bool          = False


# ── RecoverySequencer ────────────────────────────────────────────────────────

class RecoverySequencer:
    """
    Tick-based FSM for executing reverse + turn recovery sequences.

    Parameters
    ----------
    reverse_speed_ms  : backward speed during REVERSING (m/s; stored as negative)
    reverse_distance_m: total reverse travel before turning (m)
    angular_speed_rads: yaw rate during TURNING (rad/s; positive = left)
    turn_angle_rad    : total turn angle before RETRYING (rad; default π/2)
    retry_timeout_s   : max RETRYING time per attempt before next reverse cycle
    clear_hold_s      : consecutive clear time needed to declare success
    max_retries       : maximum reverse+turn attempts before GAVE_UP
    """

    def __init__(
        self,
        reverse_speed_ms:   float = -0.15,
        reverse_distance_m: float = 0.30,
        angular_speed_rads: float = 0.60,
        turn_angle_rad:     float = 1.5708,    # π/2
        retry_timeout_s:    float = 3.0,
        clear_hold_s:       float = 0.5,
        max_retries:        int   = 3,
    ):
        self._rev_speed  = min(0.0, float(reverse_speed_ms))   # ensure negative
        self._rev_dist   = max(0.01, float(reverse_distance_m))
        self._ang_speed  = abs(float(angular_speed_rads))
        self._turn_angle = max(0.01, float(turn_angle_rad))
        self._retry_tout = max(0.1, float(retry_timeout_s))
        self._clear_hold = max(0.0, float(clear_hold_s))
        self._max_retry  = max(1, int(max_retries))

        self._state        = RecoveryState.IDLE
        self._rev_done     = 0.0   # distance reversed so far
        self._turn_done    = 0.0   # angle turned so far
        self._retry_count  = 0
        self._retry_timer  = 0.0   # time spent in RETRYING
        self._clear_timer  = 0.0   # consecutive clear time in RETRYING

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

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

    @property
    def retry_count(self) -> int:
        return self._retry_count

    def reset(self) -> None:
        """Return to IDLE and clear all counters."""
        self._state       = RecoveryState.IDLE
        self._rev_done    = 0.0
        self._turn_done   = 0.0
        self._retry_count = 0
        self._retry_timer = 0.0
        self._clear_timer = 0.0

    def tick(self, inputs: RecoveryInputs) -> RecoveryOutputs:
        prev = self._state
        out  = self._step(inputs)
        out.state         = self._state
        out.retry_count   = self._retry_count
        out.state_changed = (self._state != prev)
        if out.state_changed:
            self._on_enter(self._state)
        return out

    # ── Internal step ─────────────────────────────────────────────────────────

    def _step(self, inp: RecoveryInputs) -> RecoveryOutputs:
        out = RecoveryOutputs(state=self._state)
        dt  = max(0.0, inp.dt)

        # ── IDLE ──────────────────────────────────────────────────────────────
        if self._state == RecoveryState.IDLE:
            if inp.trigger:
                self._state = RecoveryState.REVERSING

        # ── REVERSING ─────────────────────────────────────────────────────────
        elif self._state == RecoveryState.REVERSING:
            step = abs(self._rev_speed) * dt
            self._rev_done += step
            out.cmd_linear = self._rev_speed
            out.progress   = min(1.0, self._rev_done / self._rev_dist)
            if self._rev_done >= self._rev_dist:
                self._state = RecoveryState.TURNING

        # ── TURNING ───────────────────────────────────────────────────────────
        elif self._state == RecoveryState.TURNING:
            step = self._ang_speed * dt
            self._turn_done += step
            out.cmd_angular = self._ang_speed
            out.progress    = min(1.0, self._turn_done / self._turn_angle)
            if self._turn_done >= self._turn_angle:
                self._retry_count += 1
                self._state = RecoveryState.RETRYING

        # ── RETRYING ──────────────────────────────────────────────────────────
        elif self._state == RecoveryState.RETRYING:
            self._retry_timer += dt

            if inp.threat_cleared:
                self._clear_timer += dt
                if self._clear_timer >= self._clear_hold:
                    # Success — threat has cleared
                    self._state = RecoveryState.IDLE
                    return out
            else:
                self._clear_timer = 0.0

            if self._retry_timer >= self._retry_tout:
                if self._retry_count >= self._max_retry:
                    self._state  = RecoveryState.GAVE_UP
                    out.gave_up  = True
                else:
                    self._state = RecoveryState.REVERSING

        # ── GAVE_UP ───────────────────────────────────────────────────────────
        elif self._state == RecoveryState.GAVE_UP:
            # Stay in GAVE_UP until external reset()
            out.gave_up = True

        return out

    # ── Entry actions ─────────────────────────────────────────────────────────

    def _on_enter(self, state: RecoveryState) -> None:
        if state == RecoveryState.REVERSING:
            self._rev_done  = 0.0
        elif state == RecoveryState.TURNING:
            self._turn_done = 0.0
        elif state == RecoveryState.RETRYING:
            self._retry_timer = 0.0
            self._clear_timer = 0.0
