; Helper macro for containing functionality needed to
; safely perform a G38.3 move.

o<_psng2_probe> sub

; Save and autorestore model states
M73

; Parameters for this macro
#<search_distance>  = #1
#<latch_distance>   = #2
; 0 = +X, 90 = +Y, 180 = -X, 270 = -Y
#<search_xy_angle>  = #3
; 0 = XY, 90 = Z
#<search_z_angle>   = #4
#<searchvel>        = #5
#<probevel>         = #6
#<probesettle>      = #7


; check inputs
O10 if [#<search_distance> LE 0]
    O10 return [-1]
O10 endif
O11 if [[#<search_z_angle> LT 0] OR [#<search_z_angle> GT 90]]
    O11 return [-2]
O11 endif
O12 if [#<searchvel> LE 0]
    O12 return [-3]
O12 endif
O13 if [#<probevel> LE 0]
    O13 return [-4]
O13 endif
O14 if [#<probesettle> LT 0]
    O14 return [-12]
O14 endif

#<x_f> = [COS[#<search_xy_angle>] * COS[#<search_z_angle>]]
#<y_f> = [SIN[#<search_xy_angle>] * COS[#<search_z_angle>]]
; always be probing down
#<z_f> = [-SIN[#<search_z_angle>]]

; Set incremental mode
G91

;check for error
;probe error
M66 P0 L0
O20 if [#5399 EQ 1]
    O20 return [-5]
O20 endif
;low battery
M66 P1 L0
O21 if [#5399 EQ 1]
    O21 return [-6]
O21 endif

; Check if probe is already tripped
O22 if [#<_hal[motion.probe-input]> EQ 1]
    O22 return [-7]
O22 endif

; initial position
#<x> = #<_x>
#<y> = #<_y>
#<z> = #<_z>
; Perform the Search
G38.3 X[#<search_distance> * #<x_f>] Y[#<search_distance> * #<y_f>] Z[#<search_distance> * #<z_f>] F[#<searchvel>]

; Check to see if we failed to make contact
O30 if [#5070 EQ 0]
    O30 return [-8]
O30 endif

;probe error
M66 P0 L0
O31 if [#5399 EQ 1]
    O31 return [-5]
O31 endif

; check how far we moved.  If less that latch_distance then go back to where we started instead of the latch_distance
#<dx> = [#<x> - #<_x>]
#<dy> = [#<y> - #<_y>]
#<dz> = [#<z> - #<_z>]
#<dist_moved> = [SQRT[[#<dx> * #<dx>] + [#<dy> * #<dy>] + [#<dz> * #<dz>]]]

O40 if [#<dist_moved> LT #<latch_distance>]
    #<latch_dist> = #<dist_moved>
O40 else
    #<latch_dist> = #<latch_distance>
O40 endif

; it is safe to do a G1 move here as we already move through this space without issue
G1 X[-#<latch_dist> * #<x_f>] Y[-#<latch_dist> * #<y_f>] Z[-#<latch_dist> * #<z_f>]

; let the probe settle
G4 P[#<probesettle>]

M66 E0 L0
; Should never happen but good practice
O50 if [#<_hal[motion.probe-input]> EQ 1]
    O50 return [-7]
O50 endif

; fine probe
G38.3 X[#<latch_dist> * 2 * #<x_f>] Y[#<latch_dist> * 2 * #<y_f>] Z[#<latch_dist> * 2 * #<z_f>] F[#<probevel>]

; Check to see if we failed to make contact
O60 if [#5070 EQ 0]
    O60 return [-8]
O60 endif

;probe error
M66 P0 L0
O61 if [#5399 EQ 1]
    O61 return [-5]
O61 endif

; Move away
G1 X[-#<latch_dist> * #<x_f>] Y[-#<latch_dist> * #<y_f>] Z[-#<latch_dist> * #<z_f>] F[#<searchvel>]

; let the probe settle
G4 P[#<probesettle>]

o<_psng2_probe> endsub [0]
M2