import argparse
import sys

from clients._path_setup import ensure_paths

ensure_paths()


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="Run robot client actions.")
    parser.add_argument(
        "action",
        choices=["movej", "get_pose", "torque_on", "torque_off"],
        help="Action to run",
    )
    parser.add_argument("--host", default="192.168.0.222", help="gRPC server host")
    parser.add_argument("--port", type=int, default=50052, help="gRPC server port")
    parser.add_argument("--device-id", default="hc01", help="Device ID")
    parser.add_argument("--interval", type=float, default=0.05, help="Interval seconds")
    parser.add_argument("--base-link", default="PELVIS_S", help="Base link for pose")
    parser.add_argument("--ee-link", default="R_FINGER_TIP", help="End-effector link for pose")
    parser.add_argument("--vel", type=float, default=1.0, help="MoveJ velocity")
    parser.add_argument("--acc", type=float, default=0.5, help="MoveJ acceleration")
    parser.add_argument(
        "--joint-list",
        default="",
        help="MoveJ joint list format: JOINT:RAD;JOINT:RAD",
    )
    return parser.parse_args()


def main() -> int:
    args = parse_args()
    address = f"{args.host}:{args.port}"

    if args.action == "movej":
        from clients.movej_client import MoveJClient, test_joint_list

        joint_list = test_joint_list
        if args.joint_list:
            parsed = []
            for item in args.joint_list.split(";"):
                if not item:
                    continue
                if ":" not in item:
                    continue
                name, rad_str = item.split(":", 1)
                try:
                    rad = float(rad_str)
                except ValueError:
                    rad = 0.0
                parsed.append({"joint_name": name, "rad": rad})
            if parsed:
                joint_list = parsed

        client = MoveJClient(address=address)
        try:
            client.send(joint_list, vel=args.vel, acc=args.acc, device_id=args.device_id)
            print(joint_list)
        finally:
            client.close()
        return 0

    if args.action == "get_pose":
        from clients.get_pose_client import GetPoseClient

        client = GetPoseClient(address=address)
        try:
            client.send(
                interval=args.interval,
                device_id=args.device_id,
                base_link=args.base_link,
                ee_link=args.ee_link,
            )
        finally:
            client.close()
        return 0

    if args.action == "torque_on":
        from clients.torque_on_client import TorqueOnClient

        client = TorqueOnClient(address=address)
        try:
            client.send(device_id=args.device_id)
        finally:
            client.close()
        return 0

    if args.action == "torque_off":
        from clients.torque_off_client import TorqueOffClient

        client = TorqueOffClient(address=address)
        try:
            client.send(device_id=args.device_id)
        finally:
            client.close()
        return 0

    return 1


if __name__ == "__main__":
    sys.exit(main())