o<m21> sub

; Use P-word as target pocket (if called as M21 Pn)
#<target_pocket> = #1

; --- Parameter assignments for clarity ---
#<rack_id>                = #3979
#<rack_traverse_speed>    = #3980
#<rack_z_load_height>     = #3981
#<rack_safe_z_height>     = #3982
#<rack_pocket_1_x>        = #3983
#<rack_pocket_1_y>        = #3984
#<rack_pocket_2_x>        = #3985
#<rack_pocket_2_y>        = #3986
#<pocket_1_x_clearance>   = #3987
#<pocket_1_y_clearance>   = #3988

; --- Calculate pocket position ---
#<dx> = [#<rack_pocket_2_x> - #<rack_pocket_1_x>]
#<dy> = [#<rack_pocket_2_y> - #<rack_pocket_1_y>]
#<pocket_index> = [#<target_pocket> - 1]
#<pocket_x> = [#<rack_pocket_1_x> + #<pocket_index> * #<dx>]
#<pocket_y> = [#<rack_pocket_1_y> + #<pocket_index> * #<dy>]

; --- Determine clearance position, axis, and direction ---
#<clearance_x> = #<pocket_x>
#<clearance_y> = #<pocket_y>
#<store_axis> = 0
#<store_sign> = 1

o100 if [#<rack_id> EQ 1] ; -Y to +Y, clearance +X, store -X
    #<clearance_x> = #<pocket_1_x_clearance>
    #<store_axis> = 1
    #<store_sign> = -1
o100 elseif [#<rack_id> EQ 2] ; -Y to +Y, clearance -X, store +X
    #<clearance_x> = #<pocket_1_x_clearance>
    #<store_axis> = 1
    #<store_sign> = 1
o100 elseif [#<rack_id> EQ 3] ; +Y to -Y, clearance +X, store -X
    #<clearance_x> = #<pocket_1_x_clearance>
    #<store_axis> = 1
    #<store_sign> = -1
o100 elseif [#<rack_id> EQ 4] ; +Y to -Y, clearance -X, store +X
    #<clearance_x> = #<pocket_1_x_clearance>
    #<store_axis> = 1
    #<store_sign> = 1
o100 elseif [#<rack_id> EQ 5] ; -X to +X, clearance +Y, store -Y
    #<clearance_y> = #<pocket_1_y_clearance>
    #<store_axis> = 2
    #<store_sign> = -1
o100 elseif [#<rack_id> EQ 6] ; -X to +X, clearance -Y, store +Y
    #<clearance_y> = #<pocket_1_y_clearance>
    #<store_axis> = 2
    #<store_sign> = 1
o100 elseif [#<rack_id> EQ 7] ; +X to -X, clearance +Y, store -Y
    #<clearance_y> = #<pocket_1_y_clearance>
    #<store_axis> = 2
    #<store_sign> = -1
o100 elseif [#<rack_id> EQ 8] ; +X to -X, clearance -Y, store +Y
    #<clearance_y> = #<pocket_1_y_clearance>
    #<store_axis> = 2
    #<store_sign> = 1
o100 else
    (PRINT, "Error: Invalid rack_id!")
    o<m21> return
o100 endif

; --- Motions ---
G90
G0 G53 Z[#<rack_safe_z_height>]
G0 G53 X[#<clearance_x>] Y[#<clearance_y>]
G1 G53 F[#<rack_traverse_speed>] Z[#<rack_z_load_height>]
M19
G4 P1

; Move into pocket - single axis, correct direction
o200 if [#<store_axis> EQ 1]
    G1 G53 F[#<rack_traverse_speed>] X[#<pocket_x>]
o200 else
    G1 G53 F[#<rack_traverse_speed>] Y[#<pocket_y>]
o200 endif

M24
G0 G53 Z[#<rack_safe_z_height>]

o<m21> endsub

M2