#!/usr/bin/env python3
"""
调试帧数据和显示问题
"""

import os
import sys
import time
import cv2
import numpy as np

os.environ["DISPLAY"] = ":0"

def debug_frame_data():
    """调试帧数据"""
    print("=== 调试帧数据 ===")
    
    try:
        sys.path.insert(0, '..')
        import ezai.go2 as go2
        
        robot = go2.Go2()
        robot.init()
        
        read_frame, cleanup = robot.get_video_reader(method='pipe')
        
        if read_frame is None:
            print("❌ 无法创建视频读取器")
            return
        
        frame_count = 0
        
        while frame_count < 10:  # 只读取前10帧进行调试
            frame = read_frame()
            
            if frame is not None:
                frame_count += 1
                print(f"帧 {frame_count}:")
                print(f"  形状: {frame.shape}")
                print(f"  数据类型: {frame.dtype}")
                print(f"  最小值: {frame.min()}")
                print(f"  最大值: {frame.max()}")
                print(f"  平均值: {frame.mean():.2f}")
                
                # 检查是否为有效图像
                if frame.max() > 0:
                    print("  ✅ 图像数据有效")
                    
                    # 尝试不同的显示方式
                    cv2.imshow('Original', frame)
                    
                    # 尝试调整大小
                    resized = cv2.resize(frame, (640, 360))
                    cv2.imshow('Resized', resized)
                    
                    # 尝试转换颜色空间
                    if len(frame.shape) == 3 and frame.shape[2] == 3:
                        # BGR to RGB
                        rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
                        cv2.imshow('RGB', rgb)
                        
                        # BGR to Gray
                        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                        cv2.imshow('Gray', gray)
                    
                    print("  显示了4个窗口，按任意键继续...")
                    cv2.waitKey(0)
                    cv2.destroyAllWindows()
                else:
                    print("  ❌ 图像数据无效（全黑）")
                
            else:
                print(f"帧 {frame_count + 1}: 无数据")
                time.sleep(0.1)
        
        cleanup()
        
    except Exception as e:
        print(f"调试失败: {e}")
        import traceback
        traceback.print_exc()

def test_gstreamer_output():
    """测试GStreamer输出"""
    print("\n=== 测试GStreamer输出 ===")
    
    # 直接测试GStreamer是否输出正确数据
    pipe_path = "/tmp/debug_pipe"
    
    if os.path.exists(pipe_path):
        os.remove(pipe_path)
    
    os.mkfifo(pipe_path)
    
    gst_cmd = (
        "exec gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=enx00e0986113a6 "
        "! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse "
        "! avdec_h264 ! videoconvert ! video/x-raw,format=BGR,width=1280,height=720 "
        "! filesink location=" + pipe_path + " > /dev/null 2>&1 &"
    )
    
    print("启动GStreamer...")
    os.system(gst_cmd)
    time.sleep(3)
    
    try:
        frame_size = 1280 * 720 * 3
        print(f"期望帧大小: {frame_size} 字节")
        
        with open(pipe_path, 'rb') as pipe:
            for i in range(5):
                print(f"\n读取第 {i+1} 次尝试...")
                frame_data = pipe.read(frame_size)
                print(f"  读取到 {len(frame_data)} 字节")
                
                if len(frame_data) == frame_size:
                    print("  ✅ 数据长度正确")
                    
                    # 检查数据开头
                    header = frame_data[:20]
                    print(f"  前20字节: {header}")
                    
                    # 转换为图像
                    frame = np.frombuffer(frame_data, dtype=np.uint8).reshape((720, 1280, 3))
                    print(f"  图像形状: {frame.shape}")
                    print(f"  像素值范围: {frame.min()} - {frame.max()}")
                    
                    if frame.max() > 0:
                        print("  ✅ 图像有内容")
                        cv2.imshow(f'Debug Frame {i+1}', frame)
                        cv2.waitKey(1000)  # 显示1秒
                    else:
                        print("  ❌ 图像全黑")
                else:
                    print("  ❌ 数据长度不正确")
                
                time.sleep(0.5)
                
    except Exception as e:
        print(f"测试失败: {e}")
    finally:
        os.system("pkill -f gst-launch")
        if os.path.exists(pipe_path):
            os.remove(pipe_path)
        cv2.destroyAllWindows()

if __name__ == "__main__":
    print("Go2帧数据调试程序")
    print("=" * 30)
    
    print("选择调试模式:")
    print("1. 调试EZAI帧数据")
    print("2. 测试GStreamer原始输出")
    
    try:
        choice = input("请选择 (1/2): ").strip()
        
        if choice == "1":
            debug_frame_data()
        elif choice == "2":
            test_gstreamer_output()
        else:
            print("默认调试EZAI帧数据...")
            debug_frame_data()
            
    except KeyboardInterrupt:
        print("\n调试被用户中断")
    except Exception as e:
        print(f"调试异常: {e}")