#####################################################################
# Macros for mag probe
# - ATTACH_PROBE
# - DETACH_PROBE
# - GET_PROBE_STATUS
# - QUERY_PROBE
# - PROBE
# - PROBE_ABORT
# - PROBE_ACCURACY
# - PROBE_ACCEPT
# - PROBE_CALIBRATE
# - SET_PROBE_STATUS
# - _ATTACH_PROBE
# - _DOCK_PROBE
# - _MAG_PROBE
# - _PROBE_ACTION
# - _Z_MOVE_CHECK
#####################################################################


## ATTACH_PROBE
[gcode_macro ATTACH_PROBE]
description: Attaching the MagProbe if not already attached
gcode:
  _MAG_PROBE ACTION=ATTACH
  _MAG_PROBE ACTION=CHECK_ATTACH


## DETACH_PROBE
[gcode_macro DETACH_PROBE]
description: Dock the MagProbe if not already docked
gcode:
  _MAG_PROBE ACTION=DOCK
  _MAG_PROBE ACTION=CHECK_DOCK


## GET_PROBE_STATUS
[gcode_macro GET_PROBE_STATUS]
description: Prints the current MagProbe state, valid probe states are UNKNOWN, ATTACHED and DOCKED
gcode:
  _MAG_PROBE ACTION=GET_STATUS RESPOND=1


## QUERY_PROBE
[gcode_macro QUERY_PROBE]
description: Return the status of the z-probe and store ID
rename_existing: _QUERY_PROBE_BASE
variable_id: 0
gcode:
  {% set id = params.ID|default(0) %} ; call id 0 means invalid
  _QUERY_PROBE_BASE
  SET_GCODE_VARIABLE MACRO=QUERY_PROBE VARIABLE=id VALUE={id}


## PROBE
[gcode_macro PROBE]
description: Probe Z-height at current XY position and dock/undock MagProbe
rename_existing: _PROBE_BASE
gcode:
  #####  get new parameter. #####
  {% set dock = params.DOCK|default(1)|int %} ; use DOCK=0 to omit the probe docking
  ##### get user defines  #####
  {% set z_min = printer['gcode_macro _USER_VARIABLE'].probe_z_min|float %}
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  ##### get toolhead parameters #####
  {% set act_z = printer.gcode_move.gcode_position.z|float %}
  {% set absolute_coordinates = printer.gcode_move.absolute_coordinates|lower %}
  ##### get params and prepare to send them to the base macro #####
  {% set get_params = [] %}
  {% for key in params %}
    {% set get_params = get_params.append(key + "=" + params[key])  %}
  {% endfor %}
  ##### end of definitions #####
  # as we need to return to the position with the probe we need to be at least at z_min
  G90 ; absolute positioning
  {% if act_z < z_min %}
    {action_respond_info("PROBE: High must be above %.2f" % z_min)}
    G1 Z{z_min} F900 ; move head up
  {% endif %}
  M400 ; get the buffer cleared first
  SAVE_GCODE_STATE NAME=STATE_PROBE
  ATTACH_PROBE
  RESTORE_GCODE_STATE NAME=STATE_PROBE MOVE=1 MOVE_SPEED={t_speed}
  {% if printer.toolhead.position.y|float > 270 %}
    G0 Y270
  {% endif %}
  _PROBE_BASE {get_params|join(" ")}
  G1 Z{z_min} F900 ; move head up to remove trigger
  {% if dock == 1 %}
    DETACH_PROBE
    RESTORE_GCODE_STATE NAME=STATE_PROBE MOVE=1 MOVE_SPEED={t_speed}
  {% endif %}
  {% if absolute_coordinates == 'false' %} G91 {% endif %}


