# 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
    x_steps = 3
    point_i = [x,y,z]
    Point_List = []
    Point_List.append(point_i)

    s = 1134.62
    r = 107.95

    while y < 1134.62:
        x -= x_steps
        r_sq = r*r
        x_sq = x*x
        if x < -107.95:
            break
        y = s - math.sqrt(r_sq-x_sq)
        point_i = [x,y,z]
        Point_List.append(point_i)
        
    
    x = -107.95
    y = 1134.62
    point_i = [x,y,z]
    Point_List.append(point_i)
    
    while y < 1269:
        x += x_steps
        r_sq = r*r
        x_sq = x*x
        if x >= 0:
            break
        y = s + math.sqrt(r_sq-x_sq)
        point_i = [x,y,z]
        Point_List.append(point_i)

       
    return Point_List

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 = [0,1026.67,250]
Point_End = [0,1242.57,250]
Num_Points = 100
 
# 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) 
  
ProjectionType = PROJECTION_ALONG_NORMAL_RECALC
# 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.AddMillingProject("AutoCurveFollow settings") 
prog, status = path_settings.setMillingParameters(part=new_object)