"""
light_analyser.py — Histogram-based ambient light analysis and vision mode FSM.

Vision modes (string keys used on /saltybot/vision_mode):
  'day'      — full colour, all cameras nominal
  'twilight' — dim light, start boosting IMX219 gain
  'night'    — dark, max gain + headlight on, IR emitter off
  'ir_only'  — effectively no ambient light, switch to D435i IR stereo only

Transitions use hysteresis to avoid rapid toggling:
  Promotion  (to darker mode): threshold - HYST
  Demotion   (to lighter mode): threshold + HYST
"""

from __future__ import annotations

import numpy as np
from typing import Tuple

# ── Thresholds (0–255 mean grey intensity) ───────────────────────────────────
# Modes:   ir_only < NIGHT_THR <= night < TWILIGHT_THR <= twilight < DAY_THR <= day
_TWILIGHT_THR = 110   # below → twilight
_NIGHT_THR    = 45    # below → night
_IR_ONLY_THR  = 15    # below → ir_only
_HYST         = 8     # hysteresis band (half-width)

# Ordered list of modes darkest → brightest
_MODES = ('ir_only', 'night', 'twilight', 'day')

# Per-mode thresholds: (promote_below, demote_above)
#   promote_below — if median < this, enter the next darker mode
#   demote_above  — if median > this, enter the next lighter mode
#
# Hysteresis: promote threshold is thr - HYST; demote is thr + HYST
# where thr is the boundary between THIS mode and the adjacent lighter mode.
_THRESHOLDS = {
    #          (promote_below,              demote_above)
    'day':      (_TWILIGHT_THR - _HYST,     None),
    'twilight': (_NIGHT_THR    - _HYST,     _TWILIGHT_THR + _HYST),
    'night':    (_IR_ONLY_THR  - _HYST,     _NIGHT_THR    + _HYST),
    'ir_only':  (None,                      _IR_ONLY_THR  + _HYST),
}


class LightAnalyser:
    """Stateful ambient-light classifier with hysteresis."""

    def __init__(
        self,
        initial_mode: str = 'day',
        history_len: int = 6,
    ):
        if initial_mode not in _MODES:
            raise ValueError(f'Unknown mode: {initial_mode}')
        self._mode = initial_mode
        self._history: list[float] = []
        self._history_len = history_len

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

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

    def update(self, frame: np.ndarray) -> Tuple[str, float]:
        """
        Analyse a new image frame (any colour space; converted to grey internally).

        Returns:
            (mode, mean_intensity)  —  mode string + 0–255 grey mean
        """
        intensity = self._frame_intensity(frame)
        self._history.append(intensity)
        if len(self._history) > self._history_len:
            self._history.pop(0)

        smooth = float(np.median(self._history))
        self._mode = self._next_mode(self._mode, smooth)
        return self._mode, smooth

    # ── Internal helpers ──────────────────────────────────────────────────────

    @staticmethod
    def _frame_intensity(frame: np.ndarray) -> float:
        """Return mean pixel intensity of the frame as float in [0, 255]."""
        if frame.ndim == 3:
            # RGB / BGR → greyscale
            grey = np.mean(frame, axis=2).astype(np.float32)
        else:
            grey = frame.astype(np.float32)
        return float(grey.mean())

    @staticmethod
    def _next_mode(current: str, intensity: float) -> str:
        """Apply hysteresis FSM to decide next mode."""
        idx = _MODES.index(current)

        # Try promotion (→ darker mode) first
        lo, hi = _THRESHOLDS[current]
        if lo is not None and intensity < lo:
            return _MODES[max(0, idx - 1)]

        # Try demotion (→ lighter mode)
        if hi is not None and intensity > hi:
            return _MODES[min(len(_MODES) - 1, idx + 1)]

        return current
