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 TorqueOffClient(RobotClientBase):
    """Client to send TorqueOff command"""

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

        # Call RPC
        try:
            resp = self.stub.torqueOff(req, timeout=10)
            print("TorqueOff 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("TorqueOff RPC call failed:", e)


if __name__ == "__main__":
    # Initialize client
    client = TorqueOffClient()

    # Send TorqueOff command
    client.send(device_id="hc01")

    # Close client
    client.close()