## PROBE_ABORT
[gcode_macro PROBE_ABORT]
description: Abort manual Z probing tool for Probe and dock MagProbe
variable_caller: 'unknown'
variable_absolute_coordinates: False
gcode:
  ##### get user defines  #####
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  ##### end of definitions #####
  {% if caller|lower|string == 'calib' %}
    ABORT
    DETACH_PROBE
    RESTORE_GCODE_STATE NAME=STATE_PROBE_CALIBRATE MOVE=1 MOVE_SPEED={t_speed}
    {% if absolute_coordinates == 'false' %} G91 {% endif %}
      SET_GCODE_VARIABLE MACRO=PROBE_ABORT VARIABLE=caller VALUE='"unknown"'
  {% else %}
    {action_respond_info("PROBE_ABORT: Executed while PROBE_CALIBRATE is not running")}
  {% endif %}


## PROBE_ACCURACY
[gcode_macro PROBE_ACCURACY]
description: Probe Z-height accuracy at current XY position and dock/undock MagProbe
rename_existing: _PROBE_ACCURACY_BASE
gcode:
  #####  get new parameter. #####
  {% set dock = params.DOCK|default(1)|int %} ; use DOCK=0 to omit the probe docking
  ##### get user defines  #####
  {% set z_min = printer['gcode_macro _USER_VARIABLE'].probe_z_min|float %}
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  ##### get toolhead parameters #####
  {% set act_z = printer.gcode_move.gcode_position.z|float %}
  {% set absolute_coordinates = printer.gcode_move.absolute_coordinates|lower %}
  ##### get params and prepare to send them to the base macro #####
  {% set get_params = [] %}
  {% for key in params %}
    {% set get_params = get_params.append(key + "=" + params[key])  %}
  {% endfor %}
  ##### end of definitions #####
  # as we need to return to the position with the probe we need to be at least at z_min
  G90 ; absolute positioning
  {% if act_z < z_min %}
    {action_respond_info("PROBE_ACCURACY: High must be above %.2f" % z_min)}
    G1 Z{z_min} F900 ; move head up
  {% endif %}
  M400 ; get the buffer cleared first
  SAVE_GCODE_STATE NAME=STATE_PROBE_ACCURACY
  ATTACH_PROBE
  RESTORE_GCODE_STATE NAME=STATE_PROBE_ACCURACY MOVE=1 MOVE_SPEED={t_speed}
  _PROBE_ACCURACY_BASE {get_params|join(" ")}
  {% if dock == 1 %}
    DETACH_PROBE
    RESTORE_GCODE_STATE NAME=STATE_PROBE_ACCURACY MOVE=1 MOVE_SPEED={t_speed}
  {% endif %}
  {% if absolute_coordinates == 'false' %} G91 {% endif %}


## PROBE_ACCEPT
[gcode_macro PROBE_ACCEPT]
description: Accept the current Z position for Probe and dock MagProbe
gcode:
  #####  get variables from ABORT  #####
  {% set caller = printer['gcode_macro PROBE_ABORT'].caller %}
  {% set absolute_coordinates = printer['gcode_macro PROBE_ABORT'].absolute_coordinates %}
  ##### get user defines  #####
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  ##### end of definitions #####
  {% if caller|lower|string == 'calib' %}
    ACCEPT
    DETACH_PROBE
    RESTORE_GCODE_STATE NAME=STATE_PROBE_CALIBRATE MOVE=1 MOVE_SPEED={t_speed}
    {% if absolute_coordinates == 'false' %} G91 {% endif %}
      SET_GCODE_VARIABLE MACRO=PROBE_ABORT VARIABLE=caller VALUE='"unknown"'
  {% else %}
    {action_respond_info("PROBE_ACCEPT: Executed while PROBE_CALIBRATE is not running")}
  {% endif %}




