"""system_monitor_node.py — ROS2 node for Jetson Orin system monitoring (Issue #631).

Publishes at 1 Hz:
  /saltybot/system/stats    std_msgs/String   JSON stats payload
  /saltybot/diagnostics     diagnostic_msgs/DiagnosticArray  health status

Stats source priority: jtop > tegrastats > mock (for testing).
"""

from __future__ import annotations

import json

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

from saltybot_system_monitor.jetson_stats import (
    AlertChecker,
    AlertThresholds,
    JetsonStats,
    Alert,
    make_reader,
)


class SystemMonitorNode(Node):
    """Publishes Jetson Orin hardware stats at 1 Hz."""

    def __init__(self) -> None:
        super().__init__("system_monitor_node")

        # ── Parameters ─────────────────────────────────────────────────────
        self.declare_parameter("publish_rate_hz", 1.0)
        self.declare_parameter("stats_source", "auto")   # auto | jtop | tegrastats | mock
        self.declare_parameter("cpu_warn_pct",  85.0)
        self.declare_parameter("gpu_warn_pct",  85.0)
        self.declare_parameter("temp_warn_c",   80.0)
        self.declare_parameter("disk_warn_pct", 90.0)
        self.declare_parameter("ram_warn_pct",  90.0)
        self.declare_parameter("power_warn_mw", 30_000.0)

        rate     = self.get_parameter("publish_rate_hz").value
        source   = self.get_parameter("stats_source").value

        self._thresholds = AlertThresholds(
            cpu_pct   = float(self.get_parameter("cpu_warn_pct").value),
            gpu_pct   = float(self.get_parameter("gpu_warn_pct").value),
            temp_c    = float(self.get_parameter("temp_warn_c").value),
            disk_pct  = float(self.get_parameter("disk_warn_pct").value),
            ram_pct   = float(self.get_parameter("ram_warn_pct").value),
            power_mw  = float(self.get_parameter("power_warn_mw").value),
        )

        self._reader  = make_reader(prefer=source)
        self._checker = AlertChecker()

        # ── Publishers ─────────────────────────────────────────────────────
        self._stats_pub = self.create_publisher(String, "/saltybot/system/stats", 10)
        self._diag_pub  = self.create_publisher(
            DiagnosticArray, "/saltybot/diagnostics", 10
        )

        # ── Timer ──────────────────────────────────────────────────────────
        period = 1.0 / max(rate, 0.1)
        self._timer = self.create_timer(period, self._on_timer)

        self.get_logger().info(
            f"SystemMonitorNode started (source={source}, rate={rate} Hz)"
        )

    # ── Timer callback ─────────────────────────────────────────────────────

    def _on_timer(self) -> None:
        stats = self._reader.read()
        if stats is None:
            self.get_logger().warning("Stats reader returned None — skipping cycle")
            return

        alerts = self._checker.check(stats, self._thresholds)

        self._publish_stats(stats, alerts)
        self._publish_diagnostics(stats, alerts)

    # ── Stats publisher ────────────────────────────────────────────────────

    def _publish_stats(self, stats: JetsonStats, alerts: list) -> None:
        payload = {
            "timestamp": stats.timestamp,
            "source":    stats.source,
            "cpu": {
                "avg_pct": stats.cpu_avg_pct,
                "cores":   [
                    {"index": c.index, "usage_pct": c.usage_pct, "freq_mhz": c.freq_mhz}
                    for c in stats.cpu_cores
                ],
            },
            "gpu": {
                "pct":      stats.gpu_pct,
                "freq_mhz": stats.gpu_freq_mhz,
            },
            "ram": {
                "used_mb":  stats.ram_used_mb,
                "total_mb": stats.ram_total_mb,
                "pct":      stats.ram_pct,
            },
            "swap": {
                "used_mb":  stats.swap_used_mb,
                "total_mb": stats.swap_total_mb,
            },
            "temperatures": stats.temperatures,
            "temp_max_c":   stats.temp_max_c,
            "fan_pct":      stats.fan_pct,
            "power": {
                "total_mw": stats.power_total_mw,
                "cpu_mw":   stats.power_cpu_mw,
                "gpu_mw":   stats.power_gpu_mw,
            },
            "disk": {
                "used_gb":  stats.disk_used_gb,
                "total_gb": stats.disk_total_gb,
                "pct":      stats.disk_pct,
            },
            "alerts": [
                {
                    "component": a.component,
                    "message":   a.message,
                    "level":     a.level,
                    "value":     a.value,
                    "threshold": a.threshold,
                }
                for a in alerts
            ],
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._stats_pub.publish(msg)

    # ── DiagnosticArray publisher ──────────────────────────────────────────

    def _publish_diagnostics(self, stats: JetsonStats, alerts: list) -> None:
        now = self.get_clock().now().to_msg()
        diag = DiagnosticArray()
        diag.header.stamp = now

        # Overall system status — worst level among alerts
        worst = 0
        for a in alerts:
            if a.level > worst:
                worst = a.level

        overall = DiagnosticStatus()
        overall.name    = "saltybot/system"
        overall.hardware_id = "jetson_orin"
        overall.level   = worst
        overall.message = "OK" if worst == 0 else (
            "WARN" if worst == Alert.WARN else "ERROR"
        )
        overall.values  = self._stats_to_kv(stats)
        diag.status.append(overall)

        # One status per alert
        for a in alerts:
            s = DiagnosticStatus()
            s.name        = f"saltybot/system/{a.component}"
            s.hardware_id = "jetson_orin"
            s.level       = a.level
            s.message     = a.message
            s.values      = [
                KeyValue(key="value",     value=str(a.value)),
                KeyValue(key="threshold", value=str(a.threshold)),
            ]
            diag.status.append(s)

        self._diag_pub.publish(diag)

    # ── Helper: stats → KeyValue list ──────────────────────────────────────

    @staticmethod
    def _stats_to_kv(stats: JetsonStats) -> list:
        pairs = [
            ("source",       stats.source),
            ("cpu_avg_pct",  f"{stats.cpu_avg_pct:.1f}"),
            ("gpu_pct",      f"{stats.gpu_pct:.1f}"),
            ("gpu_freq_mhz", str(stats.gpu_freq_mhz)),
            ("ram_pct",      f"{stats.ram_pct:.1f}"),
            ("ram_used_mb",  str(stats.ram_used_mb)),
            ("ram_total_mb", str(stats.ram_total_mb)),
            ("temp_max_c",   f"{stats.temp_max_c:.1f}"),
            ("fan_pct",      f"{stats.fan_pct:.1f}"),
            ("power_total_mw", str(stats.power_total_mw)),
            ("disk_pct",     f"{stats.disk_pct:.1f}"),
            ("disk_used_gb", f"{stats.disk_used_gb:.1f}"),
            ("disk_total_gb", f"{stats.disk_total_gb:.1f}"),
        ]
        for name, temp in stats.temperatures.items():
            pairs.append((f"temp_{name}_c", f"{temp:.1f}"))
        return [KeyValue(key=k, value=v) for k, v in pairs]


def main(args=None) -> None:
    rclpy.init(args=args)
    node = SystemMonitorNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
