#!/usr/bin/env python3

import sys
import os
import time
import keyboard
import grpc
import numpy as np

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, JointCmd, Pose3D, RobotCartesian, RobotJointIndexDirection

def test_humanoid_robot():
    """循环获取所有电机状态，按q退出，每个电机信息单独一行并按指定格式打印"""
    print("电机状态监控 (按q退出)\n")
    client = CMVRGrpcClient("192.168.0.222:50052")
    
    if not client.is_connected():
        print("连接服务器失败")
        return

    humanoidRobot = client.get_humanoid_robot("hc01")
    if humanoidRobot is None:
        print("获取机器人失败")
        return

    try:
        while True:
            if keyboard.is_pressed('q'):
                print("\n退出程序...")
                break
                
            # 获取所有关节状态
            result, joint_states = humanoidRobot.get_joint_states()
            if result == CMVRErrorCode.CMVR_SUCCESS and joint_states:
                # 遍历所有状态组和电机
                for status in joint_states:
                    for j, name in enumerate(status.name):
                        pos = status.position[j] if j < len(status.position) else 0.0
                        # 每个电机信息单独一行，按指定格式打印并以逗号结尾
                        print(f'{{"{name}",  {pos:.6f}}},')
            
            time.sleep(0.5)
            # 如需每次刷新屏幕可取消下面一行注释
            # os.system('cls' if os.name == 'nt' else 'clear')

    except Exception as e:
        print(f"\n错误: {e}")
        import traceback
        traceback.print_exc()

    finally:
        print("监控结束")


if __name__ == "__main__":
    test_humanoid_robot()