import sys
from google.protobuf import timestamp_pb2

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

from generated.cmvr.api import humanoid_robot_command_pb2 as pb
from generated.cmvr.api import common_pb2
from generated.cmvr.api import humanoid_robot_service_pb2_grpc as rpc
from clients.base_client import RobotClientBase
import math
from typing import List, Dict, Literal



class MoveJClient(RobotClientBase):
    """Client to send MoveJ commands to the robot"""

    def send(self, joint_list, vel=1.0, acc=0.5, device_id="hc01"):
        """
        joint_list: list of dicts, e.g.,
            [
                {"joint_name": "L_SHOULDER_P", "rad": 0.0},
                {"joint_name": "L_SHOULDER_R", "rad": -1.31873},
                ...
            ]
        vel: velocity
        acc: acceleration
        device_id: device ID
        """
        # Construct JointCmd list
        cmds = [pb.JointCmd(joint_name=j["joint_name"], rad=j["rad"], vel=vel) for j in joint_list]

        # Construct request header
        header = common_pb2.CommandHeader.Request()
        header.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        header.timestamp.CopyFrom(ts)

        # Construct MoveJ request
        req = pb.MoveJ.Request(
            header=header,
            vel=vel,
            acc=acc,
            cmds=cmds
        )

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





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

    end_joint_list = [

        {'joint_name': 'R_WRIST_R', 'rad': 0.32396421236},
        {'joint_name': 'R_WRIST_Y', 'rad': 0.098841140792},
        {'joint_name': 'R_WRIST_P', 'rad': -17.626296421526},
        {'joint_name': 'R_ELBOW_R', 'rad': 1.502654735917},
        {'joint_name': 'R_SHOULDER_Y', 'rad': -4.76186081004},
        {'joint_name': 'R_SHOULDER_R', 'rad': -0.484371520195},
        {'joint_name': 'R_SHOULDER_P', 'rad': 0.154464081527},
    ]


    start_joint_list = [
        {'joint_name': 'R_WRIST_R', 'rad': 0.315772223376},
        {'joint_name': 'R_WRIST_Y', 'rad': 0.08855986238},
        {'joint_name': 'R_WRIST_P', 'rad': -17.291334250373},
        {'joint_name': 'R_ELBOW_R', 'rad': 1.66502982975},
        {'joint_name': 'R_SHOULDER_Y', 'rad': -4.758606796339},
        {'joint_name': 'R_SHOULDER_R', 'rad': -0.830841394993},
        {'joint_name': 'R_SHOULDER_P', 'rad': -0.007418353872},
    ]

    # client.send(start_joint_list, vel=2.0, acc=1.0)
    while True:
        client.send(start_joint_list, vel=2.0, acc=1.0)
        client.send(end_joint_list, vel=1.5, acc=1.5)
    # client.send(end_joint_list, vel=1.5, acc=1.5)

    # Send MoveJ command
    # client.send(start, vel=0.8, acc=0.8)
    # client.send(end, vel=0.8, acc=0.8)
    # client.send(left_arm, vel=head_speed, acc=0.8)
    # while True:
    #     client.send(action_hello_start, vel=5.0, acc=0.8)
    #     client.send(action_hello_end, vel=5.0, acc=0.8)
    # client.send(action_touch_start, vel=1.5, acc=0.8)
    # client.send(action_touch_set, vel=1.5, acc=0.8)
    # client.send(action_touch_start, vel=1.5, acc=0.8)
    # client.send(head_dd, vel=1.5, acc=0.8)
    # client.send(joint_list, vel=1.0, acc=0.8)
    # while True:
    #     client.send(action_hello_start,vel=5.0, acc=0.8)
    #     client.send(action_hello_end,vel=5.0, acc=0.8)
    #     client.send(action_hello_start, vel=5.0, acc=0.8)


    # Close client
    client.close()