# This macro was provided by discord user Garrettwp to whom i give my thanks for sharing it with me.
# I have tweaked it a lot.
#
# this macro is  based on the great Annex magprobe dockable probe macros "#Originally developed by Mental, modified for better use on K-series printers by RyanG and Trails"
# that macro can be found here https://github.com/Annex-Engineering/Annex-Engineering_Other_Printer_Mods/blob/master/All_Printers/Microswitch_Probe/Klipper_Macros/dockable_probe_macros.cfg
#
# by standing on the shoulders of giants, lets see if we can see further

[gcode_macro User_Variables]
variable_verbose:               True  # Enable verbose output
variable_travel_speed:          200   # how fast all other travel moves will be performed when running these macros
variable_dock_speed:            50    # how fast should the toolhead move when docking the probe for the final movement
variable_release_speed:         100    # how fast should the toolhead move to release the hold of the magnets after docking
variable_z_drop_speed:          20    # how fast the z will lower when moving to the z location to clear the probe
variable_home_z_height:         15    # Z when homing

# if a separate Z endstop switch is in
# use, specify the coordinates of the switch here (Voron).
# Set to 0 to have the probe move to center of bed
variable_z_endstop_x:           231
variable_z_endstop_y:           350
#variable_z_endstop_x:          0
#variable_z_endstop_y:          0

# location to park the toolhead
variable_park_toolhead:         True  # Enable toolhead parking
variable_parkposition_x:        175
variable_parkposition_y:        175
variable_parkposition_z:        10

#dock location
variable_docklocation_x:        12    # X Dock position
variable_docklocation_y:        350   # Y Dock position
variable_docklocation_z:        15    # Z dock position
variable_dockarmslenght:        30    # Dock arms lenght, toolhead movement necessary to clear the dock arms

#Umbilical to help untangle the umbilical in difficult situations
variable_umbilical_x:           15    #X umbilical position
variable_umbilical_y:           15    #Y umbilical position
variable_umbilical:             False #should we untabgle the umbilical

# Do not modify below
gcode:
    {% set Mx = printer['configfile'].config["stepper_x"]["position_max"]|float %}
    {% set My = printer['configfile'].config["stepper_y"]["position_max"]|float %}
    {% set Ox = printer['configfile'].config["probe"]["x_offset"]|float %}
    {% set Oy = printer['configfile'].config["probe"]["y_offset"]|float %}
    {% set Oz = printer['configfile'].config["probe"]["z_offset"]|float %}

    # if docklocation_z is zero, use Home Z height for safety
    {% if docklocation_z == 0 %}
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=docklocation_z VALUE={ home_z_height }
    {% endif %}

    # If x, y coordinates are set for z endstop, assign them
    {% if z_endstop_x != 0 or z_endstop_y != 0 %}
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=z_endstop_x VALUE={ z_endstop_x }
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=z_endstop_y VALUE={ z_endstop_y }

    # if no x, y coordinates for z endstop, assume probe is endstop and move toolhead to center of bed
    {% else %}
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=z_endstop_x VALUE={ (Mx * 0.5) - Ox }
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=z_endstop_y VALUE={ (My * 0.5) - Oy }
    {% endif %}


[gcode_macro Probe_Variables]
variable_probe_attached:            False
variable_probe_state:               False
variable_probe_lock:                False
variable_z_endstop_x:               0
variable_z_endstop_y:               0
gcode:


[gcode_macro Homing_Variables]
#default_parameter_reset:            0
gcode:
    {% set R  = params.RESET|default(0)|float %}

    {% if R %}
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_lock VALUE={ False }
   {% endif %}

# Attach probe and lock it
[gcode_macro Attach_Probe_Lock]
gcode:
    Attach_Probe
    SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_lock VALUE={ True }


# Dock probe and lock it
[gcode_macro Dock_Probe_Unlock]
gcode:
    SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_lock VALUE={ False }
    Dock_Probe

# Unlock Probe
[gcode_macro Probe_Unlock]
gcode:
    SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_lock VALUE={ False }

# Lock Probe
[gcode_macro Probe_Lock]
gcode:
    SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_lock VALUE={ True }
