"""
/safety/watchdog Node
Agent ROS Bridge v0.6.1 - Week 2 Implementation

Monitors system health and triggers failsafe actions.
1kHz heartbeat monitoring with 1ms timeout.
"""

import threading
import time
from collections.abc import Callable
from typing import Any


class WatchdogNode:
    """
    Watchdog Node

    Monitors critical nodes via heartbeat messages.
    Auto-triggers emergency stop on timeout.

    Timing Requirements:
    - Heartbeat frequency: 1kHz (1ms period)
    - Timeout threshold: 1ms (1 missed heartbeat)
    """

    # Constants
    MONITORING_FREQUENCY_HZ = 1000  # 1kHz
    MONITORING_PERIOD_MS = 1.0  # 1ms
    TIMEOUT_THRESHOLD_MS = 1.0  # 1ms timeout

    # Topics
    HEARTBEAT_TOPIC = "/safety/watchdog_heartbeat"
    STATUS_TOPIC = "/safety/watchdog_status"

    def __init__(self):
        """Initialize Watchdog Node"""
        self._monitored_nodes: dict[str, dict[str, Any]] = {}
        self._heartbeats: dict[str, dict[str, Any]] = {}
        self._timeout_log: list[dict[str, Any]] = []
        self._lock = threading.Lock()
        self._seq = 0

        # Callbacks
        self.heartbeat_publisher: Callable | None = None
        self.status_publisher: Callable | None = None
        self.emergency_stop_callback: Callable | None = None

    def register_node(self, node_id: str, critical: bool = True) -> None:
        """
        Register a node for monitoring

        Args:
            node_id: Unique node identifier
            critical: Whether node is critical (triggers e-stop on timeout)
        """
        with self._lock:
            self._monitored_nodes[node_id] = {"critical": critical, "registered_at": time.time()}
            self._heartbeats[node_id] = {"seq": 0, "timestamp": 0, "status": 0}

    def unregister_node(self, node_id: str) -> None:
        """
        Unregister a node from monitoring

        Args:
            node_id: Node to unregister
        """
        with self._lock:
            self._monitored_nodes.pop(node_id, None)
            self._heartbeats.pop(node_id, None)

    def process_heartbeat(self, node_id: str, seq: int, timestamp: float, status: int = 0) -> None:
        """
        Process a heartbeat from a monitored node

        Args:
            node_id: Node that sent heartbeat
            seq: Sequence number
            timestamp: Heartbeat timestamp
            status: Node status (0=OK, 1=WARNING, 2=ERROR)
        """
        with self._lock:
            if node_id not in self._monitored_nodes:
                return

            self._heartbeats[node_id] = {
                "seq": seq,
                "timestamp": timestamp,
                "status": status,
                "received_at": time.time(),
            }

    def check_timeouts(self) -> list[str]:
        """
        Check for timed out nodes

        Returns:
            List of node IDs that have timed out
        """
        timed_out = []
        current_time = time.time()

        with self._lock:
            for node_id, node_info in self._monitored_nodes.items():
                heartbeat = self._heartbeats.get(node_id, {})
                last_timestamp = heartbeat.get("timestamp", 0)

                # Calculate time since last heartbeat
                elapsed_ms = (current_time - last_timestamp) * 1000

                if elapsed_ms > self.TIMEOUT_THRESHOLD_MS:
                    timed_out.append(node_id)

                    # Log timeout
                    timeout_event = {
                        "timestamp": current_time,
                        "node": node_id,
                        "elapsed_ms": elapsed_ms,
                        "critical": node_info.get("critical", False),
                    }
                    self._timeout_log.append(timeout_event)

                    # Trigger e-stop for critical nodes
                    if node_info.get("critical", False) and self.emergency_stop_callback:
                        self.emergency_stop_callback(
                            reason=f"Watchdog timeout: {node_id}", source="watchdog"
                        )

        return timed_out

    def create_heartbeat(self) -> dict[str, Any]:
        """
        Create a watchdog heartbeat message

        Returns:
            Heartbeat message dictionary
        """
        self._seq += 1
        return {
            "seq": self._seq,
            "timestamp": time.time(),
            "source": "/safety/watchdog",
            "status": 0,  # OK
        }

    def publish_heartbeat(self) -> None:
        """Publish watchdog heartbeat"""
        heartbeat = self.create_heartbeat()
        if self.heartbeat_publisher:
            self.heartbeat_publisher(heartbeat)

    def get_status(self) -> dict[str, Any]:
        """
        Get watchdog status

        Returns:
            Status dictionary
        """
        with self._lock:
            return {
                "monitored_nodes": list(self._monitored_nodes.keys()),
                "node_count": len(self._monitored_nodes),
                "timestamp": time.time(),
            }

    def publish_status(self) -> None:
        """Publish watchdog status"""
        status = self.get_status()
        if self.status_publisher:
            self.status_publisher(status)

    @property
    def monitored_nodes(self) -> set[str]:
        """Get set of monitored node IDs"""
        with self._lock:
            return set(self._monitored_nodes.keys())

    @property
    def heartbeats(self) -> dict[str, dict[str, Any]]:
        """Get heartbeat data (copy)"""
        with self._lock:
            return self._heartbeats.copy()

    @property
    def timeout_log(self) -> list[dict[str, Any]]:
        """Get timeout log (copy)"""
        with self._lock:
            return self._timeout_log.copy()

    def get_node_health(self, node_id: str) -> dict[str, Any] | None:
        """
        Get health status for a specific node

        Args:
            node_id: Node to check

        Returns:
            Health status dictionary or None
        """
        with self._lock:
            if node_id not in self._monitored_nodes:
                return None

            heartbeat = self._heartbeats.get(node_id, {})
            last_timestamp = heartbeat.get("timestamp", 0)
            elapsed_ms = (time.time() - last_timestamp) * 1000

            return {
                "node_id": node_id,
                "last_heartbeat": last_timestamp,
                "elapsed_ms": elapsed_ms,
                "timed_out": elapsed_ms > self.TIMEOUT_THRESHOLD_MS,
                "status": heartbeat.get("status", 0),
            }
