"""rosbag_recorder_node.py — Trigger-based ROS2 bag recorder.
Issue #332

Subscribes to /saltybot/record_trigger (Bool).  True starts recording;
False stops it.  Auto-stop fires after ``auto_stop_s`` seconds if still
running.  Recording is performed by spawning a ``ros2 bag record``
subprocess which is sent SIGINT for graceful shutdown and SIGKILL if it
does not exit within ``stop_timeout_s``.

Status values
─────────────
  "idle"       — not recording
  "recording"  — subprocess active, writing to bag file
  "stopping"   — SIGINT sent, waiting for subprocess to exit
  "error"      — subprocess died unexpectedly; new trigger retries

Subscriptions
─────────────
  /saltybot/record_trigger   std_msgs/Bool  — True = start, False = stop

Publications
────────────
  /saltybot/recording_status   std_msgs/String  — status value (see above)

Parameters
──────────
  trigger_topic    (str,   "/saltybot/record_trigger")
  status_topic     (str,   "/saltybot/recording_status")
  topics           (str,   "")   comma-separated topic list;
                                 empty string → record all topics (-a)
  bag_dir          (str,   "/tmp/saltybot_bags")
  bag_prefix       (str,   "saltybot")
  auto_stop_s      (float, 60.0)  0 = no auto-stop
  stop_timeout_s   (float, 5.0)   force-kill after this many seconds
  compression      (bool,  False) enable zstd file compression
  max_bag_size_mb  (float, 0.0)   0 = unlimited
  poll_rate        (float, 2.0)   state-machine check frequency (Hz)
"""

from __future__ import annotations

import datetime
import os
import signal
import subprocess
import threading
import time
from typing import List, Optional, Tuple

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


# ── Status constants ───────────────────────────────────────────────────────────

STATUS_IDLE      = "idle"
STATUS_RECORDING = "recording"
STATUS_STOPPING  = "stopping"
STATUS_ERROR     = "error"


# ── Pure helpers ───────────────────────────────────────────────────────────────

def make_bag_path(bag_dir: str, prefix: str) -> str:
    """Return a timestamped output path for a new bag (no file created)."""
    ts = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
    return os.path.join(bag_dir, f"{prefix}_{ts}")


def parse_topics(topics_str: str) -> List[str]:
    """Parse a comma-separated topic string into a clean list.

    Returns an empty list when *topics_str* is blank (meaning record-all).
    """
    if not topics_str or not topics_str.strip():
        return []
    return [t.strip() for t in topics_str.split(",") if t.strip()]


def compute_recording_transition(
    state: str,
    trigger: Optional[bool],
    proc_running: bool,
    now: float,
    record_start_t: float,
    stop_start_t: float,
    auto_stop_s: float,
    stop_timeout_s: float,
) -> Tuple[str, bool]:
    """Pure state-machine step — no I/O, no ROS.

    Parameters
    ----------
    state           : current status string
    trigger         : latest trigger value (True=start, False=stop, None=none)
    proc_running    : whether the recorder subprocess is alive
    now             : current monotonic time (s)
    record_start_t  : monotonic time recording began (0 if not recording)
    stop_start_t    : monotonic time STOPPING began (0 if not stopping)
    auto_stop_s     : auto-stop after this many seconds (0 = disabled)
    stop_timeout_s  : force-kill if stopping > this long (0 = disabled)

    Returns
    -------
    (new_state, force_kill)
      force_kill=True signals the caller to SIGKILL the process.
    """
    if state == STATUS_IDLE:
        if trigger is True:
            return STATUS_RECORDING, False
        return STATUS_IDLE, False

    if state == STATUS_RECORDING:
        if not proc_running:
            return STATUS_ERROR, False
        if trigger is False:
            return STATUS_STOPPING, False
        if (auto_stop_s > 0 and record_start_t > 0
                and (now - record_start_t) >= auto_stop_s):
            return STATUS_STOPPING, False
        return STATUS_RECORDING, False

    if state == STATUS_STOPPING:
        if not proc_running:
            return STATUS_IDLE, False
        if (stop_timeout_s > 0 and stop_start_t > 0
                and (now - stop_start_t) >= stop_timeout_s):
            return STATUS_IDLE, True    # force-kill
        return STATUS_STOPPING, False

    # STATUS_ERROR
    if trigger is True:
        return STATUS_RECORDING, False
    return STATUS_ERROR, False


