# Createdate: 2023/03/08

[force_move]
    enable_force_move: true

[gcode_macro xyz_ready]
variable_x_ready: 0
variable_y_ready: 0
variable_z_ready: 0
variable_xy_moved: 0
variable_z_moved: 0
variable_safe_z: 3
gcode:

[gcode_macro _IF_HOME_Z]
gcode:
  {% if printer['gcode_macro xyz_ready'].z_ready|int == 1 %}
    {% if printer.toolhead.position.z|int < 5 %}
      {% set z_park = 5.0 - printer.toolhead.position.z|int %}
      G91
      G1 z{z_park} F600
      G90
    {% endif %}
  {% else %}
    {% if printer['gcode_macro xyz_ready'].z_moved|int == 0 %}
      {% if printer.print_stats.z_pos|float <= 20.0 or printer.print_stats.power_loss == 1 %}
        FORCE_MOVE STEPPER=stepper_z DISTANCE={printer["gcode_macro PRINTER_PARAM"].z_safe_g28} VELOCITY=10
      {% else %}
        FORCE_MOVE STEPPER=stepper_z DISTANCE=0.1 VELOCITY=10
      {% endif %}
      SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=z_moved VALUE=1
    {% endif %}
  {% endif %}

[gcode_macro _IF_MOVE_XY]
gcode:
  _IF_HOME_Z
  {% if printer['gcode_macro xyz_ready'].xy_moved|int == 0 %}
    {action_respond_info("move xy\n")}
    # FORCE_MOVE STEPPER=stepper_x DISTANCE=14 VELOCITY=50
    FORCE_MOVE STEPPER=stepper_y DISTANCE=-14 VELOCITY=50
    FORCE_MOVE STEPPER=stepper_y DISTANCE=7 VELOCITY=50
    SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=xy_moved VALUE=1
    G4 P1000
  {% endif %}

[gcode_macro _HOME_X]
gcode:
  _IF_MOVE_XY

  {% if printer['gcode_macro xyz_ready'].x_ready|int == 1 %}
    {% if (printer.configfile.settings['stepper_x'].position_max - printer.toolhead.position.x)|round < 10 %}
      {% set x_park = (10 - (printer.configfile.settings['stepper_x'].position_max - printer.toolhead.position.x))|round %}
      {% if x_park > 0 %}
        G91
        G1 x-{x_park} F3600
        G90
        G4 P1000
      {% endif %}
    {% endif %}
  {% endif %}
  
  # SET_TMC_FIELD FIELD=SGTHRS STEPPER=stepper_y VALUE=70
  # SET_TMC_FIELD FIELD=SGTHRS STEPPER=stepper_x VALUE=70
  # Home
  G28 X
  SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=x_ready VALUE=1
  # Move away
  G91
  G1 x-10 F3600
  G90
  # Wait just a second (give StallGuard registers time to clear)
  G4 P2000

[gcode_macro _HOME_Y]
gcode:
  _IF_MOVE_XY

  {% if printer['gcode_macro xyz_ready'].y_ready|int == 1 %}
    {% if printer.toolhead.position.y|round < 9 %}
      {% set y_park = 9 - printer.toolhead.position.y|round %}
      {% if y_park > 0 %}
        G91
        G1 y{y_park} F3600
        G90
        G4 P1000
      {% endif %}
    {% endif %}
  {% endif %}

  # SET_TMC_FIELD FIELD=SGTHRS STEPPER=stepper_y VALUE=60
  # SET_TMC_FIELD FIELD=SGTHRS STEPPER=stepper_x VALUE=60
  # Home
  G28 Y
  SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=y_ready VALUE=1
  # Move away
  G91
  G1 y10 F3600
  G90
  # Wait just a second (give StallGuard registers time to clear)
  G4 P2000

[gcode_macro _HOME_Z]
gcode:
  {% if printer['gcode_macro xyz_ready'].y_ready|int == 1 %}
    {% if printer['gcode_macro xyz_ready'].x_ready|int == 1 %}
      _IF_HOME_Z
    {% endif %}
  {% endif %}
  {% if printer.print_stats.z_pos|float >= 210.0 %}
    FORCE_MOVE STEPPER=stepper_z DISTANCE=-8 VELOCITY=10
  {% endif %}

  {% set POSITION_X = printer.configfile.settings['stepper_x'].position_max/2 %}
  {% set POSITION_Y = printer.configfile.settings['stepper_y'].position_max/2 %}
  # BED_MESH_SET_DISABLE
  G91
  {% set x_park = POSITION_X - printer.toolhead.position.x|int %}
  {% set y_park = POSITION_Y - printer.toolhead.position.y|int %}
  {action_respond_info("x_park = %s \n" % (x_park))}
  {action_respond_info("y_park = %s \n" % (y_park))}
  G1 x{x_park} y{y_park} F3600
  G90
  # G4 P500

  G28 Z
  SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=z_ready VALUE=1
  # BED_MESH_SET_ENABLE

[homing_override]
axes: xyz
# set_position_x: 0
# set_position_y: 0
# set_position_z:0
gcode:
  M220 S100
  # SET_TMC_FIELD STEPPER=stepper_x FIELD=en_spreadcycle VALUE=0
  # SET_TMC_FIELD STEPPER=stepper_y FIELD=en_spreadcycle VALUE=0
  BED_MESH_CLEAR
  {% set x_axes = printer.toolhead.homed_axes %}
  {% if x_axes is defined and x_axes[0] is defined %}
    {action_respond_info(
        "x_axes: %s \n"
        % (x_axes))}
  {% else %}
    SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=x_ready VALUE=0
    SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=y_ready VALUE=0
    SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=z_ready VALUE=0
    SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=xy_moved VALUE=0
    SET_GCODE_VARIABLE MACRO=xyz_ready VARIABLE=z_moved VALUE=0
    {action_respond_info(
        "x_axes is NULL\n")}
  {% endif %}

  {% if x_axes is defined and x_axes[0] is defined and x_axes[1] is defined %}
    {action_respond_info("x_axes: %s \n"% (x_axes))}
    {% set position_y = printer.toolhead.position.y|int %}
    {% set max_y_position = printer["gcode_macro PRINTER_PARAM"].max_y_position|int %}
    {% if position_y >= max_y_position %}
      G91
      G0 Y-10 F3600
      G90
    {% endif %}
  {% endif %}

  {% set home_all = 'X' not in params and 'Y' not in params %}
  {% if 'Z' in params and 'X' not in params and 'Y' not in params %}
    {% if x_axes[0] is defined and x_axes[1] is defined %}
      {% set home_all = 0 %}
    {% endif %}
  {% endif %}

  {% if home_all or 'X' in params %}
    _HOME_X
  {% endif %}

  {% if home_all or 'Y' in params %}
    _HOME_Y
  {% endif %}

  {% if home_all or 'X' in params %}
    _HOME_X
  {% endif %}

  {% if home_all or 'Y' in params %}
    _HOME_Y
  {% endif %}

  {% if home_all or 'Z' in params %}
    _HOME_Z
    BED_MESH_PROFILE LOAD="default"
  {% endif %}
  # SET_TMC_FIELD STEPPER=stepper_x FIELD=en_spreadcycle VALUE=1
  # SET_TMC_FIELD STEPPER=stepper_y FIELD=en_spreadcycle VALUE=1
  # SET_TMC_FIELD FIELD=SGTHRS STEPPER=stepper_x VALUE=0
  # SET_TMC_FIELD FIELD=SGTHRS STEPPER=stepper_y VALUE=0
  G4 P500