"""
修复版本的Go2视频读取器
"""

import os
import time
import numpy as np
import cv2

class PipeVideoReader:
    """管道视频读取器类"""
    
    def __init__(self, interface):
        self.interface = interface
        self.pipe_path = "/tmp/go2_video_pipe"
        self.width, self.height = 1280, 720
        self.frame_size = self.width * self.height * 3
        self.pipe = None
        self._setup_pipe()
    
    def _setup_pipe(self):
        """设置管道"""
        # 清理旧管道
        if os.path.exists(self.pipe_path):
            os.remove(self.pipe_path)
        
        # 创建管道
        os.mkfifo(self.pipe_path)
        print(f"创建视频管道: {self.pipe_path}")
        
        # 启动GStreamer
        gst_cmd = (
            "exec gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=" + self.interface + " "
            "! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse "
            "! avdec_h264 ! videoconvert ! video/x-raw,format=BGR,width=1280,height=720 "
            "! filesink location=" + self.pipe_path + " > /dev/null 2>&1 &"
        )
        
        print("启动GStreamer视频流...")
        os.system(gst_cmd)
        time.sleep(3)
        print("GStreamer视频流启动成功")
    
    def read_frame(self):
        """读取一帧"""
        try:
            if self.pipe is None:
                self.pipe = open(self.pipe_path, 'rb')
            
            frame_data = self.pipe.read(self.frame_size)
            if len(frame_data) == self.frame_size:
                frame = np.frombuffer(frame_data, dtype=np.uint8).reshape((self.height, self.width, 3))
                return frame
            else:
                return None
        except:
            return None
    
    def cleanup(self):
        """清理资源"""
        if self.pipe is not None:
            try:
                self.pipe.close()
            except:
                pass
            self.pipe = None
        
        os.system("pkill -f gst-launch")
        if os.path.exists(self.pipe_path):
            os.remove(self.pipe_path)
        print("视频管道已关闭")

def create_video_reader(interface):
    """创建视频读取器函数"""
    reader = PipeVideoReader(interface)
    return reader.read_frame, reader.cleanup