import sys
import time
from google.protobuf import timestamp_pb2

sys.path.append("../generated")

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


class GetJointStateClient(RobotClientBase):
    """Continuous client to fetch robot joint states"""

    def send(self, interval=0.5, device_id="hc01"):
        """Continuously fetch joint states and print in fixed order"""
        try:
            while True:
                # Construct request
                req = pb.JointRequest()
                req.header.device_id = device_id
                timestamp = timestamp_pb2.Timestamp()
                timestamp.GetCurrentTime()
                req.header.timestamp.CopyFrom(timestamp)

                # Call RPC with timeout
                try:
                    resp = self.stub.getJointState(req, timeout=10)
                except Exception as e:
                    print(f"RPC call failed: {e}")
                    time.sleep(interval)
                    continue

                # Fixed order for printing
                joint_order = [
                    "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
                    "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
                    "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
                    "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
                    "HEAD_P", "HEAD_Y","HEAD_R"
                ]


                # Map response to dictionary
                joint_dict = {}
                for state in resp.state:
                    for name, pos in zip(state.name, state.position):
                        joint_dict[name] = round(pos, 12)

                joint_list = [{"joint_name": name, "rad": joint_dict.get(name, 0.0)}
                              for name in joint_order]

                # Print timestamp
                print(f"[{time.strftime('%H:%M:%S')}]")
                # Print each joint, last one without comma
                for i, j in enumerate(joint_list):
                    if i < len(joint_list) - 1:
                        print(f"{j},")
                    else:
                        print(f"{j}")

                time.sleep(interval)

        except KeyboardInterrupt:
            print("\nStopped fetching joint states.")


if __name__ == "__main__":
    client = GetJointStateClient()
    try:
        client.send(interval=0.5)
    finally:
        client.close()