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 dexhand_command_pb2 as pb


handshake = [
        {"id": 0, "value": 0.0},
        {"id": 1, "value": 0.0},
        {"id": 2, "value": 0.0},
        {"id": 3, "value": 0.0},
        {"id": 4, "value": 0.0},
        {"id": 5, "value": 1.0},
    ]

hand_open = [
        {"id": 0, "value": 0.7},
        {"id": 1, "value": 0.7},
        {"id": 2, "value": 0.7},
        {"id": 3, "value": 0.7},
        {"id": 4, "value": 0.7},
        {"id": 5, "value": 1.0},
    ]

handshake_1 = [
        {"id": 0, "value": 0.0},
        {"id": 1, "value": 0.0},
        {"id": 2, "value": 0.0},
        {"id": 3, "value": 0.0},
    ]

hand_touch = [
    {"id": 0, "value": 0},
    {"id": 1, "value": 0},
    {"id": 2, "value": 0},
    {"id": 3, "value": 1.0},
    {"id": 4, "value": 0.3},
    {"id": 5, "value": 0.3},
]

class DexHandClient(RobotClientBase):
    """DexHand control client"""

    def set_angles(self, angles: list, device_id="hc01"):
        """
        angles: list of dicts, e.g.,
            [
                {"id": 0, "value": 0.5},
                {"id": 1, "value": 0.8},
                ...
            ]
        device_id: device ID
        """
        # Construct request header
        header = common_pb2.CommandHeader.Request()
        header.device_id = device_id
        ts = timestamp_pb2.Timestamp()
        ts.GetCurrentTime()
        header.timestamp.CopyFrom(ts)

        # Construct FreedomValue list
        values = [pb.FreedomValue(id=a["id"], value=a["value"]) for a in angles]

        # Construct request
        req = pb.SetDexHandAnglesCommand.Request(
            header=header,
            values=values
        )

        # Call RPC
        try:
            resp = self.stub_hand.SetDexHandAngle(req, timeout=10)
            print("SetDexHandAngle RPC call succeeded")
            print(f"Success: {resp.header.success}")
            print(f"Error message: {resp.header.error_message}")
            print(f"Timestamp: {resp.header.timestamp.seconds}")
        except Exception as e:
            print("SetDexHandAngle RPC call failed:", e)


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

    # Example: set finger angles
    angles = [
        {"id": 0, "value": 0.0},
        {"id": 1, "value": 0.0},
        {"id": 2, "value": 0.0},
        {"id": 3, "value": 1.0},

    ]

    # angles = [
    #
    #     {"id": 4, "value": 0.0},
    #     {"id": 5, "value": 1.0},
    #
    # ]

    # client.set_angles(hand_open, device_id="hand2")
    client.set_angles(hand_open, device_id="hand2")
    # client.set_angles(handshake, device_id="hand1")

    # Close client
    client.close()