#!/usr/bin/env python3
"""
Go2视频流测试程序 - 仅显示摄像头画面
适用于灵芯派系统测试
"""

import os
import sys
import time
import cv2
import numpy as np
from threading import Thread

import ezai.go2 as go2

def test_video_stream(go2_robot):
    """测试视频流，直接使用OpenCV显示"""
    print("开始测试视频流...")
    
    while True:
        try:
            # 读取图像
            image = go2_robot.read_image()
            
            if image is None:
                print("无法读取图像，检查摄像头连接")
                time.sleep(0.1)
                continue
            
            # 显示图像
            cv2.imshow('Go2视频流', image)
            
            # 按q退出
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
                
        except Exception as e:
            print(f"视频流错误: {e}")
            time.sleep(0.1)

def main():
    # 设置显示环境（灵芯派需要）
    os.environ["DISPLAY"] = ":0"
    
    print("初始化Go2机器人...")
    
    # 创建Go2实例
    go2_robot = go2.Go2()
    print(f"网络接口: {go2_robot.interface}")
    
    # 初始化连接
    try:
        go2_robot.init()
        print("Go2连接成功")
    except Exception as e:
        print(f"Go2连接失败: {e}")
        return
    
    # 检查视频流是否可用
    try:
        test_image = go2_robot.read_image()
        if test_image is None:
            print("错误：无法读取视频流，请检查摄像头连接")
            return
        print("视频流检测成功")
    except Exception as e:
        print(f"视频流检测失败: {e}")
        return
    
    # 直接使用OpenCV显示视频流
    try:
        test_video_stream(go2_robot)
    except KeyboardInterrupt:
        print("程序被中断")
    finally:
        cv2.destroyAllWindows()
        print("程序结束")

if __name__ == "__main__":
    main()