from robodk import robolink
from robodk import robomath

RDK = robolink.Robolink()

currentProg = RDK.ItemList(8)[0]

# Look at the instruction at index 6 - the first linear move in my case
n = 6
currentInst = currentProg.Instruction(n)
instName   = currentInst[0]
instType   = currentInst[1]
moveType   = currentInst[2]
isJoint    = currentInst[3]
targetPose = currentInst[4]
joints     = currentInst[5]
print(f'Index = {n}   Name={instName}   Type={instType}   moveType={moveType}')

# Get the current target position
position = targetPose.Pos()
x = position[0]
y = position[1]
z = position[2]
print(f'Original -> {x:.6f} {y:.6f} {z:.6f}')

# Change the position and update the target Mat
newX = x + 150
newY = y + 0
newZ = z + 200
print(f'Updated -> {x:.6f} {y:.6f} {z:.6f}')
targetPose = targetPose.setPos([newX, newY, newZ])

# Update the instruction
currentProg.setInstruction(n, instName, instType, moveType, isJoint, targetPose, joints)

# Just for kicks, print the new position to see if it stuck
currentInst = currentProg.Instruction(n)
targetPose = currentInst[4]
position = targetPose.Pos()
x = position[0]
y = position[1]
z = position[2]
print(f'Checked -> {x:.6f} {y:.6f} {z:.6f}')

# Try to update the display
result = currentProg.Update(1, 1000, -1, -1)
validCount = result[0]
progTime   = result[1]
progDist   = result[2]
validRatio = result[3]
message    = result[4]

print(f'Valid lines = {validCount}   Time={progTime:.1f}   Distance={progDist:.1f}   Ratio={validRatio:.3f}')
print(message)