"""bag_recorder_node.py — ROS2 bag recording manager (Issue #615).

Upgrades the Issue #488 basic recorder with:
  • Motion-triggered auto-record (/cmd_vel non-zero → start, 30s idle → stop)
  • Auto-split at 1 GB or 10 min
  • USB/NVMe storage path selection with disk monitoring
  • Disk-usage-triggered cleanup (>80% → delete bags >7 days old)
  • JSON status topic at 1 Hz

Services (Trigger):
    /saltybot/bag_recorder/start   /saltybot/bag_recorder/stop
    /saltybot/bag_recorder/split   (plus legacy /saltybot/start_recording etc.)

Topics:
    SUB  /cmd_vel                       (geometry_msgs/Twist)
    PUB  /saltybot/bag_recorder/status  (std_msgs/String, JSON)
"""

from __future__ import annotations

import json, os, shutil, signal, subprocess, threading, time
from datetime import datetime
from pathlib import Path
from typing import List, Optional

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from std_srvs.srv import Trigger

from saltybot_bag_recorder.bag_policy import BagPolicy, DiskInfo, MotionState, StorageSelector


class _BagSession:
    """Tracks one recording session (possibly multiple split segments)."""

    def __init__(self, bag_dir: Path, session_name: str) -> None:
        self.bag_dir       = bag_dir
        self.session_name  = session_name
        self.start_time    = time.monotonic()
        self.split_count   = 0
        self.proc: Optional[subprocess.Popen] = None
        self.segment_start = time.monotonic()
        self._lock         = threading.Lock()

    def segment_name(self) -> str:
        return f"{self.session_name}_s{self.split_count:03d}"

    def segment_path(self) -> Path:
        return self.bag_dir / self.segment_name()

    def segment_size_bytes(self) -> int:
        p = self.segment_path()
        if not p.exists():
            return 0
        return sum(f.stat().st_size for f in p.rglob("*") if f.is_file())

    def segment_duration_s(self) -> float:
        return time.monotonic() - self.segment_start

    def session_duration_s(self) -> float:
        return time.monotonic() - self.start_time

    def is_alive(self) -> bool:
        with self._lock:
            return self.proc is not None and self.proc.poll() is None

    def stop_proc(self, timeout: float = 5.0) -> None:
        with self._lock:
            if self.proc is None:
                return
            try:
                self.proc.send_signal(signal.SIGINT)
                self.proc.wait(timeout=timeout)
            except subprocess.TimeoutExpired:
                self.proc.kill()
                self.proc.wait()
            except OSError:
                pass
            self.proc = None


