from robolink import *
from robodk import *
import threading

# Start communication with RoboDK
RDK = Robolink()

# Ask the user to select the robot
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
FRAME = ROBOT.getLink(ITEM_TYPE_FRAME)
TOOL = ROBOT.getLink(ITEM_TYPE_TOOL)

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

# Ask the user to select the csv file
csv_file = getOpenFile(RDK.getParam('PATH_OPENSTATION'))

# Specify file codec
codec = 'utf-8-sig'

# Load joint data from csv file
def load_targets(csv_file):
    joint_data = []
    with open(csv_file.name, 'r', encoding=codec) as file:
        for line in file:
            values = line.strip().split(',')
            joints = [float(j) for j in values]
            joint_data.append(joints)
    return joint_data

# Convert joint data to cartesian coordinates and print the results
def joints_to_cartesian(joints):
    pose = ROBOT.SolveFK(joints)
    pos, rvec = Pose_2_TxyzRxyz(pose)
    orient = tr2rpy(rvec)
    print("Joint coordinates: ", joints)
    print("Cartesian coordinates: ", [pos[0], pos[1], pos[2]])
    print("Orientation (in degrees): ", [orient[0]*180/pi, orient[1]*180/pi, orient[2]*180/pi])
    print()

# Load joint data and convert to cartesian coordinates
joint_data = load_targets(csv_file)
for joints in joint_data:
    joints_to_cartesian(joints)