import grpc
from typing import Tuple

from . import generated
from .enums import CMVRErrorCode
from .models import DexHandState, RH56DFTPDexHand

class DexHandClient:
    """灵巧手客户端"""

    def __init__(self, device_id: str, stub):
        self.device_id = device_id
        self.stub = stub

    def _create_command_header(self):
        """创建命令头"""
        header = generated.common_pb2.CommandHeader.Request()
        header.device_id = self.device_id
        header.timestamp.GetCurrentTime()
        return header

    def get_status(self) -> Tuple[CMVRErrorCode, DexHandState]:
        """获取灵巧手状态"""
        try:
            request = generated.dexhand_service_pb2.GetDexHandStateCommand.Request()
            request.header.CopyFrom(self._create_command_header())

            response = self.stub.GetStatus(request)

            if not response.header.success:
                return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState()

            state = DexHandState(
                is_initialized=response.state.is_initialized
            )

            # 填充手指状态
            for i, hand_state in enumerate(response.state.hands):
                if i < len(state.hands):
                    state.hands[i] = RH56DFTPDexHand(
                        angle=hand_state.angle,
                        speed=hand_state.speed,
                        force=hand_state.force,
                        position=hand_state.position,
                        current=hand_state.current,
                        temperature=hand_state.temperature,
                        error=hand_state.error,
                        error_message=list(hand_state.error_message)
                    )

            return CMVRErrorCode.CMVR_SUCCESS, state

        except grpc.RpcError as e:
            print(f"获取灵巧手状态失败: {e}")
            return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState()

    def set_position(self, dof_id: int, value: float) -> CMVRErrorCode:
        """设置位置"""
        try:
            request = generated.dexhand_service_pb2.SetDexHandPositionsCommand.Request()
            request.header.CopyFrom(self._create_command_header())

            # 添加自由度值
            freedom_value = request.values.add()
            freedom_value.id = dof_id
            freedom_value.value = value

            response = self.stub.SetDexHandPos(request)

            return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED

        except grpc.RpcError as e:
            print(f"设置位置失败: {e}")
            return CMVRErrorCode.CMVR_RPC_FAILED

    def set_preset_act(self, preset_act_id: int) -> CMVRErrorCode:
        """设置预设动作"""
        try:
            request = generated.dexhand_service_pb2.SetDexHandPresetActCommand.Request()
            request.header.CopyFrom(self._create_command_header())
            request.presetactid = preset_act_id

            response = self.stub.SetDexHandPresetAct(request)

            return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED

        except grpc.RpcError as e:
            print(f"设置预设动作失败: {e}")
            return CMVRErrorCode.CMVR_RPC_FAILED