"""
EZAI-Go2 诊断工具
详细检查Go2连接和视频流问题
"""

import time
import ezai.go2 as go2

def diagnose_go2():
    """完整诊断Go2连接问题"""
    print("=== EZAI-Go2 完整诊断 ===")
    
    # 1. 检查导入
    print("1. 检查模块导入...")
    try:
        print("   ezai.go2 导入成功")
        print(f"   Go2 类: {go2.Go2}")
    except Exception as e:
        print(f"   模块导入失败: {e}")
        return
    
    # 2. 创建实例
    print("\n2. 创建Go2实例...")
    try:
        robot = go2.Go2()
        print("   Go2实例创建成功")
    except Exception as e:
        print(f"   实例创建失败: {e}")
        return
    
    # 3. 检查网络接口
    print("\n3. 检查网络接口...")
    if hasattr(robot, 'interface') and robot.interface:
        print(f"   找到网络接口: {robot.interface}")
    else:
        print("   未找到网络接口")
        return
    
    # 4. 检查初始化前的方法
    print("\n4. 检查可用方法...")
    methods = [method for method in dir(robot) if not method.startswith('_')]
    print(f"   可用方法: {methods}")
    
    # 5. 尝试初始化
    print("\n5. 尝试初始化...")
    try:
        robot.init()
        print("   初始化成功")
    except Exception as e:
        print(f"   初始化失败: {e}")
        import traceback
        traceback.print_exc()
        return
    
    # 6. 检查初始化后的属性
    print("\n6. 检查初始化后的属性...")
    attributes = ['sport_client', 'cap', 'video_client']
    for attr in attributes:
        if hasattr(robot, attr):
            value = getattr(robot, attr)
            print(f"   {attr}: {type(value)} - {value}")
        else:
            print(f"   {attr}: 不存在")
    
    # 7. 测试运动控制
    print("\n7. 测试运动控制...")
    try:
        if hasattr(robot, 'Move'):
            print("   Move 方法存在")
            # 不实际移动，只测试方法调用
            print("   运动控制方法可用")
        else:
            print("   Move 方法不存在")
    except Exception as e:
        print(f"   运动控制测试失败: {e}")
    
    # 8. 测试视频流
    print("\n8. 测试视频流...")
    if hasattr(robot, 'cap') and robot.cap is not None:
        print("   视频流对象存在")
        
        # 检查视频流状态
        try:
            is_opened = robot.cap.isOpened()
            print(f"   视频流状态: {'打开' if is_opened else '关闭'}")
            
            if is_opened:
                # 尝试读取一帧
                ret, frame = robot.cap.read()
                if ret:
                    print(f"   测试帧读取成功: {frame.shape}")
                else:
                    print("   测试帧读取失败")
            else:
                print("   视频流未打开")
                
        except Exception as e:
            print(f"   视频流测试失败: {e}")
    else:
        print("   视频流对象不存在或为None")
    
    # 9. 网络诊断
    print("\n9. 网络诊断...")
    try:
        import socket
        import netifaces
        
        # 检查网络接口列表
        interfaces = netifaces.interfaces()
        print(f"   可用网络接口: {interfaces}")
        
        # 检查指定接口的IP
        if robot.interface in interfaces:
            addrs = netifaces.ifaddresses(robot.interface)
            if netifaces.AF_INET in addrs:
                ip_info = addrs[netifaces.AF_INET][0]
                print(f"   接口IP: {ip_info['addr']}")
            else:
                print("   接口无IPv4地址")
        
        # 测试组播连接
        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        sock.settimeout(2.0)
        try:
            # 尝试绑定到组播地址
            sock.bind(('230.1.1.1', 1720))
            print("   组播端口绑定成功")
        except Exception as e:
            print(f"   组播端口绑定失败: {e}")
        finally:
            sock.close()
            
    except Exception as e:
        print(f"   网络诊断失败: {e}")
    
    # 10. 清理
    print("\n10. 清理资源...")
    try:
        if hasattr(robot, 'disconnect'):
            robot.disconnect()
            print("   连接已断开")
        if hasattr(robot, 'cap') and robot.cap is not None:
            robot.cap.release()
            print("   视频流已释放")
    except Exception as e:
        print(f"   清理失败: {e}")
    
    print("\n=== 诊断完成 ===")

def test_gstreamer():
    """测试GStreamer管道"""
    print("\n=== GStreamer 测试 ===")
    
    try:
        import cv2
        
        # 测试GStreamer支持
        print("1. 检查OpenCV GStreamer支持...")
        gstreamer_support = cv2.getCvBuildInformation().find('GStreamer') != -1
        print(f"   GStreamer支持: {'是' if gstreamer_support else '否'}")
        
        if not gstreamer_support:
            print("   OpenCV未编译GStreamer支持，无法使用视频流")
            return
        
        # 测试GStreamer管道
        interface = "enx00e0986113a6"  # 你的接口
        gstreamer_str = (
            f"udpsrc address=230.1.1.1 port=1720 multicast-iface={interface} "
            "! application/x-rtp, media=video, encoding-name=H264 "
            "! rtph264depay ! h264parse "
            "! avdec_h264 "
            "! videoscale "
            "! video/x-raw,width=480,height=320 "
            "! videoconvert ! video/x-raw, format=BGR "
            "! appsink drop=1"
        )
        
        print(f"2. 测试GStreamer管道...")
        print(f"   管道: {gstreamer_str}")
        
        cap = cv2.VideoCapture(gstreamer_str, cv2.CAP_GSTREAMER)
        if cap.isOpened():
            print("   GStreamer管道打开成功")
            
            # 尝试读取几帧
            for i in range(3):
                ret, frame = cap.read()
                if ret:
                    print(f"   帧{i+1}: 成功 {frame.shape}")
                else:
                    print(f"   帧{i+1}: 失败")
                time.sleep(0.5)
            
            cap.release()
        else:
            print("   GStreamer管道打开失败")
            
    except Exception as e:
        print(f"   GStreamer测试失败: {e}")
        import traceback
        traceback.print_exc()

if __name__ == "__main__":
    print("选择诊断模式:")
    print("1. Go2完整诊断")
    print("2. GStreamer测试")
    print("3. 全部诊断")
    
    try:
        choice = input("请选择 (1/2/3): ").strip()
        
        if choice == "1":
            diagnose_go2()
        elif choice == "2":
            test_gstreamer()
        elif choice == "3":
            diagnose_go2()
            test_gstreamer()
        else:
            print("运行Go2完整诊断...")
            diagnose_go2()
            
    except KeyboardInterrupt:
        print("\n诊断被用户中断")
    except Exception as e:
        print(f"诊断异常: {e}")
        import traceback
        traceback.print_exc()