from robodk.robolink import *
from robodk.robodialogs import *
from robodk.robofileio import *



LOAD_AS_PROGRAM = True
REFERENCE_NAME = 'Reffere to'
TARGET_NAME = 'Home'
RDK = Robolink()
path_file = r'C:/Users/giahn/OneDrive/Έγγραφα/Σχολή/ΔΙΠΛΩΜΑ/main/Test/program.csv'
data = LoadList(path_file)
program_name = getFileName(path_file)
#robot = RDK.Item('Comau Smart5 NJ 110-3.0', ITEM_TYPE_ROBOT)
robot = RDK.Item('', ITEM_TYPE_ROBOT)
frame = RDK.Item(REFERENCE_NAME, ITEM_TYPE_FRAME)
if not frame.Valid():
    raise Exception("Failed to retrieve the reference frame. Check frame configuration.")
robot.setPoseFrame(frame)
 
target = RDK.Item(TARGET_NAME, ITEM_TYPE_TARGET)
if not target.Valid():
    raise Exception("Target not found. Ensure the name is correct.")

if not target.Valid():
    raise Exception("Home target is not valid. Set a home target named: %s" % TARGET_NAME)

# Set the robot to the home position


# Get the pose reference from the home target
robot.setJoints(target.Joints())
pose_ref = robot.Pose()

if LOAD_AS_PROGRAM:
    # Add a new program
    program = RDK.AddProgram(program_name, robot)

    # Turn off rendering (faster)
    RDK.Render(False)
    program.ShowInstructions(False)
    current_speed = None
    target = None

    program.setPoseFrame(frame)
    program.setPoseTool(robot.PoseTool())

    # Iterate through all the points
    for i in range(len(data)):
        pi = pose_ref
        pi.setPos(data[i])

        # Update speed if there is a 4th column
        if len(data[i]) >= 3:
            speed = data[i][3]
            # Update the program if the speed is different than the previously set speed
            if type(speed) != str and speed != current_speed:
                program.setSpeed(speed)
                current_speed = speed

        target = RDK.AddTarget('T%i' % i, frame)
        target.setPose(pi)
        pi = target

        # Add a linear movement (with the exception of the first point which will be a joint movement)
        if i == 0:
            program.MoveJ(pi)
        else:
            program.MoveL(pi)

        # Update from time to time to notify the user about progress
        if i % 100 == 0:
            program.ShowTargets(False)
            RDK.ShowMessage("Loading %s: %.1f %%" % (program_name, 100 * i / len(data)), False)
            RDK.Render()

    program.ShowTargets(False)

else:
    # Very important: Make sure we set the reference frame and tool frame so that the robot is aware of it
    robot.setPoseFrame(frame)
    robot.setPoseTool(robot.PoseTool())

    # Remember the speed so that we don't set it with every instruction
    current_speed = None

    # Iterate through all the points
    for i in range(len(data)):
        pi = pose_ref
        pi.setPos(data[i][0:3])

        # Update speed if there is a 4th column
        if len(data[i]) >= 3:
            speed = data[i][3]
            # Update the program if the speed is different than the previously set speed
            if type(speed) != str and speed != current_speed:
                robot.setSpeed(speed)
                current_speed = speed

        # Add a linear movement (with the exception of the first point which will be a joint movement)
        if i == 0:
            robot.MoveJ(pi)
        else:
            robot.MoveL(pi)

        # Update from time to time to notify the user about progress
        #if i % 200 == 0:
        RDK.ShowMessage("Program %s: %.1f %%" % (program_name, 100 * i / len(data)), False)

RDK.ShowMessage("Done", False)
print("Done")