import time

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 arm_command_pb2 as pb
from cmvr.api import arm_service_pb2_grpc as rpc


class GetPoseClient(RobotClientBase):
    """Continuous client to fetch robot pose"""

    def send(self, interval=0.5, device_id="right_arm", base_link="PELVIS_S", ee_link="R_FINGER_TIP"):
        """Continuously fetch pose and print in a fixed order"""
        try:
            while True:
                # Construct request
                req = pb.GetPose.Request()
                req.header.device_id = device_id
                req.base_link = base_link
                req.ee_link = ee_link
                timestamp = timestamp_pb2.Timestamp()
                timestamp.GetCurrentTime()
                req.header.timestamp.CopyFrom(timestamp)

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

                # Extract the pose information from the response
                pose = resp.pose
                pose_data = {
                    "x": round(pose.x, 6),
                    "y": round(pose.y, 6),
                    "z": round(pose.z, 6),
                    "rx": round(pose.rx, 6),
                    "ry": round(pose.ry, 6),
                    "rz": round(pose.rz, 6),
                }

                # Print timestamp
                print(f"[{time.strftime('%H:%M:%S')}]")
                # Print pose information
                print(f"Pose: {pose_data}")

                time.sleep(interval)

        except KeyboardInterrupt:
            print("\nStopped fetching pose.")

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