# Type help("robodk.robolink") or help("robodk.robomath") for more information
# Press F5 to run the script
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/robodk.html
# Note: It is not required to keep a copy of this file, your Python script is saved with your RDK project

# You can also use the new version of the API:
from robodk import robolink    # RoboDK API
from robodk import robomath    # Robot toolbox
RDK = robolink.Robolink()

# Forward and backwards compatible use of the RoboDK API:
# Remove these 2 lines to follow python programming guidelines
from robodk import *      # RoboDK API
from robolink import *    # Robot toolbox
# Link to RoboDK
# RDK = Robolink()


from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations
RDK = Robolink()

# Ask the user to select a program:
prog = RDK.ItemUserPick("Select a Program to modify", ITEM_TYPE_PROGRAM)
if not prog.Valid():
   print("Operation cancelled or no programs available")
   quit()

# Ask the user to enter a function call that will be added after each movement:
print("Program selected: " + prog.Name())
ins_call = 'SetExtSpeed'
#mbox("Enter a program call to add after each movement", entry="SynchRobot")
if not ins_call:
   print("Operation cancelled")
   quit()

# Iterate through all the instructions in a program:
ins_id = 0
ins_count = prog.InstructionCount()
while ins_id < ins_count:
   # Retrieve instruction
   ins_nom, ins_type, move_type, isjointtarget, pose, joints = prog.Instruction(ins_id)
   if ins_type == INS_TYPE_CHANGESPEED:
       # Select the movement instruction as a reference
       prog.InstructionSelect(ins_id)
       value = prog.Instruction(ins_id)
       name = value[0]
       import re
       matches = re.findall("[+-]?\d+\.\d+", name)

       # Add a new program call
       prog.RunInstruction(ins_call + '('  + matches[0] + ')', INSTRUCTION_CALL_PROGRAM)
       
       # Advance one additional instruction as we just added another instruction
       ins_id = ins_id + 1
       ins_count = ins_count + 1
       
   ins_id = ins_id + 1