#[servo cutter_gantry_servo]
#pin: PB6
#initial_angle: 180


#[gcode_macro gantry_servo_down]
#gcode:
#    SET_SERVO SERVO=cutter_gantry_servo ANGLE=55

#[gcode_macro gantry_servo_up]
#gcode:
#    SET_SERVO SERVO=cutter_gantry_servo ANGLE=180


[gcode_macro _MMU_CUT_TIP]
description: Cut filament by pressing the cutter on a pin with a horizontal movement. 

# Distance to retract prior to making the cut, this reduces wasted filament but might cause clog 
# if set too large and/or if there are gaps in the hotend assembly 
# This must be less than the distance from the nozzle to the cutter.
variable_retract_length: 33

# The location of the pin, this should be the position of the toolhead when the cutter 
# just lightly touches the pin
variable_pin_loc_x: 14
variable_pin_loc_y: 357

# The starting and end positions when making the cut
# In particular, instead of making the cut by traveling to the pin location above, 
# we leave a small safety margin along X-axis to avoid scratching on the pin when traveling
# This should also give a small distance to produce some momentum when pressing on the pin 
variable_pin_park_x_dist: 5.0

# Position of the toolhead when the cutter is fully compressed
# Should leave a small headroom (e.g., should be a bit larger than 0, or whatever xmin is) to avoid banging the toolhead or gantry
variable_pin_loc_x_compressed: 0

#  Retract length and speed after the cut so that eh cutter can go back into its origin position
variable_rip_length: 1
variable_rip_spd: 750

# Pushback of the remaining tip from the cold end into the hotend
variable_pushback_length: 0
variable_push_dwell_time: 50 # time to dwell between the pushback

# Speed related settings
# Note that if the cut speed is too fast, the steppers can lose steps
# Therefore, for a cut: 
# - We first make a fast move to accumulate some momentum and get the cut blade to the initial contact with the filament
# - We then make a slow move for the actual cut to happen 
variable_travel_spd: 7800
variable_cut_fast_move_spd: 2000
variable_cut_slow_move_spd: 500
variable_evacuate_speed: 7800
variable_cut_dwell_time: 50 # time to dwell at the cut point in ms
variable_cut_fast_move_fraction: 1.0 # fraction of the move that uses fast move 

# Safety margin for fast vs slow travel
# When traveling to the pin location, we make a safer but longer move if we closer to the pin than this specified margin
# Usually setting these to the size of the toolhead (plus a small margin) should be good enough 
variable_safe_margin_x: 30
variable_safe_margin_y: 30

# Whether or not to make a second cut
# The goal of the second cut is to avoid filament adding friction to the blade causing the lever unable to decompress
# If set to a positive value, we first retract this amount, make the second cut, then extrude back
# Set to 0 to disable to second cut
# You should only enable this if you cannot get the lever to reliabily decompress.
variable_second_cut_retract_length: 0

# Whether to eject the filament at the end
variable_final_eject: 0

# Currently ERCF software assumes that tip forming move always moves the extruder (and consequently the encoder)
# Ideally, we should bring formal support of filament cutters to the ERCF software
# In the short term, we perform a "hack" where we wiggle the extruder a little after cutting, 
# if the extruder did not make enough movement during the filament cutting phase.
variable_extruder_minimum_movement: 0 # the minimum extruder movement required for this tip forming move, this depends on the minimum distance that the encoder can reliably detect
                                      # setting it to a lower value requires the ERCF encoder to be more sensitive.
variable_extruder_wiggle_length: 10 # the length to retract -> extrude during a wiggle, this needs to be large enough for the encoder to detect the movement
variable_extruder_wiggle_repeat: 3 # number of repeats for the wiggle

# Optionally move to location immediately after making the cut
#variable_x_after_toolchange: -1000
#variable_y_after_toolchange: -1000

variable_output_park_pos: 37.5 #  this should be set to the distance from the nozzle tip to the blade for filametrix type cutter

