from robolink import *
from robodk import *
import time
import argparse

RDK = robolink.Robolink()


def establish_connection() -> RDK.Item:
    robot = RDK.Item("", robolink.ITEM_TYPE_ROBOT)

    RDK.ShowMessage("Trying to connect to %s..." % robot.Name())
    robot.Connect()
    time.sleep(3)
    return robot


def move(robot: RDK.Item, num_movements: int, target_names: list[str]) -> None:
    status, status_msg = robot.ConnectedState()
    while status_msg == "Ready":
        for i in range(num_movements):
            robot.MoveJ(RDK.Item(target_names[i]))
            RDK.ShowMessage("Completed movement %s" % i)
        robot.Disconnect()


def get_pos(robot: RDK.Item):
    return robot.Pose()


def make_parser():
    parser = argparse.ArgumentParser()
    parser.add_argument("num_movements", type=int)
    parser.add_argument("target_names", nargs="+", metavar="string")
    return parser


def main():
    parser = make_parser()
    args = parser.parse_args()

    robot = establish_connection()
    move(robot=robot, num_movements=args.num_movements, target_names=args.target_names)
    print(get_pos(robot=robot))


if __name__ == "__main__":
    main()