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

RDK = robolink.Robolink()


def offline(num_movements:int, target_names: list[str]) -> None:
    robot = RDK.Item("", robolink.ITEM_TYPE_ROBOT)
    for i in range(num_movements):
            robot.MoveJ(RDK.Item(target_names[i]))
            RDK.ShowMessage("Completed movement %s" % i)


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.PoseAbs()


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()

    offline(num_movements=args.num_movements, target_names=args.target_names)


if __name__ == "__main__":
    main()