import argparse
from clients._path_setup import ensure_paths

ensure_paths()

from google.protobuf import timestamp_pb2

from clients.base_client import RobotClientBase
from cmvr.api import common_pb2
from cmvr.api import humanoid_robot_command_pb2 as pb
from cmvr.api import humanoid_robot_service_pb2_grpc as rpc


class CalibrateZeroQClient(RobotClientBase):
    """Client to send calibrateZeroQ command"""

    def send(self, device_id="hc01", joint_name="xxx"):
        # Construct request header
        header = common_pb2.CommandHeader.Request()
        header.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        header.timestamp.CopyFrom(ts)

        # Construct CalibrateZeroQ request
        req = pb.CalibrateZeroQ.Request(
            header=header,
            joint_name=joint_name
        )

        # Call RPC
        try:
            resp = self.stub.calibrateZeroQ(req, timeout=10)
            print("calibrateZeroQ RPC call succeeded")
            print(f"Success: {resp.success}")
            print(f"Error message: {resp.error_message}")
            print(f"Timestamp: {resp.timestamp.seconds}")
        except Exception as e:
            print("calibrateZeroQ RPC call failed:", e)


if __name__ == "__main__":
    # 创建命令行参数解析器
    parser = argparse.ArgumentParser(description='Send calibrateZeroQ command to robot')
    parser.add_argument('--device_id', type=str, default='hc01',
                        help='Device ID (default: hc01)')
    parser.add_argument('--joint_name', type=str, required=True,
                        help='Joint name to calibrate zero position')

    # 解析命令行参数
    args = parser.parse_args()

    # 初始化客户端
    client = CalibrateZeroQClient()

    # 发送calibrateZeroQ命令，使用命令行传入的参数
    client.send(device_id=args.device_id, joint_name=args.joint_name)

    # 关闭客户端
    client.close()