"""
_camera_power_manager.py — Adaptive camera power mode FSM (Issue #375).

Pure-Python library (no ROS2 / hardware dependencies) for full unit-test
coverage without hardware.

Five Power Modes
----------------
SLEEP  (0) — charging / idle.
    Sensors: none (or 1 CSI at 1 fps as wake trigger).
    ~150 MB RAM.

SOCIAL (1) — parked / socialising.
    Sensors: C920 webcam + face UI.
    ~400 MB RAM.

AWARE  (2) — indoor, slow walking, <5 km/h.
    Sensors: front CSI + RealSense + LIDAR.
    ~850 MB RAM.

ACTIVE (3) — sidewalk / bike path, 5–15 km/h.
    Sensors: front+rear CSI + RealSense + LIDAR + UWB.
    ~1.15 GB RAM.

FULL   (4) — street / high-speed >15 km/h, or crossing.
    Sensors: all 4 CSI + RealSense + LIDAR + UWB.
    ~1.55 GB RAM.

Transition Logic
----------------
Automatic transitions are speed-driven with hysteresis to prevent flapping:

  Upgrade thresholds (instantaneous):
    SOCIAL → AWARE  : speed ≥ 0.3 m/s (~1 km/h, any motion)
    AWARE  → ACTIVE : speed ≥ 1.4 m/s (~5 km/h)
    ACTIVE → FULL   : speed ≥ 4.2 m/s (~15 km/h)

  Downgrade thresholds (held for downgrade_hold_s before applying):
    FULL   → ACTIVE : speed < 3.6 m/s (~13 km/h, 2 km/h hysteresis)
    ACTIVE → AWARE  : speed < 1.1 m/s (~4 km/h)
    AWARE  → SOCIAL : speed < 0.1 m/s and idle ≥ idle_to_social_s

Scenario overrides (bypass hysteresis, instant):
  CROSSING  → FULL immediately and held until scenario clears
  EMERGENCY → FULL immediately (also signals speed reduction upstream)
  INDOOR    → cap at AWARE (never ACTIVE or FULL indoors)
  PARKED    → SOCIAL immediately
  OUTDOOR   → normal speed-based logic

Battery low override:
  battery_pct < battery_low_pct → cap at AWARE to save power

Safety invariant:
  Rear CSI is a safety sensor at speed — it is always on in ACTIVE and FULL.
  Any mode downgrade during CROSSING is blocked.
"""

from __future__ import annotations

import time
from dataclasses import dataclass
from enum import IntEnum
from typing import Optional


# ── Mode enum ─────────────────────────────────────────────────────────────────

class CameraMode(IntEnum):
    SLEEP  = 0
    SOCIAL = 1
    AWARE  = 2
    ACTIVE = 3
    FULL   = 4

    @property
    def label(self) -> str:
        return self.name


# ── Scenario enum ─────────────────────────────────────────────────────────────

class Scenario(str):
    """
    Known scenario strings.  The FSM accepts any string; these are the
    canonical values that trigger special behaviour.
    """
    UNKNOWN   = 'unknown'
    PARKED    = 'parked'
    INDOOR    = 'indoor'
    OUTDOOR   = 'outdoor'
    CROSSING  = 'crossing'
    EMERGENCY = 'emergency'


# ── Sensor configuration per mode ─────────────────────────────────────────────

@dataclass(frozen=True)
class ActiveSensors:
    csi_front:  bool = False
    csi_rear:   bool = False
    csi_left:   bool = False
    csi_right:  bool = False
    realsense:  bool = False
    lidar:      bool = False
    uwb:        bool = False
    webcam:     bool = False

    @property
    def active_count(self) -> int:
        return sum([
            self.csi_front, self.csi_rear, self.csi_left, self.csi_right,
            self.realsense, self.lidar, self.uwb, self.webcam,
        ])