# ── Subprocess wrapper ─────────────────────────────────────────────────────────

class BagRecorderProcess:
    """Thin wrapper around a ``ros2 bag record`` subprocess."""

    def __init__(self) -> None:
        self._proc: Optional[subprocess.Popen] = None

    def start(self, topics: List[str], output_path: str,
              compression: bool = False,
              max_size_mb: float = 0.0) -> bool:
        """Launch the recorder.  Returns False if already running or on error."""
        if self.is_running():
            return False

        cmd = ["ros2", "bag", "record", "--output", output_path]
        if topics:
            cmd += topics
        else:
            cmd += ["--all"]
        if compression:
            cmd += ["--compression-mode", "file",
                    "--compression-format", "zstd"]
        if max_size_mb > 0:
            cmd += ["--max-bag-size",
                    str(int(max_size_mb * 1024 * 1024))]

        try:
            self._proc = subprocess.Popen(
                cmd,
                stdout=subprocess.DEVNULL,
                stderr=subprocess.PIPE,
                preexec_fn=os.setsid,
            )
            return True
        except Exception:
            self._proc = None
            return False

    def stop(self) -> None:
        """Send SIGINT to the process group for graceful shutdown."""
        if self._proc is not None and self._proc.poll() is None:
            try:
                os.killpg(os.getpgid(self._proc.pid), signal.SIGINT)
            except (ProcessLookupError, PermissionError, OSError):
                pass

    def kill(self) -> None:
        """Send SIGKILL to the process group for forced shutdown."""
        if self._proc is not None:
            try:
                os.killpg(os.getpgid(self._proc.pid), signal.SIGKILL)
            except (ProcessLookupError, PermissionError, OSError):
                pass
            self._proc = None

    def is_running(self) -> bool:
        return self._proc is not None and self._proc.poll() is None

    def reset(self) -> None:
        """Clear internal proc reference (call after graceful exit)."""
        self._proc = None


# ── ROS2 node ──────────────────────────────────────────────────────────────────

