(author: Chris P)
(version: 0.2)
(date: 06/27/24)

o<toolchange> sub

; Parameter #3989 is used to track if the carousel 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
; #<number_of_pockets>: The number of pockets the ATC platter has, This is user set in the INI file via #<_ini[atc]pockets>
; #<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 carousel 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)

; default to a 12 pocket ATC (matching DynATC Widget behaviour), then update based on INI settings
#<number_of_pockets> = 12
o110 if [EXISTS[#<_ini[atc]pockets>]]
    #<number_of_pockets> = #<_ini[atc]pockets>
o110 endif

#<atc_z_tool_change_height> = -3.9000
o120 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
    #<atc_z_tool_change_height> = #<_ini[atc]z_tool_change_height>
o120 endif
#<atc_z_tool_clearance_height> = [#<_ini[AXIS_Z]MAX_LIMIT>-0.01]
o130 if [EXISTS[#<_ini[atc]z_tool_clearance_height>]]
    #<atc_z_tool_clearance_height> = #<_ini[atc]z_tool_clearance_height>
o130 endif

; assign the variables passed by M6 change_prolog to some parameters
#100 = #<selected_tool>
#110 = #<tool_in_spindle>
#120 = #<selected_pocket>
#121 = #<current_pocket>
; NOTE:
;      The legacy names *selected_pocket* and *current_pocket* actually reference
;      a sequential tooldata index for tool items loaded from a tool
;      table ([EMCIO]TOOL_TABLE) or via a tooldata database ([EMCIO]DB_PROGRAM)
;      see Linuxcnc docs for further information on how these are used

o140 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
o140 endif

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

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

o160 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]
o160 while [#130 GT 0]
o170 if [#<next_pocket> EQ 0] ; if tool is not found, aborts and sends a message
    (abort, Tool T%d#<selected_tool> not found in carousel)
o170 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

o180 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spindle
    o181 if [#<open_pocket> EQ 0] ; If there is a tool in the spindle, checks if there is an open pocket
        (abort, Carousel is full, cant put away tool T#<tool_in_spindle> in into carousel)
    o181 endif
    M10 P[#<open_pocket>] ; move carousel to an open pocket
    M21 ; puts the tool in spindle away into the open pocket
    (DEBUG, EVAL[vcp.getWidget{"dynatc"}.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
o180 endif

G90
G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height

o190 if [#<selected_tool> GT 0] ; selected tool is not tool0
    M10 P#<next_pocket> ; set the carousel to move to the right pocket for the selected tool
    M65 P1
    M64 P0
    M66 P1 L3 Q5 ; check carousel out position sensor
    o191 if [#5399 LT 0]
        M65 P0 ; turn off the solenoid to send atc to tool change
        (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
    o191 endif
    M65 P0

    (DEBUG, EVAL[vcp.getWidget{"dynatc"}.store_tool{#<next_pocket>, 0}])
    M22 ; Carousel out
    #150 = #<next_pocket>
    #[4000 + #150] = 0 ; empty the pocket
    #3991 = #<selected_tool> ; Set persistent variable to remember tool in spindle after power cycle
o190 else
    M65 P2 ; deactive drawbar
    M65 P0 ; make sure ATC out solenoid is off
    M64 P1 ; move carousel home
    M66 P0 L3 Q4 ; check carousel in position sensor
    o192 if [#5399 LT 0]
        M65 P1 ; turn off the solenoid to send atc home
        (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
    o192 endif
    M65 P1
o190 endif

M61 Q#<selected_tool>

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

o<program_coolant> call

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

M2