(author: Chris P)
(version: 0.1)
(date: 05/29/25)

o<toolchange> sub

; Parameter #3989 is used to track if the rack is homed (M13) (volatile)
; Parameter #3990 is used to track the current tool pocket (persistently)
; Parameter #3991 is used to track the current tool loaded it in the spindle (persistently)
; Parameters #4001 to #4024 are used to track which tool is in which pocket (persistently)
; Parameter #4000 is not populated just used in the maths to calculate the above numbers
; #<atc_z_tool_change_height> The Z height your spindle needs to be at to clamp/unclamp a tool from the ATC platter (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
; #<atc_z_tool_clearance_height> The Z clearance height in machine coordinates that your spindle needs to be at to clear the tools during rack rotation (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)

(PRINT, o<toolchange> selected_tool: #<selected_tool>, tool_in_spindle: #<tool_in_spindle>, selected_pocket: #<selected_pocket>, current_pocket: #<current_pocket>, task: #<_task>)

o100 if [#<_task> EQ 0]
    (DEBUG, Task is null)
    o<toolchange> return [999]
o100 endif (this code eliminates tool missing error on load for multiple use tools)

#<number_of_pockets> = #3973
#<atc_z_tool_change_height> = #3981
#<atc_z_tool_clearance_height> = #3982

; assign the variables passed by M6 change_prolog to some parameters
#100 = #<selected_tool>
#110 = #<tool_in_spindle>
#120 = #<selected_pocket>
#121 = #<current_pocket>

o110 if [#<selected_tool> EQ #<tool_in_spindle>] ; checks if tool in the spindle is same as requested
    (PRINT, Requested tool already in spindle)
    (PRINT, o<toolchange> endsub)
    o<toolchange> endsub [1]
    M2
o110 endif

o120 if [#3991 NE #<tool_in_spindle>]
    (PRINT, o<toolchange> tool_in_spindle does not match 3991)
o120 endif

#<next_pocket> = 0 ; assigns 0 to the next pocket for a later check if the tool is found in the rack
#<open_pocket> = 0
#130 = #<number_of_pockets> ; assign test parameter the number of pockets in the rack

o130 do
    o161 if [#[4000 + #130] EQ #<selected_tool>] ; checks all pockets to see if it contains tool number requested as the new tool
        #<next_pocket> = #130 ; if tool is found in pocket, assigns the next pocket
    o161 endif
    o162 if [#[4000 + #130] EQ 0] ; checks if the pocket is empty, last pocket checked will be the lowest empty pocket number, for putting tool in spindle away.
        #<open_pocket> = #130
    o162 endif
    #130 = [#130 - 1]
o130 while [#130 GT 0]

o140 if [#<next_pocket> EQ 0] ; if tool is not found, aborts and sends a message
    (abort, Tool T%d#<selected_tool> not found in rack)
o140 endif

; now we know which pocket the next tool is sitting in
; we need to know if we need to put a tool away
; or if there is not tool in the spindle

o150 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spindle
    o151 if [#<open_pocket> EQ 0] ; If there is a tool in the spindle, checks if there is an open pocket
        (abort, rack is full, cant store tool T#<tool_in_spindle> in rack)
    o151 endif
    o<m21> call [#<open_pocket>]; puts the tool in spindle away into the open pocket
    (DEBUG, EVAL[vcp.getWidget{"rackatc"}.store_tool{#<open_pocket>, #<tool_in_spindle>}])
    #140 = #<open_pocket>
    #[4000 + #140] = #<tool_in_spindle> ; save tool number in pocket
    #3991 = 0 ; empty tool in the spindle
    M61 Q0
    G49
o150 endif

o160 if [#<selected_tool> GT 0] ; selected tool is not tool 0
    o<m22> call [#<next_pocket>] ; set the spindle to move to the right pocket for the selected tool
    (DEBUG, EVAL[vcp.getWidget{"rackatc"}.store_tool{#<next_pocket>, 0}])
    #150 = #<next_pocket>
    #[4000 + #150] = 0 ; empty the pocket
    #3991 = #<selected_tool> ; Set persistent variable to remember tool in spindle after power cycle
o160 else
    M25 ; deactive drawbar
o160 endif

M61 Q#<selected_tool>

o170 if [1 EQ 1]
    G43 H#<selected_tool>
o170 endif

o<program_coolant> call

(PRINT, o<toolchange> endsub)
o<toolchange> endsub [1]

M2