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",
            "movel",
            "speedj",
            "speedl",
            "stop_motion",
            "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="right_arm", 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="Motion velocity")
    parser.add_argument("--acc", type=float, default=0.5, help="Motion acceleration")
    parser.add_argument("--duration", type=float, default=0.2, help="Speed command duration")
    parser.add_argument("--blend-radius", type=float, default=0.0, help="MoveL blend radius")
    parser.add_argument(
        "--frame",
        choices=["base", "tool", "world", "user"],
        default="base",
        help="Cartesian command frame",
    )
    parser.add_argument(
        "--joint-list",
        default="",
        help="Joint list format: JOINT:VALUE;JOINT:VALUE",
    )
    parser.add_argument("--x", type=float, default=0.0, help="Cartesian pose x")
    parser.add_argument("--y", type=float, default=0.0, help="Cartesian pose y")
    parser.add_argument("--z", type=float, default=0.0, help="Cartesian pose z")
    parser.add_argument("--rx", type=float, default=0.0, help="Cartesian pose rx")
    parser.add_argument("--ry", type=float, default=0.0, help="Cartesian pose ry")
    parser.add_argument("--rz", type=float, default=0.0, help="Cartesian pose rz")
    parser.add_argument("--vx", type=float, default=0.0, help="Cartesian velocity x")
    parser.add_argument("--vy", type=float, default=0.0, help="Cartesian velocity y")
    parser.add_argument("--vz", type=float, default=0.0, help="Cartesian velocity z")
    parser.add_argument("--wx", type=float, default=0.0, help="Cartesian angular velocity x")
    parser.add_argument("--wy", type=float, default=0.0, help="Cartesian angular velocity y")
    parser.add_argument("--wz", type=float, default=0.0, help="Cartesian angular velocity z")
    return parser.parse_args()


def parse_joint_values(value: str, field_name: str) -> list[dict]:
    parsed = []
    for item in value.split(";"):
        if not item or ":" not in item:
            continue
        name, raw = item.split(":", 1)
        try:
            number = float(raw)
        except ValueError:
            number = 0.0
        parsed.append({"joint_name": name, field_name: number})
    return parsed


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

    if args.action == "movej":
        from clients.arm_motion_client import ArmMotionClient
        from clients.movej_client import test_joint_list

        joint_list = test_joint_list
        if args.joint_list:
            parsed = parse_joint_values(args.joint_list, "rad")
            if parsed:
                joint_list = parsed

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

    if args.action == "movel":
        from clients.arm_motion_client import ArmMotionClient

        client = ArmMotionClient(address=address)
        try:
            client.movel(
                pose={
                    "x": args.x,
                    "y": args.y,
                    "z": args.z,
                    "rx": args.rx,
                    "ry": args.ry,
                    "rz": args.rz,
                },
                vel=args.vel,
                acc=args.acc,
                blend_radius=args.blend_radius,
                frame=args.frame,
                device_id=args.device_id,
            )
        finally:
            client.close()
        return 0

    if args.action == "speedj":
        from clients.arm_motion_client import ArmMotionClient

        joint_list = parse_joint_values(args.joint_list, "velocity")
        client = ArmMotionClient(address=address)
        try:
            client.speedj(
                joint_list,
                acc=args.acc,
                duration=args.duration,
                device_id=args.device_id,
            )
            print(joint_list)
        finally:
            client.close()
        return 0

    if args.action == "speedl":
        from clients.arm_motion_client import ArmMotionClient

        client = ArmMotionClient(address=address)
        try:
            client.speedl(
                velocity={
                    "vx": args.vx,
                    "vy": args.vy,
                    "vz": args.vz,
                    "wx": args.wx,
                    "wy": args.wy,
                    "wz": args.wz,
                },
                acc=args.acc,
                duration=args.duration,
                frame=args.frame,
                device_id=args.device_id,
            )
        finally:
            client.close()
        return 0

    if args.action == "stop_motion":
        from clients.arm_motion_client import ArmMotionClient

        client = ArmMotionClient(address=address)
        try:
            client.stop_motion(device_id=args.device_id)
        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())