"""
expression_node.py — LED mood display bridge for saltybot social layer.

Subscribes
──────────
  /social/mood  (saltybot_social_msgs/Mood)
    mood      : "happy" | "curious" | "annoyed" | "playful" | "idle"
    intensity : 0.0 .. 1.0

Behaviour
─────────
  * On every /social/mood message, serialises the command as a single-line
    JSON frame and writes it over the ESP32-C3 serial link:

        {"mood":"happy","intensity":0.80}\n

  * If no mood message arrives for idle_timeout_s, sends an explicit
    idle command so the ESP32 breathing animation is synchronised with the
    node's own awareness of whether the robot is active.

  * Re-opens the serial port automatically if the device disconnects.

Parameters
──────────
  serial_port      (str)    /dev/ttyUSB0    — ESP32-C3 USB-CDC device
  baud_rate        (int)    115200
  idle_timeout_s   (float)  3.0             — seconds before sending idle
  control_rate     (float)  10.0            — Hz; how often to check idle
"""

import json
import threading
import time

import rclpy
from rclpy.node import Node

from saltybot_social_msgs.msg import Mood

try:
    import serial
    _SERIAL_AVAILABLE = True
except ImportError:
    _SERIAL_AVAILABLE = False


class ExpressionNode(Node):

    def __init__(self):
        super().__init__("expression_node")

        self.declare_parameter("serial_port",    "/dev/ttyUSB0")
        self.declare_parameter("baud_rate",      115200)
        self.declare_parameter("idle_timeout_s", 3.0)
        self.declare_parameter("control_rate",   10.0)

        self._port        = self.get_parameter("serial_port").value
        self._baud        = self.get_parameter("baud_rate").value
        self._idle_to     = self.get_parameter("idle_timeout_s").value
        rate              = self.get_parameter("control_rate").value

        self._last_cmd_t  = 0.0          # monotonic, 0 = never received
        self._lock        = threading.Lock()
        self._ser         = None

        self.create_subscription(Mood, "/social/mood", self._mood_cb, 10)

        self._timer = self.create_timer(1.0 / rate, self._tick_cb)

        if _SERIAL_AVAILABLE:
            threading.Thread(target=self._open_serial, daemon=True).start()
        else:
            self.get_logger().warn(
                "pyserial not available — serial output disabled (dry-run mode)"
            )

        self.get_logger().info(
            f"ExpressionNode ready  port={self._port} baud={self._baud} "
            f"idle_timeout={self._idle_to}s"
        )

    # ── Serial management ─────────────────────────────────────────────────────

    def _open_serial(self):
        """Background thread: keep serial port open, reconnect on error."""
        while rclpy.ok():
            try:
                with self._lock:
                    self._ser = serial.Serial(
                        self._port, self._baud, timeout=1.0
                    )
                self.get_logger().info(f"Opened serial port {self._port}")
                # Hold serial open until error
                while rclpy.ok():
                    with self._lock:
                        if self._ser is None or not self._ser.is_open:
                            break
                    time.sleep(0.5)
            except Exception as exc:
                self.get_logger().warn(
                    f"Serial {self._port} error: {exc} — retry in 3 s"
                )
                with self._lock:
                    if self._ser and self._ser.is_open:
                        self._ser.close()
                    self._ser = None
                time.sleep(3.0)

    def _send(self, mood: str, intensity: float):
        """Serialise and write one JSON line to ESP32-C3."""
        line = json.dumps({"mood": mood, "intensity": round(intensity, 3)}) + "\n"
        data = line.encode("ascii")
        with self._lock:
            if self._ser and self._ser.is_open:
                try:
                    self._ser.write(data)
                except Exception as exc:
                    self.get_logger().warn(f"Serial write error: {exc}")
                    self._ser.close()
                    self._ser = None
            else:
                # Dry-run: log the frame
                self.get_logger().debug(f"[dry-run] → {line.strip()}")

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _mood_cb(self, msg: Mood):
        intensity = max(0.0, min(1.0, float(msg.intensity)))
        self._send(msg.mood, intensity)
        self._last_cmd_t = time.monotonic()

    def _tick_cb(self):
        """Periodic check — send idle if we've been quiet too long."""
        if self._last_cmd_t == 0.0:
            return
        if (time.monotonic() - self._last_cmd_t) >= self._idle_to:
            self._send("idle", 1.0)
            # Reset so we don't flood the ESP32 with idle frames
            self._last_cmd_t = time.monotonic()


# ── Entry point ───────────────────────────────────────────────────────────────

def main(args=None):
    rclpy.init(args=args)
    node = ExpressionNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()