class BagRecorderNode(Node):
    """ROS2 bag recording manager with motion trigger and disk monitoring (Issue #615)."""

    DEFAULT_TOPICS: List[str] = [
        "/scan", "/cmd_vel", "/odom", "/tf", "/tf_static",
        "/saltybot/pose/fused_cov", "/saltybot/diagnostics",
        "/saltybot/sensor_health", "/saltybot/battery",
        "/saltybot/motor_daemon/status",
        "/camera/color/image_raw/compressed",
        "/camera/depth/image_rect_raw/compressed",
        "/imu/data",
    ]

    DEFAULT_STORAGE_PATHS: List[str] = [
        "/media/usb0", "/media/usb1",
        "/mnt/nvme/saltybot-bags",
        "~/saltybot-data/bags",
    ]

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

        self.declare_parameter("storage_paths",      self.DEFAULT_STORAGE_PATHS)
        self.declare_parameter("topics",             self.DEFAULT_TOPICS)
        self.declare_parameter("motion_trigger",     True)
        self.declare_parameter("idle_timeout_s",     30.0)
        self.declare_parameter("split_size_gb",      1.0)
        self.declare_parameter("split_duration_min", 10.0)
        self.declare_parameter("storage_format",     "mcap")
        self.declare_parameter("warn_disk_pct",      70.0)
        self.declare_parameter("cleanup_disk_pct",   80.0)
        self.declare_parameter("cleanup_age_days",   7)
        self.declare_parameter("min_free_gb",        2.0)
        self.declare_parameter("status_hz",          1.0)
        self.declare_parameter("check_hz",           0.033)

        self._topics         = list(self.get_parameter("topics").value)
        self._motion_trigger = self.get_parameter("motion_trigger").value
        self._storage_fmt    = self.get_parameter("storage_format").value

        self._policy = BagPolicy(
            split_size_gb      = self.get_parameter("split_size_gb").value,
            split_duration_min = self.get_parameter("split_duration_min").value,
            cleanup_age_days   = int(self.get_parameter("cleanup_age_days").value),
            cleanup_disk_pct   = self.get_parameter("cleanup_disk_pct").value,
            warn_disk_pct      = self.get_parameter("warn_disk_pct").value,
            min_free_gb        = self.get_parameter("min_free_gb").value,
        )
        self._motion = MotionState(
            idle_timeout_s = self.get_parameter("idle_timeout_s").value,
        )
        self._selector = StorageSelector(
            paths       = list(self.get_parameter("storage_paths").value),
            min_free_gb = self.get_parameter("min_free_gb").value,
        )

        self._bag_dir:  Optional[Path]        = None
        self._session:  Optional[_BagSession] = None
        self._lock:     threading.Lock        = threading.Lock()
        self._recording: bool                 = False

        self._status_pub = self.create_publisher(
            String, "/saltybot/bag_recorder/status", 10)

        self.create_service(Trigger, "/saltybot/bag_recorder/start", self._svc_start)
        self.create_service(Trigger, "/saltybot/bag_recorder/stop",  self._svc_stop)
        self.create_service(Trigger, "/saltybot/bag_recorder/split", self._svc_split)
        # Legacy (Issue #488 compatibility)
        self.create_service(Trigger, "/saltybot/start_recording", self._svc_start)
        self.create_service(Trigger, "/saltybot/stop_recording",  self._svc_stop)
        self.create_service(Trigger, "/saltybot/save_bag",        self._svc_split)

        if self._motion_trigger:
            self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)

        hz = self.get_parameter("status_hz").value
        self.create_timer(1.0 / hz, self._publish_status)
        chz = self.get_parameter("check_hz").value
        self.create_timer(1.0 / chz, self._periodic_check)
        self.create_timer(60.0, self._disk_maintenance)

        self.get_logger().info(
            f"BagRecorder ready | motion={self._motion_trigger} "
            f"idle={self._motion.idle_timeout_s:.0f}s "
            f"split={self._policy.split_size_bytes//1024**3}GB/"
            f"{int(self._policy.split_duration_s//60)}min "
            f"fmt={self._storage_fmt} topics={len(self._topics)}"
        )

    def _on_cmd_vel(self, msg: Twist) -> None:
        now = time.monotonic()
        self._motion.update(msg.linear.x, msg.angular.z, now)
        with self._lock:
            if not self._recording and self._motion.should_start_recording(now):
                self.get_logger().info("Motion detected — auto-starting bag recording")
                self._start_recording_locked()

    def _svc_start(self, _req, resp):
        with self._lock:
            if self._recording:
                resp.success = False
                resp.message = f"Already recording: {self._session.session_name}"
                return resp
            ok, msg = self._start_recording_locked()
        resp.success = ok
        resp.message = msg
        return resp

    def _svc_stop(self, _req, resp):
        with self._lock:
            if not self._recording:
                resp.success = False
                resp.message = "Not recording"
                return resp
            _, ok, msg = self._stop_recording_locked()
        resp.success = ok
        resp.message = msg
        return resp

    def _svc_split(self, _req, resp):
        with self._lock:
            if not self._recording:
                resp.success = False
                resp.message = "Not recording — nothing to split"
                return resp
            ok, msg = self._split_locked()
        resp.success = ok
        resp.message = msg
        return resp

    def _periodic_check(self) -> None:
        now = time.monotonic()
        with self._lock:
            if self._recording and self._motion_trigger:
                if self._motion.should_stop_recording(now):
                    self.get_logger().info(
                        f"Idle {self._motion.idle_seconds(now):.0f}s — auto-stopping")
                    self._stop_recording_locked()
                    return
            if self._recording and self._session is not None:
                size = self._session.segment_size_bytes()
                dur  = self._session.segment_duration_s()
                split, reason = self._policy.needs_split(size, dur)
                if split:
                    self.get_logger().info(f"Auto-split: {reason}")
                    self._split_locked()

    def _disk_maintenance(self) -> None:
        if self._bag_dir is None:
            sel = self._selector.select()
            if sel is None:
                return
            self._bag_dir = Path(sel)
        try:
            disk = DiskInfo.from_path(str(self._bag_dir))
        except OSError as exc:
            self.get_logger().warn(f"Disk check failed: {exc}")
            return
        if self._policy.should_warn_disk(disk):
            self.get_logger().warn(
                f"Disk {disk.used_pct:.1f}% > {self._policy.warn_disk_pct:.0f}%")
        if self._policy.needs_cleanup(disk):
            self.get_logger().warn(
                f"Disk {disk.used_pct:.1f}% >= {self._policy.cleanup_disk_pct:.0f}% — cleaning")
            self._cleanup_expired_bags()

    def _start_recording_locked(self):
        sel = self._selector.select()
        if sel is None:
            msg = f"No storage path with >={self._policy.min_free_gb:.1f}GB free"
            self.get_logger().error(msg)
            return False, msg
        self._bag_dir = Path(sel)
        self._bag_dir.mkdir(parents=True, exist_ok=True)
        ts = datetime.now().strftime("%Y%m%d_%H%M%S")
        self._session = _BagSession(self._bag_dir, f"saltybot_{ts}")
        ok, msg = self._launch_ros2bag_locked()
        if ok:
            self._recording = True
            self.get_logger().info(
                f"Recording: {self._session.segment_name()} -> {self._bag_dir}")
        return ok, msg

    def _stop_recording_locked(self):
        if self._session is None:
            return None, False, "No active session"
        name = self._session.session_name
        self._session.stop_proc()
        self._recording = False
        self.get_logger().info(f"Stopped: {name}")
        return name, True, f"Stopped: {name}"

    def _split_locked(self):
        if self._session is None:
            return False, "No active session"
        old = self._session.segment_name()
        self._session.stop_proc()
        self._session.split_count  += 1
        self._session.segment_start = time.monotonic()
        ok, msg = self._launch_ros2bag_locked()
        if ok:
            self.get_logger().info(f"Split: {old} -> {self._session.segment_name()}")
            return True, f"Split: {old} -> {self._session.segment_name()}"
        return ok, msg

    def _launch_ros2bag_locked(self):
        seg_path = self._session.segment_path()
        cmd = ["ros2", "bag", "record",
               "--output", str(seg_path),
               "--storage", self._storage_fmt]
        if self._storage_fmt == "sqlite3":
            cmd += ["--compression-format", "zstd", "--compression-mode", "file"]
        cmd += self._topics
        try:
            self._session.proc = subprocess.Popen(
                cmd, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE,
                preexec_fn=os.setsid)
            return True, f"Recording: {self._session.segment_name()}"
        except Exception as exc:
            msg = f"Failed to launch ros2 bag record: {exc}"
            self.get_logger().error(msg)
            return False, msg

    def _cleanup_expired_bags(self) -> None:
        if self._bag_dir is None or not self._bag_dir.exists():
            return
        now = time.time()
        for item in sorted(self._bag_dir.iterdir(),
                           key=lambda p: p.stat().st_mtime):
            if not item.is_dir() or not item.name.startswith("saltybot_"):
                continue
            if (self._session and
                    item.name.startswith(self._session.session_name)):
                continue
            age = self._policy.bag_age_days(item.stat().st_mtime, now)
            if self._policy.bag_is_expired(age):
                try:
                    shutil.rmtree(item)
                    self.get_logger().info(
                        f"Cleanup: removed {item.name} (age {age:.1f}d)")
                    try:
                        disk = DiskInfo.from_path(str(self._bag_dir))
                        if not self._policy.needs_cleanup(disk):
                            break
                    except OSError:
                        break
                except OSError as exc:
                    self.get_logger().warn(f"Cleanup failed {item.name}: {exc}")

    def _publish_status(self) -> None:
        with self._lock:
            s_name  = self._session.session_name   if self._session else None
            seg     = self._session.segment_name() if self._session else None
            splits  = self._session.split_count    if self._session else 0
            seg_sz  = self._session.segment_size_bytes() if self._session else 0
            s_dur   = self._session.session_duration_s() if self._session else 0.0
            seg_dur = self._session.segment_duration_s() if self._session else 0.0
            rec     = self._recording
            bdir    = str(self._bag_dir) if self._bag_dir else None
        now = time.monotonic()
        idle = self._motion.idle_seconds(now)
        disk_status = {}
        if self._bag_dir and self._bag_dir.exists():
            try:
                di = DiskInfo.from_path(str(self._bag_dir))
                disk_status = {
                    "path":     bdir,
                    "used_pct": round(di.used_pct, 1),
                    "free_gb":  round(di.free_gb, 2),
                    "total_gb": round(di.total_gb, 2),
                    "warn":     self._policy.should_warn_disk(di),
                    "cleanup":  self._policy.needs_cleanup(di),
                }
            except OSError:
                pass
        payload = {
            "recording":       rec,
            "session":         s_name,
            "segment":         seg,
            "split_count":     splits,
            "segment_size_mb": round(seg_sz / 1e6, 1),
            "session_dur_s":   round(s_dur, 1),
            "segment_dur_s":   round(seg_dur, 1),
            "motion_idle_s":   round(idle, 1) if idle != float("inf") else None,
            "motion_trigger":  self._motion_trigger,
            "bag_dir":         bdir,
            "disk":            disk_status,
        }
        self._status_pub.publish(String(data=json.dumps(payload)))

    def _shutdown(self) -> None:
        with self._lock:
            if self._recording:
                self._stop_recording_locked()


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