#!/usr/bin/env python3

import sys
import os
import time
import random  # 导入 random 模块

# 添加项目根目录到 Python 路径
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

# 直接导入生成的模块
from generated.cmvr.api import biohead_command_pb2
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState

def test_biohead():
    """测试仿生头功能"""
    print("=== 测试仿生头功能 ===")

    # 初始化客户端
    client = CMVRGrpcClient("192.168.1.6:50051")
    if not client.is_connected():
        print("连接服务器失败")
        return

    # 获取仿生头
    biohead = client.get_biohead("bio_head")

    # 创建表情
    expression = FacialExpressionState()

    # 设置眉毛，使用随机数并打印
    expression.left_eyebrow_outside_y = random.uniform(0, 1)
    expression.left_eyebrow_inside_y = random.uniform(0, 1)
    expression.right_eyebrow_outside_y = random.uniform(0, 1)
    expression.right_eyebrow_inside_y = random.uniform(0, 1)
    print(f"眉毛设置: 左外 {expression.left_eyebrow_outside_y}, 左内 {expression.left_eyebrow_inside_y}, 右外 {expression.right_eyebrow_outside_y}, 右内 {expression.right_eyebrow_inside_y}")

    # 设置眼睑，使用随机数并打印
    expression.left_eye_upper_lid_y = random.uniform(0, 1)
    expression.left_eye_lower_lid_y = random.uniform(0, 1)
    expression.right_eye_upper_lid_y = random.uniform(0, 1)
    expression.right_eye_lower_lid_y = random.uniform(0, 1)
    print(f"眼睑设置: 左上 {expression.left_eye_upper_lid_y}, 左下 {expression.left_eye_lower_lid_y}, 右上 {expression.right_eye_upper_lid_y}, 右下 {expression.right_eye_lower_lid_y}")

    # 设置眼球，使用随机数并打印
    expression.left_eye_ball_x = random.uniform(0, 1)
    expression.left_eye_ball_y = random.uniform(0, 1)
    expression.right_eye_ball_x = random.uniform(0, 1)
    expression.right_eye_ball_y = random.uniform(0, 1)
    print(f"眼球设置: 左X {expression.left_eye_ball_x}, 左Y {expression.left_eye_ball_y}, 右X {expression.right_eye_ball_x}, 右Y {expression.right_eye_ball_y}")

    # 设置嘴巴，使用随机数并打印
    expression.upper_lip_y = random.uniform(0, 1)
    expression.lower_lip_y = random.uniform(0, 1)
    print(f"嘴巴设置: 上唇 {expression.upper_lip_y}, 下唇 {expression.lower_lip_y}")

    # 设置下巴，使用随机数并打印
    expression.jaw_x = random.uniform(0, 1)
    expression.jaw_y = random.uniform(0, 1)
    print(f"下巴设置: X {expression.jaw_x}, Y {expression.jaw_y}")

    # 设置表情
    result = biohead.set_expression(expression)
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("设置表情成功")
    else:
        print(f"设置表情失败，错误码: {result}")

    # 测试流式控制
    print("开始流式控制表情 (5次眨眼)...")
    result = biohead.start_stream()
    if result != CMVRErrorCode.CMVR_SUCCESS:
        print(f"开始流式会话失败，错误码: {result}")
    else:
        for i in range(5):
            # 创建 protobuf 消息用于流式传输
            proto_expr = biohead_command_pb2.FacialExpression()

            # 睁眼，使用随机数并打印
            proto_expr.eyelid.left_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.left_lower_y = random.uniform(0, 1)
            proto_expr.eyelid.right_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.right_lower_y = random.uniform(0, 1)
            print(f"第{i+1}次流式睁眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}")

            result = biohead.stream_expression(proto_expr)
            if result == CMVRErrorCode.CMVR_SUCCESS:
                print(f"第{i+1}次流式睁眼成功")
            else:
                print(f"第{i+1}次流式睁眼失败，错误码: {result}")

            time.sleep(0.5)

            # 闭眼，使用随机数并打印
            proto_expr.eyelid.left_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.left_lower_y = random.uniform(0, 1)
            proto_expr.eyelid.right_upper_y = random.uniform(0, 1)
            proto_expr.eyelid.right_lower_y = random.uniform(0, 1)
            print(f"第{i+1}次流式闭眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}")

            result = biohead.stream_expression(proto_expr)
            if result == CMVRErrorCode.CMVR_SUCCESS:
                print(f"第{i+1}次流式闭眼成功")
            else:
                print(f"第{i+1}次流式闭眼失败，错误码: {result}")

            time.sleep(0.2)

        # 结束流式会话
        result = biohead.end_stream()
        if result == CMVRErrorCode.CMVR_SUCCESS:
            print("结束流式会话成功")
        else:
            print(f"结束流式会话失败，错误码: {result}")

        print("流式控制完成")

    # 获取系统状态
    result = biohead.get_system_status()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("获取系统状态成功")
    else:
        print(f"获取系统状态失败，错误码: {result}")

    # 测试紧急停止
    result = biohead.emergency_stop()
    if result == CMVRErrorCode.CMVR_SUCCESS:
        print("紧急停止成功")
    else:
        print(f"紧急停止失败，错误码: {result}")

    client.close()

if __name__ == "__main__":
    test_biohead()