"""photo_capture_node.py — Photo capture service for SaltyBot (Issue #456).

Subscribes to /camera/color/image_raw and captures photos in modes:
  - manual: voice command or gamepad button trigger
  - timelapse: configurable interval (e.g., every 30s)
  - event-triggered: on person detection, encounter, or trick execution

Features: JPEG storage, FIFO management (10GB cap), GPS overlay, NAS sync.
"""

from __future__ import annotations

import json
import os
import shutil
import threading
import time
from datetime import datetime
from pathlib import Path
from typing import Optional

try:
    import cv2
    from cv_bridge import CvBridge
except ImportError:
    cv2 = None
    CvBridge = None

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image
from std_msgs.msg import String

try:
    from saltybot_social_msgs.msg import VoiceCommand
    _HAS_VOICE = True
except ImportError:
    _HAS_VOICE = False


class PhotoCaptureNode(Node):
    """Multi-mode photo capture service for SaltyBot."""

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

        # Parameters
        self.declare_parameter("photo_dir", "/home/seb/saltybot-data/photos/")
        self.declare_parameter("max_storage_gb", 10.0)
        self.declare_parameter("timelapse_interval", 30.0)
        self.declare_parameter("enable_timelapse", True)
        self.declare_parameter("enable_event_capture", True)
        self.declare_parameter("enable_gps_overlay", False)
        self.declare_parameter("enable_nas_sync", False)
        self.declare_parameter("nas_path", "/mnt/nas/saltybot-photos/")
        self.declare_parameter("quality", 95)

        self._photo_dir = Path(self.get_parameter("photo_dir").value)
        self._max_storage_bytes = self.get_parameter("max_storage_gb").value * 1e9
        self._timelapse_interval = self.get_parameter("timelapse_interval").value
        self._enable_timelapse = self.get_parameter("enable_timelapse").value
        self._enable_event_capture = self.get_parameter("enable_event_capture").value
        self._enable_gps = self.get_parameter("enable_gps_overlay").value
        self._enable_nas_sync = self.get_parameter("enable_nas_sync").value
        self._nas_path = Path(self.get_parameter("nas_path").value)
        self._quality = self.get_parameter("quality").value

        self._photo_dir.mkdir(parents=True, exist_ok=True)

        # QoS
        sensor_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST, depth=10
        )
        reliable_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST, depth=10
        )

        # Publishers
        self._photo_pub = self.create_publisher(
            String, "/saltybot/photo_captured", reliable_qos
        )

        # Subscribers
        self._image_sub = self.create_subscription(
            Image, "/camera/color/image_raw", self._on_image, sensor_qos
        )
        self._manual_sub = self.create_subscription(
            String, "/saltybot/save_photo", self._on_manual_trigger, reliable_qos
        )
        if _HAS_VOICE:
            self._voice_sub = self.create_subscription(
                VoiceCommand, "/saltybot/voice_command",
                self._on_voice_command, reliable_qos
            )
        self._trick_sub = self.create_subscription(
            String, "/saltybot/trick_state", self._on_trick_state, reliable_qos
        )

        # State
        self._latest_frame: Optional[Image] = None
        self._bridge = CvBridge() if CvBridge else None
        self._last_timelapse_time = time.time()
        self._lock = threading.Lock()

        # Timers
        if self._enable_timelapse:
            self.create_timer(self._timelapse_interval, self._capture_timelapse)
        self.create_timer(60.0, self._check_storage)

        self.get_logger().info(
            f"photo_capture_node ready (dir={self._photo_dir})"
        )

    def _on_image(self, msg: Image) -> None:
        """Cache latest camera frame."""
        with self._lock:
            self._latest_frame = msg

    def _on_manual_trigger(self, msg: String) -> None:
        """Handle manual photo capture trigger."""
        self.get_logger().info(f"Manual capture: {msg.data}")
        self._capture_photo(f"manual-{msg.data}")

    def _on_voice_command(self, msg) -> None:
        """Handle voice commands for photo capture."""
        if not _HAS_VOICE:
            return
        intent = msg.intent.lower()
        if "photo" in intent or "capture" in intent or "picture" in intent:
            self._capture_photo("voice-command")

    def _on_trick_state(self, msg: String) -> None:
        """Capture photo on trick execution."""
        if not self._enable_event_capture:
            return
        state = msg.data.lower()
        if "executing:" in state:
            trick = state.split(":")[-1]
            self._capture_photo(f"event-{trick}")

    def _capture_timelapse(self) -> None:
        """Periodic timelapse capture."""
        if self._enable_timelapse:
            self._capture_photo("timelapse")

    def _capture_photo(self, mode: str) -> None:
        """Capture and save photo with metadata."""
        with self._lock:
            if self._latest_frame is None or not cv2 or not self._bridge:
                return

            try:
                cv_image = self._bridge.imgmsg_to_cv2(
                    self._latest_frame, desired_encoding="bgr8"
                )
                timestamp = datetime.now()
                filename = f"{timestamp.strftime('%Y%m%d_%H%M%S_%f')[:-3]}_{mode}.jpg"
                filepath = self._photo_dir / filename

                if self._enable_gps:
                    self._add_overlay(cv_image, timestamp, mode)

                cv2.imwrite(
                    str(filepath), cv_image,
                    [cv2.IMWRITE_JPEG_QUALITY, self._quality]
                )

                metadata = {
                    "timestamp": timestamp.isoformat(),
                    "mode": mode,
                    "size": filepath.stat().st_size,
                }
                self._photo_pub.publish(String(data=json.dumps(metadata)))
                self.get_logger().info(f"Captured: {filename}")

                if self._enable_nas_sync:
                    self._sync_to_nas(filepath)

            except Exception as e:
                self.get_logger().error(f"Capture failed: {e}")

    def _add_overlay(self, image: any, timestamp: datetime, mode: str) -> None:
        """Add timestamp and mode overlay to image."""
        if not cv2:
            return
        height, width = image.shape[:2]
        timestamp_str = timestamp.strftime("%Y-%m-%d %H:%M:%S")
        cv2.putText(image, timestamp_str, (10, 30),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
        cv2.putText(image, f"Mode: {mode}", (10, height - 10),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)

    def _check_storage(self) -> None:
        """Monitor storage and purge oldest photos if exceeding capacity."""
        try:
            total_size = sum(
                f.stat().st_size for f in self._photo_dir.glob("*.jpg")
            )
            if total_size > self._max_storage_bytes:
                self.get_logger().warn(
                    f"Storage limit exceeded, purging oldest"
                )
                files = sorted(
                    self._photo_dir.glob("*.jpg"),
                    key=lambda f: f.stat().st_mtime
                )
                for file in files:
                    if total_size <= self._max_storage_bytes * 0.9:
                        break
                    total_size -= file.stat().st_size
                    file.unlink()
        except Exception as e:
            self.get_logger().error(f"Storage check failed: {e}")

    def _sync_to_nas(self, filepath: Path) -> None:
        """Asynchronous WiFi sync to NAS."""
        def sync_worker():
            try:
                shutil.copy2(filepath, self._nas_path / filepath.name)
            except Exception as e:
                self.get_logger().warn(f"NAS sync failed: {e}")
        thread = threading.Thread(target=sync_worker, daemon=True)
        thread.start()


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


if __name__ == '__main__':
    main()
