import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix
from std_srvs.srv import Trigger
import os
import datetime
import cv2  # 需要安装 python3-opencv
import threading

class CameraNode(Node):
    def __init__(self):
        super().__init__('camera_driver_node')

        # 声明参数
        self.declare_parameter('video_device', 0) # OpenCV通常使用索引号，如 0 代表 /dev/video0
        self.declare_parameter('save_dir', './camera_data')
        self.declare_parameter('gps_topic', '/gps_info')
        self.declare_parameter('interval', 5.0)
        self.declare_parameter('image_width', 640)
        self.declare_parameter('image_height', 480)
        
        # 新增调试参数
        self.declare_parameter('frame_rate', 30.0)
        self.declare_parameter('fourcc', 'MJPG')
        self.declare_parameter('auto_exposure', True)
        self.declare_parameter('exposure_value', -5.0) # 默认值根据实际情况调整

        # 获取参数
        self.camera_index = self.get_parameter('video_device').value
        self.save_dir = self.get_parameter('save_dir').value
        self.gps_topic = self.get_parameter('gps_topic').value
        self.interval = self.get_parameter('interval').value

        # 初始化 OpenCV 摄像头，强制使用 V4L2 后端
        self.cap = cv2.VideoCapture(self.camera_index, cv2.CAP_V4L2)
        
        # 设置格式
        fourcc_str = self.get_parameter('fourcc').value
        if len(fourcc_str) == 4:
            self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*fourcc_str))
            
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.get_parameter('image_width').value)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.get_parameter('image_height').value)
        self.cap.set(cv2.CAP_PROP_FPS, self.get_parameter('frame_rate').value)
        
        # 设置曝光
        auto_exp = self.get_parameter('auto_exposure').value
        if auto_exp:
            self.cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 3) # 通常 3=Auto, 1=Manual (具体值取决于后端，V4L2通常是 3 和 1)
        else:
            self.cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 1) # 手动模式
            self.cap.set(cv2.CAP_PROP_EXPOSURE, self.get_parameter('exposure_value').value)

        if not self.cap.isOpened():
            self.get_logger().error("无法打开摄像头！")
            return

        # 创建目录
        os.makedirs(self.save_dir, exist_ok=True)

        self.latest_gps = None
        self.latest_frame = None
        self.frame_lock = threading.Lock() # 线程锁，防止读写冲突

        # 启动一个后台线程持续读取图片流
        self.capture_thread = threading.Thread(target=self._stream_reader, daemon=True)
        self.capture_thread.start()

        # 订阅/服务/定时器
        self.gps_sub = self.create_subscription(NavSatFix, self.gps_topic, self.gps_callback, 10)
        self.srv = self.create_service(Trigger, 'trigger_photo', self.trigger_callback)
        self.timer = self.create_timer(self.interval, self.timer_callback)

    def _stream_reader(self):
        """后台线程：不断清空缓冲区并保存最新的一帧"""
        while rclpy.ok() and self.cap.isOpened():
            ret, frame = self.cap.read()
            if ret:
                with self.frame_lock:
                    self.latest_frame = frame

    def gps_callback(self, msg):
        self.latest_gps = msg

    def trigger_callback(self, request, response):
        success, message = self.take_photo()
        response.success = success
        response.message = message
        return response

    def timer_callback(self):
        if self.interval > 0:
            self.take_photo()

    def take_photo(self):
        with self.frame_lock:
            if self.latest_frame is None:
                return False, "No frame available yet"
            # 复制当前最新帧，防止保存时被后台线程修改
            frame_to_save = self.latest_frame.copy()

        # 处理文件名和 GPS
        now = datetime.datetime.now()
        time_str = now.strftime('%Y%m%d-%H%M%S')
        gps_str = f"lat{self.latest_gps.latitude:.6f}_lon{self.latest_gps.longitude:.6f}" if self.latest_gps else "no_gps"
        
        filename = f"{time_str}-{gps_str}.jpg"
        full_path = os.path.join(self.save_dir, filename)

        # 保存图片
        cv2.imwrite(full_path, frame_to_save)
        self.get_logger().info(f'Saved photo: {filename}')
        return True, f"Saved to {full_path}"

    def destroy_node(self):
        self.cap.release()
        super().destroy_node()

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