class RosbagRecorderNode(Node):
    """Trigger-based ROS bag recorder with auto-stop and status reporting."""

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

        self.declare_parameter("trigger_topic",   "/saltybot/record_trigger")
        self.declare_parameter("status_topic",    "/saltybot/recording_status")
        self.declare_parameter("topics",           "")
        self.declare_parameter("bag_dir",          "/tmp/saltybot_bags")
        self.declare_parameter("bag_prefix",       "saltybot")
        self.declare_parameter("auto_stop_s",      60.0)
        self.declare_parameter("stop_timeout_s",   5.0)
        self.declare_parameter("compression",      False)
        self.declare_parameter("max_bag_size_mb",  0.0)
        self.declare_parameter("poll_rate",        2.0)

        trigger_topic   = self.get_parameter("trigger_topic").value
        status_topic    = self.get_parameter("status_topic").value
        topics_str      = self.get_parameter("topics").value
        self._bag_dir   = str(self.get_parameter("bag_dir").value)
        self._bag_prefix = str(self.get_parameter("bag_prefix").value)
        self._auto_stop_s = float(self.get_parameter("auto_stop_s").value)
        self._stop_tmo_s  = float(self.get_parameter("stop_timeout_s").value)
        self._compression = bool(self.get_parameter("compression").value)
        self._max_mb      = float(self.get_parameter("max_bag_size_mb").value)
        poll_rate         = float(self.get_parameter("poll_rate").value)

        self._topics: List[str] = parse_topics(str(topics_str))

        # Recorder process — injectable for tests
        self._recorder: BagRecorderProcess = BagRecorderProcess()

        # State
        self._state          = STATUS_IDLE
        self._trigger: Optional[bool] = None
        self._record_start_t: float   = 0.0
        self._stop_start_t:   float   = 0.0
        self._current_bag:    str     = ""
        self._lock = threading.Lock()

        qos = QoSProfile(depth=10)
        self._status_pub = self.create_publisher(String, status_topic, qos)
        self._trigger_sub = self.create_subscription(
            Bool, trigger_topic, self._on_trigger, qos
        )
        self._timer = self.create_timer(1.0 / poll_rate, self._poll_cb)

        # Publish initial state
        self._publish(STATUS_IDLE)

        topic_desc = ",".join(self._topics) if self._topics else "<all>"
        self.get_logger().info(
            f"RosbagRecorderNode ready — topics={topic_desc}, "
            f"bag_dir={self._bag_dir}, auto_stop={self._auto_stop_s}s"
        )

    # ── Subscription ───────────────────────────────────────────────────

    def _on_trigger(self, msg) -> None:
        with self._lock:
            self._trigger = bool(msg.data)

    # ── Poll / state machine ────────────────────────────────────────────

    def _poll_cb(self) -> None:
        now = time.monotonic()

        with self._lock:
            trigger      = self._trigger
            self._trigger = None          # consume
            state        = self._state
            rec_t        = self._record_start_t
            stop_t       = self._stop_start_t

        proc_running = self._recorder.is_running()

        new_state, force_kill = compute_recording_transition(
            state, trigger, proc_running, now,
            rec_t, stop_t, self._auto_stop_s, self._stop_tmo_s,
        )

        if force_kill:
            self._recorder.kill()
            self.get_logger().warn(
                f"RosbagRecorder: force-killed (stop_timeout={self._stop_tmo_s}s)"
            )

        if new_state != state:
            self._enter_state(new_state, now)

    def _enter_state(self, new_state: str, now: float) -> None:
        if new_state == STATUS_RECORDING:
            bag_path = make_bag_path(self._bag_dir, self._bag_prefix)
            started  = self._recorder.start(
                self._topics, bag_path,
                compression=self._compression,
                max_size_mb=self._max_mb,
            )
            if not started:
                new_state = STATUS_ERROR
                self.get_logger().error(
                    "RosbagRecorder: failed to start recorder subprocess"
                )
            else:
                with self._lock:
                    self._record_start_t = now
                    self._current_bag    = bag_path
                self.get_logger().info(
                    f"RosbagRecorder: recording started → {bag_path}"
                )

        elif new_state == STATUS_STOPPING:
            self._recorder.stop()
            with self._lock:
                self._stop_start_t = now
            self.get_logger().info("RosbagRecorder: stopping (SIGINT sent)")

        elif new_state == STATUS_IDLE:
            bag = self._current_bag
            with self._lock:
                self._record_start_t = 0.0
                self._stop_start_t   = 0.0
                self._current_bag    = ""
            self._recorder.reset()
            self.get_logger().info(
                f"RosbagRecorder: recording complete → {bag}"
            )

        elif new_state == STATUS_ERROR:
            self.get_logger().error(
                "RosbagRecorder: subprocess exited unexpectedly"
            )

        with self._lock:
            self._state = new_state

        self._publish(new_state)

    # ── Publish ─────────────────────────────────────────────────────────

    def _publish(self, status: str) -> None:
        msg = String()
        msg.data = status
        self._status_pub.publish(msg)

    # ── Public accessors ─────────────────────────────────────────────────

    @property
    def state(self) -> str:
        with self._lock:
            return self._state

    @property
    def current_bag(self) -> str:
        with self._lock:
            return self._current_bag


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