# AFC_XY_SAFETY_CHECK: lost-step check around tool changes (needs extras/xy_drift.py)

#####################################################################
## XY lost-step check (needs xy_drift.py + [xy_drift])
#####################################################################
# Homing resets the toolhead position, so the position after G28 cannot
# show lost steps. [xy_drift] compares the stepper step counts at the X and
# Y endstop triggers instead: every lost step shows up as drift.
# _AFC_STORE_HOME_REFERENCE (PRINT_START) stores the reference, every
# AFC_XY_SAFETY_CHECK compares against it and then stores a new reference,
# so each check reports the drift since the previous one.

[xy_drift]

[gcode_macro _AFC_STORE_HOME_REFERENCE]
gcode:
    G28 X Y
    M400
    XY_DRIFT_STORE

[gcode_macro AFC_XY_SAFETY_CHECK]
description: Re-home XY and report steps lost since the last check
variable_allowed_mm: 0.1     # one motor skipping one cycle (0.4 mm) shows as 0.2 mm in X and Y; endstop noise ~0.02
variable_soft_fail: 1        # 1 = PAUSE on drift, 0 = M112
gcode:
    {% set th = printer.toolhead %}
    M117 AFC XY check...
    SAVE_GCODE_STATE NAME=_AFC_XY_CHECK
    G90
    G1 Z{[th.position.z + 10, th.axis_maximum.z]|min} F3000
    G28 X Y
    M400
    _AFC_CHECK_CAPTURE
    RESTORE_GCODE_STATE NAME=_AFC_XY_CHECK

[gcode_macro _AFC_CHECK_CAPTURE]
gcode:
    {% set d = printer.xy_drift %}
    {% set cfg = printer["gcode_macro AFC_XY_SAFETY_CHECK"] %}
    {% set th = printer.toolhead %}
    {% set lim = cfg.allowed_mm|float %}
    {% set drift = d.ref_set and (d.x|abs > lim or d.y|abs > lim) %}
    {% if not d.ref_set %}
        RESPOND PREFIX="AFC" MSG="XY check: no reference yet (Klipper restarted?), storing one now"
    {% else %}
        RESPOND PREFIX="AFC" MSG="XY drift since last check: X={'%.3f' % d.x} Y={'%.3f' % d.y} mm (limit {lim})"
    {% endif %}
    # New reference from this homing, so the next check reports only new drift
    XY_DRIFT_STORE
    {% if not drift %}
        {% if d.ref_set %}
            RESPOND PREFIX="AFC" MSG="Position OK"
        {% endif %}
        G1 X{th.axis_maximum.x - 10} Y{th.axis_maximum.y - 10} F5000
    {% elif cfg.soft_fail|int == 1 %}
        RESPOND TYPE=error MSG="AFC: XY drift detected! X={'%.3f' % d.x} Y={'%.3f' % d.y} mm. Axes are re-homed, print PAUSED. Run AFC_XY_RESUME to continue, or CANCEL_PRINT."
        M117 AFC: XY drift! See console. Paused.
        PAUSE
    {% else %}
        RESPOND TYPE=error MSG="AFC: XY drift X={'%.3f' % d.x} Y={'%.3f' % d.y} mm -> M112"
        M112
    {% endif %}

[gcode_macro AFC_XY_RESUME]
description: Acknowledge XY drift warning and resume print
gcode:
    RESPOND PREFIX="AFC" MSG="User acknowledged drift, resuming."
    # Axes were re-homed by the check, so AFC's resume returns to the
    # correct print position.
    RESUME