#########################################
###### MACHINE VIBRATIONS ANALYSIS ######
#########################################
# Written by Frix_x#0161 #

[gcode_macro CREATE_VIBRATIONS_PROFILE]
gcode:
    {% set size = params.SIZE|default(100)|int %} # size of the circle where the angled lines are done
    {% set z_height = params.Z_HEIGHT|default(20)|int %} # z height to put the toolhead before starting the movements
    {% set max_speed = params.MAX_SPEED|default(200)|float * 60 %} # maximum feedrate for the movements
    {% set speed_increment = params.SPEED_INCREMENT|default(2)|float * 60 %} # feedrate increment between each move
    
    {% set feedrate_travel = params.TRAVEL_SPEED|default(200)|int * 60 %} # travel feedrate between moves
    {% set accel = params.ACCEL|default(3000)|int %} # accel value used to move on the pattern
    {% set accel_chip = params.ACCEL_CHIP|default("adxl345") %} # ADXL chip name in the config

    {% set keep_results = params.KEEP_N_RESULTS|default(3)|int %}
    {% set keep_csv = params.KEEP_CSV|default(True) %}

    {% set mid_x = printer.toolhead.axis_maximum.x|float / 2 %}
    {% set mid_y = printer.toolhead.axis_maximum.y|float / 2 %}
    {% set min_speed = 2 * 60 %} # minimum feedrate for the movements is set to 2mm/s
    {% set nb_speed_samples = ((max_speed - min_speed) / speed_increment + 1) | int %}

    {% set accel = [accel, printer.configfile.settings.printer.max_accel]|min %}
    {% set old_accel = printer.toolhead.max_accel %}
    {% set old_cruise_ratio = printer.toolhead.minimum_cruise_ratio %}
    {% set old_sqv = printer.toolhead.square_corner_velocity %}

    {% set kinematics = printer.configfile.settings.printer.kinematics %}


    {% if not 'xyz' in printer.toolhead.homed_axes %}
        { action_raise_error("Must Home printer first!") }
    {% endif %}

    {% if params.SPEED_INCREMENT|default(2)|float * 100 != (params.SPEED_INCREMENT|default(2)|float * 100)|int %}
        { action_raise_error("Only 2 decimal digits are allowed for SPEED_INCREMENT") }
    {% endif %}

    {% if (size / (max_speed / 60)) < 0.25 %}
        { action_raise_error("SIZE is too small for this MAX_SPEED. Increase SIZE or decrease MAX_SPEED!") }
    {% endif %}

    {action_respond_info("")}
    {action_respond_info("Starting machine vibrations profile measurement")}
    {action_respond_info("This operation can not be interrupted by normal means. Hit the \"emergency stop\" button to stop it if needed")}
    {action_respond_info("")}

    SAVE_GCODE_STATE NAME=CREATE_VIBRATIONS_PROFILE

    G90

    # Set the wanted acceleration values (not too high to avoid oscillation, not too low to be able to reach constant speed on each segments)
    SET_VELOCITY_LIMIT ACCEL={accel} MINIMUM_CRUISE_RATIO=0 SQUARE_CORNER_VELOCITY={[(accel / 1000), 5.0]|max}

    # Going to the start position
    G1 Z{z_height} F{feedrate_travel / 10}
    G1 X{mid_x } Y{mid_y} F{feedrate_travel}


    {% if kinematics == "cartesian" %}
        # Cartesian motors are on X and Y axis directly
        RESPOND MSG="Cartesian kinematics mode"
        {% set main_angles = [0, 90] %}
    {% elif kinematics == "corexy" %}
        # CoreXY motors are on A and B axis (45 and 135 degrees)
        RESPOND MSG="CoreXY kinematics mode"
        {% set main_angles = [45, 135] %}
    {% else %}
        { action_raise_error("Only Cartesian and CoreXY kinematics are supported at the moment for the vibrations measurement tool!") }
    {% endif %}

    {% set pi = (3.141592653589793) | float %}
    {% set tau = (pi * 2) | float %}


    {% for curr_angle in main_angles %}
        {% for curr_speed_sample in range(0, nb_speed_samples) %}
            {% set curr_speed = min_speed + curr_speed_sample * speed_increment %}
            {% set rad_angle_full = (curr_angle|float * pi / 180) %}

            # -----------------------------------------------------------------------------------------------------------
            # Here are some maths to approximate the sin and cos values of rad_angle in Jinja
            # Thanks a lot to Aubey! for sharing the idea of using hardcoded Taylor series and
            # the associated bit of code to do it easily! This is pure madness!
            {% set rad_angle = ((rad_angle_full % tau) - (tau / 2)) | float %}

            {% if rad_angle < (-(tau / 4)) %}
                {% set rad_angle = (rad_angle + (tau / 2)) | float %}
                {% set final_mult = (-1) %}
            {% elif rad_angle > (tau / 4) %}
                {% set rad_angle = (rad_angle - (tau / 2)) | float %}
                {% set final_mult = (-1) %}
            {% else %}
                {% set final_mult = (1) %}
            {% endif %}

            {% set sin0 = (rad_angle) %}
            {% set sin1 = ((rad_angle ** 3) / 6) | float %}
            {% set sin2 = ((rad_angle ** 5) / 120) | float %}
            {% set sin3 = ((rad_angle ** 7) / 5040) | float %}
            {% set sin4 = ((rad_angle ** 9) / 362880) | float %}
            {% set sin5 = ((rad_angle ** 11) / 39916800) | float %}
            {% set sin6 = ((rad_angle ** 13) / 6227020800) | float %}
            {% set sin7 = ((rad_angle ** 15) / 1307674368000) | float %}
            {% set sin = (-(sin0 - sin1 + sin2 - sin3 + sin4 - sin5 + sin6 - sin7) * final_mult) | float %}

            {% set cos0 = (1) | float %}
            {% set cos1 = ((rad_angle ** 2) / 2) | float %}
            {% set cos2 = ((rad_angle ** 4) / 24) | float %}
            {% set cos3 = ((rad_angle ** 6) / 720) | float %}
            {% set cos4 = ((rad_angle ** 8) / 40320) | float %}
            {% set cos5 = ((rad_angle ** 10) / 3628800) | float %}
            {% set cos6 = ((rad_angle ** 12) / 479001600) | float %}
            {% set cos7 = ((rad_angle ** 14) / 87178291200) | float %}
            {% set cos = (-(cos0 - cos1 + cos2 - cos3 + cos4 - cos5 + cos6 - cos7) * final_mult) | float %}
            # -----------------------------------------------------------------------------------------------------------

            # Reduce the segments length for the lower speed range (0-100mm/s). The minimum length is 1/3 of the SIZE and is gradually increased
            # to the nominal SIZE at 100mm/s. No further size changes are made above this speed. The goal is to ensure that the print head moves
            # enough to collect enough data for vibration analysis, without doing unnecessary distance to save time. At higher speeds, the full
            # segments lengths are used because the head moves faster and travels more distance in the same amount of time and we want enough data
            {% if curr_speed < (100 * 60) %}
                {% set segment_length_multiplier = 1/3 + 2/3 * (curr_speed / 60) / 100 %}
            {% else %}
                {% set segment_length_multiplier = 1 %}
            {% endif %}

            # Calculate angle coordinates using trigonometry and length multiplier and move to start point
            {% set dx = (size / 2) * cos * segment_length_multiplier %}
            {% set dy = (size / 2) * sin * segment_length_multiplier %}
            G1 X{mid_x - dx} Y{mid_y - dy} F{feedrate_travel}

            # Adjust the number of back and forth movements based on speed to also save time on lower speed range
            # 3 movements are done by default, reduced to 2 between 150-250mm/s and to 1 under 150mm/s.
            {% set movements = 3 %}
            {% if curr_speed < (150 * 60) %}
                {% set movements = 1 %}
            {% elif curr_speed < (250 * 60) %}
                {% set movements = 2 %}
            {% endif %}

            ACCELEROMETER_MEASURE CHIP={accel_chip}

            # Back and forth movements to record the vibrations at constant speed in both direction
            {% for n in range(movements) %}
                G1 X{mid_x + dx} Y{mid_y + dy} F{curr_speed}
                G1 X{mid_x - dx} Y{mid_y - dy} F{curr_speed}
            {% endfor %}
            
            ACCELEROMETER_MEASURE CHIP={accel_chip} NAME=an{("%.2f" % curr_angle|float)|replace('.','_')}sp{("%.2f" % (curr_speed / 60)|float)|replace('.','_')}
            G4 P300

            M400
        {% endfor %}
    {% endfor %}


    RESPOND MSG="Machine vibrations profile generation..."
    RESPOND MSG="This may take some time (3-5min)"
    RUN_SHELL_COMMAND CMD=shaketune PARAMS="--type vibrations --accel {accel|int} --kinematics {kinematics} --chip_name {accel_chip} {% if keep_csv %}--keep_csv{% endif %} --keep_results {keep_results}"

    # Restore the previous acceleration values
    SET_VELOCITY_LIMIT ACCEL={old_accel} MINIMUM_CRUISE_RATIO={old_cruise_ratio} SQUARE_CORNER_VELOCITY={old_sqv}

    RESTORE_GCODE_STATE NAME=CREATE_VIBRATIONS_PROFILE