gcode:
    {% set RETRACT_LENGTH = params.RETRACT_LENGTH | default(printer['gcode_macro _MMU_CUT_TIP']['retract_length']) | float %}
    {% set PUSHBACK_LENGTH = params.PUSHBACK_LENGTH | default(printer['gcode_macro _MMU_CUT_TIP']['pushback_length']) | float %}
    {% set FINAL_EJECT = params.FINAL_EJECT | default(printer['gcode_macro _MMU_CUT_TIP']['final_eject'], True) | int %}
    {% set current_loc_x = printer.gcode_move.gcode_position.x %}
    {% set current_loc_y = printer.gcode_move.gcode_position.y %}
    {% set push_dwell_time = printer['gcode_macro _MMU_CUT_TIP']['push_dwell_time'] | float %}

    SAVE_GCODE_STATE NAME=_MMU_CUT_TIP_state
    
    {% if ("x" not in printer.toolhead.homed_axes) or ("y" not in printer.toolhead.homed_axes) %}
        G28 X Y
    {% endif %}

    {% set prev_pa = printer.extruder.pressure_advance %}
    SET_PRESSURE_ADVANCE ADVANCE=0 # temporaily disable PA

    {% set pin_park_x_loc = pin_loc_x + pin_park_x_dist %}
    {% set pin_park_y_loc = pin_loc_y %}
    {% set _extruder_moved_dist = 0 %}

    M83 # relative extrusion
    {% if RETRACT_LENGTH > 0 %}
        G1 E-{RETRACT_LENGTH} F3000 # retract to save filament waste
        {% set _extruder_moved_dist = _extruder_moved_dist + RETRACT_LENGTH %}
    {% endif %}

#    gantry_servo_up
    
    G90 # absolute positioning
    _MOVE_TO_CUTTER_PIN PIN_PARK_X_LOC={pin_park_x_loc} PIN_PARK_Y_LOC={pin_park_y_loc}

#    gantry_servo_down
    
    # Make the main cut 
    _DO_CUT_MOTION PIN_PARK_X_LOC={pin_park_x_loc} RIP_LENGTH={rip_length}
    {% set _extruder_moved_dist = _extruder_moved_dist + rip_length %}
    
    # Optional secondary cut to help decompressing the lever
    {% if second_cut_retract_length > 0 %}
        G1 E-{second_cut_retract_length} F3000
        _DO_CUT_MOTION PIN_PARK_X_LOC={pin_park_x_loc} RIP_LENGTH=-1
        G1 E{second_cut_retract_length} F3000
    {% endif %}

#    gantry_servo_up


    # pushback of the tip residual into the hotend
    {% if PUSHBACK_LENGTH > 0 %} 
        G1 E{PUSHBACK_LENGTH} F1000 
#        G4 P{push_dwell_time}
#        G1 E-{PUSHBACK_LENGTH} F1000
        {% set _extruder_moved_dist = _extruder_moved_dist - PUSHBACK_LENGTH %}
    {% endif %}
    
    {% if FINAL_EJECT == 1 %}
        G1 E-80 F3000
        {% set _extruder_moved_dist = _extruder_moved_dist + 80 %}
    {% endif %}

    {% if _extruder_moved_dist < extruder_minimum_movement %}
        # make some wiggle moves so that the encoder catch them
        {% for _ in range(extruder_wiggle_repeat) %}
            G1 E-{extruder_wiggle_length} F3000
            G1 E{extruder_wiggle_length} F3000
        {% endfor %}
    {% endif %}

    SET_PRESSURE_ADVANCE ADVANCE={prev_pa} # restore PA

#    {% set move_to_next_loc = ((x_after_toolchange != -1000) and (y_after_toolchange != -1000)) | int %}
#    {% if move_to_next_loc %}
#        G1 X{x_after_toolchange} Y{y_after_toolchange} F{travel_spd}
#    {% endif %}

 #   _SET_LOC_AFTER_TOOLCHANGE # clears the location
    RESTORE_GCODE_STATE NAME=_MMU_CUT_TIP_state MOVE_SPEED=250


