"""
EZAI-Go2 视频流获取示例
演示如何初始化Go2机器狗并获取实时视频流
"""

import os
import time
import cv2
import ezai.go2 as go2
import ezai.ui as ui









def get_video_stream(window, image_label, status_label, go2_robot):
    """获取并显示Go2视频流"""
    consecutive_failures = 0
    max_failures = 10
    
    # 检查视频流是否初始化
    if go2_robot.cap is None:
        status_label.set_text("状态: 视频流未初始化，请检查Go2连接")
        print("视频流未初始化，停止视频流线程")
        return
    
    while window.running:
        try:
            # 获取Go2机器狗视频流
            image = go2_robot.read_image()
            
            if image is not None:
                # 显示视频流
                image_label.set_image(image)
                status_label.set_text(f"状态: 视频流正常 | 分辨率: {image.shape[1]}x{image.shape[0]}")
                consecutive_failures = 0  # 重置失败计数
            else:
                consecutive_failures += 1
                if consecutive_failures < max_failures:
                    status_label.set_text(f"状态: 无法获取视频流 ({consecutive_failures}/{max_failures})")
                else:
                    status_label.set_text("状态: 视频流连接失败，请检查Go2连接")
                    time.sleep(1)  # 长时间等待
            
        except Exception as e:
            consecutive_failures += 1
            status_label.set_text(f"状态: 视频流异常 - {str(e)[:30]}...")
            print(f"视频流异常: {e}")
            
        # 控制帧率，避免过度占用CPU
        time.sleep(0.03)  # 约30fps
        
        
        



def main():
    """主函数"""
    # 设置显示环境
    os.environ["DISPLAY"] = ":0"
    
    # 创建UI窗口
    window = ui.OpenCVUI("Go2 Video Stream", 640, 520)
    
    # 显示视频流的标签
    image_label = ui.Label(10, 10, 620, 400, align="center")
    window.add_widget(image_label)
    
    # 显示状态信息的标签
    status_label = ui.Label(10, 420, 620, 40, text="状态: 初始化中...", align="center", font_size=16)
    window.add_widget(status_label)
    
    # 退出按钮
    quit_button = ui.Button(270, 470, 100, 30, "退出", font_size=14, command=window.destroy)
    window.add_widget(quit_button)
    
    # 初始化Go2机器狗
    print("正在初始化Go2机器狗...")
    go2_robot = go2.Go2()
    
    # 显示网络接口信息
    if go2_robot.interface:
        print(f"找到网络接口: {go2_robot.interface}")
        status_label.set_text(f"状态: 已连接到 {go2_robot.interface}")
    else:
        print("未找到Go2网络接口")
        status_label.set_text("状态: 未找到Go2网络接口")
    
    # 初始化连接
    try:
        go2_robot.init()
        print("Go2初始化成功")
        status_label.set_text(f"状态: Go2已连接 ({go2_robot.interface})")
    except Exception as e:
        print(f"Go2初始化失败: {e}")
        status_label.set_text(f"状态: 连接失败 - {e}")
        # 即使初始化失败，也继续运行，让用户可以看到错误信息
    
    # 启动视频流线程
    from threading import Thread
    thread = Thread(target=get_video_stream, args=(window, image_label, status_label, go2_robot))
    thread.daemon = True
    thread.start()
    
    # 启动主循环
    print("视频流已启动，按退出按钮关闭")
    window.mainloop()

if __name__ == "__main__":
    main()