# Attach Probe Routine
[gcode_macro Attach_Probe]
gcode:
    # Get probe attach status
    {% set P = printer["gcode_macro Probe_Variables"].probe_attached %}
    {% set L = printer["gcode_macro Probe_Variables"].probe_lock %}
    {% set V = printer["gcode_macro User_Variables"].verbose %}
    # Get Docking location
    {% set Dx = printer["gcode_macro User_Variables"].docklocation_x %}
    {% set Dy = printer["gcode_macro User_Variables"].docklocation_y %}
    {% set Dz = printer["gcode_macro User_Variables"].docklocation_z %}
    {% set Da = printer["gcode_macro User_Variables"].dockarmslenght %}
    # Safe Z for travel
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}
    #Set speed
    {% set St = printer["gcode_macro User_Variables"].travel_speed * 60    %}

    #prior to saving actual position, check if its necessary to move to a safe Z
    #that has enought overhead for the attached probe
    {% if (printer.toolhead.position.z < Hzh) %}
        G1 Z{Hzh} F1200
    {% endif %}
    M400 # mandatory to save the new safe position
    #allows the docking position to be independent of the Z offset, necessary for bed mounted probes
    SAVE_GCODE_STATE name=_attachProbe
    SET_GCODE_OFFSET Z=0

    # if x and y are not homed
    {% if not 'xy' in printer.toolhead.homed_axes %}
        { action_raise_error("Must Home X and Y Axis First!") }

    # If probe not attached and locked
    {% elif not P and not L %}
        {% if V %}
            { action_respond_info("Attaching Probe") }
        {% endif %}
        G90

        {% if (not 'z' in printer.toolhead.homed_axes) %}
            {% if V %}
                { action_respond_info("Resetting Z position to zero") }
            {% endif %}
            SET_KINEMATIC_POSITION Z=0
        {% endif %}

        {% if (printer.toolhead.position.z < Hzh) %}
            G1 Z{Hzh} F1200
        {% endif %}

        Umbilical_Path

        # Probe entry location
        G1 X{Dx} Y{Dy|int - Da|int} Z{Hzh} F{St}
        #lower to Z dock
        G1 Z{Dz} F600
        # pickup from Probe location
        G1 X{Dx} Y{Dy} F1800
        # Probe entry location
        G1 X{Dx} Y{Dy|int - Da|int} F6000
        #Go to Z safe distance
        G1 Z{Hzh} F600
        Park_Toolhead

        CheckProbe action=attach

    {% elif L %}
        {% if V %}
            { action_respond_info("Probe locked!") }
        {% endif %}

        # Probe attached, do nothing
        CheckProbe action=query

    {% else %}
        {% if V %}
            { action_respond_info("Probe already attached!") }
        {% endif %}

        # Probe attached, do nothing
        CheckProbe action=query

    {% endif %}
    #reverts to the original Z offset
    RESTORE_GCODE_STATE name=_attachProbe


# Dock Probe Routine
[gcode_macro Dock_Probe]
gcode:
    # Get probe attach status
    {% set P = printer["gcode_macro Probe_Variables"].probe_attached %}
    {% set L = printer["gcode_macro Probe_Variables"].probe_lock %}
    {% set V = printer["gcode_macro User_Variables"].verbose %}
    # Get Docking location
    {% set Dx = printer["gcode_macro User_Variables"].docklocation_x %}
    {% set Dy = printer["gcode_macro User_Variables"].docklocation_y %}
    {% set Dz = printer["gcode_macro User_Variables"].docklocation_z %}
    {% set Da = printer["gcode_macro User_Variables"].dockarmslenght %}
    # Safe Z for travel
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}
    # Set speed
    {% set St = printer["gcode_macro User_Variables"].travel_speed * 60    %}
    {% set Sd = printer["gcode_macro User_Variables"].dock_speed * 60    %}
    {% set Sr = printer["gcode_macro User_Variables"].release_speed * 60    %}
    {% set Sz = printer["gcode_macro User_Variables"].z_drop_speed * 60    %}

    M400 # mandatory to save the new safe position
    #allows the docking position to be independent of the Z offset, necessary for bed mounted probes
    SAVE_GCODE_STATE name=_dockProbe
    SET_GCODE_OFFSET Z=0

    {% if not 'xyz' in printer.toolhead.homed_axes %}
        { action_raise_error("Must Home X, Y and Z Axis First!") }

    # If probe not attached and not locked
    {% elif P and not L %}
        {% if V %}
            { action_respond_info("Docking Probe") }
        {% endif %}
        G90

        {% if (printer.toolhead.position.z < Hzh) %}
            G1 Z{Hzh} F{Sz}
        {% endif %}

        Umbilical_Path

        # Probe entry location
        G1 X{Dx} Y{Dy|int - Da|int} Z{Hzh} F{St}
        #lower to Z dock
        G1 Z{Dz} F{Sz}
        # Drop Probe to Probe location
        G1 X{Dx} Y{Dy} F{Sd}
        # Probe decoupling
        G1 X{Dx|int + 40} Y{Dy} F{Sr}
        #Go to Z safe distance
        G1 X{Dx|int + 40} Y{Dy|int - 5} Z{Hzh} F{St}

        Park_Toolhead

        CheckProbe action=dock

    {% elif L %}
        {% if V %}
            { action_respond_info("Probe locked!") }
        {% endif %}

        # Probe docked, do nothing
        CheckProbe action=query

    {% else %}
        {% if V %}
            { action_respond_info("Probe already docked!") }
        {% endif %}

        # Probe docked, do nothing
        CheckProbe action=query

    {% endif %}
    #reverts to the original Z offset
    RESTORE_GCODE_STATE name=_dockProbe



