# Copyright (C) 2022 Justin Schuh <code@justinschuh.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.

[gcode_macro park]
description: Park the toolhead
  Usage: PARK [P=<0|1|2>] [X=<pos>] [Y=<pos>] [Z=<pos>] [LAZY=<1|0>]
gcode:
  {% set km = printer["gcode_macro _km_globals"] %}
  {% set LAZY = params.LAZY|default(1)|int %}
  {% if printer.toolhead.homed_axes != "xyz" %}
    {% if LAZY %}
      M118 checkpoint PARK G28 01
      G28 O
      M118 checkpoint PARK G28 1
    {% else %}
      {action_raise_error("Must home axes first.")}
    {% endif %}
  {% endif %}

  # Z position type from G27 (if below, absolute, relative)
  {% set P = (params.P|default(2))|int %} # Default to 2 because it's sanest.
  {% set X = params.X|default(km.park_x)|float %}
  {% set Y = params.Y|default(km.park_y)|float %}
  {% set Z = params.Z|default(km.park_z)|float %}

  _CHECK_KINEMATIC_LIMITS X="{X}" Y="{Y}" Z="{Z}"
  _PARK_INNER X="{X}" Y="{Y}" Z="{Z}" P="{P}" LAZY="{LAZY}"
  # Dummy argument block for Mainsail
  {% set dummy = None if True else "
  {% set dummy = params.P|default(mode=<0|1|2>)|int %}
  {% set dummy = params.X|default(X position)|int %}
  {% set dummy = params.Y|default(Y position)|int %}
  {% set dummy = params.Z|default(Z position)|int %}
  " %} # End argument block for Mainsail

[gcode_macro _park_inner]
gcode:
  {% set km = printer["gcode_macro _km_globals"] %}
  {% set travel_speed_xy = km.travel_speed_xy %}
  {% set travel_speed_z = km.travel_speed_z %}

  {% set position = printer.gcode_move.gcode_position %}

  # Use the taller of the highest printed layer or the current Z height, which
  # should helps crashes e.g. when a sequential print in progress.
  {% set clearance_z = (printer["gcode_macro _km_layer_run"].clearance_z,
                        position.z) | max %}
  {% set P = params.P|int %}
  {% set X = params.X|float %}
  {% set Y = params.Y|float %}
  {% set Z = params.Z|float %}
  {% set LAZY = params.LAZY|int %}

  # Convert everything to absolute coordinates.
  # P == 1 is absolute, so needs no adjustment.
  {% if P == 0 %} # Move absolute to Z if below current Z
    {% if clearance_z > Z %}
      {% set Z = clearance_z %}
    {% endif %}
  {% elif P == 2 %} # Move Z relative
    {% set Z = Z + clearance_z %}
  {% elif P != 1 %}
    {action_raise_error("Invalid parameter P=%i. Value must be 0, 1, or 2." |
        format(P)) }
  {% endif %}

  # Clamp to the printer limits.
  {% set Z = ((Z, printer.toolhead.axis_maximum.z)|min,
              printer.toolhead.axis_minimum.z)|max %}

  # Don't move if it's a lazy park and we're already in position.
  {% if (not LAZY) or P != 2 or X != position.x or Y != position.y 
        or Z < clearance_z %}
    SAVE_GCODE_STATE NAME=_KM_PARK
    G90
    G0 Z{Z} F{travel_speed_z}
    G0 X{X} Y{Y} F{travel_speed_xy}
    RESTORE_GCODE_STATE NAME=_KM_PARK MOVE=0
  {% endif %}


[gcode_macro g27]
description: Parks the toolhead.
  Usage: G27 [P=<0|1|2>]
gcode:
  # Wraps any arguments for the PARK macro and defaults P=0 for Marlin compat.
  PARK P={params.P|default(0)} {% for k in params|reject("in", "GP") %}{
    ' '~k~'="'~params[k]~'"'
    }{% endfor %}