# Canonical sensor set for each mode
MODE_SENSORS: dict[CameraMode, ActiveSensors] = {
    CameraMode.SLEEP:  ActiveSensors(),
    CameraMode.SOCIAL: ActiveSensors(webcam=True),
    CameraMode.AWARE:  ActiveSensors(csi_front=True, realsense=True, lidar=True),
    CameraMode.ACTIVE: ActiveSensors(csi_front=True, csi_rear=True,
                                     realsense=True, lidar=True, uwb=True),
    CameraMode.FULL:   ActiveSensors(csi_front=True, csi_rear=True,
                                     csi_left=True, csi_right=True,
                                     realsense=True, lidar=True, uwb=True),
}

# Speed thresholds (m/s)
_SPD_MOTION       = 0.3           # ~1 km/h — any meaningful motion
_SPD_ACTIVE_UP    = 5.0  / 3.6   # 5  km/h upgrade to ACTIVE
_SPD_FULL_UP      = 15.0 / 3.6   # 15 km/h upgrade to FULL
_SPD_ACTIVE_DOWN  = 4.0  / 3.6   # 4  km/h downgrade from ACTIVE (hysteresis gap)
_SPD_FULL_DOWN    = 13.0 / 3.6   # 13 km/h downgrade from FULL

# Scenario strings that force FULL
_FORCE_FULL = frozenset({Scenario.CROSSING, Scenario.EMERGENCY})
# Scenarios that cap the mode
_CAP_AWARE  = frozenset({Scenario.INDOOR})
_CAP_SOCIAL = frozenset({Scenario.PARKED})


# ── FSM result ────────────────────────────────────────────────────────────────

@dataclass
class ModeDecision:
    mode:               CameraMode
    sensors:            ActiveSensors
    trigger_speed_mps:  float
    trigger_scenario:   str
    scenario_override:  bool


# ── FSM ───────────────────────────────────────────────────────────────────────