# Quad Gantry Level
[gcode_macro QUAD_GANTRY_LEVEL]
rename_existing:             _QUAD_GANTRY_LEVEL
gcode:
    BED_MESH_CLEAR
    {% set V = printer["gcode_macro User_Variables"].verbose %}
    {% if V %}
        { action_respond_info("QG Level") }
    {% endif %}

    CheckProbe action=query
    Attach_Probe

    _QUAD_GANTRY_LEVEL {% for p in params
            %}{'%s=%s ' % (p, params[p])}{%
            endfor %}
    Dock_Probe


# Z Tilt Adjust
#[gcode_macro Z_TILT_ADJUST]
#rename_existing:             _Z_TILT_ADJUST
#gcode:
#    {% set V = printer["gcode_macro User_Variables"].verbose %}
#    {% if V %}
#        { action_respond_info("Z Tilt Adjust") }
#    {% endif %}
#
#    CheckProbe action=query
#    Attach_Probe
#
#    _Z_TILT_ADJUST {% for p in params
#          %}{'%s=%s ' % (p, params[p])}{%
#         endfor %}
#    G28 Z0
#    Dock_Probe


# Screws Tilt Adjust
#gcode_macro SCREWS_TILT_CALCULATE]
#rename_existing:             _SCREWS_TILT_CALCULATE
#gcode:
#    {% set V = printer["gcode_macro User_Variables"].verbose %}
#    {% if V %}
#        { action_respond_info("Screws Tilt Adjust") }
#    {% endif %}
#
#    CheckProbe action=query
#    Attach_Probe
#
#    _SCREWS_TILT_CALCULATE {% for p in params
#          %}{'%s=%s ' % (p, params[p])}{%
#         endfor %}
#
#    Dock_Probe

# Bed Mesh Calibrate
[gcode_macro BED_MESH_CALIBRATE]
rename_existing:             _BED_MESH_CALIBRATE
gcode:
    {% set V = printer["gcode_macro User_Variables"].verbose %}
    {% if V %}
        { action_respond_info("Bed Mesh Calibrate") }
    {% endif %}

    CheckProbe action=query
    Attach_Probe

    _BED_MESH_CALIBRATE {% for p in params
           %}{'%s=%s ' % (p, params[p])}{%
          endfor %}

    Dock_Probe


