import sys
import tty
import termios
from google.protobuf import timestamp_pb2

# sys.path.append("../generated")
sys.path.append("/home/lgv/cmvr/0-workspace/grpc_client/generated")  # 指向 generated
sys.path.append("/home/lgv/cmvr/0-workspace/grpc_client/generated/cmvr")  # 指向 cmvr 顶层

from cmvr.api import humanoid_robot_service_pb2_grpc as rpc
from cmvr.api import humanoid_robot_command_pb2 as pb
from cmvr.api import common_pb2


from base_client import RobotClientBase


class TorqueClient(RobotClientBase):
    """Client to control torque: on/off"""

    def torque_on(self, device_id="hc01"):
        req = common_pb2.CommandHeader.Request()
        req.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        req.timestamp.CopyFrom(ts)
        try:
            resp = self.stub.torqueOn(req, timeout=self.timeout)
            print("TorqueOn 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("TorqueOn RPC call failed:", e)

    def torque_off(self, device_id="hc01"):
        req = common_pb2.CommandHeader.Request()
        req.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        req.timestamp.CopyFrom(ts)
        try:
            resp = self.stub.torqueOff(req, timeout=self.timeout)
            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)


def get_single_key():
    """Read a single key from stdin without Enter"""
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch


if __name__ == "__main__":
    client = TorqueClient()
    print("Press 'o' for TorqueOn, 'p' for TorqueOff, 'q' to quit.")

    try:
        while True:
            key = get_single_key()
            if key.lower() == 'o':
                client.torque_on()
            elif key.lower() == 'p':
                client.torque_off()
            elif key.lower() == 'q':
                print("Exiting.")
                break
    finally:
        client.close()