; These are the parameters given to the subroutine
; #1 x
; #2 y
; #3 z
; #4 a
; #5 safe-Z
; #6 acceleration

o<LML> sub

    ; set acceleration
    m171 p#6

    ; find lateral distance
    #<dx> = [#1 - #<_x>]
    #<dy> = [#2 - #<_y>]
    #<tr> = sqrt[[#<dx>*#<dx>]+[#<dy>*#<dy>]]

    ; increase vertical max based on lateral distance, but only as far as z=20
    #<extraz> = [#<tr> * 0.5]
    #5 = [#5 + #<extraz>]
    o1 if [#5 gt 20]
        #5 = 20
    o1 endif

    ; spread rotation evenly over segments 2 and 3
    #<vr> = [#5 - #3]                  ; down-going vertical distance
    #<rr> = [#4 - #<_a>]               ; total rotation required
    #<f> = [#<tr> / [#<vr> + #<tr>]]   ; horizontal fraction of total distance of segments 2 and 3
    #<r1> = [#<_a> + [#<f> * #<rr>]]   ; desired rotation at end of segment 2

    ;(debug, t: #<tr>, v: #<vr>, r: #<rr> )

    ; lift to safe-Z only if necessary
    o2 if [#5 gt #<_z>]
        g1 z#5
    o2 endif

    ; backlash compensation waypoint
    #<blx> = #1
    #<bly> = #2

    ; z waypoint to ensure pure-vertical arrival at destination
    ; an offset of 5mm will ensure that at least the last 2.5mm is purely vertical even with blending
    #<pvz> = [#3 + 5]

    ; check if x backlash compensation is needed
    o3 if [#<dx> gt 0]
        #<blx> = [#1 + 2]
    o3 endif

    ; check if y backlash compensation is needed
    o4 if [#<dy> gt 0]
        #<bly> = [#2 + 2]
    o4 endif

    g1 x#<blx> y#<bly> a#<r1>  ; main horizontal movement to backlash comp waypoint, rotation waypoint, safe-Z
    g1 x#1 y#2 z#<pvz> a#4     ; main down-going move to final x,y,rotation and pure-vertical start z
    g1 z#3                     ; pure vertical

o<LML> endsub