## PROBE_CALIBRATE
[gcode_macro PROBE_CALIBRATE]
description: Calibrate the probes z_offset and undock MagProbe
rename_existing: _PROBE_CALIBRATE_BASE
gcode:
  ##### get user defines  #####
  {% set z_min = printer['gcode_macro _USER_VARIABLE'].probe_z_min|float %}
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  ##### get toolhead parameters #####
  {% set act_z = printer.gcode_move.gcode_position.z|float %}
  {% set absolute_coordinates = printer.gcode_move.absolute_coordinates|lower %}
  ##### get params and prepare to send them to the base macro #####
  {% set get_params = [] %}
  {% for key in params %}
    {% set get_params = get_params.append(key + "=" + params[key])  %}
  {% endfor %}
  ##### end of definitions #####
  # as we need to return to the position with the probe we need to be at least at z_min
  G90 ; absolute positioning
  {% if act_z < z_min %}
    {action_respond_info("PROBE_CALIBRATE: High must be above %.2f" % z_min)}
    G1 Z{z_min} F900 ; move head up
  {% endif %}
  M400 ; get the buffer cleared first
  SAVE_GCODE_STATE NAME=STATE_PROBE_CALIBRATE
  ATTACH_PROBE
  RESTORE_GCODE_STATE NAME=STATE_PROBE_CALIBRATE MOVE=1 MOVE_SPEED={t_speed}
  SET_GCODE_VARIABLE MACRO=PROBE_ABORT VARIABLE=caller VALUE='"calib"'
  SET_GCODE_VARIABLE MACRO=PROBE_ABORT VARIABLE=absolute_coordinates VALUE='"{absolute_coordinates}"'
  _PROBE_CALIBRATE_BASE {get_params|join(" ")}


## SET_PROBE_STATUS
[gcode_macro SET_PROBE_STATUS]
description: Manually specify MagProbe status, valid probe states are UNKNOWN, ATTACHED and DOCKED
variable_state: 'unknown'
gcode:
  {% if 'STATE' in params|upper and
        (params.STATE|lower == 'unknown' or params.STATE|lower == 'attached' or params.STATE|lower == 'docked') %}
    SET_GCODE_VARIABLE MACRO=SET_PROBE_STATUS VARIABLE=state VALUE='"{params.STATE|lower}"'
    SET_GCODE_VARIABLE MACRO=_MAG_PROBE VARIABLE=state VALUE='"{params.STATE|lower}"'
  {% else %}
    {% set state = params.STATE|default('none') %}
    {action_raise_error("Invalid probe state: %s. Valid probe states are [UNKNOWN, ATTACHED, DOCKED]" % state|upper)}
  {% endif %}


## _ATTACH_PROBE
[gcode_macro _ATTACH_PROBE]
description: Helper: Attach MagProbe
gcode:
  ##### Get user defines #####
  {% set dock_x = printer['gcode_macro _USER_VARIABLE'].probe_dock_x %}
  {% set dock_y = printer['gcode_macro _USER_VARIABLE'].probe_dock_y %}
  {% set dock_z = printer['gcode_macro _USER_VARIABLE'].probe_dock_z %}
  {% set undock_x = printer['gcode_macro _USER_VARIABLE'].probe_undock_x %}
  {% set undock_y = printer['gcode_macro _USER_VARIABLE'].probe_undock_y %}
  {% set undock_z = printer['gcode_macro _USER_VARIABLE'].probe_undock_z %}
  {% set z_min = printer['gcode_macro _USER_VARIABLE'].probe_z_min|float %}
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  {% set d_speed = printer['gcode_macro _USER_VARIABLE'].probe_dock_speed|float * 60 %}
  ##### get toolhead position #####
  {% set act_z = printer.toolhead.position.z|float %}
  ##### end of definitions #####
  SAVE_GCODE_STATE NAME=STATE_ATTACH_PROBE
  SET_GCODE_OFFSET Z=0.0                ; reset offset - will be restored
  G90                                   ; absolute positioning
  {% if act_z < z_min %}
    G0 Z{z_min} F1500                   ; move head up
  {% endif %}

  ##### gantry dock:
  G0 X{undock_x} Y{undock_y} F{t_speed} ; move next to mag-probe
  G0 X{dock_x} F{d_speed}               ; move sideways to attach probe
  G0 Y{dock_y} F{d_speed}               ; move out of holder

  RESTORE_GCODE_STATE NAME=STATE_ATTACH_PROBE