# Probe Calibrate
[gcode_macro PROBE_CALIBRATE]
rename_existing:             _PROBE_CALIBRATE
gcode:
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}
    {% set Sz = printer["gcode_macro User_Variables"].z_drop_speed * 60 %}
    {% set St = printer["gcode_macro User_Variables"].travel_speed %}

    # Go to Z safe distance before saving location in order to
    # avoid crashing the probe on the bed when coming back
    G1 Z{Hzh} F{Sz}
    M400 # mandatory to save the new safe position
    SAVE_GCODE_STATE NAME=_original_nozzle_location
    
    CheckProbe action=query
    Attach_Probe

    # Restore nozzle location to probe the right place
    RESTORE_GCODE_STATE NAME=_original_nozzle_location MOVE=1 MOVE_SPEED={St}

    _PROBE_CALIBRATE {% for p in params
            %}{'%s=%s ' % (p, params[p])}{%
           endfor %}

	#store current nozzle location
	SAVE_GCODE_STATE NAME=_original_nozzle_location  
    Dock_Probe
	
    # Restore nozzle location again at the end
    RESTORE_GCODE_STATE NAME=_original_nozzle_location MOVE=1 MOVE_SPEED={St}


# Probe Accuracy
[gcode_macro PROBE_ACCURACY]
rename_existing:             _PROBE_ACCURACY
gcode:
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}
    {% set Sz = printer["gcode_macro User_Variables"].z_drop_speed * 60 %}
    {% set St = printer["gcode_macro User_Variables"].travel_speed %}

    # Go to Z safe distance before saving location in order to
    # avoid crashing the probe on the bed when coming back
    G1 Z{Hzh} F{Sz}
    M400 # mandatory to save the new safe position
    SAVE_GCODE_STATE NAME=_original_nozzle_location

    CheckProbe action=query
    Attach_Probe

    # Restore nozzle location to probe the right place
    RESTORE_GCODE_STATE NAME=_original_nozzle_location MOVE=1 MOVE_SPEED={St}

    _PROBE_ACCURACY {% for p in params
            %}{'%s=%s ' % (p, params[p])}{%
           endfor %}

	#store current nozzle location
	SAVE_GCODE_STATE NAME=_original_nozzle_location 
	
	Dock_Probe
	
    # Restore nozzle location again at the end
    RESTORE_GCODE_STATE NAME=_original_nozzle_location MOVE=1 MOVE_SPEED={St}


# enable to SET_KINEMATIC_POSITION for Z hop
[force_move]
enable_force_move: True

# Homeing Override
[homing_override]
axes:                             xyz
gcode:
    BED_MESH_CLEAR
    # collect user state variables
    User_Variables
    {% set V = printer["gcode_macro User_Variables"].verbose %}
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}

    {% if (not 'z' in printer.toolhead.homed_axes) %}
        SET_KINEMATIC_POSITION Z=0
        G90
        G0 Z{Hzh} F600
    {% endif %}

    CheckProbe action=query

    # reset parameters
    {% set X, Y, Z = False, False, False %}

    # which axes have been requested for homing
    {% if not 'X' in params
        and not 'Y' in params
        and not 'Z' in params %}

        {% set X, Y, Z = True, True, True %}

    {% else %}
        {% if 'X' in params %}
            {% set X = True %}
        {% endif %}

        {% if 'Y' in params %}
            {% set Y = True %}
        {% endif %}

        {% if 'Z' in params %}
            {% set Z = True %}
        {% endif %}

        {% if 'X' in params
          and 'Y' in params
          and 'Z' in params %}
            # reset homing state variables
            # if homing all axes
            Homing_Variables reset=1
         {% endif %}

    {% endif %}

    # Home x
    {% if X %}
        {% if V %}
            { action_respond_info("Homing X") }
        {% endif %}
        G28 X0
    {% endif %}

    # Home y
    {% if Y %}
        {% if V %}
            { action_respond_info("Homing Y") }
        {% endif %}
        G28 Y0
    {% endif %}

    # Home z
    {% if Z %}
        {% if V %}
            { action_respond_info("Homing Z") }
        {% endif %}

        # if probe is configured as endstop, attach it
        {% if printer['configfile'].config["stepper_z"]["endstop_pin"] == 'probe:z_virtual_endstop' %}
            Attach_Probe
        {% endif %}

        Home_Z

        #Wipe the nozzle and Home Again
        #G1 Y350 X85 Z4 F7000
        #G1 Y348 X115 Z4 F7000
        #G1 Y350 X85 Z4 F7000
        #G1 Y348 X115 Z4 F7000
        #G1 Y350 X85 Z4 F7000
        #G1 Y348 X115 Z4 F7000
        #G1 Y350 X85 Z4 F7000
        #G1 Y348 X115 Z4 F7000
        CLEAN_NOZZLE
        Home_Z

        # if probe is configured as endstop, dock it
        {% if printer['configfile'].config["stepper_z"]["endstop_pin"] == 'probe:z_virtual_endstop' %}
            Dock_Probe
        {% endif %}
    {% endif %}
  CheckProbe action=query

    # park the toolhead
    Park_Toolhead

