from robolink import *    # RoboDK API
from robodk import *      # Robot toolbox
import csv
import os

# -------------------------------
# CONFIGURATION SECTION
# -------------------------------

CSV_FILE = 'C:/RoboDK/Library/Scripts/import_csv.csv'  # ← UPDATE to your CSV path
FRAME_NAME = 'CSV_Frame'
ROBOT_NAME = 'Yaskawa GP25'

# -------------------------------
# CONNECT TO RoboDK
# -------------------------------

RDK = Robolink()

# Get the robot by name (should match station robot name)
robot = RDK.Item(ROBOT_NAME, ITEM_TYPE_ROBOT)
if not robot.Valid():
    raise Exception(f'Robot "{ROBOT_NAME}" not found in RoboDK station.')

# Get or create the reference frame
frame = RDK.Item(FRAME_NAME, ITEM_TYPE_FRAME)
if not frame.Valid():
    frame = RDK.AddFrame(FRAME_NAME)
    print(f"Created reference frame: {FRAME_NAME}")
else:
    print(f"Using existing frame: {FRAME_NAME}")

robot.setPoseFrame(frame)

# -------------------------------
# READ CSV AND CREATE TARGETS
# -------------------------------

if not os.path.exists(CSV_FILE):
    raise FileNotFoundError(f"CSV file not found: {CSV_FILE}")

with open(CSV_FILE, newline='') as csvfile:
    reader = csv.reader(csvfile)
    headers = next(reader)  # Skip header line
    

    for i, row in enumerate(reader):
        if len(row) < 6:
            print(f"Skipping row {i+1}: not enough values.")
            continue

        try:
            x, y, z = float(row[0]), float(row[1]), float(row[2])
            rx, ry, rz = float(row[3]), float(row[4]), float(row[5])

            # Build the pose using translation and XYZ Euler angles
            pose = transl(x, y, z) * rotx(radians(rx)) * roty(radians(ry)) * rotz(radians(rz))
            # Create a target
            target_name = f"GP25_Target_{i+1}"
            target = RDK.AddTarget(target_name, frame)
            target.setAsCartesian()
            target.setPose(pose)

            print(f"Created target {target_name} at ({x}, {y}, {z})")

        except Exception as e:
            print(f"Error processing row {i+1}: {e}")