# Type help("robolink") or help("robodk") 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/index.html
# Note: It is not required to keep a copy of this file, your python script is saved with the station
from robolink import *    # API to communicate with RoboDK
from robodk import *      # basic matrix operations
RUN_ON_ROBOT = True
# Start communication with RoboDK
RDK = Robolink()

# Ask the user to select the robot (ignores the popup if only 
ROBOT = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)

# Check if the user selected a robot
if not ROBOT.Valid():
    quit()

# Automatically retrieve active reference and tool
TOOL = ROBOT.getLink(ITEM_TYPE_TOOL)

FRAME = RDK.ItemUserPick('Select a reference frame', ITEM_TYPE_FRAME)
 #set the robot frame picked by user 
ROBOT.setPoseFrame(FRAME)

if not FRAME.Valid() or not TOOL.Valid():
    raise Exception("Select appropriate FRAME and TOOL references")

csv_file = 'C:/stepgage.csv'
codec = 'utf-8' #'ISO-8859-1'

def load_targets(strfile):
    csvdata = LoadList(strfile, ',', codec)
    poses = []
    idxs = []
    
    
    pose=Mat(4,4)
    for i in range(0, len(csvdata)):
        
        pose=Mat(4,4)
        xyz = csvdata[i][0:3]
        vx =    csvdata[i][3:6]
        vy =    csvdata[i][6:9]
        pose.setVX(vx)
        pose.setVY(vy)
        pose.setVZ(cross(vy,vx))
        pose.setPos(xyz)
        poses.append(pose)
        idxs.append(i)
       
    return poses, idxs

def load_targets_GUI(strfile):
    poses, idxs =load_targets(strfile)
    
    program_name = getFileName(strfile)
    program_name = program_name.replace('-','_').replace(' ','_')
    program = RDK.Item(program_name, ITEM_TYPE_PROGRAM)
    if program.Valid():
        program.Delete()
    program = RDK.AddProgram(program_name, ROBOT)
    program.setFrame(FRAME)
    program.setTool(TOOL)
    ROBOT.MoveJ(ROBOT.JointsHome())
    
    
    for pose, idx in zip(poses, idxs):
        name = '%s-%i' % (program_name, idx)
        target = RDK.Item(name, ITEM_TYPE_TARGET)
        if target.Valid():
            target.Delete()
        target = RDK.AddTarget(name, FRAME, ROBOT)
        target.setPose(pose)
        
        try:
          
            program.MoveJ(target)
        except:
            print('Warning: %s can not be reached. It will not be added to the program' % name)
            
# Recommended mode of operation:
# 1-Double click the python file creates a program in RoboDK station
# 2-Generate program generates the program directly

MAKE_GUI_PROGRAM = False

ROBOT.setFrame(FRAME)
ROBOT.setTool(TOOL)


if RDK.RunMode() == RUNMODE_SIMULATE:
    MAKE_GUI_PROGRAM = True
    # MAKE_GUI_PROGRAM = mbox('Do you want to create a new program? If not, the robot will just move along the tagets', 'Yes', 'No')
else:
    # if we run in program generation mode just move the robot
    MAKE_GUI_PROGRAM = False



if MAKE_GUI_PROGRAM:
    RDK.Render(False) # Faster if we turn render off
    load_targets_GUI(csv_file)