class CameraPowerFSM:
    """
    Finite state machine for camera power mode management.

    Parameters
    ----------
    downgrade_hold_s    : seconds a downgrade condition must persist before
                          the mode drops (hysteresis).  Default 5.0 s.
    idle_to_social_s    : seconds of near-zero speed before AWARE → SOCIAL.
                          Default 30.0 s.
    battery_low_pct     : battery level below which mode is capped at AWARE.
                          Default 20.0 %.
    clock               : callable() → float for monotonic time (injectable
                          for tests; defaults to time.monotonic).
    """

    def __init__(
        self,
        downgrade_hold_s:  float = 5.0,
        idle_to_social_s:  float = 30.0,
        battery_low_pct:   float = 20.0,
        clock=None,
    ) -> None:
        self._downgrade_hold  = downgrade_hold_s
        self._idle_to_social  = idle_to_social_s
        self._battery_low_pct = battery_low_pct
        self._clock           = clock or time.monotonic

        self._mode: CameraMode = CameraMode.SLEEP
        self._downgrade_pending_since: Optional[float] = None
        self._idle_since: Optional[float] = None

        # Last input values (for reporting)
        self._last_speed:    float = 0.0
        self._last_scenario: str   = Scenario.UNKNOWN

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

    @property
    def mode(self) -> CameraMode:
        return self._mode

    def update(
        self,
        speed_mps:    float,
        scenario:     str   = Scenario.UNKNOWN,
        battery_pct:  float = 100.0,
    ) -> ModeDecision:
        """
        Evaluate inputs and return the current mode decision.

        Parameters
        ----------
        speed_mps   : current speed in m/s (non-negative)
        scenario    : current operating scenario string
        battery_pct : battery charge level 0–100

        Returns
        -------
        ModeDecision with the resolved mode and active sensor set.
        """
        now = self._clock()
        speed_mps = max(0.0, float(speed_mps))
        self._last_speed    = speed_mps
        self._last_scenario = scenario

        scenario_override = False

        # ── 1. Hard overrides (no hysteresis) ─────────────────────────────
        if scenario in _FORCE_FULL:
            self._mode = CameraMode.FULL
            self._downgrade_pending_since = None
            self._idle_since = None
            scenario_override = True
            return self._decision(scenario, scenario_override)

        if scenario in _CAP_SOCIAL:
            self._mode = CameraMode.SOCIAL
            self._downgrade_pending_since = None
            self._idle_since = None
            scenario_override = True
            return self._decision(scenario, scenario_override)

        # ── 2. Compute desired mode from speed ────────────────────────────
        desired = self._speed_to_mode(speed_mps)

        # ── 3. Apply scenario caps ────────────────────────────────────────
        if scenario in _CAP_AWARE:
            desired = min(desired, CameraMode.AWARE)

        # ── 4. Apply battery low cap ──────────────────────────────────────
        if battery_pct < self._battery_low_pct:
            desired = min(desired, CameraMode.AWARE)

        # ── 5. Idle timer: delay AWARE → SOCIAL when briefly stopped ─────────
        # _speed_to_mode already returns SOCIAL for speed < _SPD_MOTION.
        # When in AWARE (or above) and nearly stopped, hold at AWARE until
        # the idle timer expires to avoid flapping at traffic lights.
        if speed_mps < 0.1 and self._mode >= CameraMode.AWARE:
            if self._idle_since is None:
                self._idle_since = now
            if now - self._idle_since < self._idle_to_social:
                # Timer not yet expired — enforce minimum AWARE
                desired = max(desired, CameraMode.AWARE)
            # else: timer expired — let desired remain SOCIAL naturally
        else:
            self._idle_since = None

        # ── 6. Apply hysteresis for downgrades ────────────────────────────
        if desired < self._mode:
            # Downgrade requested — start hold timer on first detection, then
            # check on every call (including the first, so hold=0 is instant).
            if self._downgrade_pending_since is None:
                self._downgrade_pending_since = now
            if now - self._downgrade_pending_since >= self._downgrade_hold:
                self._mode = desired
                self._downgrade_pending_since = None
            # else: hold not expired — keep current mode
        else:
            # Upgrade or no change — apply immediately, cancel any pending downgrade
            self._downgrade_pending_since = None
            self._mode = desired

        return self._decision(scenario, scenario_override)

    def reset(self, mode: CameraMode = CameraMode.SLEEP) -> None:
        """Reset FSM state (e.g. on node restart)."""
        self._mode = mode
        self._downgrade_pending_since = None
        self._idle_since = None

    # ── Helpers ───────────────────────────────────────────────────────────────

    def _speed_to_mode(self, speed_mps: float) -> CameraMode:
        """Map speed to desired mode using hysteresis thresholds."""
        if self._mode == CameraMode.FULL:
            # Downgrade from FULL uses lower threshold
            if speed_mps >= _SPD_FULL_DOWN:
                return CameraMode.FULL
            elif speed_mps >= _SPD_ACTIVE_DOWN:
                return CameraMode.ACTIVE
            elif speed_mps >= _SPD_MOTION:
                return CameraMode.AWARE
            else:
                return CameraMode.AWARE  # idle handled separately

        elif self._mode == CameraMode.ACTIVE:
            if speed_mps >= _SPD_FULL_UP:
                return CameraMode.FULL
            elif speed_mps >= _SPD_ACTIVE_DOWN:
                return CameraMode.ACTIVE
            elif speed_mps >= _SPD_MOTION:
                return CameraMode.AWARE
            else:
                return CameraMode.AWARE

        else:
            # SLEEP / SOCIAL / AWARE — use upgrade thresholds
            if speed_mps >= _SPD_FULL_UP:
                return CameraMode.FULL
            elif speed_mps >= _SPD_ACTIVE_UP:
                return CameraMode.ACTIVE
            elif speed_mps >= _SPD_MOTION:
                return CameraMode.AWARE
            else:
                return CameraMode.SOCIAL

    def _decision(self, scenario: str, override: bool) -> ModeDecision:
        return ModeDecision(
            mode              = self._mode,
            sensors           = MODE_SENSORS[self._mode],
            trigger_speed_mps = self._last_speed,
            trigger_scenario  = scenario,
            scenario_override = override,
        )