## _DOCK_PROBE
[gcode_macro _DOCK_PROBE]
description: Helper: Dock MagProbe
gcode:
  ##### Get user defines #####
  {% set dock_x = printer['gcode_macro _USER_VARIABLE'].probe_dock_x %}
  {% set dock_y = printer['gcode_macro _USER_VARIABLE'].probe_dock_y %}
  {% set dock_z = printer['gcode_macro _USER_VARIABLE'].probe_dock_z %}
  {% set undock_x = printer['gcode_macro _USER_VARIABLE'].probe_undock_x %}
  {% set undock_y = printer['gcode_macro _USER_VARIABLE'].probe_undock_y %}
  {% set undock_z = printer['gcode_macro _USER_VARIABLE'].probe_undock_z %}
  {% set z_min = printer['gcode_macro _USER_VARIABLE'].probe_z_min|float %}
  {% set t_speed = printer['gcode_macro _USER_VARIABLE'].probe_travel_speed|float * 60 %}
  {% set d_speed = printer['gcode_macro _USER_VARIABLE'].probe_dock_speed|float * 60 %}
  ##### get toolhead position #####
  {% set act_z = printer.toolhead.position.z|float %}
  ##### end of definitions #####
  SAVE_GCODE_STATE NAME=STATE_DOCK_PROBE
  SET_GCODE_OFFSET Z=0.0                ; reset offset - will be restored
  G90                                   ; absolute positioning
  {% if act_z < z_min %}
    G0 Z{z_min} F900                    ; move head up
  {% endif %}

  ###################################################################
  #        !!! Caution !!!
  #
  # Adapt for your needs if needed !!
  ###################################################################

  ##### Example for a bed dock:
  #G0 X{dock_x} Y{dock_y} F{t_speed}     ; move to mag-probe
  #G0 Z{dock_z} F1500 FORCE              ; move down to probe
  #G0 Y{undock_y} F{d_speed}             ; move into the holder
  #G0 Z{undock_z} F1500                  ; move upwards to remove probe

  ##### Example for a gantry dock:
  G0 X{dock_x} Y{dock_y} F{t_speed}     ; move in front of mag-probe
  G0 Y{undock_y} F{d_speed}             ; move into the holder
  G0 X{undock_x} F{d_speed}             ; move sideways to remove probe

  RESTORE_GCODE_STATE NAME=STATE_DOCK_PROBE


## MAG_PROBE
# QUERY_PROBE must run direct before _PROBE_ACTION
# that relation is insured by the caller id
[gcode_macro _MAG_PROBE]
description: Helper: Query MagProbe state and request action
variable_state: 'unknown'
variable_id: 0
gcode:
  ##### add RESPOND if specified #####
  {% if 'RESPOND' in params|upper %}
    {% set respond = "RESPOND=" + params.RESPOND %}
  {% else %}
    {% set respond = "" %}
  {% endif %}
  ##### generate an id not equal to 0 #####
  {% if id == 0 %}
    {% set id = 1 %}
  {% else %}
    {% set id = id + 1 %}
  {% endif %}
  ##### end of definition #####
  QUERY_PROBE ID={id}
  _PROBE_ACTION ACTION={params.ACTION} ID={id} {respond}
  SET_GCODE_VARIABLE MACRO=_MAG_PROBE VARIABLE=id VALUE={id}
  M400


