from robodk import robolink
import sys
import os


# Move a robot along a line given a start and end point by steps
# This macro shows three different ways of programming a robot using a Python script and the RoboDK API

# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
# For more information visit:
# https://robodk.com/doc/en/PythonAPI/robodk.html#robolink-py

# Default parameters:
P_START = [1765.0, 1.0409497792752507e-14, 1010.0, -3.1415926535897913, -1.4988010832439613e-15, -3.1415926535897927]  # Start point with respect to the robot base frame
P_END = [1765.0, 1.0409497792752507e-14, 1510.0, -3.1415926535897913, -1.4988010832439613e-15, -1]  # End point with respect to the robot base frame
NUM_POINTS = 10  # Number of points to interpolate


# Function definition to create a list of points (line)
def MakePoints(xStart, xEnd, numPoints):
    """Generates a list of points"""
    # if len(xStart) != 3 or len(xEnd) != 3:
    #     raise Exception("Start and end point must be 3-dimensional vectors")
    if numPoints < 2:
        raise Exception("At least two points are required")

    # Starting Points
    pt_list = []
    x = xStart[0]
    y = xStart[1]
    z = xStart[2]
    wx = xStart[3]
    wy = xStart[4]
    wz = xStart[5]

    # How much we add/subtract between each interpolated point
    x_steps = (xEnd[0] - xStart[0]) / (numPoints - 1)
    y_steps = (xEnd[1] - xStart[1]) / (numPoints - 1)
    z_steps = (xEnd[2] - xStart[2]) / (numPoints - 1)
    wx_steps = (xEnd[3] - xStart[3]) / (numPoints - 1)
    wy_steps = (xEnd[4] - xStart[4]) / (numPoints - 1)
    wz_steps = (xEnd[5] - xStart[5]) / (numPoints - 1)

    # Incrementally add to each point until the end point is reached
    for i in range(numPoints):
        point_i = [x, y, z, wx, wy, wz]  # create a point
        #append the point to the list
        pt_list.append(point_i)
        x = x + x_steps
        y = y + y_steps
        z = z + z_steps
        wx = wx + wx_steps
        wy = wy + wy_steps
        wz = wz + wz_steps
    return pt_list


# Copied from forum
def PoseSplit(pose1, pose2, delta_mm=10):
   """Split the move between 2 poses given delta_mm increments"""
   pose_delta = invH(pose1) * pose2
   distance = norm(pose_delta.Pos())   
   if distance <= delta_mm:
       return [pose2]
   pose_list = []
  
   x,y,z,w,p,r = Pose_2_KUKA(pose_delta)       
  
   steps = max(1,int(distance/delta_mm)) 
   xd = x/steps
   yd = y/steps
   zd = z/steps
   wd = w/steps
   pd = p/steps
   rd = r/steps
   for i in range(steps-1):
       factor = i+1
       pose_list.append( pose1 * KUKA_2_Pose([xd*factor,yd*factor,zd*factor,wd*factor,pd*factor,rd*factor]) )
      
   return pose_list

#---------------------------------------------------
#--------------- PROGRAM START ---------------------
from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations

# Generate the points curve path
POINTS = MakePoints(P_START, P_END, NUM_POINTS)   # Option 1
# POINTS = PoseSplit(KUKA_2_Pose(P_START), KUKA_2_Pose(P_END))   # Option 2

# Initialize the RoboDK API
RDK = Robolink()

# turn off auto rendering (faster)
RDK.Render(False)

# Promt the user to select a robot (if only one robot is available it will select that robot automatically)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)

# Turn rendering ON before starting the simulation
RDK.Render(True)

# Abort if the user hits Cancel
if not robot.Valid():
    quit()

# Retrieve the robot reference frame
reference = robot.Parent()

# Use the robot base frame as the active reference
robot.setPoseFrame(reference)

# get the current orientation of the robot (with respect to the active reference frame and tool frame)
pose_ref = robot.Pose()
print(Pose_2_TxyzRxyz(pose_ref))
# a pose can also be defined as xyzwpr / xyzABC
#pose_ref = TxyzRxyz_2_Pose([100,200,300,0,0,pi])


#-------------------------------------------------------------
# Option 2: Create the program on the graphical user interface
# Turn off rendering
RDK.Render(False)
prog = RDK.AddProgram('AutoProgram')

# Iterate through all the points
for i in range(NUM_POINTS):
    # add a new target and keep the reference to it
    ti = RDK.AddTarget('Auto Target %i' % (i + 1))
    # use the reference pose and update the XYZ position
    pose_i = pose_ref
    pose_i.setPos(POINTS[i])
    ti.setPose(pose_i)
    # force to use the target as a Cartesian target
    ti.setAsCartesianTarget()

    # Optionally, add the target as a Linear/Joint move in the new program
    prog.MoveJ(ti)

# Turn rendering ON before starting the simulation
RDK.Render(True)