#####################################################################
## 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

#####################################################################
## unloading help
#####################################################################

[gcode_macro LANE_FEED_UNTIL_HUB_OPEN]
variable_lane: 1
variable_step: -50
variable_max_dist: 2000
variable_moved: 0
variable_active: 0

gcode:
    {% set lane = params.LANE|default(1)|int %}
    {% set step = params.STEP|default(-50)|float %}
    {% set max_dist = params.MAX_DIST|default(2000)|float %}

    SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=lane VALUE={lane}
    SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=step VALUE={step}
    SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=max_dist VALUE={max_dist}
    SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=moved VALUE=0
    SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=active VALUE=1

    RESPOND PREFIX="info" MSG="START lane={lane} step={step} max={max_dist}"

    UPDATE_DELAYED_GCODE ID=_lane_feed_loop DURATION=0.05

[delayed_gcode _lane_feed_loop]
gcode:
    {% set m = printer["gcode_macro LANE_FEED_UNTIL_HUB_OPEN"] %}

    {% if m.active == 1 %}

        LANE_MOVE LANE=lane{m.lane} DISTANCE={m.step}

        {% set new_moved = m.moved + m.step %}
        SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=moved VALUE={new_moved}

        RESPOND PREFIX="info" MSG="MOVE lane={m.lane} step={m.step} moved={new_moved}"

        QUERY_ENDSTOPS

        UPDATE_DELAYED_GCODE ID=_lane_eval DURATION=0.03

    {% else %}
        RESPOND PREFIX="info" MSG="feed_loop inactive"
    {% endif %}

[delayed_gcode _lane_eval]
gcode:
    {% set m = printer["gcode_macro LANE_FEED_UNTIL_HUB_OPEN"] %}

    {% if m.active == 1 %}

        {% set hub = printer.query_endstops.last_query.get("hub", "unknown") %}
        
        # RESPOND PREFIX="info" MSG="QUERY hub={hub} raw={printer.query_endstops.last_query.get('hub', 'MISSING')}"
        # RESPOND PREFIX="info" MSG="last_query: {printer.query_endstops.last_query}"

        {% if hub == "open" or hub == 0 %}
            RESPOND PREFIX="info" MSG="HUB TRIGGERED → STOP"

            SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=active VALUE=0

            LANE_MOVE LANE=lane{m.lane} DISTANCE=-100

        {% elif m.moved|abs >= m.max_dist %}

            RESPOND PREFIX="warning" MSG="MAX DIST STOP reached={m.moved}"

            SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=active VALUE=0

        {% else %}

            RESPOND PREFIX="info" MSG="CONTINUE"

            UPDATE_DELAYED_GCODE ID=_lane_feed_loop DURATION=0.03

        {% endif %}

    {% else %}
        RESPOND PREFIX="info" MSG="eval inactive"
    {% endif %}

[gcode_macro LANE_FEED_ABORT]
gcode:
    SET_GCODE_VARIABLE MACRO=LANE_FEED_UNTIL_HUB_OPEN VARIABLE=active VALUE=0
    UPDATE_DELAYED_GCODE ID=_lane_feed_loop DURATION=0
    UPDATE_DELAYED_GCODE ID=_lane_eval DURATION=0
    RESPOND PREFIX="warning" MSG="ABORTED"

[gcode_macro HOME]
gcode:
    G90
    G4 P2000
    G28 X0 Y0 Z0
    G1 X175 Y175 Z10 F1200

[gcode_macro SET_ACTIVE_SPOOL]
gcode:
  {% if params.ID %}
    {% set id = params.ID|int %}
    {action_call_remote_method(
       "spoolman_set_active_spool",
       spool_id=id
    )}
  {% else %}
    {action_respond_info("Parameter 'ID' is required")}
  {% endif %}

[gcode_macro CLEAR_ACTIVE_SPOOL]
gcode:
  {action_call_remote_method(
    "spoolman_set_active_spool",
    spool_id=None
  )}

# [gcode_macro TEST_AMS_STRESS]
# gcode:
#   {% for i in range(500) %}
#     T0 ; Load tool 0
#     G4 P5000 ; Wait 5s
#     T1 ; Unload
#     G4 P3000 ; Wait 3s
#   {% endfor %}