## _PROBE_ACTION
[gcode_macro _PROBE_ACTION]
description: Helper: Perform MagProbe action
gcode:
  ##### get params and defaults #####
  {% set default_respond = printer['gcode_macro _USER_VARIABLE'].respond_probe_action|default(1)|int %}
  {% set respond = params.RESPOND|default(default_respond)|int %}
  {% set action = params.ACTION|lower %}
  {% set id = params.ID|default(0)|int %} ; call id 0 means invalid
  ##### get probe variables #####
  {% set probe_id = printer['gcode_macro QUERY_PROBE'].id|default(0)|int %}
  {% set man_state = printer['gcode_macro SET_PROBE_STATUS'].state|lower %}
  ##### generate state #####
  {% if man_state != 'unknown' %}
    SET_GCODE_VARIABLE MACRO=SET_PROBE_STATUS VARIABLE=state VALUE='"unknown"'
    {% set state = man_state %}
    {% if respond == 1 %}
      {action_respond_info("MagProbe: State was set to %s by SET_PROBE_STATUS"% man_state|upper)}
    {% endif %}
  {% elif id == 0 or id != probe_id %}
    {action_raise_error("MagProbe: Call ID invalid or does not match QUERY_PROBE call ID")}
  {% elif printer.probe.last_query|lower == 'false' %}
    {action_raise_error("MagProbe: Please execute QUERY_PROBE first")}
  {% else %}
    {% if printer.probe.last_query|int == 0 %}
      {% set state = 'attached' %}
    {% else %}
      {% set state = 'docked' %}
    {% endif %}
  {% endif %}
  ##### end of defines #####
  SET_GCODE_VARIABLE MACRO=_MAG_PROBE VARIABLE=state VALUE='"{state}"'
  {% if action == 'attach' %}
    {% if state == 'docked' %}
      {% if respond == 1 %}
        {action_respond_info("MagProbe: Attach Probe")}
      {% endif %}
      _ATTACH_PROBE
    {% else %}
      {% if respond == 1 %}
        {action_respond_info("MagProbe: already attached")}
      {% endif %}
    {% endif %}
  {% elif action == 'dock' %}
    {% if state == 'attached' %}
      {% if respond == 1 %}
        {action_respond_info("MagProbe: Dock Probe")}
      {% endif %}
      _DOCK_PROBE
    {% else %}
      {% if respond == 1 %}
        {action_respond_info("MagProbe: already docked")}
      {% endif %}
    {% endif %}
  {% elif action == 'check_dock' %}
    {% if state != 'docked' %}
      {action_raise_error("MagProbe: dock failed!")}
    {% endif %}
  {% elif action == 'check_attach' %}
    {% if state != 'attached' %}
      {action_raise_error("MagProbe: attach failed!")}
    {% endif %}
  {% elif action == 'get_status' %}
    {% if respond == 1 %}
      {action_respond_info("MagProbe Status: %s" % state)}
    {% endif %}
  {% else %}
    {action_raise_error("MagProbe: action not defined")}
  {% endif %}


## _Z_MOVE_CHECK
[gcode_macro _Z_MOVE_CHECK]
description: Helper: Check limit and perform move
gcode:
  ##### define defaults ######
  {% set caller = params.CALLER|default('G0')|upper %}
  {% set force = params.FORCE|default(0)|int %}
  ##### z values #####
  {% set z_min = printer['gcode_macro _USER_VARIABLE'].probe_z_min|float %}
  {% set z_act = printer.toolhead.position.z|float %}
  ##### MagProbe state #####
  {% set probe_state = printer['gcode_macro _MAG_PROBE'].state|lower %}
  ##### get params and prepare to send them to the base macro #####
  {% set get_params = [] %}
  {% for key in params %}
    {% if key is not in ['Z', 'CALLER', 'FORCE'] %}
      {% set get_params = get_params.append(key + params[key]) %}
    {% elif key is in ['Z'] %}
      {% if force == 1 %} ;manual override of probe check
        {% set get_params = get_params.append(key + params[key]) %}
      {% elif probe_state ==  'unknown' %}
        {action_raise_error("%s: MagProbe state %s run \"_MAG_PROBE ACTION=GET_STATUS\"" % (caller, probe_state|upper))}
      {% elif probe_state ==  'docked' %}
        {% set get_params = get_params.append(key + params[key]) %}
      {% elif probe_state ==  'attached' %}
        ##### define move target position depending on absolute_coordinates #####
        {% if printer.gcode_move.absolute_coordinates|lower == 'true' %}
          {% set z_target = params.Z|float %}
        {% else %}
          {% set z_target = z_act + params.Z|float %}
        {% endif %}
        ##### decide if a Z move can be executed #####
        {% if z_target > z_min or z_act < z_target %}
          {% set get_params = get_params.append(key + params[key]) %}
        {% else %}
          {action_respond_info("%s: Z Moves lower than %.1fmm not allowed with installed probe" % (caller,z_min))}
        {% endif %}
      {% else %}
        {action_raise_error("%s: MagProbe state %s not valid" % (caller, probe_state|upper))}
      {% endif %}
    {% endif %}
  {% endfor %}
  ##### end of definitions #####
  {caller}.1 {get_params|join(" ")}