# umbilical path setup
[gcode_macro Umbilical_Path]
gcode:
    {% set Ux = printer["gcode_macro User_Variables"].umbilical_x %}
    {% set Uy = printer["gcode_macro User_Variables"].umbilical_y %}
    {% set U = printer["gcode_macro User_Variables"].umbilical    %}
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}
    {% set St = printer["gcode_macro User_Variables"].travel_speed * 60    %}

    {% if U %}
        # Used to give the umbilical a better path to follow and coil properly if dock is tight in space
        G1 X{Ux} Y{Uy} Z{Hzh} F{St}
    {% endif %}


# Home Z Routine
[gcode_macro Home_Z]
gcode:
    {% set Zx = printer["gcode_macro Probe_Variables"].z_endstop_x         %}
    {% set Zy = printer["gcode_macro Probe_Variables"].z_endstop_y         %}
    {% set Hzh = printer["gcode_macro User_Variables"].home_z_height|float %}
    {% set St = printer["gcode_macro User_Variables"].travel_speed * 60    %}
    {% set Sz = printer["gcode_macro User_Variables"].z_drop_speed * 60    %}

    # if x and y are not homed yet, raise error
    {% if not 'xy' in printer.toolhead.homed_axes %}
        { action_raise_error("Must Home X and Y Axis First!") }

    {% else %}
        {% if (not 'z' in printer.toolhead.homed_axes) %}
            {% if V %}
                { action_respond_info("Resetting Z position to zero") }
            {% endif %}
            SET_KINEMATIC_POSITION Z=0
        {% endif %}

        # move tool to safe homing position and home Z axis
        # location of z endstop
        G1 X{Zx} Y{Zy} Z{Hzh} F{St}
        G28 Z0
        G1 Z{Hzh} F{Sz}

    {% endif %}


# check to see if probe is where it is supposed to be after
# attaching/docking maneuver and set homing error or shutdown
[gcode_macro CheckProbe]
variable_probe_state:           0
#default_parameter_action:
gcode:
    Query_Probe
    SetProbeState action={ ACTION }


# due to how templates are evaluated, we have query endstops in one
# macro and call another macro to make decisions based on the result
[gcode_macro SetProbeState]
#default_parameter_action:
gcode:
    {% set P = printer.probe.last_query %}
    {% set V = printer["gcode_macro User_Variables"].verbose %}

    # If triggered (true), probe not attached
    {% if P %}
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_attached VALUE={ False }

    # If not triggered (false), probe attached
    {% else %}
        SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_attached VALUE={ True }
    {% endif %}

    {% if params.ACTION == 'query' %}
          SET_GCODE_VARIABLE MACRO=Probe_Variables VARIABLE=probe_state VALUE={ P }
    {% endif %}

    # if probe fails to attach/detach
    # if not docked
    {% if (not P and params.ACTION == 'dock') %}
        { action_raise_error("Probe dock failed!") }
    {% endif %}

    # if not attached
    {% if P and params.ACTION == 'attach' %}
        { action_raise_error("Probe attach failed!") }
    {% endif %}

# Park Toolhead Routine
[gcode_macro Park_Toolhead]
gcode:
    {% set P = printer["gcode_macro User_Variables"].park_toolhead %}
    {% set Px = printer["gcode_macro User_Variables"].parkposition_x %}
    {% set Py = printer["gcode_macro User_Variables"].parkposition_y %}
    {% set Pz = printer["gcode_macro User_Variables"].parkposition_z %}
    {% set St = printer["gcode_macro User_Variables"].travel_speed * 60 %}
    {% set V = printer["gcode_macro User_Variables"].verbose %}

    {% if (P and 'xyz' in printer.toolhead.homed_axes) %}
        {% if V %}
            { action_respond_info("Parking Toolhead") }
        {% endif %}
        G90
        G1 X{Px} Y{Py} Z{Pz} F{St}
    {% endif %}