#[gcode_macro _SET_LOC_AFTER_TOOLCHANGE]
#description: Set the location to travel after making a cut
#gcode:
#    # Note that we use an extremely negative number to represent that we not moving anywhere afte toolchange
#    {% set x_after_toolchange = params.X | default (-1000) | float %}
#    {% set y_after_toolchange = params.Y | default (-1000) | float %}
#    SET_GCODE_VARIABLE macro=_MMU_CUT_TIP variable=x_after_toolchange value={x_after_toolchange}
#    SET_GCODE_VARIABLE macro=_MMU_CUT_TIP variable=y_after_toolchange value={y_after_toolchange}


[gcode_macro _MOVE_TO_CUTTER_PIN]
description: helper to move the toolhead to the target pin in either safe or faster way, depending on toolhead clearance.
gcode:
    {% set safe_margin_x = printer['gcode_macro _MMU_CUT_TIP']['safe_margin_x'] | float %}
    {% set safe_margin_y = printer['gcode_macro _MMU_CUT_TIP']['safe_margin_y'] | float %}
    {% set travel_spd = printer['gcode_macro _MMU_CUT_TIP']['travel_spd'] | float %}
    {% set pin_park_x_loc = params.PIN_PARK_X_LOC | float %}
    {% set pin_park_y_loc = params.PIN_PARK_Y_LOC | float %}

    G90 # absolute positioning
    {% if ((printer.gcode_move.gcode_position.x - pin_park_x_loc) | abs < safe_margin_x) or ((printer.gcode_move.gcode_position.y - pin_park_y_loc | float) | abs < safe_margin_y) %}
        # Make a safe but slower travel move
        G1 X{pin_park_x_loc} F{travel_spd}
        G1 Y{pin_park_y_loc} F{travel_spd}
    {% else %}
        G1 X{pin_park_x_loc} Y{pin_park_y_loc} F{travel_spd}
    {% endif %}


[gcode_macro _DO_CUT_MOTION]
description: helper to do a single horizontal cut movement
gcode:
    {% set pin_loc_x_compressed = printer['gcode_macro _MMU_CUT_TIP']['pin_loc_x_compressed'] | float %}
    {% set cut_fast_move_fraction = printer['gcode_macro _MMU_CUT_TIP']['cut_fast_move_fraction'] | float %}
    {% set cut_fast_move_spd = printer['gcode_macro _MMU_CUT_TIP']['cut_fast_move_spd'] | float %}
    {% set cut_slow_move_spd = printer['gcode_macro _MMU_CUT_TIP']['cut_slow_move_spd'] | float %}
    {% set cut_dwell_time = printer['gcode_macro _MMU_CUT_TIP']['cut_dwell_time'] | float %}
    {% set evacuate_speed = printer['gcode_macro _MMU_CUT_TIP']['evacuate_speed'] | float %}
    {% set pin_park_x_loc = params.PIN_PARK_X_LOC | float %}
    {% set rip_spd = printer['gcode_macro _MMU_CUT_TIP']['rip_spd'] | float %}
    {% set rip_length = params.RIP_LENGTH | default(-1) | float %}
    {% set fast_slow_transition_loc = (pin_loc_x_compressed - pin_park_x_loc) * cut_fast_move_fraction + pin_park_x_loc | float %}
    G1 X{fast_slow_transition_loc} F{cut_fast_move_spd} # make a fast move to initiate contact of the blade with the filament
    G1 X{pin_loc_x_compressed} F{cut_slow_move_spd} # do the cut in slow move
    G4 P{cut_dwell_time}
    {% if rip_length > 0 %}
        G1 E-{rip_length} F{rip_spd}
    {% endif %}
    G1 X{pin_park_x_loc} F{evacuate_speed} # evacuate