"""thermal_node.py — Jetson CPU/GPU thermal monitor.
Issue #205

Reads every /sys/class/thermal/thermal_zone* sysfs entry, publishes a JSON
blob on /saltybot/thermal at a configurable rate (default 1 Hz), and logs
ROS2 WARN / ERROR when zone temperatures exceed configurable thresholds.

Published topic:
  /saltybot/thermal  (std_msgs/String, JSON)

JSON schema:
  {
    "ts": <float unix seconds>,
    "zones": [
      {"zone": "CPU-therm", "index": 0, "temp_c": 42.5},
      ...
    ],
    "max_temp_c": 55.0,
    "throttled": false,
    "warn": false
  }

Parameters:
  publish_rate_hz   (float, 1.0)  — publish rate
  warn_temp_c       (float, 75.0) — log WARN above this temperature
  throttle_temp_c   (float, 85.0) — log ERROR and set throttled=true above this
  thermal_root      (str, "/sys/class/thermal") — sysfs thermal root (override for tests)
"""

from __future__ import annotations

import json
import os
import time
from typing import List, Optional

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import String


def read_thermal_zones(root: str) -> List[dict]:
    """Return a list of {zone, index, temp_c} dicts from sysfs."""
    zones = []
    try:
        entries = sorted(os.listdir(root))
    except OSError:
        return zones
    for entry in entries:
        if not entry.startswith("thermal_zone"):
            continue
        try:
            idx = int(entry[len("thermal_zone"):])
        except ValueError:
            continue
        zone_dir = os.path.join(root, entry)
        try:
            with open(os.path.join(zone_dir, "type")) as f:
                zone_type = f.read().strip()
        except OSError:
            zone_type = entry
        try:
            with open(os.path.join(zone_dir, "temp")) as f:
                temp_mc = int(f.read().strip())   # millidegrees Celsius
            temp_c = round(temp_mc / 1000.0, 1)
        except (OSError, ValueError):
            continue
        zones.append({"zone": zone_type, "index": idx, "temp_c": temp_c})
    return zones


class ThermalNode(Node):
    """Reads Jetson thermal zones and publishes /saltybot/thermal at 1 Hz."""

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

        self.declare_parameter("publish_rate_hz",  1.0)
        self.declare_parameter("warn_temp_c",      75.0)
        self.declare_parameter("throttle_temp_c",  85.0)
        self.declare_parameter("thermal_root",     "/sys/class/thermal")

        self._rate        = self.get_parameter("publish_rate_hz").value
        self._warn_t      = self.get_parameter("warn_temp_c").value
        self._throttle_t  = self.get_parameter("throttle_temp_c").value
        self._root        = self.get_parameter("thermal_root").value

        qos = QoSProfile(depth=10)
        self._pub = self.create_publisher(String, "/saltybot/thermal", qos)
        self._timer = self.create_timer(1.0 / self._rate, self._publish)

        self.get_logger().info(
            f"ThermalNode ready (rate={self._rate} Hz, "
            f"warn={self._warn_t}°C, throttle={self._throttle_t}°C, "
            f"root={self._root})"
        )

    def _publish(self) -> None:
        zones = read_thermal_zones(self._root)
        if not zones:
            self.get_logger().warn("No thermal zones found — check thermal_root param")
            return

        max_temp = max(z["temp_c"] for z in zones)
        throttled = max_temp >= self._throttle_t
        warn      = max_temp >= self._warn_t

        payload = {
            "ts":         time.time(),
            "zones":      zones,
            "max_temp_c": max_temp,
            "throttled":  throttled,
            "warn":       warn,
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._pub.publish(msg)

        if throttled:
            self.get_logger().error(
                f"THERMAL THROTTLE: {max_temp}°C >= {self._throttle_t}°C"
            )
        elif warn:
            self.get_logger().warn(
                f"Thermal warning: {max_temp}°C >= {self._warn_t}°C"
            )


def main(args: Optional[list] = None) -> None:
    rclpy.init(args=args)
    node = ThermalNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
