"""Subsystem state tracking for the diagnostics aggregator.

Each SubsystemState tracks one logical subsystem (motors, battery, etc.)
across potentially multiple source topics.  Status priority:
  ERROR > WARN > STALE > OK > UNKNOWN
"""

import time
from dataclasses import dataclass, field
from typing import Optional

# ---------------------------------------------------------------------------
# Status constants — ordered by severity (higher index = more severe)
# ---------------------------------------------------------------------------
STATUS_UNKNOWN = "UNKNOWN"
STATUS_OK      = "OK"
STATUS_STALE   = "STALE"
STATUS_WARN    = "WARN"
STATUS_ERROR   = "ERROR"

# ---------------------------------------------------------------------------
# Subsystem registry and diagnostic keyword routing
# ---------------------------------------------------------------------------
SUBSYSTEM_NAMES: list[str] = [
    "motors", "battery", "imu", "uwb", "lidar", "camera", "can_bus", "estop"
]

# (keywords-tuple, subsystem-name) — first match wins, lower-cased comparison
DIAG_KEYWORD_MAP: list[tuple[tuple[str, ...], str]] = [
    (("vesc", "motor", "esc", "fsesc"),               "motors"),
    (("battery", "ina219", "lvc", "coulomb", "vbat"),  "battery"),
    (("imu", "mpu6000", "bno055", "icm42688", "gyro", "accel"), "imu"),
    (("uwb", "dw1000", "dw3000"),                      "uwb"),
    (("lidar", "rplidar", "sick", "laser"),             "lidar"),
    (("camera", "realsense", "oak", "webcam"),          "camera"),
    (("can", "can_bus", "can_driver"),                  "can_bus"),
    (("estop", "safety", "emergency", "e-stop"),        "estop"),
]


def keyword_to_subsystem(name: str) -> Optional[str]:
    """Map a diagnostic status name to a subsystem key, or None."""
    lower = name.lower()
    for keywords, subsystem in DIAG_KEYWORD_MAP:
        if any(kw in lower for kw in keywords):
            return subsystem
    return None


_SEVERITY: dict[str, int] = {
    STATUS_UNKNOWN: 0,
    STATUS_OK:      1,
    STATUS_STALE:   2,
    STATUS_WARN:    3,
    STATUS_ERROR:   4,
}


def worse(a: str, b: str) -> str:
    """Return the more severe of two status strings."""
    return a if _SEVERITY.get(a, 0) >= _SEVERITY.get(b, 0) else b


def ros_level_to_status(level: int) -> str:
    """Convert diagnostic_msgs/DiagnosticStatus.level to a status string."""
    return {0: STATUS_OK, 1: STATUS_WARN, 2: STATUS_ERROR}.get(level, STATUS_UNKNOWN)


# ---------------------------------------------------------------------------
# Transition log entry
# ---------------------------------------------------------------------------
@dataclass
class Transition:
    """A logged status change for one subsystem."""
    subsystem: str
    from_status: str
    to_status: str
    message: str
    timestamp_iso: str      # ISO-8601 wall-clock
    monotonic_s: float      # time.monotonic() at the transition


# ---------------------------------------------------------------------------
# Per-subsystem state
# ---------------------------------------------------------------------------
@dataclass
class SubsystemState:
    """Live health state for one logical subsystem."""

    name: str
    stale_timeout_s: float = 5.0    # seconds without update → STALE

    # Mutable state
    status: str = STATUS_UNKNOWN
    message: str = ""
    last_updated_mono: float = field(default_factory=lambda: 0.0)
    last_change_mono: float  = field(default_factory=lambda: 0.0)
    previous_status: str = STATUS_UNKNOWN

    def update(self, new_status: str, message: str, now_mono: float) -> Optional[Transition]:
        """Apply a new status, record a transition if the status changed.

        Returns a Transition if the status changed, else None.
        """
        from datetime import datetime, timezone

        self.last_updated_mono = now_mono

        if new_status == self.status:
            self.message = message
            return None

        transition = Transition(
            subsystem=self.name,
            from_status=self.status,
            to_status=new_status,
            message=message,
            timestamp_iso=datetime.now(timezone.utc).isoformat(),
            monotonic_s=now_mono,
        )

        self.previous_status = self.status
        self.status = new_status
        self.message = message
        self.last_change_mono = now_mono
        return transition

    def apply_stale_check(self, now_mono: float) -> Optional[Transition]:
        """Mark STALE if no update received within stale_timeout_s.

        Returns a Transition if newly staled, else None.
        """
        if self.last_updated_mono == 0.0:
            # Never received any data — leave as UNKNOWN
            return None
        if (now_mono - self.last_updated_mono) > self.stale_timeout_s:
            if self.status not in (STATUS_STALE, STATUS_UNKNOWN):
                return self.update(STATUS_STALE, "No data received", now_mono)
        return None

    def to_dict(self, now_mono: float) -> dict:
        age = (now_mono - self.last_updated_mono) if self.last_updated_mono > 0 else None
        return {
            "status":           self.status,
            "message":          self.message,
            "age_s":            round(age, 2) if age is not None else None,
            "previous_status":  self.previous_status,
        }
