# This program will create points and project those points on a window blank; x,y - coordinates form a sine wave
# Variables
# x                                           x-coordinate in mm
# y                                           y-coordinate in mm
# z                                           z-coordinate in mm
# y_steps                                     y-coordinate increments between points
# x_steps                                     x-

def Make_Points(Point_Start, Point_End, Num_Points):

    # Extract the Starting Points
    Point_List = []
    x = Point_Start[0]
    y = Point_Start[1]
    z = Point_Start[2]


    # Calculate the distance between each consecutive waypoint
    y_steps = abs((Point_End[1]-Point_Start[1])/(Num_Points-1))
    point_i = [x,y,z]
    Point_List = []
    Point_List.append(point_i)

    while x < 381:
        for i in range(9):
            y -= y_steps
            if y < 1000:
                break
            point_i = [x,y,z]
            Point_List.append(point_i)
             
       
        x += 20
        if x > 381:
            break
        point_i = [x,y,z]
        Point_List.append(point_i)

        for i in range(9):
            y += y_steps
            if y > 1381:
                break
            point_i = [x,y,z]
            Point_List.append(point_i)

        x += 20
        if x > 381:
            break
        point_i = [x,y,z]
        Point_List.append(point_i)
        
    return Point_List
import numpy as np
from robolink import *      # Import the robolink library 
from robodk import *        # Import RoboDK robotics toolbox 
from random import seed # Only if needed 
from random import randint # Only if needed
from math import sqrt


# Starting Parameters
Point_Start = [5,1361,20]
Point_End = [5,1010,20]
Num_Points = 10
 
# Generate points that will be used to create sine wave path 
POINTS = Make_Points(Point_Start, Point_End, Num_Points)

 
# Open communication with simulator 
RDK = Robolink() 
  
# Turn off auto rendering 
RDK.Render(False) 
  
# Automatically delete previously generated items (Auto tag) 
list_items = RDK.ItemList() 
for item in list_items: 
    if item.Name().startswith('Auto'): 
        item.Delete() 
  
# Turn rendering ON before starting the simulation 
RDK.Render(True) 
 
object_curve = RDK.AddCurve(POINTS)
#for i in range(len(POINTS)):
    # add a new target
 #   ti = RDK.AddTarget('Auto Target %i' % (i+1))

RDK.ShowMessage("The points are: " + str(POINTS))
 
length = len(POINTS)
matrix = np.zeros((3,length),float)
#mat_points = np.array(POINTS)
for r in range(length):
    point_r = POINTS[r]
    #for n in range(length):
    matrix[0,r] = point_r[0]
    matrix[1,r] = point_r[1]
    matrix[2,r] = point_r[2]
        

RDK.ShowMessage("The matrix is :" + str(matrix))

#points = RDK.AddPoints(mat_points,'PART',False,0)
  
ProjectionType = PROJECTION_ALONG_NORMAL
# Available values include: 
#PROJECTION_NONE                = 0 # No curve projection 
#PROJECTION_CLOSEST             = 1 # The projection will be the closest point on the surface 
#PROJECTION_ALONG_NORMAL        = 2 # The projection will be done along the normal. 
#PROJECTION_ALONG_NORMAL_RECALC = 3 # The projection will be done along the normal. Furthermore, the normal will be recalculated according to the surface normal. 
#PROJECTION_CLOSEST_RECALC      = 4 # The projection will be the closest point on the surface and the normals will be recalculated 
#PROJECTION_RECALC              = 5 # The normals are recalculated according to the surface normal of the closest projection 
  
#------------------------------------------------------------- 
# Ask the user to provide the object with the features 
object_features = RDK.ItemUserPick("Select object with the features to project (curves and/or points)", ITEM_TYPE_OBJECT) 
if not object_features.Valid(): 
    quit() 
  
# Ask the user to provide the object with the surface used as a reference 
object_surface = RDK.ItemUserPick("Select Surface Object to project features", ITEM_TYPE_OBJECT) 
if not object_surface.Valid(): 
    quit() 
  
# Create a duplicate copy of the surface object 
object_surface.Copy() 
new_object = RDK.Paste(object_surface.Parent()) 
new_object.setName("Recalculated Normals") 
new_object.setVisible(True) 
  
# Hide the objects used to build the new object with the desired curves 
object_features.setVisible(False) 
object_surface.setVisible(False) 
  
# Turn Off rendering (faster) 
RDK.Render(False) 
  
# Add all curves, projected as desired (iterate through all curves until no more curves are found) 
  
curve_points, name_feature = object_features.GetPoints(FEATURE_CURVE, 0) 
print(name_feature) 
     
curve_points_proj = RDK.ProjectPoints(curve_points, object_surface, ProjectionType) 
   
RDK.AddCurve(curve_points_proj, new_object, True, PROJECTION_NONE) 
  
# Turn On rendering (Optional) 
RDK.Render(True) 
print("Done") 
path_settings = RDK.AddMachiningProject("AutoCurveFollow settings") 
prog, status = path_settings.setMachiningParameters(part=new_object)
RDK.ShowMessage("The size of the list of points is: " +str(len(POINTS)))