gcode.c

/*
  gcode.c - rs274/ngc parser.

  Part of grblHAL

  Copyright (c) 2017-2026 Terje Io
  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud

  grblHAL is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  grblHAL is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
*/

#include <math.h>
#include <string.h>
#include <stdlib.h>

#include "hal.h"
#include "motion_control.h"
#include "protocol.h"
#include "state_machine.h"

#if NGC_PARAMETERS_ENABLE
#include "ngc_params.h"
#endif

#if NGC_EXPRESSIONS_ENABLE
#include "ngc_expr.h"
#include "ngc_flowctrl.h"
#ifndef NGC_N_ASSIGN_PARAMETERS_PER_BLOCK
#define NGC_N_ASSIGN_PARAMETERS_PER_BLOCK 10
#endif
#endif

// NOTE: Max line number is defined by the g-code standard to be 99999. It seems to be an
// arbitrary value, and some GUIs may require more. So we increased it based on a max safe
// value when converting a float (7.2 digit precision) to an integer.
#define MAX_LINE_NUMBER 10000000

#define MACH3_SCALING

// Do not change, must be same as axis indices
#define I_VALUE X_AXIS
#define J_VALUE Y_AXIS
#define K_VALUE Z_AXIS

typedef struct {
    modal_groups_t command;
    bool update_spindle[N_SYS_SPINDLE];
} modal_restore_actions_t;

typedef enum {
    AxisCommand_None = 0,
    AxisCommand_NonModal,
    AxisCommand_MotionMode,
    AxisCommand_ToolLengthOffset,
    AxisCommand_Scaling
} axis_command_t;

typedef struct {
    parameter_words_t parameter;
    modal_groups_t modal_group;
} word_bit_t;

typedef union {
    uint_fast8_t mask;
    struct {
        uint_fast8_t i :1,
                     j :1,
                     k :1;
    };
} ijk_words_t;

typedef struct m98_macro {
    macro_id_t id;
    line_number_t line_number;
    vfs_file_t *file;
    size_t pos;
    struct m98_macro *next;
} m98_macro_t;

// Declare gc extern struct
DCRAM parser_state_t gc_state;

#define RETURN(status) return gc_at_exit(status);

m98_macro_t *m98_macros = NULL;
static tool_data_t *pending_tool = NULL;
static output_command_t *output_commands = NULL; // Linked list
static settings_changed_ptr on_settings_changed = NULL;
static scale_factor_t scale_factor = {
    .ijk[X_AXIS] = 1.0f,
    .ijk[Y_AXIS] = 1.0f,
    .ijk[Z_AXIS] = 1.0f
#ifdef A_AXIS
  , .ijk[A_AXIS] = 1.0f
#endif
#ifdef B_AXIS
  , .ijk[B_AXIS] = 1.0f
#endif
#ifdef C_AXIS
  , .ijk[C_AXIS] = 1.0f
#endif
#ifdef U_AXIS
  , .ijk[U_AXIS] = 1.0f
#endif
#ifdef V_AXIS
  , .ijk[V_AXIS] = 1.0f
#endif
};

#if CUTTER_COMP_ENABLE

FLASHMEM __attribute__((weak)) void cc_init (void)
{
}

FLASHMEM __attribute__((weak)) bool cc_enable (gc_ccomp_t *comp_data, plane_t plane, coord_data_t *position)
{
    return comp_data->side == CComp_Off;
}

#endif

// Simple hypotenuse computation function.
inline static float hypot_f (float x, float y)
{
    return sqrtf(x * x + y * y);
}

inline static bool motion_is_lasercut (motion_mode_t motion)
{
    return motion == MotionMode_Linear || motion == MotionMode_CwArc || motion == MotionMode_CcwArc || motion == MotionMode_CubicSpline || motion == MotionMode_QuadraticSpline;
}

inline static bool no_word_value (char letter)
{
    return letter == '\0' || (letter >= 'A' && letter <= 'Z') || letter == '$';
}

parser_state_t *gc_get_state (void)
{
    return &gc_state;
}

FLASHMEM char *gc_coord_system_to_str (coord_system_id_t id)
{
    static char buf[6];

    uint8_t g5x = id + 54;

    strcat(strcpy(buf, "G"), uitoa((uint32_t)(g5x > 59 ? 59 : g5x)));
    if(g5x > 59) {
        strcat(buf, ".");
        strcat(buf, uitoa((uint32_t)(g5x - 59)));
    }

    return buf;
}

FLASHMEM static void override_disable (spindle_t *spindle, int32_t spindle_id, gc_override_flags_t flags, overrides_t *overrides)
{
    gc_override_flags_t org_flags = gc_state.modal.override_ctrl;

    if(flags.spindle_rpm_disable) {
        overrides->spindle_rpm = spindle->hal->param->override_pct;
        overrides->control.spindle_rpm_disable = org_flags.spindle_rpm_disable;
        BIT_SET(org_flags.spindle_rpm_disable, bit(spindle_id), On);
    }

    if(flags.feed_hold_disable) {
        overrides->control.feed_hold_disable = org_flags.feed_hold_disable;
        org_flags.feed_hold_disable = On;
    }

    if(flags.feed_rates_disable) {
        overrides->feed_rate = sys.override.feed_rate;
        overrides->rapid_rate = sys.override.rapid_rate;
        overrides->control.feed_rates_disable = org_flags.feed_rates_disable;
        plan_feed_override(DEFAULT_FEED_OVERRIDE, sys.override.rapid_rate);
        org_flags.feed_rates_disable = On;
    }

    mc_override_ctrl_update(org_flags);
}

FLASHMEM static void override_restore (spindle_t *spindle, int32_t spindle_id, gc_override_flags_t flags, overrides_t *overrides)
{
    if(flags.spindle_rpm) {
        if(!(gc_state.modal.override_ctrl.spindle_rpm_disable = overrides->control.spindle_rpm_disable)) {
            if(!spindle_override_disable(spindle->hal, bit_istrue(overrides->control.spindle_rpm_disable, bit(spindle_id))))
                spindle_set_override(spindle->hal, overrides->spindle_rpm);
        }
    }

    if(flags.feed_hold)
        gc_state.modal.override_ctrl.feed_hold_disable = overrides->control.feed_hold_disable;

    if(flags.feed_rates) {
        sys.override.feed_rate = overrides->feed_rate;
        if(!(gc_state.modal.override_ctrl.feed_rates_disable = overrides->control.feed_rates_disable))
            plan_feed_override(sys.override.feed_rate, sys.override.rapid_rate);
    }

    mc_override_ctrl_update(gc_state.modal.override_ctrl);
}

FLASHMEM static void set_scaling (float factor)
{
    uint_fast8_t idx = N_AXIS;
    axes_signals_t state = gc_get_g51_state();

    do {
        scale_factor.ijk[--idx] = factor;
#ifdef MACH3_SCALING
        scale_factor.xyz[idx] = 0.0f;
#endif
    } while(idx);

    gc_state.modal.scaling_active = factor != 1.0f;

    if(state.value != gc_get_g51_state().value)
        report_add_realtime(Report_Scaling);
}

float *gc_get_scaling (void)
{
    return scale_factor.ijk;
}

axes_signals_t gc_get_g51_state (void)
{
    uint_fast8_t idx = N_AXIS;
    axes_signals_t scaled = {0};

    do {
        scaled.value <<= 1;
        if(scale_factor.ijk[--idx] != 1.0f)
            scaled.value |= 0x01;

    } while(idx);

    return scaled;
}

void gc_clear_offset (coord_system_id_t id)
{
    bool clear;
    coord_system_data_t coord_data = {0};

    if((id == CoordinateSystem_G92)) {
        clear = !settings.flags.g92_is_volatile;
        gc_state.g92_offset_applied = false;
        memcpy(&gc_state.g92_offset, &coord_data, sizeof(coord_system_data_t));
#if COMPATIBILITY_LEVEL <= 1
    } else if((clear = (id < CoordinateSystem_G59_1 || id > CoordinateSystem_G59_3 || bit_isfalse(settings.offset_lock.mask, bit(id - CoordinateSystem_G59_1))))) {
#else
    } else {
        clear = true;
#endif
        if(id == gc_state.modal.g5x_offset.id)
            memcpy(&gc_state.modal.g5x_offset.data, &coord_data, sizeof(coord_system_data_t));
    }

    if(clear)
        settings_write_coord_data(id, &coord_data);
}

float gc_get_offset (uint_fast8_t idx, bool real_time)
{
    offset_id_t offset_id;

    if(real_time &&
        !(settings.status_report.machine_position && settings.status_report.sync_on_wco_change) &&
          (offset_id = st_get_offset_id()) >= 0)
        return gc_state.modal.g5x_offset.data.coord.values[idx] + gc_state.offset_queue[offset_id].values[idx] + gc_state.modal.tool_length_offset[idx];
    else
        return gc_state.modal.g5x_offset.data.coord.values[idx] + gc_state.g92_offset.coord.values[idx] + gc_state.modal.tool_length_offset[idx];
}

inline static float gc_get_block_offset (parser_block_t *gc_block, uint_fast8_t idx)
{
    return gc_block->modal.g5x_offset.data.coord.values[idx] + gc_state.g92_offset.coord.values[idx] + gc_state.modal.tool_length_offset[idx];
}

FLASHMEM void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offset)
{
    bool tlo_changed = false;

    switch(mode) {

        case ToolLengthOffset_Cancel:
            idx = N_AXIS;
            do {
                idx--;
                tlo_changed |= gc_state.modal.tool_length_offset[idx] != 0.0f;
                gc_state.modal.tool_length_offset[idx] = 0.0f;
                if(grbl.tool_table.n_tools == 0)
                    gc_state.tool->offset.values[idx] = 0.0f;
            } while(idx);
            break;

        case ToolLengthOffset_EnableDynamic:
            {
                float new_offset = offset / settings.axis[idx].steps_per_mm;
                tlo_changed |= gc_state.modal.tool_length_offset[idx] != new_offset;
                gc_state.modal.tool_length_offset[idx] = new_offset;
                if(grbl.tool_table.n_tools == 0)
                    gc_state.tool->offset.values[idx] = new_offset;
            }
            break;

        default:
            break;
    }

    gc_state.modal.tool_offset_mode = mode;

    if(tlo_changed) {
        report_add_realtime(Report_ToolOffset);
        system_flag_wco_change();
    }
}

plane_t *gc_get_plane_data (plane_t *plane, plane_select_t select)
{
    switch (select) {

        case PlaneSelect_XY:
            plane->axis_0 = X_AXIS;
            plane->axis_1 = Y_AXIS;
            plane->axis_linear = Z_AXIS;
            break;

        case PlaneSelect_ZX:
            plane->axis_0 = Z_AXIS;
            plane->axis_1 = X_AXIS;
            plane->axis_linear = Y_AXIS;
            break;

        default: // case PlaneSelect_YZ:
            plane->axis_0 = Y_AXIS;
            plane->axis_1 = Z_AXIS;
            plane->axis_linear = X_AXIS;
    }

    return plane;
}

FLASHMEM axes_signals_t gc_claim_axis_words (parser_block_t *gc_block, axes_signals_t validate)
{
    PROGMEM static const parameter_words_t wordmap[] = {
        { .x = On },
        { .y = On },
        { .z = On },
#ifdef A_AXIS
        { .a = On },
#endif
#ifdef B_AXIS
        { .b = On },
#endif
#ifdef C_AXIS
        { .c = On },
#endif
#ifdef U_AXIS
        { .u = On },
#endif
#ifdef V_AXIS
        { .v = On },
#endif
#ifdef W_AXIS
        { .w = On }
#endif
    };

    uint_fast8_t idx = N_AXIS;
    axes_signals_t axes = {};

    do {
        idx--;
        if(gc_block->words.mask & wordmap[idx].mask) {
            if(validate.mask == 0 || (bit_true(validate.mask, bit(idx)) && !isnan(gc_block->values.xyz[idx]))) {
                bit_true(axes.mask, bit(idx));
                gc_block->words.mask &= ~wordmap[idx].mask;
            }
        }
    } while(idx);

    return axes;
}

#if ENABLE_ACCELERATION_PROFILES

//Acceleration Profiles for G187 P[x] in percent of maximum machine acceleration.
float gc_get_accel_factor (uint8_t profile)
{
    PROGMEM static const float lookup[] = {
        1.0f,   // 100% - Roughing - Max Acceleration Default
        0.8f,   // 80% - Semi Roughing
        0.6f,   // 60% - Semi Finish
        0.4f,   // 40% - Finish
        0.2f    // 20% - Slow AF Mode
    };

    return lookup[profile >= (sizeof(lookup) / sizeof(float)) ? 0 : profile];
}

#endif // ENABLE_ACCELERATION_PROFILES

#if NGC_PARAMETERS_ENABLE

FLASHMEM static parameter_words_t macro_arguments_push (gc_values_t *values, parameter_words_t words, gc_values_t *target)
{
    // NOTE: this array has to match the parameter_words_t order!
    PROGMEM static const uint8_t offset[] = {
       0, // $
#ifdef A_AXIS
       offsetof(gc_values_t, xyz[A_AXIS]),
#else
       offsetof(gc_values_t, a),
#endif
#ifdef B_AXIS
       offsetof(gc_values_t, xyz[B_AXIS]),
#else
       offsetof(gc_values_t, b),
#endif
#ifdef C_AXIS
       offsetof(gc_values_t, xyz[C_AXIS]),
#else
       offsetof(gc_values_t, c),
#endif
       offsetof(gc_values_t, ijk[I_VALUE]),
       offsetof(gc_values_t, ijk[J_VALUE]),
       offsetof(gc_values_t, ijk[K_VALUE]),
       offsetof(gc_values_t, d),
       offsetof(gc_values_t, e),
       offsetof(gc_values_t, f),
       0, // G
       offsetof(gc_values_t, h),
       0, // L
       offsetof(gc_values_t, m),
       0, // N
       0, // O
       0, // P
       offsetof(gc_values_t, q),
       offsetof(gc_values_t, r),
       offsetof(gc_values_t, s),
       offsetof(gc_values_t, t),
#ifdef U_AXIS
       offsetof(gc_values_t, xyz[U_AXIS]),
#else
       offsetof(gc_values_t, u),
#endif
#ifdef V_AXIS
       offsetof(gc_values_t, xyz[V_AXIS]),
#else
       offsetof(gc_values_t, v),
#endif
#ifdef W_AXIS
       offsetof(gc_values_t, xyz[W_AXIS]),
#else
       offsetof(gc_values_t, w),
#endif
       offsetof(gc_values_t, xyz[X_AXIS]),
       offsetof(gc_values_t, xyz[Y_AXIS]),
       offsetof(gc_values_t, xyz[Z_AXIS])
    };

    uint_fast8_t idx = 1;
    parameter_words_t g65_words = {0};

    words.mask >>= 1; // Skip $-word value

    while(words.mask) {
        if((words.mask & 1) && offset[idx]) {
            g65_words.value |= (1 << idx);
            if(target)
                *(float *)((uint8_t *)target + offset[idx]) = *(float *)((uint8_t *)values + offset[idx]);
            else
                ngc_param_set((ngc_param_id_t)idx, *(float *)((uint8_t *)values + offset[idx]));
        }
        idx++;
        words.mask >>= 1;
    }

    return g65_words;
}

FLASHMEM static modal_restore_actions_t *get_state_restore_commands (gc_modal_t *modal, gc_modal_snapshot_t *snapshot)
{
    static modal_restore_actions_t actions = {0};

    if(snapshot) {

        bool cp_state;
        int_fast8_t idx;

        // M3,M4,M4 and S
        for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
            cp_state = memcmp(&snapshot->modal.spindle[idx], &modal->spindle[idx], offsetof(spindle_t, hal)) != 0;
            actions.update_spindle[idx] = gc_state.modal.spindle[idx].hal &&
                                           (bit_istrue(modal->override_ctrl.spindle_rpm_disable, bit(idx)) != bit_istrue(snapshot->modal.override_ctrl.spindle_rpm_disable, bit(idx)) ||
                                             modal->spindle[idx].hal->param->override_pct != snapshot->override.spindle_rpm[idx] ||
                                              cp_state != 0);
            if(cp_state != 0)
                memcpy(&modal->spindle[idx], &snapshot->modal.spindle[idx], offsetof(spindle_t, hal));
        }

        // G17,G18,G19
        if((actions.command.G2 = modal->plane_select != snapshot->modal.plane_select))
            modal->plane_select = snapshot->modal.plane_select;

        // G90,G91
        if((actions.command.G3 = modal->distance_incremental != snapshot->modal.distance_incremental))
            modal->distance_incremental = snapshot->modal.distance_incremental;

        // G4 not supported: G90.1,G91.1

        // G93,G94,G95
        if((actions.command.G5 = modal->feed_mode != snapshot->modal.feed_mode))
            modal->feed_mode = snapshot->modal.feed_mode;

        // G20,G21
        if((actions.command.G6 = modal->units_imperial != snapshot->modal.units_imperial))
            modal->units_imperial = snapshot->modal.units_imperial;

        // G7 not supported: G40,G41,G41.1,G42,G42.1

#if CUTTER_COMP_ENABLE
        // G7: G40,G41,G41.1,G42,G42.1
        if((actions.command.G7 = modal->cutter_comp.side != snapshot->modal.cutter_comp.side ||
                                 modal->cutter_comp.radius != snapshot->modal.cutter_comp.radius)) // ?? use memcmp
            memcpy(&modal->cutter_comp, &snapshot->modal.cutter_comp, sizeof(gc_ccomp_t));
#else
        // G7 not supported: G40,G41,G41.1,G42,G42.1
#endif

        // G43,G43.1,G49
        if((actions.command.G8 = modal->tool_offset_mode != snapshot->modal.tool_offset_mode))
            modal->tool_offset_mode = snapshot->modal.tool_offset_mode;

        if(memcmp(&snapshot->modal.tool_length_offset, &modal->tool_length_offset, sizeof(modal->tool_length_offset)) != 0) {
            actions.command.G8 = On;
            memcpy(&modal->tool_length_offset, &snapshot->modal.tool_length_offset, sizeof(modal->tool_length_offset));
        }

        // G98,G99
        if((actions.command.G10 = modal->retract_mode != snapshot->modal.retract_mode))
            modal->retract_mode = snapshot->modal.retract_mode;

        // G54-G59.x
        if((actions.command.G12 = modal->g5x_offset.id != snapshot->modal.g5x_offset.id))
            settings_read_coord_data((modal->g5x_offset.id = snapshot->modal.g5x_offset.id), &modal->g5x_offset.data);

#if ENABLE_PATH_BLENDING
        // G61,G61.1,G64
        if((actions.command.G13 = modal->control != snapshot->modal.control))
            modal->control = snapshot->modal.control;
#endif

        // G96,G97
        if((actions.command.G14 = modal->spindle->rpm_mode != snapshot->modal.spindle->rpm_mode)) // !!
            modal->spindle->rpm_mode = snapshot->modal.spindle->rpm_mode;

        // G7,G8
        if((actions.command.G15 = modal->diameter_mode != snapshot->modal.diameter_mode))
            modal->diameter_mode = snapshot->modal.diameter_mode;

        // M7,M8,M9
        if((actions.command.M8 = modal->coolant.value != snapshot->modal.coolant.value))
            modal->coolant.value = snapshot->modal.coolant.value;

        // M50-M53
        if((actions.command.M9 = modal->override_ctrl.value != snapshot->modal.override_ctrl.value))
            modal->override_ctrl.value = sys.override.control.value = snapshot->modal.override_ctrl.value;

        if(grbl.on_modal_state_action)
            grbl.on_modal_state_action(ModalState_Restore, actions.command, snapshot);
    }

    return &actions;
}

FLASHMEM bool gc_modal_state_restore (gc_modal_snapshot_t *snapshot)
{
    bool ok = false;

    if((ok = !!snapshot && !ABORTED)) {

        uint_fast8_t idx;
        modal_restore_actions_t *actions = get_state_restore_commands(&gc_state.modal, snapshot);

        gc_state.feed_rate = snapshot->modal.feed_rate;
        sys.override.control = snapshot->modal.override_ctrl;

        if(actions->command.M9)
            plan_feed_override(snapshot->override.feed_rate, snapshot->override.rapid_rate);

        if(actions->command.M8)
            coolant_restore(gc_state.modal.coolant, settings.coolant.on_delay);

#if CUTTER_COMP_ENABLE
        if(actions->command.G7) { // ?? add state validation++
            plane_t plane;
            gc_get_plane_data(&plane, gc_state.modal.plane_select);
            cc_enable(&gc_state.modal.cutter_comp, plane, (coord_data_t *)gc_state.position);
        }
#endif

        if(actions->command.G12) {
            report_add_realtime(Report_GWCO);
            system_flag_wco_change();
        }

        if(actions->command.G15)
            report_add_realtime(Report_LatheXMode);

        for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
            if(actions->update_spindle[idx]) {
                if(!spindle_override_disable(gc_state.modal.spindle[idx].hal, bit_istrue(gc_state.modal.override_ctrl.spindle_rpm_disable, bit(idx))))
                    spindle_set_override(gc_state.modal.spindle[idx].hal, snapshot->override.spindle_rpm[idx]);
                spindle_restore(gc_state.modal.spindle[idx].hal, gc_state.modal.spindle[idx].state, gc_state.modal.spindle[idx].rpm, settings.spindle.on_delay);
            }
        }
    }

    return ok;
}

#endif // NGC_PARAMETERS_ENABLE

FLASHMEM static m98_macro_t *macro_find (macro_id_t id)
{
    m98_macro_t *sub;

    if((sub = m98_macros)) do {
        if(sub->id == id)
            break;
    } while((sub = sub->next));

    return sub;
}

FLASHMEM size_t gc_macro_get_pos (macro_id_t id, vfs_file_t *file)
{
    m98_macro_t *sub = macro_find(id);

    return sub && sub->file == file ? sub->pos : 0;
}

/*
bool gc_macros_validate (void)
{
    bool ok = true;
    m98_macro_t *sub;

    if((sub = m98_macros)) do {
        ok = sub->pos != 0;
    } while(ok && (sub = sub->next));

    return ok;
}
*/

FLASHMEM static void macros_clear (void)
{
    m98_macro_t *next;

    if(m98_macros) do {
        next = m98_macros->next;
        free(m98_macros);
    } while((m98_macros = next));
}

FLASHMEM static status_code_t macro_add (macro_id_t id, line_number_t line_number, vfs_file_t *file)
{
    m98_macro_t *sub = macro_find(id);

    if(sub == NULL && (sub = calloc(1, sizeof(m98_macro_t)))) {
        sub->id = id;
        sub->file = file;
        sub->line_number = line_number;
        if(m98_macros) {
            m98_macro_t *add = m98_macros;
            while(add->next)
                add = add->next;
            add->next = sub;
        } else
            m98_macros = sub;
    }

    return sub ? Status_OK : Status_FlowControlOutOfMemory;
}

FLASHMEM static status_code_t macro_call (macro_id_t macro, line_number_t line_number, parameter_words_t args, uint8_t repeats)
{
#if NGC_PARAMETERS_ENABLE
    ngc_named_param_set("_value", 0.0f);
    ngc_named_param_set("_value_returned", 0.0f);
#endif

    status_code_t status = grbl.on_macro_execute(macro, line_number, args, repeats);

#if NGC_PARAMETERS_ENABLE
    if(status != Status_Handled)
        ngc_call_pop();
#endif
    return status == Status_Unhandled ? Status_GcodeValueOutOfRange : (status == Status_Handled ? Status_OK : status);
}

static status_code_t gc_at_exit (status_code_t status)
{
    if(!(status == Status_OK || status == Status_Handled)) {

        pending_tool = NULL;
        gc_state.g43_pending = (tool_id_t)-1;

        // Clear any pending output commands
        gc_clear_output_commands(output_commands);

        // Clear any registered M98 macros
        macros_clear();

#if NGC_PARAMETERS_ENABLE

        // Clear the G66 arguments stack
        while(gc_state.g66_args) {
            g66_arguments_t *args = gc_state.g66_args->prev;
            free(gc_state.g66_args);
            gc_state.g66_args = args;
        }

#endif
    }

    return status;
}

static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed)
{
    if(changed.spindle || changed.restore_defaults)
        gc_spindle_off();

    on_settings_changed(settings, changed);
}

FLASHMEM void gc_init (bool stop)
{
#if COMPATIBILITY_LEVEL > 1
        memset(&gc_state, 0, sizeof(parser_state_t));
        gc_state.tool = grbl.tool_table.get_tool(0)->data;
        if(grbl.tool_table.n_tools == 0)
            memset(gc_state.tool, 0, sizeof(tool_data_t));
#else
    if(sys.cold_start) {
        memset(&gc_state, 0, sizeof(parser_state_t));
        if(settings.flags.tool_persistent)
            gc_state.tool = grbl.tool_table.get_tool(settings.tool_id)->data;
        if(gc_state.tool == NULL)
            gc_state.tool = grbl.tool_table.get_tool(0)->data;
        if(grbl.tool_table.n_tools == 0) {
            memset(gc_state.tool, 0, sizeof(tool_data_t));
            if(settings.flags.tool_persistent)
                gc_state.tool->tool_id = settings.tool_id;
        }
#if CUTTER_COMP_ENABLE
        grbl.mc_line = mc_line;
        grbl.mc_arc = mc_arc;
#endif
    } else {

        coord_data_t tlo;
        coord_system_id_t coord_system_id = gc_state.modal.g5x_offset.id;
        tool_offset_mode_t tool_offset_mode = gc_state.modal.tool_offset_mode;

        memcpy(&tlo, gc_state.modal.tool_length_offset, sizeof(coord_data_t));
        memset(&gc_state, 0, offsetof(parser_state_t, g92_offset));
        memcpy(gc_state.modal.tool_length_offset, &tlo, sizeof(coord_data_t));
        gc_state.tool_pending = gc_state.tool->tool_id;

        if(hal.tool.select)
            hal.tool.select(gc_state.tool, false);

        if(stop) {
            // Restore offsets, tool offset mode
            gc_state.modal.g5x_offset.id = coord_system_id;
            gc_state.modal.tool_offset_mode = tool_offset_mode;
        }
    }
#endif

    if(on_settings_changed == NULL) {
        on_settings_changed = grbl.on_settings_changed;
        grbl.on_settings_changed = onSettingsChanged;
    }

    // Clear any pending output commands etc...
    gc_at_exit(Status_UserException);

    gc_state.modal.override_ctrl = sys.override.control;    // Load default override status
    gc_state.spindle = &gc_state.modal.spindle[0];
    gc_state.modal.spindle[0].hal = spindle_get(0);

//    if(settings.flags.lathe_mode)
//        gc_state.modal.plane_select = PlaneSelect_ZX;

    set_scaling(1.0f);

#if CUTTER_COMP_ENABLE

    plane_t plane;
    gc_get_plane_data(&plane, gc_state.modal.plane_select);

    cc_init();
    cc_enable(&gc_state.modal.cutter_comp, plane, (coord_data_t *)gc_state.position);

#endif

    // Load default G54 coordinate system.
    if (!settings_read_coord_data(gc_state.modal.g5x_offset.id, &gc_state.modal.g5x_offset.data))
        grbl.report.status_message(Status_SettingReadFail);

    if(sys.cold_start && !settings.flags.g92_is_volatile) {
        if(!settings_read_coord_data(CoordinateSystem_G92, &gc_state.g92_offset))
            grbl.report.status_message(Status_SettingReadFail);
        else
            memcpy(&gc_state.offset_queue[gc_state.offset_id], gc_state.g92_offset.coord.values, sizeof(coord_data_t));
    }

    if(grbl.on_wco_changed && (!sys.cold_start ||
                                !is0_position_vector(gc_state.modal.g5x_offset.data.coord.values) ||
                                 !is0_position_vector(gc_state.g92_offset.coord.values)))
        grbl.on_wco_changed();

#if NGC_EXPRESSIONS_ENABLE
    ngc_flowctrl_init();
#endif
#if NGC_PARAMETERS_ENABLE
    ngc_params_init();
#endif
#if ENABLE_ACCELERATION_PROFILES
    gc_state.modal.acceleration_factor = gc_get_accel_factor(0); // Initialize machine with default
#endif


    if(grbl.on_parser_init)
        grbl.on_parser_init(&gc_state);
}

inline static bool is_single_spindle_block (parser_block_t *gc_block, modal_groups_t command_words)
{
    return gc_block->words.s ||
            (command_words.G1 && (gc_block->modal.motion == MotionMode_SpindleSynchronized ||
                                   gc_block->modal.motion == MotionMode_RigidTapping ||
                                    gc_block->modal.motion == MotionMode_Threading)) ||
             (command_words.G5 && gc_block->modal.feed_mode == FeedMode_UnitsPerRev) ||
               command_words.G14 ||
               (command_words.M9 && gc_block->override_command == Override_SpindleSpeed);
}

// Set dynamic laser power mode to PPI (Pulses Per Inch)
// Returns true if driver uses hardware implementation.
// Driver support for pulsing the laser on signal is required for this to work.
FLASHMEM bool gc_laser_ppi_enable (uint_fast16_t ppi, uint_fast16_t pulse_length)
{
    gc_state.is_laser_ppi_mode = ppi > 0 && pulse_length > 0;

    return grbl.on_laser_ppi_enable && grbl.on_laser_ppi_enable(ppi, pulse_length);
}

FLASHMEM spindle_t *gc_spindle_get (spindle_num_t spindle)
{
#if N_SYS_SPINDLE > 1
    return spindle < 0 ? gc_state.spindle : &gc_state.modal.spindle[spindle];
#else
    return &gc_state.modal.spindle[0];
#endif
}

FLASHMEM void gc_spindle_off (void)
{
    uint_fast8_t idx;
    for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
        memset(&gc_state.modal.spindle[idx], 0, offsetof(spindle_t, hal));
    }

    spindle_all_off(false);
    report_add_realtime(Report_Spindle);
}


FLASHMEM void gc_coolant (coolant_state_t state)
{
    gc_state.modal.coolant = state;
    hal.coolant.set_state(gc_state.modal.coolant);
    report_add_realtime(Report_Coolant);
}

static void add_offset (const coord_data_t *offset)
{
    gc_state.offset_id = (gc_state.offset_id + 1) & (MAX_OFFSET_ENTRIES - 1);
    memcpy(&gc_state.offset_queue[gc_state.offset_id], offset, sizeof(coord_data_t));
    system_flag_wco_change();
}

FLASHMEM static tool_data_t *tool_get_pending (tool_id_t tool_id, char **message)
{
    static tool_data_t tool_data = {0};

    if(grbl.tool_table.n_tools) {

        tool_table_entry_t *tool = grbl.tool_table.get_tool(tool_id);

        if(message && tool->name) {
            if((*message = malloc(strlen(tool->name) + 1)))
                strcpy(*message, tool->name);
        }

        return tool->data;
    }

    memcpy(&tool_data, gc_state.tool, sizeof(tool_data_t));
    tool_data.tool_id = tool_id;

    return &tool_data;
}

static inline void tool_set (tool_data_t *tool)
{
    if(grbl.tool_table.n_tools)
        gc_state.tool = tool;
    else
        gc_state.tool->tool_id = tool->tool_id;

    pending_tool = NULL;
}

FLASHMEM static void tool_offset_apply (tool_offset_mode_t mode, tool_id_t tool, float *values, axes_signals_t axis_words)
{
    uint8_t idx = N_AXIS;
    bool tlo_changed = false;
    tool_data_t *tool_data = NULL;

    gc_state.g43_pending = (tool_id_t)-1;
    gc_state.modal.tool_offset_mode = mode;

    if(gc_state.modal.tool_offset_mode == ToolLengthOffset_Enable || gc_state.modal.tool_offset_mode == ToolLengthOffset_ApplyAdditional)
        tool_data = grbl.tool_table.get_tool(tool)->data;

    do {
        idx--;
        switch(gc_state.modal.tool_offset_mode) {

            case ToolLengthOffset_Cancel: // G49
                tlo_changed |= gc_state.modal.tool_length_offset[idx] != 0.0f;
                gc_state.modal.tool_length_offset[idx] = 0.0f;
                break;

            case ToolLengthOffset_Enable: // G43
                if(gc_state.modal.tool_length_offset[idx] != tool_data->offset.values[idx]) {
                    tlo_changed = true;
                    gc_state.modal.tool_length_offset[idx] = tool_data->offset.values[idx];
                }
                break;

            case ToolLengthOffset_ApplyAdditional: // G43.2
                tlo_changed |= tool_data->offset.values[idx] != 0.0f;
                gc_state.modal.tool_length_offset[idx] += tool_data->offset.values[idx];
                break;

            case ToolLengthOffset_EnableDynamic: // G43.1
                if(bit_istrue(axis_words.mask, bit(idx)) && gc_state.modal.tool_length_offset[idx] != values[idx]) {
                    tlo_changed = true;
                    gc_state.modal.tool_length_offset[idx] = values[idx];
                }
                break;
        }
    } while(idx);

    if(tlo_changed) {
        report_add_realtime(Report_ToolOffset);
        system_flag_wco_change();
    }
}

// For core use only, called internally and from state machine
FLASHMEM void gc_tool_changed (void)
{
    if(pending_tool)
        tool_set(pending_tool);

    if(gc_state.g43_pending > 0)
        tool_offset_apply(ToolLengthOffset_Enable, gc_state.g43_pending, NULL, (axes_signals_t){0});

    grbl.on_tool_changed(gc_state.tool);

    report_add_realtime(Report_Tool);
}

// Add output command to linked list
FLASHMEM static bool add_output_command (output_command_t *command)
{
    output_command_t *add_cmd;

    if((add_cmd = malloc(sizeof(output_command_t)))) {

        memcpy(add_cmd, command, sizeof(output_command_t));

        if(output_commands == NULL)
            output_commands = add_cmd;
        else {
            output_command_t *cmd = output_commands;
            while(cmd->next)
                cmd = cmd->next;
            cmd->next = add_cmd;
        }
    }

    return add_cmd != NULL;
}

// Free linked list of output commands
FLASHMEM void gc_clear_output_commands (output_command_t *cmd)
{
    while(cmd) {
        output_command_t *next = cmd->next;
        free(cmd);
        cmd = next;
    }
}

static gc_thread_data thread;

FLASHMEM static status_code_t init_sync_motion (plan_line_data_t *pl_data, float pitch)
{
    if(pl_data->spindle.hal->get_data == NULL)
        RETURN(Status_GcodeUnsupportedCommand); // [Spindle not sync capable]

    pl_data->condition.inverse_time = Off;
    pl_data->feed_rate = gc_state.distance_per_rev = pitch;
    pl_data->spindle.css = NULL;                    // Switch off CSS.
    pl_data->overrides = sys.override.control;      // Use current override flags and
    pl_data->overrides.sync = On;                   // set to sync overrides on execution of motion.

    // Disable feed rate and spindle overrides for the duration of the cycle.
    pl_data->overrides.spindle_rpm_disable = sys.override.control.spindle_rpm_disable = On;
    pl_data->overrides.feed_rates_disable = sys.override.control.feed_rates_disable = On;
    pl_data->spindle.hal->param->override_pct = DEFAULT_SPINDLE_RPM_OVERRIDE;
    // TODO: need for gc_state.distance_per_rev to be reset on modal change?
    float feed_rate = pl_data->feed_rate * pl_data->spindle.hal->get_data(SpindleData_RPM)->rpm;

    if(feed_rate == 0.0f)
        RETURN(Status_GcodeSpindleNotRunning); // [Spindle not running]

    if(feed_rate > settings.axis[Z_AXIS].max_rate * 0.9f)
        RETURN(Status_GcodeMaxFeedRateExceeded); // [Feed rate too high]

    return Status_OK;
}

// Output and free previously allocated message
FLASHMEM void gc_output_message (char *message)
{
    if(message) {

        if(grbl.on_gcode_message)
            grbl.on_gcode_message(message);

        if(*message)
            report_message(message, Message_Plain);

        free(message);
    }
}

// Remove whitespace, control characters, comments and if block delete is active block delete lines
// else the block delete character. Remaining characters are converted to upper case.
// If the driver handles message comments then the first is extracted and returned in a dynamically
// allocated memory block, the caller must free this after the message has been processed.

char *gc_normalize_block (char *block, status_code_t *status, char **message)
{
    char c, *s1, *s2, *comment = NULL;

    // Remove leading whitespace & control characters
    while(*block && *block <= ' ')
        block++;

    if(*block == ';' || (*block == '/' && sys.flags.block_delete_enabled)) {
        *block = '\0';
        return block;
    }

    if(*block == '/')
        block++;

    s1 = s2 = block;

    while((c = *s1) != '\0') {

        if(c > ' ') switch(c) {

            case ';':
                if(!comment) {
                    *s1 = '\0';
                    continue;
                }
                break;

            case '(':
                // TODO: generate error if a left parenthesis is found inside a comment...
                comment = s1 + 1;
                break;

            case ')':
                if(comment && !(gc_state.skip_blocks || state_get() == STATE_CHECK_MODE)) {

                    *s1 = '\0';
                    if(!hal.driver_cap.no_gcode_message_handling) {

                        if(message && *message == NULL) {

                            if(grbl.on_process_gcode_comment)
                                *message = grbl.on_process_gcode_comment(comment);

                            if(*message == NULL) {

                                size_t len = s1 - comment - 3;
                                if(!strncasecmp(comment, "MSG,", 4) && (*message = malloc(len))) {

                                    comment += 4;
                                    while(*comment == ' ') {
                                        comment++;
                                        len--;
                                    }
                                    memcpy(*message, comment, len);
                                }
                            }
                        }
                    }

                    if(*comment && (message == NULL || *message == NULL) && grbl.on_gcode_comment)
                        *status = grbl.on_gcode_comment(comment);
                }
                comment = NULL;
                break;

            default:
                if(comment == NULL)
                    *s2++ = CAPS(c);
                break;
        }
        s1++;
    }

    *s2 = '\0';

    return block;
}

// Parses and executes one block (line) of 0-terminated G-Code.
// In this function, all units and positions are converted and exported to internal functions
// in terms of (mm, mm/min) and absolute machine coordinates, respectively.

status_code_t gc_execute_block (char *block)
{
    PROGMEM static const parameter_words_t axis_words_mask = {
        .x = On,
        .y = On,
        .z = On,
#if IS_AXIS_LETTER('A')
        .a = On,
#endif
#if IS_AXIS_LETTER('B')
        .b = On,
#endif
#if IS_AXIS_LETTER('C')
        .c = On,
#endif
#if IS_AXIS_LETTER('U')
        .u = On,
#endif
#if IS_AXIS_LETTER('V')
        .v = On,
#endif
#if IS_AXIS_LETTER('W')
        .w = On
#endif
    };

    PROGMEM static const parameter_words_t pq_words = {
        .p = On,
        .q = On
    };

    PROGMEM static const parameter_words_t ij_words = {
        .i = On,
        .j = On
    };

    PROGMEM static const parameter_words_t uint_words = {
        .l = On,
        .n = On,
        .o = On,
    };

    PROGMEM static const modal_groups_t jog_groups = {
        .G0 = On,
        .G3 = On,
        .G6 = On
    };

    PROGMEM static const modal_groups_t m_groups = {
        .M4 = On,
        .M5 = On,
        .M6 = On,
        .M7 = On,
        .M8 = On,
        .M9 = On,
        .M10 = On
    };

    DCRAM static parser_block_t gc_block;

#if NGC_EXPRESSIONS_ENABLE

    PROGMEM static const parameter_words_t o_label = {
        .o = On
    };

    static ngc_param_t ngc_params[NGC_N_ASSIGN_PARAMETERS_PER_BLOCK];

    uint_fast8_t ngc_param_count = 0;

#endif // NGC_EXPRESSIONS_ENABLE

#ifdef ROTATION_ENABLE

    PROGMEM static const axes_signals_t rotate_axes[] = {
        { .x = On, .y = On },
        { .x = On, .z = On },
        { .y = On, .z = On }
    };

#endif

    char *message = NULL;
    status_code_t status = Status_OK;
    struct {
        float f;
        uint32_t o;
        float s;
        float t;
    } single_meaning_value = {0};

    bool fs_changed;
    if((fs_changed = gc_state.file_stream ? hal.stream.file == NULL : hal.stream.file != NULL))
        gc_state.file_stream = hal.stream.file != NULL;

    block = gc_normalize_block(block, &status, &message);
    if(status != Status_OK)
        RETURN(status);

    // Determine if the line is a program start/end marker.
    if(*block == CMD_PROGRAM_DEMARCATION && block[1] == '\0') {

        *block = '\0';

        if(gc_state.file_stream) {

            if(!fs_changed && !gc_state.file_run)
                status = Status_UnexpectedDemarcation;
            else if(!(gc_state.file_run = fs_changed)) {
                protocol_buffer_synchronize(); // Empty planner buffer
                grbl.report.feedback_message(Message_ProgramEnd);
                if(grbl.on_program_completed) {
                    bool check_mode = state_get() == STATE_CHECK_MODE;
                    grbl.on_program_completed(ProgramFlow_EndPercent, check_mode);
                    gc_state.file_stream = !check_mode;
                }
            }
        } else
            gc_state.file_run = !gc_state.file_run;

        if(status == Status_OK && grbl.on_file_demarcate)
            grbl.on_file_demarcate(gc_state.file_run);
    }

  /* -------------------------------------------------------------------------------------
     STEP 1: Initialize parser block struct and copy current g-code state modes. The parser
     updates these modes and commands as the block line is parsed and will only be used and
     executed after successful error-checking. The parser block struct also contains a block
     values struct, word tracking variables, and a non-modal commands tracker for the new
     block. This struct contains all of the necessary information to execute the block. */

    memset(&gc_block, 0, sizeof(gc_block));                                             // Initialize the parser block struct.
    memcpy(&gc_block.modal, &gc_state.modal, offsetof(gc_modal_t, tool_length_offset)); // Copy current modes

    int32_t spindle_id = 0;
    bool spindle_event = false;
    axis_command_t axis_command = AxisCommand_None;
    io_mcode_t port_command = (io_mcode_t)0;
    spindle_t *sspindle = gc_state.spindle;
    plane_t plane;
    coord_system_t coord_system;

    // Initialize bitflag tracking variables for axis indices compatible operations.
    axes_signals_t axis_words = {0};    // XYZ tracking
    ijk_words_t ijk_words = {0};        // IJK tracking

    // Initialize command and value words and parser flags variables.
    modal_groups_t command_words = {0};         // Bitfield for tracking G and M command words. Also used for modal group violations.
    gc_parser_flags_t gc_parser_flags = {0};    // Parser flags for handling special cases.
    parameter_words_t user_words = {0};         // User M-code words "taken"
#if NGC_EXPRESSIONS_ENABLE
    bool skip_blocks = gc_state.skip_blocks && *block != 'N';   // Parse line number if present (first word) in blocks to be skipped.
#endif

    if(*block != 'N' && hal.stream.file && grbl.on_line_number_assigned)
        gc_block.values.n = (int32_t)grbl.on_line_number_assigned((line_number_t)0);

    if(*block == '\0') {

        if(message)
            gc_output_message(message);

        return Status_OK;
    }

    // Determine if the line is a jogging motion or a normal g-code block.
    if(*block == '$') { // NOTE: `$J=` already parsed when passed to this function.
        // Set G1 and G94 enforced modes to ensure accurate error checks.
        gc_parser_flags.jog_motion = On;
        gc_block.modal.motion = MotionMode_Linear;
        gc_block.modal.feed_mode = FeedMode_UnitsPerMin;
        gc_block.spindle_modal.rpm_mode = SpindleSpeedMode_RPM;
        gc_block.values.n = JOG_LINE_NUMBER; // Initialize default line number reported during jog.
    }

  /* -------------------------------------------------------------------------------------
     STEP 2: Import all g-code words in the block. A g-code word is a letter followed by
     a number, which can either be a 'G'/'M' command or sets/assigns a command value. Also,
     perform initial error-checks for command word modal group violations, for any repeated
     words, and for negative values set for the value words F, N, P, T, and S. */

    uint_fast8_t char_counter = gc_parser_flags.jog_motion ? 3 /* Start parsing after `$J=` */ : 0;
    char letter;
    float value;
    uint32_t int_value = 0;
    uint_fast16_t mantissa = 0;
    user_mcode_type_t user_mcode = UserMCode_Unsupported;
    word_bit_t word_bit = { .parameter = {0}, .modal_group = {0} }; // Bit-value for assigning tracking variables

    while((letter = block[char_counter++]) != '\0') { // Loop until no more g-code words in block.

        // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out.

#if NGC_EXPRESSIONS_ENABLE

        status_code_t status;

        if(letter == '#') {

            if(skip_blocks)
                return Status_OK;

            if(block[char_counter] == '<') {

                char name[NGC_MAX_PARAM_LENGTH + 1];

                if((status = ngc_read_name(block, &char_counter, name)) == Status_OK) {
                    if(block[char_counter++] != '=')
                        status = Status_BadNumberFormat;    // [Expected equal sign]
                    else if((status = ngc_read_real_value(block, &char_counter, &value)) == Status_OK) {
                        if(!ngc_named_param_set(name, value))
                            status = Status_BadNumberFormat;    // [Out of memory or attempt to write RO parameter]
                    } // else: [Expected value]
                } // else: [Expected parameter name]
            } else {

                float param;

                if((status = ngc_read_real_value(block, &char_counter, &param)) == Status_OK) {
                    if(!ngc_param_is_rw((ngc_param_id_t)param))
                        status = Status_GcodeValueOutOfRange;   // [Parameter does not exist or is read only]
                    else if(block[char_counter++] != '=')
                        status = Status_BadNumberFormat;   // [Expected equal sign]
                    else if((status = ngc_read_real_value(block, &char_counter, &value)) == Status_OK) {
                        if(ngc_param_count < NGC_N_ASSIGN_PARAMETERS_PER_BLOCK) {
                            ngc_params[ngc_param_count].id = (ngc_param_id_t)param;
                            ngc_params[ngc_param_count++].value = value;
                        } else
                            RETURN(Status_BadNumberFormat);   // [Too many parameters in block]
                    } // else: [Expected parameter value]
                } // else: [Expected parameter number]
            }

            if(status != Status_OK)
                RETURN(status);

            continue;

        } else if(letter == 'O') {

            gc_block.words.n = Off; // Hack to allow line number with O word

            if(block[char_counter] == '[') {
                int32_t value;
                if((status = ngc_read_integer_value(block, &char_counter, &value)) == Status_OK) {
                    gc_block.words.o = On;
                    gc_block.values.o = (uint32_t)value;
                    char_counter++;
                } else
                    RETURN(status);
            } else if(block[char_counter] == '<') {

                char o_slabel[NGC_MAX_PARAM_LENGTH + 1];
                if((status = ngc_read_name(block, &char_counter, o_slabel)) != Status_OK)
                    RETURN(status);
                gc_block.words.o = On;
                if((gc_block.values.o = ngc_string_param_set_name(o_slabel)) == 0)
                    RETURN(Status_FlowControlOutOfMemory);
                continue;
            }
        }

        if((gc_block.words.mask & o_label.mask) && (gc_block.words.mask & ~o_label.mask) == 0) {

            if(grbl.on_line_number_assigned)
                gc_block.values.n = (int32_t)grbl.on_line_number_assigned((line_number_t)gc_block.values.n);

            char_counter--;
            RETURN(ngc_flowctrl(gc_block.values.o, (line_number_t)gc_block.values.n, block, &char_counter, &gc_state.skip_blocks));
        }

        if((letter < 'A' && letter != '$') || letter > 'Z')
            RETURN(Status_ExpectedCommandLetter); // [Expected word letter]

        if(user_mcode == UserMCode_NoValueWords && no_word_value(block[char_counter]))
            value = NAN;
        else if((status = ngc_read_real_value(block, &char_counter, &value)) != Status_OK)
            RETURN(status);

        if(skip_blocks && letter != 'O')
            return Status_OK;

        if(user_mcode != UserMCode_NoValueWords && isnan(value))
            RETURN(Status_BadNumberFormat);   // [Expected word value]

#else

        if((letter < 'A' && letter != '$') || letter > 'Z')
            RETURN(Status_ExpectedCommandLetter); // [Expected word letter]

        if(letter == 'O') {
            value = NAN;
            if((status = read_uint(block, &char_counter, &int_value)) != Status_OK)
                RETURN(status);
        } else if(!read_float(block, &char_counter, &value)) {
            if(user_mcode == UserMCode_NoValueWords)    // Valueless parameters allowed for user defined M-codes.
                value = NAN;                            // Parameter validation deferred to implementation.
            else
                RETURN(Status_BadNumberFormat);         // [Expected word value]
        }

#endif

        // Convert values to smaller uint8 significand and mantissa values for parsing this word.
        // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more
        // accurate than the NIST gcode requirement of x10 when used for commands, but not quite
        // accurate enough for value words that require integers to within 0.0001. This should be
        // a good enough compromise and catch most all non-integer errors. To make it compliant,
        // we would simply need to change the mantissa to int16, but this add compiled flash space.
        // Maybe update this later.
        if(!isnan(value)) {
            int_value = (uint32_t)truncf(value);
            mantissa = (uint_fast16_t)roundf(100.0f * (value - int_value));
        }
        // NOTE: Rounding must be used to catch small floating point errors.

#if NGC_PARAMETERS_ENABLE
        if(gc_state.g66_args && gc_state.g66_args->call == MacroCall_Modal1 && gc_state.g66_args->call_level == ngc_call_level()) {
            command_words.G16 = On;
            gc_block.macro_call = gc_state.g66_args->call;
        }
#endif

        // Check if the g-code word is supported or errors due to modal group violations or has
        // been repeated in the g-code block. If ok, update the command or record its value.
        switch(letter) {

          /* 'G' and 'M' Command Words: Parse commands and check for modal group violations.
             NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */

            case 'G': // Determine 'G' command and its modal group

                user_mcode = UserMCode_Unsupported;
                word_bit.modal_group.mask = 0;

                switch(int_value) {

                    case 7: case 8:
                        if(settings.mode == Mode_Lathe) {
                            word_bit.modal_group.G15 = On;
                            gc_block.modal.diameter_mode = int_value == 7; // TODO: find specs for implementation, only affects X calculation? reporting? current position?
                        } else
                            RETURN(Status_GcodeUnsupportedCommand); // [G7 & G8 not supported]
                        break;

                    case 10: case 28: case 30: case 92:
                        // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block.
                        // * G43.1 is also an axis command but is not explicitly defined this way.
                        if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1
                            if (axis_command)
                                RETURN(Status_GcodeAxisCommandConflict); // [Axis word/command conflict]
                            axis_command = AxisCommand_NonModal;
                        }
                        // No break. Continues to next line.

                    case 4: case 53:
                        word_bit.modal_group.G0 = On;
                        gc_block.non_modal_command = (non_modal_t)int_value;
                        if ((int_value == 28) || (int_value == 30)) {
                            if (!((mantissa == 0) || (mantissa == 10)))
                                RETURN(Status_GcodeUnsupportedCommand);
                            gc_block.non_modal_command += mantissa;
                            mantissa = 0; // Set to zero to indicate valid non-integer G command.
                        } else if (int_value == 92) {
                            if (!((mantissa == 0) || (mantissa == 10) || (mantissa == 20) || (mantissa == 30)))
                                RETURN(Status_GcodeUnsupportedCommand);
                            gc_block.non_modal_command += mantissa;
                            mantissa = 0; // Set to zero to indicate valid non-integer G command.
                        }
                        break;

                    case 33: case 76:
                        if(axis_command)
                            RETURN(Status_GcodeAxisCommandConflict); // [Axis word/command conflict]
                        axis_command = AxisCommand_MotionMode;
                        word_bit.modal_group.G1 = On;
                        gc_block.modal.motion = (motion_mode_t)int_value;
                        if(mantissa == 10) {
                            mantissa = 0;
                            gc_block.modal.motion = MotionMode_RigidTapping;
                        }
                        gc_block.modal.canned_cycle_active = false;
                        break;

                    case 38:
                        if(!(hal.probe.get_state && ((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50))))
                            RETURN(Status_GcodeUnsupportedCommand); // [probing not supported by driver or unsupported G38.x command]
                        int_value += (mantissa / 10) + 100;
                        mantissa = 0; // Set to zero to indicate valid non-integer G command.
                        // No break. Continues to next line.

                    case 0: case 1: case 2: case 3: case 5:

                        // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block.
                        // * G43.1 is also an axis command but is not explicitly defined this way.
                        if (axis_command)
                            RETURN(Status_GcodeAxisCommandConflict); // [Axis word/command conflict]
                        axis_command = AxisCommand_MotionMode;
                        // No break. Continues to next line.

                    case 80:
                        word_bit.modal_group.G1 = On;
                        if(int_value == 5 && mantissa != 0) {
                            if(mantissa == 10) {
                                gc_block.modal.motion = MotionMode_QuadraticSpline;
                                mantissa = 0; // Set to zero to indicate valid non-integer G command.
                            } else
                                RETURN(Status_GcodeUnsupportedCommand);
                        } else
                            gc_block.modal.motion = (motion_mode_t)int_value;
                        gc_block.modal.canned_cycle_active = false;
                        break;

#if LATHE_UVW_OPTION && NGC_EXPRESSIONS_ENABLE
                    case 70: case 71: case 72:
                        if(axis_command)
                            RETURN(Status_GcodeAxisCommandConflict); // [Axis word/command conflict]
                        axis_command = AxisCommand_MotionMode;
                        word_bit.modal_group.G1 = On;
                        gc_block.modal.motion = (motion_mode_t)int_value;
                        if(mantissa != 0) {
                            if(mantissa == 10 || mantissa == 20) {
                                gc_block.modal.motion = gc_block.modal.motion * 10 + mantissa / 10;
                                mantissa = 0; // Set to zero to indicate valid non-integer G command.
                            } else
                                RETURN(Status_GcodeUnsupportedCommand);
                        } else
                            gc_block.modal.canned_cycle_active = false;
                        break;
#endif

                    case 73: case 81: case 82: case 83: case 84: case 85: case 86: case 89:
                        if (axis_command)
                            RETURN(Status_GcodeAxisCommandConflict); // [Axis word/command conflict]
                        axis_command = AxisCommand_MotionMode;
                        word_bit.modal_group.G1 = On;
                        gc_block.modal.canned_cycle_active = true;
                        gc_block.modal.motion = (motion_mode_t)int_value;
                        gc_parser_flags.canned_cycle_change = gc_block.modal.motion != gc_state.modal.motion;
                        break;

                    case 17: case 18: case 19:
                        word_bit.modal_group.G2 = On;
                        gc_block.modal.plane_select = (plane_select_t)(int_value - 17);
                        break;

                    case 90: case 91:
                        if (mantissa == 0) {
                            word_bit.modal_group.G3 = On;
                            gc_block.modal.distance_incremental = int_value == 91;
                        } else {
                            word_bit.modal_group.G4 = On;
                            if ((mantissa != 10) || (int_value == 90))
                                RETURN(Status_GcodeUnsupportedCommand); // [G90.1 not supported]
                            mantissa = 0; // Set to zero to indicate valid non-integer G command.
                            // Otherwise, arc IJK incremental mode is default. G91.1 does nothing.
                        }
                        break;

                    case 93: case 94:
                        word_bit.modal_group.G5 = On;
                        gc_block.modal.feed_mode = (feed_mode_t)(94 - int_value);
                        break;

                    case 95:
                        word_bit.modal_group.G5 = On;
                        gc_block.modal.feed_mode = FeedMode_UnitsPerRev;
                        break;

                    case 20: case 21:
                        word_bit.modal_group.G6 = On;
                        gc_block.modal.units_imperial = int_value == 20;
                        break;

#if CUTTER_COMP_ENABLE
                    case 41: case 42:
                        if((gc_block.modal.cutter_comp.dynamic = mantissa == 10))
                            mantissa = 0; // Set to zero to indicate valid non-integer G command.
                        // No break. Continues to next line.
#endif
                    case 40:
                        // NOTE: When cutter comp is not supported G40 still is since it often appear in g-code program headers to setup defaults.
                        word_bit.modal_group.G7 = On;
#if CUTTER_COMP_ENABLE
                        gc_block.modal.cutter_comp.side = (ccomp_mode_t)(int_value - 40);
#endif
                        break;

                    case 43: case 49:
                        word_bit.modal_group.G8 = On;
                        // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed,
                        // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49
                        // all are explicit axis commands, regardless if they require axis words or not.
                        // NOTE: cannot find the NIST statement referenced above, changed to match LinuxCNC behaviour in build 20210513.
                        if(int_value == 49) // G49
                            gc_block.modal.tool_offset_mode = ToolLengthOffset_Cancel;
                        else if(mantissa == 0 && grbl.tool_table.n_tools) // G43
                            gc_block.modal.tool_offset_mode = ToolLengthOffset_Enable;
                        else if(mantissa == 20 && grbl.tool_table.n_tools) // G43.2
                            gc_block.modal.tool_offset_mode = ToolLengthOffset_ApplyAdditional;
                        else if(mantissa == 10) { // G43.1
                            if(axis_command)
                                RETURN(Status_GcodeAxisCommandConflict); // [Axis word/command conflict] }
                            axis_command = AxisCommand_ToolLengthOffset;
                            gc_block.modal.tool_offset_mode = ToolLengthOffset_EnableDynamic;
                        } else
                            RETURN(Status_GcodeUnsupportedCommand); // [Unsupported G43.x command]
                        mantissa = 0; // Set to zero to indicate valid non-integer G command.
                        break;

                    case 54: case 55: case 56: case 57: case 58: case 59:
                        word_bit.modal_group.G12 = On;
                        gc_block.modal.g5x_offset.id = (coord_system_id_t)(int_value - 54); // Shift to array indexing.
                        if(int_value == 59 && mantissa > 0) {
                            if(N_WorkCoordinateSystems == 9 && (mantissa == 10 || mantissa == 20 || mantissa == 30)) {
                                gc_block.modal.g5x_offset.id += mantissa / 10;
                                mantissa = 0;
                            } else
                                RETURN(Status_GcodeUnsupportedCommand); // [Unsupported G59.x command]
                        }
                        break;

#if ENABLE_PATH_BLENDING
                    case 61:
                        word_bit.modal_group.G13 = On;
                        if (mantissa != 0 || mantissa != 10)
                            RETURN(Status_GcodeUnsupportedCommand);
                        gc_block.modal.control = mantissa == 0 ? ControlMode_ExactPath : ControlMode_ExactStop;
                        break;

                    case 64:
                        word_bit.modal_group.G13 = On;
                        gc_block.modal.control = ControlMode_PathBlending; // G64
                        break;
#else
                    case 61:
                        word_bit.modal_group.G13 = On;
                        if (mantissa != 0) // [G61.1 not supported]
                            RETURN(Status_GcodeUnsupportedCommand);
                        break;
#endif

                    case 65:
#if NGC_PARAMETERS_ENABLE
                    case 66:
#endif
                        // NOTE: Fanuc style, not supported by LinuxCNC
                        if(command_words.mask & m_groups.mask) {
                            gc_block.words.m = On;
                            command_words.mask &= ~m_groups.mask;
                        }
                        if(grbl.on_macro_execute && mantissa <= (int_value == 66 ? 10 : 0)) {
                            word_bit.modal_group.G16 = On;
                            gc_block.macro_call = (macro_call_t)(int_value + mantissa * 10);
                            mantissa = 0;
                        } else
                            RETURN(Status_GcodeUnsupportedCommand);
                        break;

#if NGC_PARAMETERS_ENABLE
                    case 67:
                        // NOTE: Fanuc style, not supported by LinuxCNC
                        if(grbl.on_macro_execute) {
                            if(!(command_words.G16 && gc_state.g66_args))
                                word_bit.modal_group.G16 = On;
                            gc_block.macro_call = MacroCall_End;
                        } else
                            RETURN(Status_GcodeUnsupportedCommand);
                        break;
#endif

                    case 96: case 97:
                        if(settings.mode == Mode_Lathe) {
                            word_bit.modal_group.G14 = On;
                            gc_block.spindle_modal.rpm_mode = (spindle_rpm_mode_t)((int_value - 96) ^ 1);
                        } else
                            RETURN(Status_GcodeUnsupportedCommand);
                        break;

                    case 98: case 99:
                        word_bit.modal_group.G10 = On;
                        gc_block.modal.retract_mode = (cc_retract_mode_t)(int_value - 98);
                        break;

                    case 50: case 51:
                        axis_command = AxisCommand_Scaling;
                        word_bit.modal_group.G11 = On;
                        gc_block.modal.scaling_active = int_value == 51;
                        break;

#if ENABLE_ACCELERATION_PROFILES
                    case 187:
                        word_bit.modal_group.G0 = On;
                        gc_block.non_modal_command = (non_modal_t)int_value;
                        if(mantissa != 0)
                            RETURN(Status_GcodeUnsupportedCommand);
                        break;
#endif

                    default: RETURN(Status_GcodeUnsupportedCommand); // [Unsupported G command]
                } // end G-value switch

                if (mantissa > 0)
                    RETURN(Status_GcodeCommandValueNotInteger); // [Unsupported or invalid Gxx.x command]

                // Check for more than one command per modal group violations in the current block
                // NOTE: Variable 'word_bit' is always assigned, if the command is valid.
                if (command_words.mask & word_bit.modal_group.mask)
                    RETURN(Status_GcodeModalGroupViolation);

                command_words.mask |= word_bit.modal_group.mask;
                break;

            case 'M': // Determine 'M' command and its modal group

                gc_block.values.m = value;

                if(gc_block.macro_call == MacroCall_NonModal || gc_block.macro_call == MacroCall_Modal || gc_block.macro_call == MacroCall_Modal1) {

                    if(gc_block.words.m)
                        RETURN(Status_GcodeWordRepeated); // [Word repeated]

                    gc_block.words.m = On; // Flag to indicate parameter assigned.

                    continue;
                }

                if(int_value <= 99 && mantissa > 0)
                    RETURN(Status_GcodeCommandValueNotInteger); // [No Mxx.x commands]

                user_mcode = UserMCode_Unsupported;
                word_bit.modal_group.mask = 0;

                switch(int_value) {

                    case 0: case 1: case 2: case 30: case 60:
                        word_bit.modal_group.M4 = On;
                        switch(int_value) {

                            case 0: // M0 - program pause
                                gc_block.modal.program_flow = ProgramFlow_Paused;
                                break;

                            case 1: // M1 - program pause
                                if(hal.signals_cap.stop_disable ? !hal.control.get_state().stop_disable : !sys.flags.optional_stop_disable)
                                    gc_block.modal.program_flow = ProgramFlow_OptionalStop;
                                break;

                            default: // M2, M30, M60 - program end and reset
                                gc_block.modal.program_flow = (program_flow_t)int_value;
                        }
                        break;

                    case 3: case 4: case 5:
                        word_bit.modal_group.M7 = On;
                        gc_block.spindle_modal.state.on = !(int_value == 5);
                        gc_block.spindle_modal.state.ccw = int_value == 4;
//                        sys.override_delay.spindle = On; TODO: only when spindle sync?
                        break;

                    case 6:
                        {
                            atc_status_t atc = hal.tool.atc_get_state();
                            if(atc != ATC_None || settings.tool_change.mode != ToolChange_Ignore) {
                                if(atc == ATC_None ? !!hal.stream.suspend_read : (atc == ATC_Online && hal.tool.change)) {
                                    word_bit.modal_group.M6 = On;
                                    gc_block.tool_action = ToolAction_Change;
                                } else
                                    RETURN(Status_GcodeUnsupportedCommand); // [Unsupported M command]
                            }
                        }
                        break;

                    case 7: case 8: case 9:
                        word_bit.modal_group.M8 = On;
//                        sys.override_delay.coolant = On; TODO: ?
                        gc_parser_flags.set_coolant = On;
                        switch(int_value) {

                            case 7:
                                if(!hal.coolant_cap.mist)
                                    RETURN(Status_GcodeUnsupportedCommand);
                                gc_block.modal.coolant.mist = On;
                                break;

                            case 8:
                                // TODO: check driver cap?
                                gc_block.modal.coolant.flood = On;
                                break;

                            case 9:
                                gc_block.modal.coolant.value = 0;
                                break;
                        }
                        break;

                    case 56:
                        if(!settings.parking.flags.enable_override_control) // TODO: check if enabled?
                            RETURN(Status_GcodeUnsupportedCommand); // [Unsupported M command]
                        // no break
                    case 48: case 49: case 50: case 51: case 53:
                        word_bit.modal_group.M9 = On;
                        gc_block.override_command = (override_mode_t)int_value;
                        break;

                    case 61:
                        word_bit.modal_group.M6 = On; //??
                        gc_block.tool_action = ToolAction_Set;
                        break;

                    case 62:
                    case 63:
                    case 64:
                    case 65:
                        if(!ioports_can_do().digital_out || ioports_unclaimed(Port_Digital, Port_Output) == 0)
                            RETURN(Status_GcodeUnsupportedCommand); // [Unsupported M command]
                        word_bit.modal_group.M5 = On;
                        port_command = (io_mcode_t)int_value;
                        break;

                    case 66:
                        if(!ioports_can_do().wait_on_input || (ioports_unclaimed(Port_Digital, Port_Input) == 0 &&
                                                                ioports_unclaimed(Port_Analog, Port_Input) == 0))
                            RETURN(Status_GcodeUnsupportedCommand); // [Unsupported M command]
                        word_bit.modal_group.M5 = On;
                        port_command = (io_mcode_t)int_value;
                        break;

                    case 67:
                    case 68:
                        if(!ioports_can_do().analog_out || ioports_unclaimed(Port_Analog, Port_Output) == 0)
                            RETURN(Status_GcodeUnsupportedCommand); // [Unsupported M command]
                        word_bit.modal_group.M5 = On;
                        port_command = (io_mcode_t)int_value;
                        break;

#if NGC_PARAMETERS_ENABLE
                    case 70: case 71: case 72: case 73:
                        //word_bit.modal_group.G0 = On; ??
                        gc_block.state_action = (modal_state_action_t)int_value;
                        break;
#endif

                    case 98:
                        if(mantissa != 0 || grbl.on_macro_execute == NULL)
                            RETURN(Status_GcodeUnsupportedCommand);
                        word_bit.modal_group.G16 = On;
                        gc_block.macro_call = MacroCall_NonModal98;
                        break;

                    case 99:
                        if(mantissa != 0 || !(!!hal.stream.file || !!grbl.on_macro_return))
                            RETURN(Status_GcodeUnsupportedCommand);
                        word_bit.modal_group.M4 = On;
                        gc_block.modal.program_flow = ProgramFlow_Return;
                        break;

                    default:
                        if(grbl.user_mcode.check && (user_mcode = grbl.user_mcode.check((user_mcode_t)(mantissa ? int_value * 100 + mantissa : int_value)))) {
                            gc_block.user_mcode = (user_mcode_t)(mantissa ? int_value * 100 + mantissa : int_value);
                            word_bit.modal_group.M10 = On;
                        } else
                            RETURN(Status_GcodeUnsupportedCommand); // [Unsupported M command]
                } // end M-value switch

                // Check for more than one command per modal group violations in the current block
                // NOTE: Variable 'word_bit' is always assigned, if the command is valid.
                if (command_words.mask & word_bit.modal_group.mask)
                    RETURN(Status_GcodeModalGroupViolation);

                command_words.mask |= word_bit.modal_group.mask;
                break;

            // NOTE: All remaining letters assign values.
            default:

                /* Non-Command Words: This initial parsing phase only checks for repeats of the remaining
                legal g-code words and stores their value. Error-checking is performed later since some
                words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */

                word_bit.parameter.mask = 0;

                switch(letter) {

                    case 'A':
                        word_bit.parameter.a = On;
#ifdef A_AXIS
                        axis_words.a = On;
                        gc_block.values.xyz[A_AXIS] = value;
#else
                        gc_block.values.a = value;
#endif
                        break;

                    case 'B':
                        word_bit.parameter.b = On;
#ifdef B_AXIS
                        axis_words.b = On;
                        gc_block.values.xyz[B_AXIS] = value;
#else
                        gc_block.values.b = value;
#endif
                        break;

                    case 'C':
                        word_bit.parameter.c = On;
#ifdef C_AXIS
                        axis_words.c = On;
                        gc_block.values.xyz[C_AXIS] = value;
#else
                        gc_block.values.c = value;
#endif
                        break;

                    case 'D':
                        word_bit.parameter.d = On;
                        gc_block.values.d = value;
                        break;

                    case 'E':
                        word_bit.parameter.e = On;
                        gc_block.values.e = value;
                        break;

                    case 'F':
                        word_bit.parameter.f = On;
                        gc_block.values.f = value;
                        break;

                    case 'H':
                        word_bit.parameter.h = On;
                        gc_block.values.h = value;
                        break;

                    case 'I':
                        ijk_words.i = On;
                        word_bit.parameter.i = On;
                        gc_block.values.ijk[I_VALUE] = value;
                        break;

                    case 'J':
                        ijk_words.j = On;
                        word_bit.parameter.j = On;
                        gc_block.values.ijk[J_VALUE] = value;
                        break;

                    case 'K':
                        ijk_words.k = On;
                        word_bit.parameter.k = On;
                        gc_block.values.ijk[K_VALUE] = value;
                        break;

                    case 'L':
                        if (mantissa > 0)
                            RETURN(Status_GcodeCommandValueNotInteger);
                        word_bit.parameter.l = On;
                        gc_block.values.l = isnan(value) ? 0xFF : (uint8_t)int_value;
                        break;

                    case 'N':
                        word_bit.parameter.n = On;
                        gc_block.values.n = (int32_t)truncf(value);
#if NGC_EXPRESSIONS_ENABLE
                        skip_blocks = gc_state.skip_blocks;
#endif
                        break;

                    case 'O':
                        if(mantissa == 0) {
                            m98_macro_t *macro;

                            if(hal.stream.state.m98_macro_prescan && (macro = macro_find((macro_id_t)int_value))) {
                                macro->file = hal.stream.file;
                                macro->pos = vfs_tell(macro->file);
                                return Status_OK;
                            } else {
                                word_bit.parameter.o = On;
                                gc_block.values.o = int_value;
                            }
                        } else
                            RETURN(Status_GcodeCommandValueNotInteger);
                        break;

                    case 'P': // NOTE: For certain commands, P value must be an integer.
                        word_bit.parameter.p = On;
                        gc_block.values.p = value;
                        break;

                    case 'Q': // may be used for user defined mcodes or G61,G76
                        word_bit.parameter.q = On;
                        gc_block.values.q = value;
                        break;

                    case 'R':
                        word_bit.parameter.r = On;
                        gc_block.values.r = value;
                        break;

                    case 'S':
                        word_bit.parameter.s = On;
                        gc_block.values.s = value;
                        break;

                    case 'T':
                        word_bit.parameter.t = On;
                        gc_block.values.t = value;
                        break;

#if LATHE_UVW_OPTION
                    case 'U':
                        axis_words.x = On;
                        word_bit.parameter.x = word_bit.parameter.u = On;
                        gc_block.values.uvw[X_AXIS] = (gc_block.values.u = value) / 2.0f; // U is always a diameter
                        break;

                    case 'V':
                        axis_words.y = On;
                        word_bit.parameter.y = word_bit.parameter.v = On;
                        gc_block.values.uvw[Y_AXIS] = gc_block.values.v = value;
                        break;

                    case 'W':
                        axis_words.z = On;
                        word_bit.parameter.z = word_bit.parameter.w = On;
                        gc_block.values.uvw[Z_AXIS] = gc_block.values.w = value;
                        break;
#else

                    case 'U':
                        word_bit.parameter.u = On;
#ifdef U_AXIS
                        axis_words.u = On;
                        gc_block.values.xyz[U_AXIS] = value;
#else
                        gc_block.values.u = value;
#endif
                        break;

                    case 'V':
                        word_bit.parameter.v = On;
#ifdef V_AXIS
                        axis_words.v = On;
                        gc_block.values.xyz[V_AXIS] = value;
#else
                        gc_block.values.v = value;
#endif
                        break;

                    case 'W':
                        word_bit.parameter.w = On;
#ifdef W_AXIS
                        axis_words.w = On;
                        gc_block.values.xyz[W_AXIS] = value;
#else
                        gc_block.values.w = value;
#endif
                        break;

#endif // !LATHE_UVW_OPTION

                    case 'X':
                        axis_words.x = On;
                        word_bit.parameter.x = On;
                        gc_block.values.xyz[X_AXIS] = value;
                        break;

                    case 'Y':
                        axis_words.y = On;
                        word_bit.parameter.y = On;
                        gc_block.values.xyz[Y_AXIS] = value;
                        break;

                    case 'Z':
                        axis_words.z = On;
                        word_bit.parameter.z = On;
                        gc_block.values.xyz[Z_AXIS] = value;
                        break;

                    case '$':
                        if(mantissa > 0)
                            RETURN(Status_GcodeCommandValueNotInteger);
                        word_bit.parameter.$ = On;
                        gc_block.values.$ = (int32_t)value;
                        break;

                    default: RETURN(Status_GcodeUnsupportedCommand);

                } // end parameter letter switch

                // NOTE: Variable 'word_bit' is always assigned, if the non-command letter is valid.
                if (gc_block.words.mask & word_bit.parameter.mask)
                    RETURN(Status_GcodeWordRepeated); // [Word repeated]

                // Check for invalid negative values for words H, L, N, O, and T.
                // NOTE: Negative value check is done here simply for code-efficiency.
                if ((word_bit.parameter.mask & uint_words.mask) && value < 0.0f)
                    RETURN(Status_NegativeValue); // [Word value cannot be negative]

                gc_block.words.mask |= word_bit.parameter.mask; // Flag to indicate parameter assigned.

        } // end main letter switch
    }

    // Parsing complete!


  /* -------------------------------------------------------------------------------------
     STEP 3: Error-check all commands and values passed in this block. This step ensures all of
     the commands are valid for execution and follows the NIST standard as closely as possible.
     If an error is found, all commands and values in this block are dumped and will not update
     the active system g-code modes. If the block is ok, the active system g-code modes will be
     updated based on the commands of this block, and signal for it to be executed.

     Also, we have to pre-convert all of the values passed based on the modes set by the parsed
     block. There are a number of error-checks that require target information that can only be
     accurately calculated if we convert these values in conjunction with the error-checking.
     This relegates the next execution step as only updating the system g-code modes and
     performing the programmed actions in order. The execution step should not require any
     conversion calculations and would only require minimal checks necessary to execute.
  */

  /* NOTE: At this point, the g-code block has been parsed and the block line can be freed.
     NOTE: It's also possible, at some future point, to break up STEP 2, to allow piece-wise
     parsing of the block on a per-word basis, rather than the entire block. This could remove
     the need for maintaining a large string variable for the entire block and free up some memory.
     To do this, this would simply need to retain all of the data in STEP 1, such as the new block
     data struct, the modal group and value bitflag tracking variables, and axis array indices
     compatible variables. This data contains all of the information necessary to error-check the
     new g-code block when the EOL character is received. However, this would break grblHAL's startup
     lines in how it currently works and would require some refactoring to make it compatible.
  */

 /*
  * Order of execution as per RS274-NGC_3 table 8:
  *
  *      1. comment (includes message)
  *      2. set feed rate mode (G93, G94 - inverse time or per minute)
  *      3. set feed rate (F)
  *      4. set spindle speed (S)
  *      5. select tool (T)
  *      6. change tool (M6)
  *      7. spindle on or off (M3, M4, M5)
  *      8. coolant on or off (M7, M8, M9)
  *      9. enable or disable overrides (M48, M49, M50, M51, M53)
  *      10. dwell (G4)
  *      11. set active plane (G17, G18, G19)
  *      12. set length units (G20, G21)
  *      13. cutter radius compensation on or off (G40, G41, G42)
  *      14. cutter length compensation on or off (G43, G49)
  *      15. coordinate system selection (G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3)
  *      16. set path control mode (G61, G61.1, G64)
  *      17. set distance mode (G90, G91)
  *      18. set retract mode (G98, G99)
  *      19. home (G28, G30) or
  *              change coordinate system data (G10) or
  *              set axis offsets (G92, G92.1, G92.2, G94).
  *      20. perform motion (G0 to G3, G33, G80 to G89) as modified (possibly) by G53
  *      21. stop and end (M0, M1, M2, M30, M60)
  */

  // [0. Non-specific/common error-checks and miscellaneous setup]:

    if(grbl.on_line_number_assigned)
        gc_block.values.n = (int32_t)grbl.on_line_number_assigned((line_number_t)gc_block.values.n);

    if(command_words.G16) {

        if(gc_block.macro_call
#if NGC_PARAMETERS_ENABLE
            && gc_state.g66_args == NULL
#endif
        ) {
            if(!gc_block.words.p)
                RETURN(Status_GcodeValueWordMissing); // [P word missing]
            if(gc_block.values.p > 65535.0f)
                RETURN(Status_GcodeValueOutOfRange); // [P word out of range]

            if(!gc_block.words.l || gc_block.values.l == 0)
                gc_block.values.l = 1;
        }

        gc_block.words.l = gc_block.words.p = Off;

#if NGC_PARAMETERS_ENABLE

        if(gc_block.macro_call == MacroCall_NonModal || gc_block.macro_call == MacroCall_Modal || gc_block.macro_call == MacroCall_Modal1) {

            // Remove axis and ijk words flags since values are to be passed unmodified.
            axis_words.mask = ijk_words.mask = 0;

            // Remove all modal groups except macro call.
            command_words.mask = (modal_groups_t){ .G16 = On }.mask;

            if(gc_block.macro_call == MacroCall_NonModal) {
                // TODO: add context for local storage?
                if(!ngc_call_push(&gc_state + ngc_call_level()))
                    RETURN(Status_FlowControlStackOverflow); // [Call level too deep]
                gc_block.g65_words.mask = macro_arguments_push(&gc_block.values, gc_block.words, NULL).mask;
                gc_block.words.mask &= ~gc_block.g65_words.mask; // Remove G65 arguments
            } else { // G66 and G66.1
                g66_arguments_t *args;
    //            if(gc_state.g66_args && gc_state.g66_args->call_level == ngc_call_level())
    //                  RETURN(cannot have nested g66 calls on the same call level?);
                if(gc_state.g66_args && gc_state.g66_args->call == MacroCall_Modal1 && gc_state.g66_args->call_level == ngc_call_level()) {
                    args = gc_state.g66_args;
                    gc_block.words.mask &= ~macro_arguments_push(&gc_block.values, (parameter_words_t){ .mask = gc_block.words.mask & gc_state.g66_args->words.mask }, &gc_state.g66_args->values).mask;
                } else if((args = malloc(sizeof(g66_arguments_t)))) {
                    args->prev = gc_state.g66_args;
                    gc_state.g66_args = args;
                    gc_state.g66_args->call = gc_block.macro_call;
                    gc_state.g66_args->call_level = ngc_call_level();
                    gc_state.g66_args->words.mask = gc_block.words.mask;
                    gc_block.words.mask = 0;
                    memcpy(&gc_state.g66_args->values, &gc_block.values, sizeof(gc_values_t));
                }
                if(args == NULL)
                    RETURN(Status_FlowControlStackOverflow);
                // MacroCall_Modal is not invoked on initially, MacroCall_Modal1 is.
                if(gc_block.macro_call == MacroCall_Modal)
                    return Status_OK;
            }
        }
#endif // NGC_PARAMETERS_ENABLE
    } else
        gc_block.words.m = Off;

    // Determine implicit axis command conditions. Axis words have been passed, but no explicit axis
    // command has been sent. If so, set axis command to current motion mode.
    if (axis_words.mask && !axis_command)
        axis_command = AxisCommand_MotionMode; // Assign implicit motion-mode

    if(gc_state.tool_change && axis_command == AxisCommand_MotionMode && !gc_parser_flags.jog_motion)
        RETURN(Status_GcodeToolChangePending); // [Motions (except jogging) not allowed when changing tool]

    // Check for valid line number N value.
    // Line number value cannot be less than zero (done) or greater than max line number.
    if (gc_block.words.n && gc_block.values.n > MAX_LINE_NUMBER)
        RETURN(Status_GcodeInvalidLineNumber); // [Exceeds max line number]

    // bit_false(gc_block.words,bit(Word_N)); // NOTE: Single-meaning value word. Set at end of error-checking.

    // Track for unused words at the end of error-checking.
    // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because
    // they are always used when present. This was done to save a few bytes of flash. For clarity, the
    // single-meaning value words may be removed as they are used. Also, axis words are treated in the
    // same way. If there is an explicit/implicit axis command, XYZ words are always used and are
    // are removed at the end of error-checking.

    // [0. User defined M commands ]:
    if(command_words.M10 && gc_block.user_mcode) {

        user_words.mask = gc_block.words.mask;
        if((int_value = (uint_fast16_t)grbl.user_mcode.validate(&gc_block)))
            RETURN((status_code_t)int_value);

        parameter_words_t taken_words;

        user_words.mask ^= gc_block.words.mask; // Flag "taken" words for user M-code execution

        if((taken_words.mask = user_words.mask & axis_words_mask.mask))
            axis_words.mask &= ~gc_paramwords_to_axes(taken_words).mask;

        ijk_words.mask &= ~(uint8_t)((user_words.mask >> 4) & 0b111);

        if(user_words.f) {
            single_meaning_value.f = gc_block.values.f;
            gc_block.values.f = 0.0f;
        }
        if(user_words.o) {
            single_meaning_value.o = gc_block.values.o;
            gc_block.values.o = 0;
        }
        if(user_words.s) {
            single_meaning_value.s = gc_block.values.s;
            gc_block.values.s = 0.0f;
        }
        if(user_words.t) {
            single_meaning_value.t = gc_block.values.t;
            gc_block.values.t = 0.0f;
        }
    }

    if(gc_block.words.t) {

        if(!isintf(gc_block.values.t))
            RETURN(Status_GcodeCommandValueNotInteger);

        if(gc_block.values.t < 0.0f)
            RETURN(Status_NegativeValue); // [Word value cannot be negative]

        if(grbl.tool_table.n_tools ? (grbl.tool_table.get_tool((tool_id_t)gc_block.values.t)->data == NULL) : (gc_block.values.t > MAX_TOOL_NUMBER))
            RETURN(Status_GcodeIllegalToolTableEntry);
    }

    if(gc_block.words.h) {

        if(!isintf(gc_block.values.h))
            RETURN(Status_GcodeCommandValueNotInteger);

        if(gc_block.values.h < 0.0f)
            RETURN(Status_NegativeValue); // [Word value cannot be negative]
    }

    // [1. Comments ]: MSG's may be supported by driver layer. Comment handling performed by protocol.

    // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly.
    // Feed rate is not defined after switching between G93, G94 and G95.
    // NOTE: For jogging, ignore prior feed rate mode. Enforce G94 and check for required F word.
    if(!gc_parser_flags.jog_motion) {

        // Switching to G94 or G95 from G93, so don't push last state feed rate.
        if(gc_block.modal.feed_mode != gc_state.modal.feed_mode)
            gc_state.feed_rate = 0.0f;

        if(gc_block.modal.feed_mode == FeedMode_InverseTime) { // = G93

            // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here.
            if(!gc_block.words.f && axis_command == AxisCommand_MotionMode &&
                !(gc_block.modal.motion == MotionMode_None ||
                   gc_block.modal.motion == MotionMode_Seek ||
                    gc_block.modal.motion == MotionMode_SpindleSynchronized ||
                     gc_block.modal.motion == MotionMode_RigidTapping))
                RETURN(Status_GcodeUndefinedFeedRate); // [F word missing]

            // NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would
            // accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each
            // inverse time block, since the commands that use this value already perform undefined checks. This would
            // also allow other commands, following this switch, to execute and not error out needlessly. This code is
            // combined with the above feed rate mode and the below set feed rate error-checking.

            // - In inverse time mode: Always implicitly zero the feed rate value before and after block completion.
            // NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word
            // value in the block. If no F word is passed with a motion command that requires a feed rate, this will error
            // out in the motion modes error-checking. However, if no F word is passed with NO motion command that requires
            // a feed rate, we simply move on and the state feed rate value gets updated to zero and remains undefined.

        } else if(!gc_block.words.f)
            gc_block.values.f = gc_state.feed_rate; // Push last state feed rate

    } else if(!gc_block.words.f)
        RETURN(Status_GcodeUndefinedFeedRate);

    // [3. Set feed rate ]:
    // if F word passed, ensure value is in mm/min or mm/rev depending on mode.
    if(gc_block.words.f) {
        if(gc_block.values.f < 0.0f)
            RETURN(Status_NegativeValue); // [Word value cannot be negative]
        if(gc_block.modal.feed_mode != FeedMode_InverseTime && gc_block.modal.units_imperial)
            gc_block.values.f *= MM_PER_INCH;
    }

    if(gc_block.modal.motion == MotionMode_SpindleSynchronized || gc_block.modal.motion == MotionMode_RigidTapping) {
        if(!gc_block.words.k) {
            gc_block.values.k = gc_state.distance_per_rev;
        } else {
            gc_block.words.k = Off;
            gc_block.values.k = gc_block.modal.units_imperial ? gc_block.values.ijk[K_VALUE] *= MM_PER_INCH : gc_block.values.ijk[K_VALUE];
        }
        if(gc_block.modal.motion == MotionMode_RigidTapping) {
            if(!gc_block.words.i)
                gc_block.values.ijk[I_VALUE] = 1.0f; // RPM multiplier
            else
                gc_block.words.i = Off;
        }
    }

    // bit_false(gc_block.words,bit(Word_F)); // NOTE: Single-meaning value word. Set at end of error-checking.

    // [4. Set spindle speed and address spindle ]: S or D is negative (done.)
    if(gc_block.words.$) {
        bool single_spindle_only = is_single_spindle_block(&gc_block, command_words);
        if(command_words.M7 || single_spindle_only) {
            if(gc_block.values.$ < (single_spindle_only ? 0 : -1))
                RETURN(single_spindle_only ? Status_NegativeValue : Status_GcodeValueOutOfRange);
#if N_SYS_SPINDLE > 1
            if((spindle_id = gc_block.values.$) < 0)
                sspindle = NULL;
            else {
                if(!spindle_is_enabled(spindle_id))
                    RETURN(Status_GcodeValueOutOfRange);
                if(gc_state.modal.spindle[spindle_id].hal == NULL)
                    gc_state.modal.spindle[spindle_id].hal = spindle_get(spindle_id);
                sspindle = &gc_state.modal.spindle[spindle_id];
            }
#else
            if(gc_block.values.$ > 0)
                RETURN(Status_GcodeValueOutOfRange);
#endif
            gc_block.words.$ = Off;
        }
    }
#if N_SYS_SPINDLE > 1
    // For now, remove when downstream code can handle multiple spindles?
    else if(command_words.M7 || is_single_spindle_block(&gc_block, command_words))
        sspindle = &gc_state.modal.spindle[0];
#endif

    if(gc_block.modal.feed_mode == FeedMode_UnitsPerRev && (sspindle == NULL || !sspindle->hal->get_data))
        RETURN(Status_GcodeUnsupportedCommand); // [G95 not supported]

    if(command_words.G14) {
        if(gc_block.spindle_modal.rpm_mode == SpindleSpeedMode_CSS) {
            if(!sspindle->hal->cap.variable)
                RETURN(Status_GcodeUnsupportedCommand);
            if(!gc_block.words.s) // TODO: add check for S0?
                RETURN(Status_GcodeValueWordMissing);
    // see below!! gc_block.values.s *= (gc_block.modal.units_imperial ? MM_PER_INCH * 12.0f : 1000.0f); // convert surface speed to mm/min
            if(gc_block.words.d) {
                if(gc_block.words.d && gc_block.values.d < 0.0f)
                    RETURN(Status_NegativeValue); // [Word value cannot be negative]
                sspindle->hal->param->css.max_rpm = min(gc_block.values.d, sspindle->hal->rpm_max);
                gc_block.words.d = Off;
            } else
                sspindle->hal->param->css.max_rpm = sspindle->hal->rpm_max;
        } else if(sspindle->rpm_mode == SpindleSpeedMode_CSS) {
            if(sspindle->css) {
                sspindle->css = NULL;
                protocol_buffer_synchronize(); // Empty planner buffer to ensure we get RPM at end of last CSS motion
            }
            sspindle->rpm = sspindle->hal->param->rpm; // Is it correct to restore latest spindle RPM here?
        }
        sspindle->rpm_mode = gc_block.spindle_modal.rpm_mode;
    } else if(sspindle)
        gc_block.spindle_modal.rpm_mode = sspindle->rpm_mode;

    if((spindle_event = gc_block.words.s)) {

        if(gc_block.values.s < 0.0f)
            RETURN(Status_NegativeValue); // [Word value cannot be negative]

        if(sspindle->rpm_mode == SpindleSpeedMode_CSS) {
            // Unsure what to do about S values when in SpindleSpeedMode_CSS - ignore? For now use it to (re)calculate surface speed.
            // Reinsert commented out code above if this is removed!!
            gc_block.values.s *= (gc_block.modal.units_imperial ? MM_PER_INCH * 12.0f : 1000.0f); // convert surface speed to mm/min
            sspindle->hal->param->css.surface_speed = gc_block.values.s;
        }
    } else if(sspindle)
        gc_block.values.s = sspindle->rpm_mode == SpindleSpeedMode_RPM ? sspindle->rpm : sspindle->hal->param->css.max_rpm;

    // bit_false(gc_block.words,bit(Word_S)); // NOTE: Single-meaning value word. Set at end of error-checking.

    // [5. Select tool ]: If not supported then only tracks value. T is negative (done.) Not an integer (done).
    if(gc_block.tool_action == ToolAction_Set) { // M61

        if(!gc_block.words.q)
            RETURN(Status_GcodeValueWordMissing);
        if(!isintf(gc_block.values.q))
            RETURN(Status_GcodeCommandValueNotInteger);
        if(grbl.tool_table.n_tools ? grbl.tool_table.get_tool((tool_id_t)gc_block.values.q)->data == NULL : gc_block.values.q > MAX_TOOL_NUMBER)
            RETURN(Status_GcodeIllegalToolTableEntry);

        gc_block.values.t = gc_block.values.q;
        command_words.M6 = gc_block.words.q = Off;
#if NGC_EXPRESSIONS_ENABLE
        if(hal.stream.file) {
            gc_state.tool_pending = (tool_id_t)-1; // force set tool
            if(grbl.tool_table.n_tools && !command_words.G8) {
                if((gc_block.words.h = gc_state.g43_pending > 0)) { // ?? --> >= 0
                    command_words.G8 = On;
                    gc_block.values.h = (float)gc_state.g43_pending;
                    gc_block.modal.tool_offset_mode = ToolLengthOffset_Enable;
                }
            }
            gc_state.g43_pending = (tool_id_t)-1;
        }
#endif
    } else if(!gc_block.words.t)
        gc_block.values.t = (float)gc_state.tool_pending;

    if(command_words.M5 && port_command) {

        switch(port_command) {

            case IoMCode_OutputOnSynced:
            case IoMCode_OutputOffSynced:
            case IoMCode_OutputOnImmediate:
            case IoMCode_OutputOffImmediate:
                if(!gc_block.words.p)
                    RETURN(Status_GcodeValueWordMissing);
                if(gc_block.values.p < 0.0f)
                    RETURN(Status_NegativeValue);
                if((uint32_t)gc_block.values.p >= ioports_unclaimed(Port_Digital, Port_Output))
                    RETURN(Status_GcodeValueOutOfRange);
                gc_block.output_command.is_digital = true;
                gc_block.output_command.port = (uint8_t)gc_block.values.p;
                gc_block.output_command.value = port_command == 62 || port_command == 64 ? 1.0f : 0.0f;
                gc_block.words.p = Off;
                break;

            case IoMCode_WaitOnInput:
                if(!(gc_block.words.l || gc_block.words.q))
                    RETURN(Status_GcodeValueWordMissing);

                if(gc_block.words.p && gc_block.words.e)
                    RETURN(Status_ValueWordConflict);

                if(gc_block.values.l >= (uint32_t)WaitMode_Max)
                    RETURN(Status_GcodeValueOutOfRange);

                if((wait_mode_t)gc_block.values.l != WaitMode_Immediate && gc_block.values.q == 0.0f)
                    RETURN(Status_GcodeValueOutOfRange);

                if(gc_block.words.p) {
                    if(gc_block.values.p < 0.0f)
                        RETURN(Status_NegativeValue);
                    if((uint8_t)gc_block.values.p >= ioports_unclaimed(Port_Digital, Port_Input))
                        RETURN(Status_GcodeValueOutOfRange);

                    gc_block.output_command.is_digital = true;
                    gc_block.output_command.port = (uint8_t)gc_block.values.p;
                }

                if(gc_block.words.e) {
                    if((uint8_t)gc_block.values.e >= ioports_unclaimed(Port_Analog, Port_Input))
                        RETURN(Status_GcodeValueOutOfRange);
                    if((wait_mode_t)gc_block.values.l != WaitMode_Immediate)
                        RETURN(Status_GcodeValueOutOfRange);

                    gc_block.output_command.is_digital = false;
                    gc_block.output_command.port = (uint8_t)gc_block.values.e;
                }

                gc_block.words.e = gc_block.words.l = gc_block.words.p = gc_block.words.q = Off;
                break;

            case IoMCode_AnalogOutSynced:
            case IoMCode_AnalogOutImmediate:
                if(!(gc_block.words.e || gc_block.words.q))
                    RETURN(Status_GcodeValueWordMissing);
                if((uint32_t)gc_block.values.e >= ioports_unclaimed(Port_Analog, Port_Output))
                    RETURN(Status_GcodeRPMOutOfRange);
                gc_block.output_command.is_digital = false;
                gc_block.output_command.port = (uint8_t)gc_block.values.e;
                gc_block.output_command.value = gc_block.values.q;
                gc_block.words.e = gc_block.words.q = Off;
            break;
        }
    }

    // bit_false(gc_block.words,bit(Word_T)); // NOTE: Single-meaning value word. Set at end of error-checking.

    // [6. Change tool ]:
    if(command_words.M6 && gc_block.tool_action == ToolAction_Change) {
        if((tool_id_t)gc_block.values.t == gc_state.tool->tool_id)
            command_words.M6 = Off; // Tool already in spindle, ignore.
#if CUTTER_COMP_ENABLE
        else if(gc_state.modal.cutter_comp.side)
            RETURN(Status_CutterCompConflict);
#endif
    }

    // [7. Spindle control ]:
    if(command_words.M7) {
        if(gc_block.spindle_modal.state.ccw) {
            // Check if spindle(s) support reversing direction
#if N_SYS_SPINDLE > 1
            if(sspindle == NULL) {
                uint_fast8_t idx = N_SYS_SPINDLE;
                do {
                    idx--;
                    if(gc_state.modal.spindle[idx].hal && !(gc_state.modal.spindle[idx].hal->cap.direction || gc_state.modal.spindle[idx].hal->cap.laser))
                        RETURN(Status_GcodeUnsupportedCommand);
                } while(idx);
            } else
#endif
            if(!(sspindle->hal->cap.direction || sspindle->hal->cap.laser))
                RETURN(Status_GcodeUnsupportedCommand);
        }
    } else if(sspindle)
        gc_block.spindle_modal.state = sspindle->state;

    // [8. Coolant control ]:
    command_words.M8 &= gc_block.modal.coolant.value != gc_state.modal.coolant.value;

    // [9. Override control ]:
    if(command_words.M9) {

        if(!gc_block.words.p)
            gc_block.values.p = 1.0f;
        else {
            if(gc_block.values.p < 0.0f)
                RETURN(Status_NegativeValue);
            gc_block.words.p = Off;
        }
        switch(gc_block.override_command) {

            case Override_FeedSpeedEnable:
                gc_block.modal.override_ctrl.feed_rates_disable = Off;
#if N_SYS_SPINDLE > 1
                if(spindle_id == - 1) {
                    gc_block.values.$ = N_SYS_SPINDLE;
                    do {
                        bit_true(gc_block.modal.override_ctrl.spindle_rpm_disable, bit(--gc_block.values.$));
                    } while(gc_block.values.$);
                } else
#endif
                bit_false(gc_block.modal.override_ctrl.spindle_rpm_disable, bit(spindle_id));
                break;

            case Override_FeedSpeedDisable:
                gc_block.modal.override_ctrl.feed_rates_disable = On;
#if N_SYS_SPINDLE > 1
                if(spindle_id == - 1) {
                    gc_block.values.$ = N_SYS_SPINDLE;
                    do {
                        bit_true(gc_block.modal.override_ctrl.spindle_rpm_disable, bit(--gc_block.values.$));
                    } while(gc_block.values.$);
                } else
#endif
                bit_true(gc_block.modal.override_ctrl.spindle_rpm_disable, bit(spindle_id));
                break;

            case Override_FeedRate:
                gc_block.modal.override_ctrl.feed_rates_disable = gc_block.values.p == 0.0f;
                break;

            case Override_SpindleSpeed:
#if N_SYS_SPINDLE > 1
                if(spindle_id == - 1) {
                    gc_block.values.$ = N_SYS_SPINDLE;
                    do {
                        BIT_SET(gc_block.modal.override_ctrl.spindle_rpm_disable, bit(--gc_block.values.$), gc_block.values.p == 0.0f);
                    } while(gc_block.values.$);
                } else
#endif
                BIT_SET(gc_block.modal.override_ctrl.spindle_rpm_disable, bit(spindle_id), gc_block.values.p == 0.0f);
                break;

            case Override_FeedHold:
                gc_block.modal.override_ctrl.feed_hold_disable = gc_block.values.p == 0.0f;
                break;

            case Override_Parking:
                if(settings.parking.flags.enable_override_control)
                    gc_block.modal.override_ctrl.parking_disable = gc_block.values.p == 0.0f;
                break;

            default:
                break;
        }

        command_words.M9 = gc_block.modal.override_ctrl.value != gc_state.modal.override_ctrl.value;
    }

    // [10. Dwell ]: P value missing. NOTE: See below.
    if (gc_block.non_modal_command == NonModal_Dwell) {
        if (!gc_block.words.p)
            RETURN(Status_GcodeValueWordMissing); // [P word missing]
        if(gc_block.values.p < 0.0f)
            RETURN(Status_NegativeValue);
        gc_block.words.p = Off;
    }

    // [11. Set active plane ]:
#if CUTTER_COMP_ENABLE
    if(gc_block.modal.plane_select == PlaneSelect_YZ && gc_block.modal.cutter_comp.side)
        RETURN(Status_CutterCompConflict);
#endif
    gc_get_plane_data(&plane, gc_block.modal.plane_select);

    // [12. Set length units ]: N/A
    // Pre-convert XYZ coordinate values to millimeters, if applicable.
    uint_fast8_t idx = N_AXIS;
    if (gc_block.modal.units_imperial) do { // Axes indices are consistent, so loop may be used.
        idx--;
#if N_AXIS > 3
        if (bit_istrue(axis_words.mask, bit(idx)) && bit_isfalse(settings.steppers.is_rotary.mask, bit(idx))) {
#else
        if (bit_istrue(axis_words.mask, bit(idx))) {
#endif
            gc_block.values.xyz[idx] *= MM_PER_INCH;
#if LATHE_UVW_OPTION
  #if N_AXIS > 3
            if(idx <= Z_AXIS)
  #endif
            gc_block.values.uvw[idx] *= MM_PER_INCH;
#endif
        }
    } while(idx);

    if(gc_block.modal.diameter_mode && bit_istrue(axis_words.mask, bit(X_AXIS)))
        gc_block.values.xyz[X_AXIS] /= 2.0f;

    // Scale axis words if commanded
    if(axis_command == AxisCommand_Scaling) {

#if CUTTER_COMP_ENABLE
        if(gc_block.modal.cutter_comp.side)
            RETURN(Status_CutterCompConflict);
#endif

        if(gc_block.modal.scaling_active) {

            bool report_scaling = false;

            // TODO: precheck for 0.0f and fail if found?

            gc_block.modal.scaling_active = false;

#ifdef MACH3_SCALING
            // [G51 Errors]: No axis words. TODO: add support for P (scale all with same factor)?
            if (!axis_words.mask)
                RETURN(Status_GcodeNoAxisWords); // [No axis words]

            idx = N_AXIS;
            do {
                if(bit_istrue(axis_words.mask, bit(--idx))) {
                    report_scaling |= scale_factor.ijk[idx] != gc_block.values.xyz[idx];
                    scale_factor.ijk[idx] = gc_block.values.xyz[idx];
                    bit_false(axis_words.mask, bit(idx));
                    report_add_realtime(Report_Scaling);
                }
                gc_block.modal.scaling_active = gc_block.modal.scaling_active || (scale_factor.xyz[idx] != 1.0f);
            } while(idx);

            gc_block.words.mask &= ~axis_words_mask.mask; // Remove axis words.
#else
            if (!(gc_block.words.p || ijk_words.mask))
                RETURN(Status_GcodeNoAxisWords); // [No axis words]

            idx = N_AXIS;
            do {
                if(bit_istrue(axis_words.mask, bit(--idx)))
                    scale_factor.xyz[idx] = gc_block.values.xyz[idx];
                else
                    scale_factor.xyz[idx] = gc_state.position[idx];
            } while(idx);

            gc_block.words.mask &= ~axis_words_mask.mask; // Remove axis words.

            idx = 3;
            do {
                idx--;
                if(gc_block.words.p) {
                    report_scaling |= scale_factor.ijk[idx] != gc_block.values.p;
                    scale_factor.ijk[idx] = gc_block.values.p;
                } else if(bit_istrue(ijk_words.mask, bit(idx))) {
                    report_scaling |= scale_factor.ijk[idx] != gc_block.values.ijk[idx];
                    scale_factor.ijk[idx] = gc_block.values.ijk[idx];
                }
                gc_block.modal.scaling_active = gc_block.modal.scaling_active || (scale_factor.ijk[idx] != 1.0f);
            } while(idx);

            if(gc_block.words.p)
                gc_block.words.p = Off;
            else
                gc_block.words.i = gc_block.words.j = gc_block.words.k = Off;
#endif
            report_scaling |= gc_state.modal.scaling_active != gc_block.modal.scaling_active;
            gc_state.modal.scaling_active = gc_block.modal.scaling_active;

            if(report_scaling)
                report_add_realtime(Report_Scaling);

        } else
            set_scaling(1.0f);
    }

    // Scale axis words if scaling active
    if(gc_state.modal.scaling_active) {
        idx = N_AXIS;
        do {
            if(bit_istrue(axis_words.mask, bit(--idx))) {
                if(gc_block.modal.distance_incremental)
                     gc_block.values.xyz[idx] *= scale_factor.ijk[idx];
                else
                     gc_block.values.xyz[idx] = (gc_block.values.xyz[idx] - scale_factor.xyz[idx]) * scale_factor.ijk[idx] + scale_factor.xyz[idx];
#if LATHE_UVW_OPTION
  #if N_AXIS > 3
                if(idx <= Z_AXIS)
  #endif
                gc_block.values.uvw[idx] *= scale_factor.ijk[idx];
#endif
            }
        } while(idx);
    }

    // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED. Error, if enabled while G53 is active.
    // [G40 Errors]: G2/3 arc is programmed after a G40. The linear move after disabling is less than tool diameter.
    //   NOTE: Since cutter radius compensation is never enabled, these G40 errors don't apply. grblHAL supports G40
    //   only for the purpose to not error when G40 is sent with a g-code program header to setup the default modes.

#if CUTTER_COMP_ENABLE

    // [13. Cutter radius compensation ]: G41/42 SUPPORTED XY ONLY.

    // A standalone G40 is modal-only. Drain queued compensation only when the block also carries motion.

    if(command_words.G7) {

        if(gc_block.modal.cutter_comp.side && gc_state.modal.cutter_comp.side)
            RETURN(Status_GcodeCutterCompActive);

        if(gc_block.modal.canned_cycle_active)
            RETURN(Status_CutterCompConflict);

        if(gc_block.modal.cutter_comp.side) {

            if(gc_block.modal.plane_select == PlaneSelect_YZ)
                RETURN(Status_GcodeIllegalPlane);

            if(gc_block.modal.cutter_comp.dynamic) {

                if(!gc_block.words.d)
                    RETURN(Status_GcodeValueWordMissing);
#if LATHE_UVW_OPTION
                if(gc_block.words.l) {

                    if(gc_block.modal.plane_select != PlaneSelect_ZX)
                        RETURN(Status_GcodeIllegalPlane);

                    if(!isintf(gc_block.values.l) || gc_block.values.l < 0.0f || gc_block.values.l > 9.0f)
                        RETURN(Status_GcodeValueOutOfRange); // [Illegal orientation]

                    gc_block.modal.cutter_comp.orientation = (tool_orientation_t)gc_block.values.l;
                    gc_block.words.l = Off;
                }
#endif
                gc_block.modal.cutter_comp.radius = gc_block.values.d * 0.5f;
                if(gc_block.modal.units_imperial)
                    gc_block.modal.cutter_comp.radius *= MM_PER_INCH;

            } else {

                tool_data_t *tool_data = NULL;

                if(gc_block.words.d) {

                    if(!isintf(gc_block.values.d))
                        RETURN(Status_GcodeCommandValueNotInteger);

                    if(gc_block.values.d == 0.0f || (tool_id_t)gc_block.values.d == gc_state.tool->tool_id)
                        tool_data = gc_state.tool;
                    else if(grbl.tool_table.n_tools) {
                        if((tool_data = grbl.tool_table.get_tool((tool_id_t)gc_block.values.d)->data) == NULL)
                            RETURN(Status_GcodeIllegalToolTableEntry);
                    }
                } else
                    tool_data = gc_state.tool;

                gc_block.modal.cutter_comp.radius = tool_data && tool_data->tool_id ? tool_data->radius : 0.0f;
#if LATHE_UVW_OPTION
                gc_block.modal.cutter_comp.orientation = tool_data && tool_data->tool_id ? tool_data->orientation : ToolPos_Undefined;
#endif
            }

            gc_block.words.d = Off;
        }

        gc_state.ccomp_off = gc_state.modal.cutter_comp.side && !gc_block.modal.cutter_comp.side;
    }

#endif

    // [14. Tool length compensation ]: G43.1 and G49 are always supported, G43 and G43.2 if grbl.tool_table.n_tools > 0
    // [G43.1 Errors]: Motion command in same line.
    // [G43.2 Errors]: Tool number not in the tool table,
    if (command_words.G8) { // Indicates called in block.

#if TOOL_LENGTH_OFFSET_AXIS >= 0
        // NOTE: Although not explicitly stated so, G43.1 should be applied to only one valid
        // axis that is configured (in config.h). There should be an error if the configured axis
        // is absent or if any of the other axis words are present.
        if(gc_block.modal.tool_offset_mode == ToolLengthOffset_EnableDynamic) {
            if (axis_words.mask ^ bit(TOOL_LENGTH_OFFSET_AXIS))
                RETURN(Status_GcodeG43DynamicAxisError);
        }
#endif

        switch(gc_block.modal.tool_offset_mode) {

            case ToolLengthOffset_EnableDynamic:
                if(!axis_words.mask)
                    RETURN(Status_GcodeG43DynamicAxisError);
                break;

            case ToolLengthOffset_Enable:
                if(grbl.tool_table.n_tools) {
                    if(gc_block.words.h) {
                        if(gc_block.values.h == 0.0f && !settings.macro_atc_flags.random_toolchanger)
                            gc_block.values.h = gc_block.values.t; // !! no tool clear offset
                        if(!grbl.tool_table.get_tool((tool_id_t)gc_block.values.h)->data)
                            RETURN(Status_GcodeIllegalToolTableEntry);
                        gc_block.words.h = Off;
                    } else if(!command_words.M6)
                        gc_block.values.h = gc_block.words.t ? gc_block.values.t : (float)gc_state.tool->tool_id;
                } else
                    RETURN(Status_GcodeUnsupportedCommand);
                break;

            case ToolLengthOffset_ApplyAdditional:
                if(grbl.tool_table.n_tools) {
                    if(gc_block.words.h) {
                        if(gc_block.values.h == 0.0f || !grbl.tool_table.get_tool((tool_id_t)gc_block.values.h)->data)
                            RETURN(Status_GcodeIllegalToolTableEntry);
                        gc_block.words.h = Off;
                    } else
                        RETURN(Status_GcodeValueWordMissing);
                } else
                    RETURN(Status_GcodeUnsupportedCommand);
                break;

            default:
                break;
        }
    }

    // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active.
    // TODO: A read of the coordinate data may require a buffer sync when the cycle
    // is active. The read pauses the processor temporarily and may cause a rare crash.
    // NOTE: If NVS buffering is active then non-volatile storage reads/writes are buffered and updates
    // delayed until no cycle is active.

    if((command_words.G12 &= gc_block.modal.g5x_offset.id != gc_state.modal.g5x_offset.id)) { // Check if called in block
#if CUTTER_COMP_ENABLE
        if(gc_block.modal.cutter_comp.side)
            RETURN(Status_CutterCompConflict);
#endif
        if(!settings_read_coord_data(gc_block.modal.g5x_offset.id, &gc_block.modal.g5x_offset.data))
            RETURN(Status_SettingReadFail);
    }

    // [16. Set path control mode ]: N/A. Only G61. G61.1 and G64 NOT SUPPORTED.
#if ENABLE_PATH_BLENDING
    if(command_words.G13) { // Check if called in block
        if(gc_block.modal.control == ControlMode_PathBlending) {
            gc_state.path_tolerance = gc_block.words.p ? gc_block.values.p : 0.0f;
            gc_state.cam_tolerance = gc_block.words.q ? gc_block.values.q : 0.0f;
            gc_block.words.p = gc_block.words.q = Off;
        } else
            gc_state.path_tolerance = gc_state.cam_tolerance = 0.0f;
    }
#endif

    // [17. Set distance mode ]: N/A. Only G91.1. G90.1 NOT SUPPORTED.

    command_words.G15 &= gc_block.modal.diameter_mode != gc_state.modal.diameter_mode;

    // [18. Set retract mode ]: N/A.

    // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets.
    // NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these
    // commands all treat axis words differently. G10 as absolute offsets or computes current position as
    // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes
    // all the current coordinate system and G92 offsets.
    switch (gc_block.non_modal_command) {

        case NonModal_Settings:;

            // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.)
            // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to N_WorkCoordinateSystems (max 9). Axis words missing.
            // [G10 L20 Errors]: P must be 0 to N_WorkCoordinateSystems (max 9). Axis words missing.
            // [G10 L0 Errors]:  No tool table or tool in spindle.
            // [G10 L1, L10, L11 Errors]: P must be 0 to grbl.tool_table.n_tools. Axis words or R word missing.

            uint8_t p_value = 0;

            if(gc_block.values.l > 0) {

                if(!(axis_words.mask || (gc_block.values.l != 20 && gc_block.words.r)))
                    RETURN(Status_GcodeNoAxisWords); // [No axis words (or R word for tool offsets)]

                if(!(gc_block.words.p || gc_block.words.l))
                    RETURN(Status_GcodeValueWordMissing); // [P/L word missing]

                if(gc_block.values.p < 0.0f)
                    RETURN(Status_NegativeValue);

#if CUTTER_COMP_ENABLE
                if(!(gc_block.values.l == 2 || gc_block.values.l == 20) && gc_block.modal.cutter_comp.side)
                    RETURN(Status_CutterCompConflict);
#endif
                p_value = (uint8_t)truncf(gc_block.values.p); // Convert p value to int.
            }

            switch(gc_block.values.l) {

                case 0:
                    if(grbl.tool_table.reload == NULL)
                        RETURN(Status_GcodeUnsupportedCommand); // [G10 L0 not supported]
                    if(gc_state.tool->tool_id != 0)
                        RETURN(Status_ToolInSPindle); // [G1 L0 can only be used when no tool is loaded]
                    if((status_code_t)(int_value = grbl.tool_table.reload()) == Status_OK) {
                        tool_data_t *tool_data;
                        if((tool_data = grbl.tool_table.get_tool((tool_id_t)gc_state.tool->tool_id)->data))
                            memcpy(&gc_state.modal.tool_length_offset, tool_data->offset.values, sizeof(gc_state.modal.tool_length_offset));
                    } else
                        RETURN((status_code_t)int_value);
                    break;

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wimplicit-fallthrough"
#endif
                case 2:
#ifndef ROTATION_ENABLE
                    if(gc_block.words.r)
                        RETURN(Status_GcodeUnsupportedCommand); // [G10 L2 R not supported]
                    // no break
#endif
                case 20:;
                    if(p_value > N_WorkCoordinateSystems)
                        RETURN(Status_GcodeUnsupportedCoordSys); // [Greater than N sys]

                    // Determine coordinate system to change and try to load from non-volatile storage.
                    coord_system.id = p_value == 0
                                       ? gc_block.modal.g5x_offset.id       // Index P0 as the active coordinate system
                                       : (coord_system_id_t)(p_value - 1);  // else adjust index to NVS coordinate data indexing.

                    if(!settings_read_coord_data(coord_system.id, &coord_system.data))
                        RETURN(Status_SettingReadFail); // [non-volatile storage read fail]

#if COMPATIBILITY_LEVEL <= 1
                    if(settings.offset_lock.mask && coord_system.id >= CoordinateSystem_G59_1 && coord_system.id <= CoordinateSystem_G59_3) {
                        if(bit_istrue(settings.offset_lock.mask, bit(coord_system.id - CoordinateSystem_G59_1)))
                            RETURN(Status_GCodeCoordSystemLocked);
                    }
  #ifdef ROTATION_ENABLE
                    if(gc_block.words.r && gc_block.values.l == 2) {
                        coord_system.data.rotation = gc_block.values.r * RADDEG;
                        gc_block.words.r = Off;
                    }
  #endif
#endif
                    // Pre-calculate the coordinate data changes.
                    idx = N_AXIS;
                    do { // Axes indices are consistent, so loop may be used.
                        // Update axes defined only in block. Always in machine coordinates. Can change non-active system.
                        if(bit_istrue(axis_words.mask, bit(--idx))) {
                            if(gc_block.values.l == 20)
                                // L20: Update coordinate system axis at current position (with modifiers) with programmed value
                                // WPos = MPos - WCS - G92 - TLO  ->  WCS = MPos - G92 - TLO - WPos
                                coord_system.data.coord.values[idx] = gc_state.position[idx] - gc_block.values.xyz[idx] - gc_state.g92_offset.coord.values[idx] - gc_state.modal.tool_length_offset[idx];
                            else // L2: Update coordinate system axis to programmed value.
                                coord_system.data.coord.values[idx] = gc_block.values.xyz[idx];
                        } // else, keep current stored value.
                    } while(idx);
                    break;
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
                case 1: case 10:
#if COMPATIBILITY_LEVEL <= 1
                case 11:
#endif
                    if(grbl.tool_table.n_tools) {

                        tool_data_t *tool_data;

                        if((tool_data = grbl.tool_table.get_tool((tool_id_t)p_value)->data) == NULL)
                            RETURN(Status_GcodeIllegalToolTableEntry); // [Greater than max allowed tool number or not in tool table]
#if LATHE_UVW_OPTION
                        if(gc_block.words.q) {
                            if(!isintf(gc_block.values.q) || gc_block.values.q < 0.0f || gc_block.values.q > 9.0f)
                                RETURN(Status_GcodeValueOutOfRange); // [Illegal orientation]
                            tool_data->orientation = (tool_orientation_t)gc_block.values.q;
                            gc_block.words.q = Off;
                        }
                        if(gc_block.words.i) {
                            tool_data->front_angle = gc_block.values.ijk[I_VALUE];
                            gc_block.words.i = Off;
                        }
                        if(gc_block.words.j) {
                            tool_data->back_angle = gc_block.values.ijk[J_VALUE];
                            gc_block.words.j = Off;
                        }
#endif
                        if(gc_block.words.r) {
                            tool_data->radius = gc_block.modal.units_imperial ? gc_block.values.r * MM_PER_INCH : gc_block.values.r;
                            gc_block.words.r = Off;
                        }
#if COMPATIBILITY_LEVEL <= 1
                        coord_system_data_t g59_3_offset;
                        if(gc_block.values.l == 11 && !settings_read_coord_data(CoordinateSystem_G59_3, &g59_3_offset))
                            RETURN(Status_SettingReadFail);
#endif
                        idx = N_AXIS;
                        do {
                            if(bit_istrue(axis_words.mask, bit(--idx))) {
                                if(gc_block.values.l == 1)
                                    tool_data->offset.values[idx] = gc_block.values.xyz[idx];
                                else if(gc_block.values.l == 10)
                                    tool_data->offset.values[idx] = gc_state.position[idx] - gc_state.modal.g5x_offset.data.coord.values[idx] - gc_state.g92_offset.coord.values[idx] - gc_block.values.xyz[idx];
#if COMPATIBILITY_LEVEL <= 1
                                else if(gc_block.values.l == 11)
                                    tool_data->offset.values[idx] = g59_3_offset.coord.values[idx] - gc_block.values.xyz[idx];
#endif
    //                            if(gc_block.values.l != 1)
    //                                tool_table[p_value].offset[idx] -= gc_state.modal.tool_length_offset[idx];
                            } else if(gc_block.values.l == 10 || gc_block.values.l == 11)
                                tool_data->offset.values[idx] = gc_state.modal.tool_length_offset[idx];

                            // else, keep current stored value.
                        } while(idx);

                        if(gc_block.values.l == 1)
                            grbl.tool_table.set_tool(tool_data);
                    } else
                        RETURN(Status_GcodeUnsupportedCommand);
                    break;

                default:
                    RETURN(Status_GcodeUnsupportedCommand); // [Unsupported L]
            }
            gc_block.words.l = gc_block.words.p = Off;
            break;

        case NonModal_SetCoordinateOffset:

            // [G92 Errors]: No axis words.
            if (!axis_words.mask)
                RETURN(Status_GcodeNoAxisWords); // [No axis words]

            // Update axes defined only in block. Offsets current system to defined value. Does not update when
            // active coordinate system is selected, but is still active unless G92.1 disables it.
            idx = N_AXIS;
            do { // Axes indices are consistent, so loop may be used.
                if(bit_istrue(axis_words.mask, bit(--idx))) {
            // WPos = MPos - WCS - G92 - TLO  ->  G92 = MPos - WCS - TLO - WPos
                    gc_block.values.xyz[idx] = gc_state.position[idx] - gc_block.modal.g5x_offset.data.coord.values[idx] - gc_block.values.xyz[idx] - gc_state.modal.tool_length_offset[idx];
                } else
                    gc_block.values.xyz[idx] = gc_state.g92_offset.coord.values[idx];
            } while(idx);
            break;
            
#if ENABLE_ACCELERATION_PROFILES
        case NonModal_SetAccelerationProfile:
            if(gc_block.words.e)
                RETURN(Status_GcodeUnsupportedCommand);

            if(gc_block.words.p && (gc_block.values.p < 1.0f || gc_block.values.p > 5.0f))
                RETURN(Status_GcodeValueOutOfRange);

            gc_state.modal.acceleration_factor = gc_get_accel_factor(gc_block.words.p ? (uint8_t)gc_block.values.p - 1 : 0);
            gc_block.words.p = Off;
            break;
#endif
        default:

            // At this point, the rest of the explicit axis commands treat the axis values as the traditional
            // target position with the coordinate system offsets, G92 offsets, absolute override, and distance
            // modes applied. This includes the motion mode commands. We can now pre-compute the target position.
            // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
            if((axis_words.mask || gc_block.modal.motion == MotionMode_CwArc || gc_block.modal.motion == MotionMode_CcwArc) && axis_command != AxisCommand_ToolLengthOffset) { // TLO block any axis command.

#ifdef ROTATION_ENABLE
                axes_signals_t r_axes = {0}, r_around;
                uint_fast8_t idx_0 = 0, idx_1 = 0;

                if(!(gc_parser_flags.jog_motion || gc_block.non_modal_command == NonModal_AbsoluteOverride) && (r_axes.mask = (gc_block.modal.g5x_offset.data.rotation == 0.0f ? 0 : (axis_words.mask & (r_around.mask = rotate_axes[gc_block.modal.plane_select].mask))))) {

                    if(r_axes.mask != r_around.mask) {
                        point_3d_t pos;
                        memcpy(&pos, gc_state.position, sizeof(point_3d_t));
                        idx_0 = ffs(r_axes.mask) - 1;
                        idx_1 = ffs((r_axes.mask ^ r_around.mask) & r_around.mask) - 1;
                        pos.values[idx_0] -= (gc_state.g92_offset.coord.values[idx_0] + gc_state.modal.tool_length_offset[idx_0]);
                        pos.values[idx_1] -= (gc_state.g92_offset.coord.values[idx_1] + gc_state.modal.tool_length_offset[idx_1]);
                        rotate((coord_data_t *)&pos, plane, -gc_block.modal.g5x_offset.data.rotation);
                        gc_block.values.xyz[idx_1] = pos.values[idx_1] - gc_block.modal.g5x_offset.data.coord.values[idx_1];
                        axis_words.mask |= r_around.mask;
                    } else {
                        idx_0 = ffs(r_around.mask) - 1;
                        bit_false(r_around.mask, bit(idx_0));
                        idx_1 = ffs(r_around.mask) - 1;
                    }

                    gc_block.values.xyz[plane.axis_0] += gc_block.modal.g5x_offset.data.coord.values[plane.axis_0];
                    gc_block.values.xyz[plane.axis_1] += gc_block.modal.g5x_offset.data.coord.values[plane.axis_1];

                    rotate((coord_data_t *)gc_block.values.xyz, plane, gc_block.modal.g5x_offset.data.rotation);
                }
#endif

                idx = N_AXIS;

                do { // Axes indices are consistent, so loop may be used to save flash space.
                    if(bit_isfalse(axis_words.mask, bit(--idx)))
                        gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position.
                    else if(gc_block.non_modal_command != NonModal_AbsoluteOverride) {
                        // Update specified value according to distance mode or ignore if absolute override is active.
                        // NOTE: G53 is never active with G28/30 since they are in the same modal group.
                        // Apply coordinate offsets based on distance mode.
#if LATHE_UVW_OPTION
  #if N_AXIS > 3
                        if(idx <= Z_AXIS && bit_istrue(axis_words.mask, bit(idx)) && bit_istrue(gc_block.words.mask, bit(idx + 21))) {
  #else
                        if(bit_istrue(axis_words.mask, bit(idx)) && bit_istrue(gc_block.words.mask, bit(idx + 21))) {
  #endif
                            bit_false(gc_block.words.mask, bit(idx + 21));
                            gc_block.values.xyz[idx] = gc_state.position[idx] + gc_block.values.uvw[idx];
                        } else
#endif
                        if(gc_block.modal.distance_incremental)
                            gc_block.values.xyz[idx] += gc_state.position[idx];
                        else {  // Absolute mode
#ifdef ROTATION_ENABLE
                            if(r_axes.mask && (idx == idx_0 || idx == idx_1))
                                gc_block.values.xyz[idx] += gc_state.g92_offset.coord.values[idx] + gc_state.modal.tool_length_offset[idx];
                            else
#endif
                                gc_block.values.xyz[idx] += gc_get_block_offset(&gc_block, idx);
                        }
                    }
                } while(idx);
            }

            // Check remaining non-modal commands for errors.
            switch (gc_block.non_modal_command) {

                case NonModal_GoHome_0: // G28
                case NonModal_GoHome_1: // G30
                    // [G28/30 Errors]: Cutter compensation is enabled.
                    // Retrieve G28/30 go-home position data (in machine coordinates) from non-volatile storage
#if CUTTER_COMP_ENABLE
                    if(gc_block.modal.cutter_comp.side)
                        RETURN(Status_CutterCompConflict);
#endif
                    if(!settings_read_coord_data(gc_block.non_modal_command == NonModal_GoHome_0 ? CoordinateSystem_G28 : CoordinateSystem_G30, &coord_system.data))
                        RETURN(Status_SettingReadFail);

                    if(axis_words.mask) {
                        // Move only the axes specified in secondary move.
                        idx = N_AXIS;
                        do {
                            if (bit_isfalse(axis_words.mask, bit(--idx)))
                                coord_system.data.coord.values[idx] = gc_state.position[idx];
                        } while(idx);
                    } else
                        axis_command = AxisCommand_None; // Set to none if no intermediate motion.
                    break;

                case NonModal_SetHome_0: // G28.1
                case NonModal_SetHome_1: // G30.1
                    // [G28.1/30.1 Errors]: Cutter compensation is enabled.
                    // NOTE: If axis words are passed here, they are interpreted as an implicit motion mode.
                    break;

                case NonModal_ResetCoordinateOffset:
                    // NOTE: If axis words are passed here, they are interpreted as an implicit motion mode.
                    break;

                case NonModal_AbsoluteOverride:
                    // [G53 Errors]: G0 and G1 are not active. Cutter compensation is enabled.
                    // NOTE: All explicit axis word commands are in this modal group. So no implicit check necessary.
#if CUTTER_COMP_ENABLE
                    if(gc_block.modal.cutter_comp.side)
                        RETURN(Status_CutterCompConflict);
#endif
                    if (!(gc_block.modal.motion == MotionMode_Seek || gc_block.modal.motion == MotionMode_Linear))
                        RETURN(Status_GcodeG53InvalidMotionMode); // [G53 G0/1 not active]
                    break;


                default:
                    break;
            }
    } // end gc_block.non_modal_command

    // [20. Motion modes ]:
    if (gc_block.modal.motion == MotionMode_None) {

        // [G80 Errors]: Axis word are programmed while G80 is active.
        // NOTE: Even non-modal commands or TLO that use axis words will throw this strict error.
        if (axis_words.mask && axis_command != AxisCommand_NonModal) // [No axis words allowed]
            RETURN(Status_GcodeAxisWordsExist);

        gc_block.modal.retract_mode = CCRetractMode_Previous;

    // Check remaining motion modes, if axis word are implicit (exist and not used by G10/28/30/92), or
    // was explicitly commanded in the g-code block.
    } else if (axis_command == AxisCommand_MotionMode) {

        gc_parser_flags.motion_mode_changed = gc_block.modal.motion != gc_state.modal.motion;

        if (gc_block.modal.motion == MotionMode_Seek) {
            // [G0 Errors]: Axis letter not configured or without real value (done.)
            // Axis words are optional. If missing, set axis command flag to ignore execution.
            if (!axis_words.mask)
                axis_command = AxisCommand_None;

        // All remaining motion modes (all but G0 and G80), require a valid feed rate value. In units per mm mode,
        // the value must be positive. In inverse time mode, a positive value must be passed with each block.
        } else {
#if CUTTER_COMP_ENABLE
            if(gc_state.ccomp_off) {

                if(!(gc_block.modal.motion == MotionMode_Linear || gc_block.modal.motion == MotionMode_Seek))
                    RETURN(Status_CutterCompConflict);

                point_2d_t move = { .x = gc_block.values.ijk[plane.axis_0] - gc_state.position[plane.axis_0], .y = gc_block.values.ijk[plane.axis_1] - gc_state.position[plane.axis_1] };
                if(hypot_f(move.x, move.y) < gc_state.modal.cutter_comp.radius * 2.0f)
                    RETURN(Status_CutterCompConflict);

                gc_state.ccomp_off = false;
            }
#endif
            if(!gc_block.modal.canned_cycle_active)
                gc_block.modal.retract_mode = CCRetractMode_Previous;

            // Initial(?) check for spindle running for moves in G96 mode
            if(gc_block.spindle_modal.rpm_mode == SpindleSpeedMode_CSS && (!gc_block.spindle_modal.state.on || gc_block.values.s == 0.0f))
                 RETURN(Status_GcodeSpindleNotRunning);

            // Check if feed rate is defined for the motion modes that require it.
            if(gc_block.modal.motion == MotionMode_SpindleSynchronized || gc_block.modal.motion == MotionMode_RigidTapping) {

                if(!sspindle->hal->get_data)
                    RETURN(Status_GcodeUnsupportedCommand); // [G33, G33.1]

                if(gc_block.values.k == 0.0f)
                    RETURN(Status_GcodeValueOutOfRange); // [No distance (pitch) given]

                // Ensure spindle speed is at 100% - any override will be disabled on execute.
                gc_parser_flags.spindle_force_sync = On;

            } else if(gc_block.modal.motion == MotionMode_Threading) {

                // Fail if cutter radius comp is active

                if(!sspindle->hal->get_data)
                    RETURN(Status_GcodeUnsupportedCommand); // [G76 not supported]

                if(gc_block.modal.plane_select != PlaneSelect_ZX)
                    RETURN(Status_GcodeIllegalPlane); // [Plane not ZX]

                if(axis_words.mask & ~(bit(X_AXIS)|bit(Z_AXIS)))
                    RETURN(Status_GcodeUnusedWords); // [Only X and Z axis words allowed]

                if(gc_block.words.r && gc_block.values.r < 1.0f)
                    RETURN(Status_GcodeValueOutOfRange);

                if(!axis_words.z || !(gc_block.words.i || gc_block.words.j || gc_block.words.k || gc_block.words.p))
                    RETURN(Status_GcodeValueWordMissing);

                if(gc_block.values.p < 0.0f || gc_block.values.ijk[J_VALUE] < 0.0f || gc_block.values.ijk[K_VALUE] < 0.0f)
                    RETURN(Status_NegativeValue);

                if(gc_block.values.ijk[I_VALUE] == 0.0f ||
                    gc_block.values.ijk[J_VALUE] == 0.0f ||
                     gc_block.values.ijk[K_VALUE] <= gc_block.values.ijk[J_VALUE] ||
                      (gc_block.words.l && (gc_taper_type)gc_block.values.l > Taper_Both))
                    RETURN(Status_GcodeValueOutOfRange);

                if(sspindle->rpm < sspindle->hal->rpm_min || sspindle->rpm > sspindle->hal->rpm_max)
                    RETURN(Status_GcodeRPMOutOfRange);

                if(gc_block.modal.motion != gc_state.modal.motion) {
                    memset(&thread, 0, sizeof(gc_thread_data));
                    thread.depth_degression = 1.0f;
                }

                thread.pitch = gc_block.values.p;
                thread.z_final = gc_block.values.xyz[Z_AXIS];
                thread.cut_direction = gc_block.values.ijk[I_VALUE] < 0.0f ? -1.0f : 1.0f;
                thread.peak = fabsf(gc_block.values.ijk[I_VALUE]);
                thread.initial_depth = gc_block.values.ijk[J_VALUE];
                thread.depth = gc_block.values.ijk[K_VALUE];

                if(gc_block.modal.units_imperial) {
                    thread.peak *= MM_PER_INCH;
                    thread.initial_depth *= MM_PER_INCH;
                    thread.depth *= MM_PER_INCH;
                }

                if(gc_block.modal.diameter_mode) {
                    thread.peak /= 2.0f;
                    thread.initial_depth /= 2.0f;
                    thread.depth /= 2.0f;
                }

                //scaling?

                if(axis_words.x) {
                    thread.main_taper_height = gc_block.values.xyz[X_AXIS] - gc_get_block_offset(&gc_block, X_AXIS);
                    gc_block.values.p = fabsf(thread.z_final - gc_state.position[Z_AXIS]);
                    thread.pitch = thread.pitch * hypot_f(thread.main_taper_height, gc_block.values.p) / gc_block.values.p;
                }

                if(gc_block.words.h)
                    thread.spring_passes = (uint_fast16_t)gc_block.values.h;

                if(gc_block.words.l)
                    thread.end_taper_type = (gc_taper_type)gc_block.values.l;

                if(gc_block.words.e)
                    thread.end_taper_length = gc_block.values.e;

                if(thread.end_taper_length <= 0.0f || thread.end_taper_type == Taper_None) {
                    thread.end_taper_length = 0.0f;
                    thread.end_taper_type = Taper_None;
                    // TODO: fail?
                }

                if(thread.end_taper_type != Taper_None && thread.end_taper_length > fabsf(thread.z_final - gc_state.position[Z_AXIS]) / 2.0f)
                    RETURN(Status_GcodeValueOutOfRange);

                if(gc_block.words.r)
                    thread.depth_degression = gc_block.values.r;

                if(gc_block.words.q)
                    thread.infeed_angle = gc_block.values.q;

                // Ensure spindle speed is at 100% - any override will be disabled on execute.
                gc_parser_flags.spindle_force_sync = On;

                gc_block.words.e = gc_block.words.h = gc_block.words.i = gc_block.words.j = gc_block.words.k = gc_block.words.l = gc_block.words.p = gc_block.words.q = gc_block.words.r = Off;

            } else if(gc_block.values.f == 0.0f)
                RETURN(Status_GcodeUndefinedFeedRate); // [Feed rate undefined]

            if(gc_block.modal.canned_cycle_active) {

                if(gc_parser_flags.canned_cycle_change) {
#if CUTTER_COMP_ENABLE
                    if(gc_block.modal.cutter_comp.side)
                        RETURN(Status_CutterCompConflict);
#endif
                    if(gc_state.modal.feed_mode == FeedMode_InverseTime)
                        RETURN(Status_InvalidStatement);

                    if(!gc_block.words.r)
                        RETURN(Status_GcodeValueWordMissing);

                    if(!(axis_words.mask & bit(plane.axis_linear)))
                        RETURN(Status_GcodeValueWordMissing);

                    gc_state.canned.dwell = 0.0f;
                    gc_state.canned.xyz[plane.axis_0] = 0.0f;
                    gc_state.canned.xyz[plane.axis_1] = 0.0f;
                    gc_state.canned.rapid_retract = On;
                    gc_state.canned.spindle_off = Off;
                }
#if N_AXIS > 3
                if(axis_words.mask & settings.steppers.is_rotary.mask)
                    RETURN(Status_GcodeAxisCommandConflict);
#endif

                if(!gc_block.words.l)
                    gc_block.values.l = 1;
                else if(gc_block.values.l <= 0)
                    RETURN(Status_NonPositiveValue); // [L <= 0]

                if(gc_block.words.r)
                    gc_state.canned.retract_position = gc_block.values.r * (gc_block.modal.units_imperial ? MM_PER_INCH : 1.0f) +
                                                        (gc_block.modal.distance_incremental
                                                          ? gc_state.position[plane.axis_linear]
                                                          : gc_get_block_offset(&gc_block, plane.axis_linear));

                idx = N_AXIS;
                do {
                    if(bit_istrue(axis_words.mask, bit(--idx))) {
                        gc_state.canned.xyz[idx] = gc_block.values.xyz[idx];
                        if(idx != plane.axis_linear)
                            gc_state.canned.xyz[idx] -= gc_state.position[idx];
                        else if(gc_block.modal.distance_incremental)
                            gc_state.canned.xyz[idx] = gc_state.canned.retract_position + (gc_state.canned.xyz[idx] - gc_state.position[idx]);
                    }
                } while(idx);

                if(gc_state.canned.retract_position < gc_state.canned.xyz[plane.axis_linear])
                    RETURN(Status_GcodeInvalidRetractPosition);

                gc_block.words.r = gc_block.words.l = Off; // Remove single-meaning value words.

                switch (gc_block.modal.motion) {

                    case MotionMode_CannedCycle86:
                    case MotionMode_CannedCycle89:
                        gc_state.canned.spindle_off = gc_block.modal.motion == MotionMode_CannedCycle86;
                        gc_state.canned.rapid_retract = gc_block.modal.motion == MotionMode_CannedCycle86;
                        // no break

                    case MotionMode_CannedCycle82:
                    case MotionMode_CannedCycle84:
                        if(gc_block.modal.motion == MotionMode_CannedCycle84) {
                            if(!sspindle->hal->cap.at_speed)
                                RETURN(Status_GcodeUnsupportedCommand);
                            gc_state.canned.rapid_retract = Off;
                        }
                        if(gc_block.words.p) {
                            if(gc_block.values.p < 0.0f)
                                RETURN(Status_NegativeValue);
                            gc_state.canned.dwell = gc_block.values.p;
                            gc_block.words.p = Off; // Remove single-meaning value word.
                        } else if(gc_parser_flags.canned_cycle_change)
                            RETURN(Status_GcodeValueWordMissing);
                        // no break

                    case MotionMode_CannedCycle85:
                    case MotionMode_CannedCycle81:
                        gc_state.canned.delta = - gc_state.canned.xyz[plane.axis_linear] + gc_state.canned.retract_position;
                        if(gc_block.modal.motion == MotionMode_CannedCycle85)
                            gc_state.canned.rapid_retract = Off;
                        break;

                    case MotionMode_DrillChipBreak:
                    case MotionMode_CannedCycle83:
                        if(gc_block.words.q) {
                            if(gc_block.values.q <= 0.0f)
                                RETURN(Status_NegativeValue); // [Q <= 0]
                            gc_state.canned.delta = gc_block.values.q * (gc_block.modal.units_imperial ? MM_PER_INCH : 1.0f);
                            gc_block.words.q = Off; // Remove single-meaning value word.
                        } else if(gc_parser_flags.canned_cycle_change)
                            RETURN(Status_GcodeValueWordMissing);
                        gc_state.canned.dwell = 0.25f;
                        break;

                    default:
                        break;

                } // end switch gc_state.canned.motion
            }

            switch (gc_block.modal.motion) {

                case MotionMode_Linear:
                    // [G1 Errors]: Feed rate undefined. Axis letter not configured or without real value.
                    // Axis words are optional. If missing, set axis command flag to ignore execution.
                    if (!axis_words.mask)
                        axis_command = AxisCommand_None;
                    break;

                case MotionMode_CwArc:
                    gc_parser_flags.arc_is_clockwise = On;
                    // No break intentional.

                case MotionMode_CcwArc:
                    // [G2/3 Errors All-Modes]: Feed rate undefined.
                    // [G2/3 Radius-Mode Errors]: No axis words in selected plane. Target point is same as current.
                    // [G2/3 Offset-Mode Errors]: No axis offsets in selected plane. The radius to the current
                    //   point and the radius to the target point differs more than 0.002mm (EMC def. 0.5mm OR 0.005mm and 0.1% radius).
                    // [G2/3 Full-Circle-Mode Errors]: No offsets programmed. P must be an integer.
                    // NOTE: Both radius and offsets are required for arc tracing and are pre-computed with the error-checking.

                    if(gc_block.words.r) { // Arc Radius Mode
                        if(!axis_words.mask)
                            RETURN(Status_GcodeNoAxisWords); // [No axis words]

                        if(!(axis_words.mask & (bit(plane.axis_0)|bit(plane.axis_1))))
                            RETURN(Status_GcodeNoAxisWordsInPlane); // [No axis words in plane]
                    }

                    if(gc_block.words.p) { // Number of turns
                        if(!isintf(gc_block.values.p))
                            RETURN(Status_GcodeCommandValueNotInteger); // [P word is not an integer]
                        gc_block.arc_turns = (uint32_t)truncf(gc_block.values.p);
                        if(gc_block.arc_turns == 0)
                            RETURN(Status_GcodeValueOutOfRange); // [P word is 0]
                        gc_block.words.p = Off;
                    } else
                        gc_block.arc_turns = 1;

                    // Calculate the change in position along each selected axis
                    float x, y;
                    x = gc_block.values.xyz[plane.axis_0] - gc_state.position[plane.axis_0]; // Delta x between current position and target
                    y = gc_block.values.xyz[plane.axis_1] - gc_state.position[plane.axis_1]; // Delta y between current position and target

                    if(gc_state.modal.scaling_active && scale_factor.ijk[plane.axis_0] * scale_factor.ijk[plane.axis_1] < 0.0f)
                        gc_parser_flags.arc_is_clockwise = !gc_parser_flags.arc_is_clockwise;

                    if(gc_block.words.r) { // Arc Radius Mode

                        gc_block.words.r = Off;

                        if (isequal_position_vector(gc_state.position, gc_block.values.xyz))
                            RETURN(Status_GcodeInvalidTarget); // [Invalid target]

                        // Convert radius value to proper units.
                        if (gc_block.modal.units_imperial)
                            gc_block.values.r *= MM_PER_INCH;

                        if(gc_state.modal.scaling_active)
                            gc_block.values.r *= (scale_factor.ijk[plane.axis_0] > scale_factor.ijk[plane.axis_1]
                                                   ? scale_factor.ijk[plane.axis_0]
                                                   : scale_factor.ijk[plane.axis_1]);

                        /*  We need to calculate the center of the circle that has the designated radius and passes
                             through both the current position and the target position. This method calculates the following
                             set of equations where [x,y] is the vector from current to target position, d == magnitude of
                             that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
                             the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
                             length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
                             [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.

                             d^2 == x^2 + y^2
                             h^2 == r^2 - (d/2)^2
                             i == x/2 - y/d*h
                             j == y/2 + x/d*h

                                                                                  O <- [i,j]
                                                                               -  |
                                                                     r      -     |
                                                                         -        |
                                                                      -           | h
                                                                   -              |
                                                     [0,0] ->  C -----------------+--------------- T  <- [x,y]
                                                               | <------ d/2 ---->|

                             C - Current position
                             T - Target position
                             O - center of circle that pass through both C and T
                             d - distance from C to T
                             r - designated radius
                             h - distance from center of CT to O

                             Expanding the equations:

                             d -> sqrt(x^2 + y^2)
                             h -> sqrt(4 * r^2 - x^2 - y^2)/2
                             i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
                             j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2

                             Which can be written:

                             i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
                             j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2

                             Which we for size and speed reasons optimize to:

                             h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
                             i = (x - (y * h_x2_div_d))/2
                             j = (y + (x * h_x2_div_d))/2
                         */

                        // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
                        // than d. If so, the sqrt of a negative number is complex and error out.
                        float h_x2_div_d = 4.0f * gc_block.values.r * gc_block.values.r - x * x - y * y;

                        if (h_x2_div_d < 0.0f)
                            RETURN(Status_GcodeArcRadiusError); // [Arc radius error] TODO: this will fail due to limited float precision...

                        // Finish computing h_x2_div_d.
                        h_x2_div_d = -sqrtf(h_x2_div_d) / hypot_f(x, y); // == -(h * 2 / d)

                        // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
                        if (gc_block.modal.motion == MotionMode_CcwArc)
                            h_x2_div_d = -h_x2_div_d;

                        /* The counter clockwise circle lies to the left of the target direction. When offset is positive,
                           the left hand circle will be generated - when it is negative the right hand circle is generated.

                                                                               T  <-- Target position

                                                                               ^
                                    Clockwise circles with this center         |          Clockwise circles with this center will have
                                    will have > 180 deg of angular travel      |          < 180 deg of angular travel, which is a good thing!
                                                                     \         |          /
                        center of arc when h_x2_div_d is positive ->  x <----- | -----> x <- center of arc when h_x2_div_d is negative
                                                                               |
                                                                               |

                                                                               C  <-- Current position
                        */
                        // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
                        // even though it is advised against ever generating such circles in a single line of g-code. By
                        // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
                        // travel and thus we get the unadvisably long arcs as prescribed.
                        if (gc_block.values.r < 0.0f) {
                            h_x2_div_d = -h_x2_div_d;
                            gc_block.values.r = -gc_block.values.r; // Finished with r. Set to positive for mc_arc
                        }
                        // Complete the operation by calculating the actual center of the arc
                        gc_block.values.ijk[plane.axis_0] = 0.5f * (x - (y * h_x2_div_d));
                        gc_block.values.ijk[plane.axis_1] = 0.5f * (y + (x * h_x2_div_d));

                    } else { // Arc Center Format Offset Mode

                        if (!(ijk_words.mask & (bit(plane.axis_0)|bit(plane.axis_1))))
                            RETURN(Status_GcodeNoOffsetsInPlane);// [No offsets in plane]

                        gc_block.words.i = gc_block.words.j = gc_block.words.k = Off;

                        // Convert IJK values to proper units.
                        if (gc_block.modal.units_imperial) {
                            idx = 3;
                            do { // Axes indices are consistent, so loop may be used to save flash space.
                                idx--;
                                if (ijk_words.mask & bit(idx))
                                    gc_block.values.ijk[idx] *= MM_PER_INCH;
                            } while(idx);
                        }

                        // Scale values if scaling active - NOTE: only incremental mode is supported
                        if(gc_state.modal.scaling_active) {
                            idx = 3;
                            do {
                                if (ijk_words.mask & bit(--idx))
                                    gc_block.values.ijk[idx] *= scale_factor.ijk[idx];
                            } while(idx);
                        }

#ifdef ROTATION_ENABLE
                        if(gc_block.modal.g5x_offset.data.rotation != 0.0f)
                            rotate((coord_data_t *)gc_block.values.ijk, plane, gc_block.modal.g5x_offset.data.rotation);
#endif

                        // Arc radius from center to target
                        x -= gc_block.values.ijk[plane.axis_0]; // Delta x between circle center and target
                        y -= gc_block.values.ijk[plane.axis_1]; // Delta y between circle center and target
                        float target_r = hypot_f(x, y);

                        // Compute arc radius for mc_arc. Defined from current location to center.
                        gc_block.values.r = hypot_f(gc_block.values.ijk[plane.axis_0], gc_block.values.ijk[plane.axis_1]);

                        // Compute difference between current location and target radii for final error-checks.
                        float delta_r = fabsf(target_r - gc_block.values.r);
                        if (delta_r > 0.005f) {
                            if (delta_r > 0.5f)
                                RETURN(Status_GcodeInvalidTarget); // [Arc definition error] > 0.5mm
                            if (delta_r > (0.001f * gc_block.values.r))
                                RETURN(Status_GcodeInvalidTarget); // [Arc definition error] > 0.005mm AND 0.1% radius
                        }
                    }
                    break;

                case MotionMode_CubicSpline:
                    // [G5 Errors]: Feed rate undefined.
                    // [G5 Plane Errors]: The active plane is not G17.
                    // [G5 Offset Errors]: P and Q are not both specified.
                    // [G5 Offset Errors]: Just one of I or J are specified.
                    // [G5 Offset Errors]: I or J are unspecified in the first of a series of G5 commands.
                    // [G5 Axisword Errors]: An axis other than X or Y is specified.
                    if(gc_block.modal.plane_select != PlaneSelect_XY)
                        RETURN(Status_GcodeIllegalPlane); // [The active plane is not G17]

                    if (axis_words.mask & ~(bit(X_AXIS)|bit(Y_AXIS)))
                        RETURN(Status_GcodeAxisCommandConflict); // [An axis other than X or Y is specified]

                    if((gc_block.words.mask & pq_words.mask) != pq_words.mask)
                        RETURN(Status_GcodeValueWordMissing); // [P and Q are not both specified]

                    if(gc_parser_flags.motion_mode_changed && (gc_block.words.mask & ij_words.mask) != ij_words.mask)
                        RETURN(Status_GcodeValueWordMissing); // [I or J are unspecified in the first of a series of G5 commands]

                    if(!(gc_block.words.i || gc_block.words.j)) {
                        gc_block.values.ijk[I_VALUE] = - gc_block.values.p;
                        gc_block.values.ijk[J_VALUE] = - gc_block.values.q;
                    } else {
                        // Convert I and J values to proper units.
                        if (gc_block.modal.units_imperial) {
                            gc_block.values.ijk[I_VALUE] *= MM_PER_INCH;
                            gc_block.values.ijk[J_VALUE] *= MM_PER_INCH;
                        }
                        // Scale values if scaling active
                        if(gc_state.modal.scaling_active) {
                            gc_block.values.ijk[I_VALUE] *= scale_factor.ijk[X_AXIS];
                            gc_block.values.ijk[J_VALUE] *= scale_factor.ijk[Y_AXIS];
                        }
                    }
                    // Convert P and Q values to proper units.
                    if (gc_block.modal.units_imperial) {
                        gc_block.values.p *= MM_PER_INCH;
                        gc_block.values.q *= MM_PER_INCH;
                    }
                    // Scale values if scaling active
                    if(gc_state.modal.scaling_active) {
                        gc_block.values.p *= scale_factor.ijk[X_AXIS];
                        gc_block.values.q *= scale_factor.ijk[Y_AXIS];
                    }
                    gc_state.modal.spline_pq[X_AXIS] = gc_block.values.p;
                    gc_state.modal.spline_pq[Y_AXIS] = gc_block.values.q;
                    gc_block.words.p = gc_block.words.q = gc_block.words.i = gc_block.words.j = Off;
                    break;

                case MotionMode_QuadraticSpline:
                    // [G5.1 Errors]: Feed rate undefined.
                    // [G5.1 Plane Errors]: The active plane is not G17.
                    // [G5.1 Offset Errors]: Just one of I or J are specified.
                    // [G5.1 Offset Errors]: I or J are unspecified in the first of a series of G5 commands.
                    // [G5.1 Axisword Errors]: An axis other than X or Y is specified.
                    if(gc_block.modal.plane_select != PlaneSelect_XY)
                        RETURN(Status_GcodeIllegalPlane); // [The active plane is not G17]

                    if (axis_words.mask & ~(bit(X_AXIS)|bit(Y_AXIS)))
                        RETURN(Status_GcodeAxisCommandConflict); // [An axis other than X or Y is specified]

                    if((gc_block.words.mask & ij_words.mask) != ij_words.mask)
                        RETURN(Status_GcodeValueWordMissing); // [I or J are unspecified]

                    if(gc_block.values.ijk[I_VALUE] == 0.0f && gc_block.values.ijk[I_VALUE] == 0.0f)
                        RETURN(Status_GcodeValueOutOfRange); // [I or J are zero]

                    // Convert I and J values to proper units.
                    if (gc_block.modal.units_imperial) {
                        gc_block.values.ijk[I_VALUE] *= MM_PER_INCH;
                        gc_block.values.ijk[J_VALUE] *= MM_PER_INCH;
                    }
                    // Scale values if scaling active
                    if(gc_state.modal.scaling_active) {
                        gc_block.values.ijk[I_VALUE] *= scale_factor.ijk[X_AXIS];
                        gc_block.values.ijk[J_VALUE] *= scale_factor.ijk[Y_AXIS];
                    }
                    gc_block.words.i = gc_block.words.j = Off;
                    break;

                case MotionMode_ProbeTowardNoError:
                case MotionMode_ProbeAwayNoError:
                    gc_parser_flags.probe_is_no_error = On;
                    // No break intentional.

                case MotionMode_ProbeToward:
                case MotionMode_ProbeAway:
                    gc_parser_flags.probe_is_away = gc_block.modal.motion == MotionMode_ProbeAway || gc_block.modal.motion == MotionMode_ProbeAwayNoError;
                    // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate
                    //   is undefined. Probe is triggered. NOTE: Probe check moved to probe cycle. Instead of returning
                    //   an error, it issues an alarm to prevent further motion to the probe. It's also done there to
                    //   allow the planner buffer to empty and move off the probe trigger before another probing cycle.
#if CUTTER_COMP_ENABLE
                    if(gc_block.modal.cutter_comp.side)
                        RETURN(Status_CutterCompConflict);
#endif
                    if(!axis_words.mask)
                        RETURN(Status_GcodeNoAxisWords); // [No axis words]

                    if(isequal_position_vector(gc_state.position, gc_block.values.xyz))
                        RETURN(Status_GcodeInvalidTarget); // [Invalid target]

                    if(gc_block.words.p) {
                        if(hal.probe.select) switch((int32_t)gc_block.values.p) {

                            case Probe_Default:
                                gc_block.select_probe = hal.driver_cap.probe;
                                break;

                            case Probe_Toolsetter:
                                gc_block.select_probe = hal.driver_cap.toolsetter;
                                break;

                            case Probe_2:
                                gc_block.select_probe = hal.driver_cap.probe2;
                                break;
                        }
                        if(gc_block.values.p != 0.0f && !gc_block.select_probe)
                            RETURN(Status_GcodeValueOutOfRange); // [Invalid probe]
                        gc_block.words.p = Off;
                    }
                    break;

#if LATHE_UVW_OPTION && NGC_EXPRESSIONS_ENABLE

                case MotionMode_LatheFinishing:
                    if(gc_state.modal.plane_select != PlaneSelect_ZX)
                        RETURN(Status_GcodeIllegalPlane);
                    if(!gc_block.words.q)
                        RETURN(Status_GcodeValueWordMissing); // [No subroutine]
                    if(!gc_block.words.d)
                        gc_block.values.d = 0.0f;
                    if(!gc_block.words.e)
                        gc_block.values.e = 0.0f;
                    if(!gc_block.words.p)
                        gc_block.values.p = 1.0f;
                    gc_block.words.d = gc_block.words.e = gc_block.words.p = gc_block.words.q = Off;
                    break;

                case MotionMode_LatheRoughingZ:
                case MotionMode_LatheRoughingZ1:
                case MotionMode_LatheRoughingZ2:
                case MotionMode_LatheRoughingX:
                case MotionMode_LatheRoughingX1:
                case MotionMode_LatheRoughingX2:
                    if(gc_state.modal.plane_select != PlaneSelect_ZX)
                        RETURN(Status_GcodeIllegalPlane);
                    if(!gc_block.words.q)
                        RETURN(Status_GcodeValueWordMissing); // [No subroutine]
                    if(!gc_block.words.d)
                        gc_block.values.d = 0.0f;
                    if(!gc_block.words.i)
                        gc_block.values.ijk[0] = 1.0f;
                    if(!gc_block.words.r)
                        gc_block.values.r = 0.5f;
                     gc_block.words.d = gc_block.words.i = gc_block.words.q = gc_block.words.r = Off;
                    break;
#endif
                default:
                    break;

            } // end switch gc_block.modal.motion
        }
    }

    // [21. Program flow ]: No error checks required.

    // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc
    // radius mode, or axis words that aren't used in the block.
    if(gc_parser_flags.jog_motion) // Jogging only uses the F feed rate and XYZ value words. N is valid, but S and T are invalid.
        gc_block.words.n = gc_block.words.f = Off;
    else
        gc_block.words.n = gc_block.words.f = gc_block.words.s = gc_block.words.t = Off;

#if CUTTER_COMP_ENABLE || (LATHE_UVW_OPTION && NGC_EXPRESSIONS_ENABLE)
    if(grbl.on_pre_gcode_execute) {
        status_code_t status;
        if(!gc_parser_flags.jog_motion)
            command_words.G1 |= gc_block.modal.motion != MotionMode_None && axis_command == AxisCommand_MotionMode; // TODO: always set?
        if((status = grbl.on_pre_gcode_execute(&command_words, &gc_state, &gc_block, sspindle)) != Status_Unhandled)
            return status;
    }
#endif

    if(axis_command)
        gc_block.words.mask &= ~axis_words_mask.mask; // Remove axis words.

    if(gc_block.words.mask)
        RETURN(Status_GcodeUnusedWords); // [Unused words]

    /* -------------------------------------------------------------------------------------
     STEP 4: EXECUTE!!
     Assumes that all error-checking has been completed and no failure modes exist. We just
     need to update the state and execute the block according to the order-of-execution.
    */

    // Initialize planner data struct for motion blocks.
    plan_line_data_t plan_data = {0};
    plan_data.offset_id = gc_state.offset_id;
    plan_data.overrides = gc_state.modal.override_ctrl;
    plan_data.condition.target_validated = plan_data.condition.target_valid = sys.soft_limits.mask == 0;
#if ENABLE_ACCELERATION_PROFILES
    plan_data.acceleration_factor = gc_state.modal.acceleration_factor;
#endif

    // Intercept jog commands and complete error checking for valid jog commands and execute.
    // NOTE: G-code parser state is not updated, except the position to ensure sequential jog
    // targets are computed correctly. The final parser position after a jog is updated in
    // protocol_execute_realtime() when jogging completes or is canceled.
    if(gc_parser_flags.jog_motion) {

        // Only distance and unit modal commands and G53 absolute override command are allowed.
        // NOTE: Feed rate word and axis word checks have already been performed in STEP 3.
        if(command_words.mask & ~jog_groups.mask)
            RETURN(Status_InvalidJogCommand);

        if(!(gc_block.non_modal_command == NonModal_AbsoluteOverride || gc_block.non_modal_command == NonModal_NoAction))
            RETURN(Status_InvalidJogCommand);

#if N_SYS_SPINDLE > 1
        spindle_t *spindle = sspindle ? sspindle : gc_state.modal.spindle;
#else
        spindle_t *spindle = &gc_block.modal.spindle[0];
#endif

        // Initialize planner data to current spindle and coolant modal state.
        memcpy(&plan_data.spindle, spindle, sizeof(spindle_t));
        plan_data.condition.coolant = gc_state.modal.coolant;
        plan_data.condition.is_rpm_rate_adjusted = gc_state.is_rpm_rate_adjusted || (spindle->state.ccw && spindle->hal->cap.laser);

        if((status_code_t)(int_value = (uint_fast16_t)mc_jog_execute(&plan_data, &gc_block, gc_state.position)) == Status_OK)
            memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_state.position));

        RETURN((status_code_t)int_value);
    }

    bool check_mode = state_get() == STATE_CHECK_MODE;

    if(command_words.G16) switch(gc_block.macro_call) {

        case MacroCall_NonModal:
            RETURN(macro_call((macro_id_t)gc_block.values.p, (line_number_t)gc_block.values.n, gc_block.g65_words, gc_block.values.l));
            break;

#if NGC_PARAMETERS_ENABLE
        case MacroCall_End:
            if(gc_state.g66_args) {
                g66_arguments_t *args = gc_state.g66_args->prev;
                free(gc_state.g66_args);
                gc_state.g66_args = args;
            }
            break;
#endif

        default:
            break;
    }

    // If in laser mode, setup laser power based on current and past parser conditions.
    if(sspindle && sspindle->hal->cap.laser) {

        if(!motion_is_lasercut(gc_block.modal.motion))
            gc_parser_flags.laser_disable = On;

        // Any motion mode with axis words is allowed to be passed from a spindle speed update.
        // NOTE: G1 and G0 without axis words sets axis_command to none. G28/30 are intentionally omitted.
        // TODO: Check sync conditions for M3 enabled motions that don't enter the planner. (zero length).
        if(axis_words.mask && (axis_command == AxisCommand_MotionMode))
            gc_parser_flags.laser_is_motion = On;
        else if(sspindle->state.on && !sspindle->state.ccw) {
            // M3 constant power laser requires planner syncs to update the laser when changing between
            // a G1/2/3 motion mode state and vice versa when there is no motion in the line.
            if(motion_is_lasercut(gc_state.modal.motion)) {
                if(gc_parser_flags.laser_disable)
                    gc_parser_flags.spindle_force_sync = On; // Change from G1/2/3 motion mode.
            } else if(!gc_parser_flags.laser_disable) // When changing to a G1 motion mode without axis words from a non-G1/2/3 motion mode.
                gc_parser_flags.spindle_force_sync = On;
        }

        gc_state.is_rpm_rate_adjusted = sspindle->state.ccw && !gc_parser_flags.laser_disable;
    }

    // [0. Non-specific/common error-checks and miscellaneous setup]:
    // NOTE: If no line number is present, the value is zero.
    plan_data.line_number = gc_state.line_number = (line_number_t)gc_block.values.n; // Record data for planner use.

    // [1. Comments feedback ]: Extracted in protocol.c if HAL entry point provided
    if(message && !check_mode && (plan_data.message = malloc(strlen(message) + 1)))
        strcpy(plan_data.message, message);

    // [2. Set feed rate mode ]:
    if(command_words.G5)
        gc_state.modal.feed_mode = gc_block.modal.feed_mode;

    // [3. Set feed rate ]:
    gc_state.feed_rate = gc_block.values.f; // Always copy this value. See feed rate error-checking.

    // [4. Set spindle speed ]:

#if N_SYS_SPINDLE > 1
  if(sspindle) {
#endif // N_SYS_SPINDLE > 1

    if(sspindle->rpm_mode == SpindleSpeedMode_CSS) {
        if(gc_block.modal.motion != MotionMode_None && gc_block.modal.motion != MotionMode_Seek) {
            sspindle->css = &sspindle->hal->param->css;
            sspindle->css->axis = plane.axis_1;
            sspindle->css->tool_offset = gc_get_offset(plane.axis_1, false);
            float pos = gc_state.position[plane.axis_1] - sspindle->css->tool_offset;
            gc_block.values.s = pos <= 0.0f ? sspindle->css->max_rpm : min(sspindle->css->max_rpm, sspindle->css->surface_speed / (pos * (float)(2.0f * M_PI)));
//??            gc_parser_flags.spindle_force_sync = On;
        } else {
            if(sspindle->css) {
                sspindle->css = NULL;
                protocol_buffer_synchronize(); // Empty planner buffer to ensure we get RPM at end of last CSS motion
            }
            gc_block.values.s = sspindle->rpm; //sspindle.hal->param->rpm; // Keep current RPM
        }
    }

    if(sspindle->rpm != gc_block.values.s || gc_parser_flags.spindle_force_sync) {
        if(sspindle->state.on && !gc_parser_flags.laser_is_motion) {
            sspindle->hal->param->rpm = gc_block.values.s;
            protocol_buffer_synchronize();
            spindle_set_state(sspindle->hal, sspindle->state, gc_parser_flags.laser_disable ? 0.0f : gc_block.values.s);
        }
        sspindle->rpm = gc_block.values.s; // Update spindle speed state.
    }

    // NOTE: Pass zero spindle speed for all restricted laser motions.
    plan_data.spindle.rpm = gc_parser_flags.laser_disable ? 0.0f : gc_block.values.s;

#if N_SYS_SPINDLE > 1
  }
#endif

    //
    // [5. Select tool ]: Only tracks tool value if ATC or manual tool change is not possible.
    if(gc_state.tool_pending != (tool_id_t)gc_block.values.t) {

        gc_state.tool_pending = (tool_id_t)gc_block.values.t;

        if(!check_mode) {

            bool set_current;
            tool_data_t *tool = tool_get_pending(gc_state.tool_pending, NULL);

            // If M6 not available or M61 commanded set new tool immediately
            if((set_current = gc_block.tool_action == ToolAction_Set || (hal.driver_cap.atc
                                                                          ? !hal.tool.change
                                                                          : settings.tool_change.mode == ToolChange_Ignore ||
                                                                             !(hal.stream.suspend_read || hal.tool.change)))) {

                tool_set(tool);

                if(grbl.on_tool_selected) {

                    spindle_state_t state = sspindle ? sspindle->state : (spindle_state_t){0};

                    grbl.on_tool_selected(tool);

                    if(sspindle && state.value != sspindle->state.value) {
                        command_words.M7 = On;
                        gc_block.spindle_modal.state = sspindle->state;
                    }
                }

                gc_tool_changed();
            }

            // Prepare tool carousel when available
            if(hal.tool.select)
                hal.tool.select(tool, !set_current);
            else
                report_add_realtime(Report_Tool);
        }
    }

    // [5a. HAL pin I/O ]: M62 - M68. (Modal group M10)

    if(port_command) {

        switch(port_command) {

            case IoMCode_OutputOnSynced:
            case IoMCode_OutputOffSynced:
                add_output_command(&gc_block.output_command);
                break;

            case IoMCode_OutputOnImmediate:
            case IoMCode_OutputOffImmediate:
                ioport_digital_out(gc_block.output_command.port, gc_block.output_command.value != 0.0f);
                break;

            case IoMCode_WaitOnInput:
                sys.var5399 = ioport_wait_on_input((io_port_type_t)gc_block.output_command.is_digital, gc_block.output_command.port, (wait_mode_t)gc_block.values.l, gc_block.values.q);
                report_add_realtime(Report_M66Result);
                break;

            case IoMCode_AnalogOutSynced:
                add_output_command(&gc_block.output_command);
                break;

            case IoMCode_AnalogOutImmediate:
                ioport_analog_out(gc_block.output_command.port, gc_block.output_command.value);
                break;
        }
    }

    // [6. Change tool ]: Delegated to (possible) driver implementation
    if(command_words.M6 && gc_block.tool_action == ToolAction_Change && !check_mode) {

        pending_tool = tool_get_pending(gc_state.tool_pending, plan_data.message ? NULL : &plan_data.message);

        protocol_buffer_synchronize();

        if(plan_data.message) {
            gc_output_message(plan_data.message);
            plan_data.message = NULL;
        }

        if(pending_tool->tool_id != gc_state.tool->tool_id) {

            if(grbl.on_tool_selected) {

                spindle_state_t state = sspindle ? sspindle->state : (spindle_state_t){0};

                grbl.on_tool_selected(pending_tool);

                if(sspindle && state.value != sspindle->state.value) {
                    command_words.M7 = On;
                    gc_block.spindle_modal.state = sspindle->state;
                }
            }

            bool macro_toolchange = false;

            if(hal.tool.change) { // ATC
                if((int_value = (uint_fast16_t)hal.tool.change(&gc_state)) != Status_OK) {
#if NGC_EXPRESSIONS_ENABLE
                    if(!(macro_toolchange = (int_value == Status_Unhandled)))
#endif
                        RETURN((status_code_t)int_value);
                }
                report_add_realtime(Report_Tool);
            } else { // Manual
                int_value = (uint_fast16_t)Status_OK;
                gc_state.tool_change = true;
                system_set_exec_state_flag(EXEC_TOOL_CHANGE);   // Set up program pause for manual tool change
                protocol_execute_realtime();                    // Execute...
            }

            if(!(macro_toolchange || state_get() == STATE_TOOL_CHANGE))
                gc_tool_changed();
            else if(grbl.tool_table.n_tools && command_words.G8 && gc_block.modal.tool_offset_mode == ToolLengthOffset_Enable) {
                command_words.G8 = Off;
                gc_state.g43_pending = gc_block.values.h > 0.0f ? (tool_id_t)gc_block.values.h : pending_tool->tool_id;
            }
        }
    }

    // [7. Spindle control ]:
    // Update spindle control and apply spindle speed when enabling it in this block.
    // NOTE: All spindle state changes are synced, even in laser mode. Also, plan_data,
    // rather than gc_state, is used to manage laser state for non-laser motions.
    if(command_words.M7) {

        bool spindle_ok;

#if N_SYS_SPINDLE > 1

        if(sspindle == NULL) {

            idx = N_SYS_SPINDLE;
            do {
                spindle_t *sys_spindle = &gc_state.modal.spindle[--idx];
                if(sys_spindle->hal) {

                    if((spindle_ok = sys_spindle->state.value != gc_block.spindle_modal.state.value)) {

                        if((spindle_ok = spindle_set_state_synced(sys_spindle->hal, gc_block.spindle_modal.state, sys_spindle->rpm, sys_spindle->rpm_mode))) {
                            if((sys_spindle->state = sys_spindle->hal->param->state = gc_block.spindle_modal.state).on)
                                sspindle = sys_spindle;
                        }
                    }

                    if((spindle_ok ? false : (!spindle_ok || spindle_event)) && grbl.on_spindle_programmed)
                        grbl.on_spindle_programmed(sys_spindle->hal, sys_spindle->state, sys_spindle->rpm, sys_spindle->rpm_mode);
                }
            } while(idx);

            spindle_event = false;

        } else

#endif // N_SYS_SPINDLE > 1

        if((spindle_ok = sspindle->state.value != gc_block.spindle_modal.state.value)) {

            if((spindle_ok = spindle_set_state_synced(sspindle->hal, gc_block.spindle_modal.state, plan_data.spindle.rpm, sspindle->rpm_mode)))
                sspindle->state = sspindle->hal->param->state = gc_block.spindle_modal.state;

            spindle_event = !spindle_ok;
        }

        sys.override.control.spindle_wait_disable = Off;

        if(spindle_event && grbl.on_spindle_programmed)
            grbl.on_spindle_programmed(sspindle->hal, sspindle->state, plan_data.spindle.rpm, sspindle->rpm_mode);
    }

    if(sspindle != NULL)
        gc_state.spindle = sspindle; // for now

#if NGC_PARAMETERS_ENABLE

    // [7.1 Modal state actions ]:
    switch(gc_block.state_action) {

        case ModalState_Save:
        case ModalState_SaveAutoRestore:;
            gc_override_values_t override = {
                .feed_rate = sys.override.feed_rate,
                .rapid_rate = sys.override.rapid_rate,
            };

            for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
                if(gc_state.modal.spindle[idx].hal)
                    override.spindle_rpm[idx] = gc_state.modal.spindle[idx].hal->param->override_pct;
            }

            if(!ngc_modal_state_save(&gc_state.modal, &override, gc_state.feed_rate, gc_block.state_action == ModalState_SaveAutoRestore))
                RETURN(Status_FlowControlOutOfMemory); // [Out of memory] TODO: allocate memory during validation? Static allocation?
            break;

        case ModalState_Invalidate:
            ngc_modal_state_invalidate();
            break;

        case ModalState_Restore:;
            gc_modal_snapshot_t *snapshot;

            if((snapshot = ngc_modal_state_get())) {

                modal_restore_actions_t *actions = get_state_restore_commands(&gc_block.modal, snapshot);

                gc_state.feed_rate = snapshot->modal.feed_rate;

                if(actions->command.G5)
                    gc_state.modal.feed_mode = gc_block.modal.feed_mode;

                if(actions->command.G8) {

                    actions->command.G8 = Off;
                    system_flag_wco_change();
                    report_add_realtime(Report_ToolOffset);

                    gc_state.modal.tool_offset_mode = gc_block.modal.tool_offset_mode;
                    memcpy(&gc_state.modal.tool_length_offset, &snapshot->modal.tool_length_offset, sizeof(gc_state.modal.tool_length_offset));
                }

                if(actions->command.M9) {
                    actions->command.M9 = Off;
                    plan_feed_override(snapshot->override.feed_rate, snapshot->override.rapid_rate);
                }

                if(actions->command.M8) {
                    actions->command.M8 = Off;
                    coolant_restore((gc_state.modal.coolant = gc_block.modal.coolant), settings.coolant.on_delay);
                }

                for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
                    if(actions->update_spindle[idx]) {
                        memcpy(&gc_state.modal.spindle[idx], &gc_block.modal.spindle[idx], offsetof(spindle_t, hal));
                        if(!spindle_override_disable(gc_state.modal.spindle[idx].hal, bit_istrue(gc_state.modal.override_ctrl.spindle_rpm_disable, bit(idx))))
                            spindle_set_override(gc_state.modal.spindle[idx].hal, snapshot->override.spindle_rpm[idx]);
                        spindle_restore(gc_state.modal.spindle[idx].hal, gc_state.modal.spindle[idx].state, gc_state.modal.spindle[idx].rpm, settings.spindle.on_delay);
                    }
                }

                command_words.mask |= actions->command.mask;
            }
            break;

        default:
            break;
    }

#endif // NGC_PARAMETERS_ENABLE

    // Record data for planner use.

    plan_data.feed_rate = gc_state.feed_rate;

    if(gc_state.modal.feed_mode == FeedMode_InverseTime)
        plan_data.condition.inverse_time = On; // Set condition flag for planner use.

    plan_data.spindle.hal = gc_state.spindle->hal;
    memcpy(&plan_data.spindle, gc_state.spindle, offsetof(spindle_t, rpm)); // Record data for planner use.

// TODO: Recheck spindle running in CCS mode (is_rpm_pos_adjusted == On)?

    plan_data.spindle.state = gc_state.spindle->state; // Set condition flag for planner use.
    plan_data.condition.is_rpm_rate_adjusted = gc_state.is_rpm_rate_adjusted;
    plan_data.condition.is_laser_ppi_mode = gc_state.is_rpm_rate_adjusted && gc_state.is_laser_ppi_mode;

    // [8. Coolant control ]:
    if(command_words.M8) {
    // NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states
    // can exist at the same time, while coolant disable clears all states.
        if(coolant_set_state_synced(gc_block.modal.coolant))
            gc_state.modal.coolant = gc_block.modal.coolant;
    }

    plan_data.condition.coolant = gc_state.modal.coolant; // Set condition flag for planner use.

    sys.override_delay.flags = 0;

    // [9. Override control ]:
    if(command_words.M9) {

        if(gc_state.modal.override_ctrl.feed_rates_disable != gc_block.modal.override_ctrl.feed_rates_disable) {
            sys.override.control.feed_rates_disable = gc_block.modal.override_ctrl.feed_rates_disable;
            plan_feed_override(0, 0);
        }

        gc_state.modal.override_ctrl = gc_block.modal.override_ctrl;

        mc_override_ctrl_update(gc_state.modal.override_ctrl); // NOTE: must be called last!
    }

    // [9a. User defined M commands ]:
    if(gc_block.user_mcode && !check_mode) {

        if(gc_block.user_mcode_sync)
            protocol_buffer_synchronize(); // Ensure user defined mcode is executed when specified in program.

        gc_block.words.mask = user_words.mask;
        gc_block.values.f = single_meaning_value.f;
        gc_block.values.o = single_meaning_value.o;
        gc_block.values.s = single_meaning_value.s;
        gc_block.values.t = single_meaning_value.t;
        grbl.user_mcode.execute(state_get(), &gc_block);
        gc_block.words.mask = 0;
    }

    // [10. Dwell ]:
    if(gc_block.non_modal_command == NonModal_Dwell)
        mc_dwell(gc_block.values.p);

    // [11. Set active plane ]:
    if(command_words.G2)
        gc_state.modal.plane_select = gc_block.modal.plane_select;

    // [12. Set length units ]:
    if(command_words.G6)
        gc_state.modal.units_imperial = gc_block.modal.units_imperial;

#if CUTTER_COMP_ENABLE

    // [13. Cutter radius compensation ]:
    if(command_words.G7) {

        if(!cc_enable(&gc_block.modal.cutter_comp, plane, (coord_data_t *)gc_state.position)) {
            gc_block.modal.cutter_comp.side = CComp_Off;
            RETURN(Status_GcodeUnsupportedCommand);
        }

        memcpy(&gc_state.modal.cutter_comp, &gc_block.modal.cutter_comp, sizeof(gc_ccomp_t));
    }
#else
    // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED
#endif

    // [14. Tool length compensation ]: G43, G43.1 and G49 supported. G43 supported when grbl.tool_table.n_tools > 0.
    // NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms
    // of execution. The error-checking step would simply load the offset value into the correct
    // axis of the block XYZ value array.
    if(command_words.G8)
        tool_offset_apply(gc_block.modal.tool_offset_mode, (tool_id_t)gc_block.values.h, gc_block.values.xyz, axis_words);

    // [15. Coordinate system selection ]:
    if(command_words.G12) {
        memcpy(&gc_state.modal.g5x_offset, &gc_block.modal.g5x_offset, sizeof(coord_system_t));
        report_add_realtime(Report_GWCO);
        system_flag_wco_change();
    }

    // [16. Set path control mode ]: G61.1/G64 NOT SUPPORTED
    // gc_state.modal.control = gc_block.modal.control; // NOTE: Always default.
#if ENABLE_PATH_BLENDING
    if(command_words.G13)
        gc_state.modal.control = gc_block.modal.control;
#endif

    // [17. Set distance mode ]:
    if(command_words.G3)
        gc_state.modal.distance_incremental = gc_block.modal.distance_incremental;

    if(command_words.G15) {
        gc_state.modal.diameter_mode = gc_block.modal.diameter_mode;
        report_add_realtime(Report_LatheXMode);
    }

    // [18. Set retract mode ]:
    if(command_words.G10)
        gc_state.modal.retract_mode = gc_block.modal.retract_mode;

    // [19. Go to predefined position, set G10, or set axis offsets ]:
    switch(gc_block.non_modal_command) {

        case NonModal_Settings:
            if(gc_block.values.l == 2 || gc_block.values.l == 20) {

                settings_write_coord_data(coord_system.id, &coord_system.data);

                // Update system coordinate system if currently active.
                if(gc_state.modal.g5x_offset.id == coord_system.id) {

#ifdef ROTATION_ENABLE

                    if(gc_state.modal.g5x_offset.data.rotation != 0.0f &&
                        gc_state.modal.g5x_offset.data.rotation != coord_system.data.rotation) {

                        gc_state.position[plane.axis_0] -= (gc_state.g92_offset.coord.values[plane.axis_0] + gc_state.modal.tool_length_offset[plane.axis_0]);
                        gc_state.position[plane.axis_1] -= (gc_state.g92_offset.coord.values[plane.axis_1] + gc_state.modal.tool_length_offset[plane.axis_1]);
                        rotate((coord_data_t *)&gc_state.position, plane, -gc_block.modal.g5x_offset.data.rotation);
                        gc_state.position[plane.axis_0] -= gc_block.modal.g5x_offset.data.coord.values[plane.axis_0];
                        gc_state.position[plane.axis_1] -= gc_block.modal.g5x_offset.data.coord.values[plane.axis_1];
                    }

                    memcpy(&gc_state.modal.g5x_offset.data, &coord_system.data, sizeof(coord_system_data_t));

                    if(gc_state.modal.g5x_offset.data.rotation != 0.0f) {
                        gc_state.position[plane.axis_0] += gc_block.modal.g5x_offset.data.coord.values[plane.axis_0];
                        gc_state.position[plane.axis_1] += gc_block.modal.g5x_offset.data.coord.values[plane.axis_1];
                        rotate((coord_data_t *)gc_state.position, plane, gc_state.modal.g5x_offset.data.rotation);
                        gc_state.position[plane.axis_0] += (gc_state.g92_offset.coord.values[plane.axis_0] + gc_state.modal.tool_length_offset[plane.axis_0]);
                        gc_state.position[plane.axis_1] += (gc_state.g92_offset.coord.values[plane.axis_1] + gc_state.modal.tool_length_offset[plane.axis_1]);
                    }
#else
                    memcpy(&gc_state.modal.g5x_offset.data, &coord_system.data, sizeof(coord_system_data_t));
#endif
                    system_flag_wco_change();
                }
            }
            break;

        case NonModal_GoHome_0:
#if N_AXIS > 3
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wimplicit-fallthrough"
#endif
            {
                axes_signals_t wrap = { (axis_words.mask & settings.steppers.is_rotary.mask) & settings.steppers.rotary_wrap.mask };
                if(gc_state.modal.distance_incremental && wrap.mask) {
                    for(idx = Z_AXIS + 1; idx < N_AXIS; idx++) {
                        if(bit_istrue(wrap.mask, bit(idx)) && gc_block.values.xyz[idx] == gc_state.position[idx])
                            gc_block.rotary_wrap.mask |= bit(idx);
                    }
                }
            }
            // No break. Continues to next line.
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#endif // N_AXIS > 3

        case NonModal_GoHome_1:
            // Move to intermediate position before going home. Obeys current coordinate system and offsets
            // and absolute and incremental modes.
            plan_data.condition.rapid_motion = On; // Set rapid motion condition flag.
            if(axis_command)
                mc_line(gc_block.values.xyz, &plan_data);
#if N_AXIS > 3
            if(gc_block.rotary_wrap.mask) {

                coord_data_t wrap_target;

                protocol_buffer_synchronize();
                memcpy(wrap_target.values, coord_system.data.coord.values, sizeof(coord_data_t));

                for(idx = Z_AXIS + 1; idx < N_AXIS; idx++) {
                    if(bit_istrue(gc_block.rotary_wrap.mask, bit(idx))) {
                        float position, delta;
                        if((wrap_target.values[idx] = fmodf(wrap_target.values[idx], 360.0f)) < 0.0f)
                            wrap_target.values[idx] = 360.0f + wrap_target.values[idx];
                        if((position = fmodf(gc_state.position[idx], 360.0f)) < 0.0)
                            position = 360.0f + position;
                        if((delta = position - wrap_target.values[idx]) < -180.0f)
                            position += 360.0f;
                        else if(delta > 180.0f)
                            position -= 360.0f;
                        sys.position[idx] = lroundf(position * settings.axis[idx].steps_per_mm);
                    }
                }

                sync_position();
                mc_line(wrap_target.values, &plan_data);
                protocol_buffer_synchronize();

                for(idx = Z_AXIS + 1; idx < N_AXIS; idx++) {
                    if(bit_istrue(gc_block.rotary_wrap.mask, bit(idx)))
                        sys.position[idx] = lroundf(coord_system.data.coord.values[idx] * settings.axis[idx].steps_per_mm);
                }

                sync_position();
            } else
#endif // N_AXIS > 3
            mc_line(coord_system.data.coord.values, &plan_data);
            memcpy(gc_state.position, coord_system.data.coord.values, sizeof(coord_data_t));
            set_scaling(1.0f);
            break;

        case NonModal_SetHome_0:
            memcpy(coord_system.data.coord.values, gc_state.position, sizeof(coord_data_t));
            settings_write_coord_data(CoordinateSystem_G28, &coord_system.data);
            break;

        case NonModal_SetHome_1:
            memcpy(coord_system.data.coord.values, gc_state.position, sizeof(coord_data_t));
            settings_write_coord_data(CoordinateSystem_G30, &coord_system.data);
            break;

        case NonModal_SetCoordinateOffset: // G92
            add_offset((coord_data_t *)gc_block.values.xyz);
            gc_state.g92_offset_applied = true; // TODO: check for all zero?
            memcpy(gc_state.g92_offset.coord.values, gc_block.values.xyz, sizeof(coord_data_t));
            gc_state.g92_offset_applied = memcmp(gc_state.g92_offset.coord.values, null_vector.values, sizeof(coord_data_t)) != 0;
            if(!settings.flags.g92_is_volatile)
                settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_offset); // Save G92 offsets to non-volatile storage
            break;

        case NonModal_ResetCoordinateOffset: // G92.1
            if(!settings.flags.g92_is_volatile)
                settings_write_coord_data(CoordinateSystem_G92, &(coord_system_data_t){0}); // Save G92 offsets to non-volatile storage
            // No break. Continues to next line.

        case NonModal_ClearCoordinateOffset: // G92.2
            add_offset(&null_vector);
            gc_state.g92_offset_applied = false;
            memcpy(gc_state.g92_offset.coord.values, null_vector.values, sizeof(coord_data_t)); // Disable G92 offsets by zeroing offset vector.
            break;

        case NonModal_RestoreCoordinateOffset:; // G92.3
            coord_system_data_t offset;
            settings_read_coord_data(CoordinateSystem_G92, &offset); // Restore G92 offsets from non-volatile storage
            add_offset(&offset.coord);
            gc_state.g92_offset_applied = memcmp(offset.coord.values, null_vector.values, sizeof(coord_data_t)) != 0;
            memcpy(gc_state.g92_offset.coord.values, offset.coord.values, sizeof(coord_data_t)); // Disable G92 offsets by zeroing offset vector.
            break;

        default:
            break;
    }

    // [20. Motion modes ]:
    // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
    // Enter motion modes only if there are axis words or a motion mode command word in the block.
    gc_state.modal.motion = gc_block.modal.motion;
    gc_state.modal.canned_cycle_active = gc_block.modal.canned_cycle_active;

    if(gc_state.modal.motion != MotionMode_None && axis_command == AxisCommand_MotionMode) {

        plan_data.output_commands = output_commands;
        plan_data.condition.no_feed_override = gc_state.modal.override_ctrl.feed_rates_disable;
#if ENABLE_PATH_BLENDING
        plan_data.cam_tolerance = gc_state.cam_tolerance;
        plan_data.path_tolerance = gc_state.path_tolerance;
#endif
#if PLANNER_ADD_MOTION_MODE
        plan_data.motion_mode = gc_state.modal.motion;
#endif
        pos_update_t gc_update_pos = GCUpdatePos_Target;

        switch(gc_state.modal.motion) {

            case MotionMode_Linear:
                if(gc_state.modal.feed_mode == FeedMode_UnitsPerRev) {
                    plan_data.condition.units_per_rev = On;
                    plan_data.spindle.state.synchronized = settings.mode != Mode_Lathe || gc_block.values.xyz[Z_AXIS] != gc_state.position[Z_AXIS];
                //??    gc_state.distance_per_rev = plan_data.feed_rate;
                    // check initial feed rate - fail if zero?
                }
#if CUTTER_COMP_ENABLE
                {
                    status_code_t status;
                    if((status = grbl.mc_line(gc_block.values.xyz, &plan_data)) != Status_Handled)
                        RETURN(status);
                }
#else
                mc_line(gc_block.values.xyz, &plan_data);
#endif
#if NGC_PARAMETERS_ENABLE
                if((command_words.G16 = gc_state.g66_args && gc_state.g66_args->call_level == ngc_call_level()))
                    gc_block.macro_call = gc_state.g66_args->call;
#endif
                break;

            case MotionMode_Seek:
                plan_data.condition.rapid_motion = On; // Set rapid motion condition flag.
#if CUTTER_COMP_ENABLE
                {
                    status_code_t status;
                    if((status = grbl.mc_line(gc_block.values.xyz, &plan_data)) != Status_Handled)
                        RETURN(status);
                }
#else
                mc_line(gc_block.values.xyz, &plan_data);
#endif
#if NGC_PARAMETERS_ENABLE
                // Run G66 macro?
                if((command_words.G16 = gc_state.g66_args && gc_state.g66_args->call_level == ngc_call_level()))
                    gc_block.macro_call = gc_state.g66_args->call;
#endif
                break;

            case MotionMode_CwArc:
            case MotionMode_CcwArc:
                if(gc_state.modal.feed_mode == FeedMode_UnitsPerRev)
                    plan_data.condition.units_per_rev = plan_data.spindle.state.synchronized = On;
#if CUTTER_COMP_ENABLE
                {
                    status_code_t status;
                    if((status = grbl.mc_arc(gc_block.values.xyz, &plan_data, gc_state.position, gc_block.values.ijk, gc_block.values.r,
                                              plane, gc_parser_flags.arc_is_clockwise ? -gc_block.arc_turns : gc_block.arc_turns)) != Status_Handled)
                        RETURN(status);
                }
#else
                mc_arc(gc_block.values.xyz, &plan_data, gc_state.position, gc_block.values.ijk, gc_block.values.r,
                        plane, gc_parser_flags.arc_is_clockwise ? -gc_block.arc_turns : gc_block.arc_turns);
#endif
                break;

            case MotionMode_CubicSpline:
                {
                    point_2d_t cp1 = {
                        .x = gc_state.position[X_AXIS] + gc_block.values.ijk[X_AXIS],
                        .y = gc_state.position[Y_AXIS] + gc_block.values.ijk[Y_AXIS]
                    };
                    point_2d_t cp2 = {
                        .x = gc_block.values.xyz[X_AXIS] + gc_state.modal.spline_pq[X_AXIS],
                        .y = gc_block.values.xyz[Y_AXIS] + gc_state.modal.spline_pq[Y_AXIS]
                    };
                    mc_cubic_b_spline(gc_block.values.xyz, &plan_data, gc_state.position, cp1.values, cp2.values);
                }
                break;

            case MotionMode_QuadraticSpline:
                {
                    point_2d_t cp1 = {
                        .x = gc_state.position[X_AXIS] + (gc_block.values.ijk[X_AXIS] * 2.0f) / 3.0f,
                        .y = gc_state.position[Y_AXIS] + (gc_block.values.ijk[Y_AXIS] * 2.0f) / 3.0f
                    };
                    point_2d_t cp2 = {
                        .x = gc_block.values.xyz[X_AXIS] + ((gc_state.position[X_AXIS] + gc_block.values.ijk[X_AXIS] - gc_block.values.xyz[X_AXIS]) * 2.0f) / 3.0f,
                        .y = gc_block.values.xyz[Y_AXIS] + ((gc_state.position[Y_AXIS] + gc_block.values.ijk[Y_AXIS] - gc_block.values.xyz[Y_AXIS]) * 2.0f) / 3.0f
                    };
                    mc_cubic_b_spline(gc_block.values.xyz, &plan_data, gc_state.position, cp1.values, cp2.values);
                }
                break;

            case MotionMode_SpindleSynchronized:
                {
                    protocol_buffer_synchronize(); // Wait until any previous moves are finished.

                    gc_override_flags_t overrides = sys.override.control; // Save current override disable status.

                    status_code_t status = init_sync_motion(&plan_data, gc_block.values.k);
                    if(status != Status_OK)
                        RETURN(status);

                    plan_data.spindle.state.synchronized = On;
                    plan_data.overrides.feed_hold_disable = plan_data.condition.no_feed_override = On; // Disable feed hold and override.

                    mc_line(gc_block.values.xyz, &plan_data);

                    mc_override_ctrl_update(overrides);  // Wait until synchronized move is finished, then restore previous override disable status.
                }
                break;

            case MotionMode_RigidTapping:
                {
                    status_code_t status;
                    gc_override_flags_t overrides = sys.override.control; // Save current override disable status.

                    protocol_buffer_synchronize(); // Wait until any previous moves are finished.

                    if((status = init_sync_motion(&plan_data, gc_block.values.k)) == Status_OK)
                        status = mc_rigid_tapping(&plan_data, (coord_data_t *)gc_block.values.xyz, (coord_data_t *)gc_state.position, gc_block.values.k, gc_block.values.ijk[I_VALUE]);

                  //  if(status != Status_GcodeUnsupportedCommand)
                        mc_override_ctrl_update(overrides);  // Wait until synchronized move is finished, then restore previous override disable status.
#if NGC_PARAMETERS_ENABLE
                    if(status != Status_GcodeUnsupportedCommand) {

                        if(!ngc_call_push(&gc_state + ngc_call_level()))
                            RETURN(Status_FlowControlStackOverflow); // [Call level too deep]
#ifdef A_AXIS
                        parameter_words_t g33_1_words = (parameter_words_t){ .i = On, .k = On, .x = axis_words.x, .y = axis_words.y, .z = axis_words.z, .a = axis_words.a };
#else
                        parameter_words_t g33_1_words = (parameter_words_t){ .i = On, .k = On, .x = axis_words.x, .y = axis_words.y, .z = axis_words.z };
#endif
                        g33_1_words.mask = macro_arguments_push(&gc_block.values, g33_1_words, NULL).mask;
                        RETURN(macro_call(331, (line_number_t)gc_block.values.n, g33_1_words, 1));
                    }
#endif
                }
                break;

            case MotionMode_Threading:
                {
                    protocol_buffer_synchronize(); // Wait until any previous moves are finished.

                    gc_override_flags_t overrides = sys.override.control; // Save current override disable status.

                    status_code_t status = init_sync_motion(&plan_data, thread.pitch);
                    if(status != Status_OK)
                        RETURN(status);

                    mc_thread(&plan_data, gc_state.position, &thread, overrides.feed_hold_disable);

                    mc_override_ctrl_update(overrides); // Wait until synchronized move is finished, then restore previous override disable status.
                }
                break;

#if LATHE_UVW_OPTION && NGC_EXPRESSIONS_ENABLE

            case MotionMode_LatheFinishing:
                {
                    lathe_cycle_arguments_t args = {
                       .motion = gc_block.modal.motion,
                       .x = gc_block.values.xyz[X_AXIS],
                       .z = gc_block.values.xyz[Z_AXIS],
                       .finish = {
                           .start_distance = gc_block.values.d,
                           .end_distance = gc_block.values.e,
                           .passes = gc_block.values.p
                       }
                    };
                    RETURN(mc_lathe_cycle(&plan_data, (coord_data_t *)gc_state.position, (uint32_t)gc_block.values.q, &args));
                }
                break;

            case MotionMode_LatheRoughingZ:
            case MotionMode_LatheRoughingZ1:
            case MotionMode_LatheRoughingZ2:
            case MotionMode_LatheRoughingX:
            case MotionMode_LatheRoughingX1:
            case MotionMode_LatheRoughingX2:
                {
                    lathe_cycle_arguments_t args = {
                       .motion = gc_block.modal.motion,
                       .x = gc_block.values.xyz[X_AXIS],
                       .z = gc_block.values.xyz[Z_AXIS],
                       .rough = {
                           .increment = gc_block.values.ijk[0],
                           .retract_distance = gc_block.values.r,
                           .remaining_distance = gc_block.values.d
                       }
                    };
                    RETURN(mc_lathe_cycle(&plan_data, (coord_data_t *)gc_state.position, (uint32_t)gc_block.values.q, &args));
                }
                break;
#endif

            case MotionMode_DrillChipBreak:
            case MotionMode_CannedCycle81:
            case MotionMode_CannedCycle82:
            case MotionMode_CannedCycle83:
            case MotionMode_CannedCycle84:;

                overrides_t overrides = {};

                if(gc_state.modal.motion == MotionMode_CannedCycle84)
                    override_disable(&plan_data.spindle, spindle_id, (gc_override_flags_t){ .feed_hold = On, .feed_rates = On, .spindle_rpm = On }, &overrides);

                plan_data.spindle.rpm = gc_block.values.s;
                gc_state.canned.retract_mode = gc_state.modal.retract_mode;
                mc_canned_drill(gc_state.modal.motion, gc_block.values.xyz, &plan_data, gc_state.position, plane, gc_block.values.l, &gc_state.canned);

                if(gc_state.modal.motion == MotionMode_CannedCycle84)
                    override_restore(&plan_data.spindle, spindle_id, (gc_override_flags_t){ .feed_hold = On, .feed_rates = On, .spindle_rpm = On }, &overrides);
                break;

            case MotionMode_ProbeToward:
            case MotionMode_ProbeTowardNoError:
            case MotionMode_ProbeAway:
            case MotionMode_ProbeAwayNoError:
                {
                    // NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So
                    // upon a successful probing cycle, the machine position and the returned value should be the same.
                    probe_id_t probe_id = Probe_Default; // initialized to shut up compiler warning
                    plan_data.condition.no_feed_override = !settings.probe.allow_feed_override;
                    if(gc_block.select_probe){
                        if((gc_block.select_probe = (probe_id_t)gc_block.values.p != (probe_id = hal.probe.get_state().probe_id))) {
                            hal.probe.select((probe_id_t)gc_block.values.p);
                            report_add_realtime(Report_ProbeId);
                        }
                    }
                    gc_update_pos = (pos_update_t)mc_probe_cycle(gc_block.values.xyz, &plan_data, gc_parser_flags);
                    if(gc_block.select_probe) {
                        hal.probe.select(probe_id);
                        report_add_realtime(Report_ProbeId);
                    }
                }
                break;

            default:
                break;
        }

        output_commands = plan_data.output_commands;

        // Do not update position on cancel (already done in protocol_exec_rt_system)
        if(sys.cancel)
            gc_update_pos = GCUpdatePos_None;

        // As far as the parser is concerned, the position is now == target. In reality the
        // motion control system might still be processing the action and the real tool position
        // in any intermediate location.
        if (gc_update_pos == GCUpdatePos_Target)
            memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_state.position)); // gc_state.position[] = gc_block.values.xyz[]
        else if (gc_update_pos == GCUpdatePos_System)
            gc_sync_position(); // gc_state.position[] = sys.position
        // == GCUpdatePos_None
    }

    if(command_words.G16) switch(gc_block.macro_call) {

#if NGC_PARAMETERS_ENABLE
        case MacroCall_Modal:
        case MacroCall_Modal1:
            if(!ngc_call_push(&gc_state + ngc_call_level()))
                RETURN(Status_FlowControlStackOverflow); // [Call level too deep]

            macro_arguments_push(&gc_state.g66_args->values, gc_state.g66_args->words, NULL);

            RETURN(macro_call((macro_id_t)gc_state.g66_args->values.p, (line_number_t)gc_block.values.n, gc_state.g66_args->words, gc_state.g66_args->values.l));
            break;
#endif

        case MacroCall_NonModal98:
            if(check_mode) {
                RETURN(macro_add((macro_id_t)gc_block.values.p, (line_number_t)gc_block.values.n, hal.stream.file));
            } else {
                RETURN(macro_call((macro_id_t)gc_block.values.p, (line_number_t)gc_block.values.n, (parameter_words_t){ .$ = On}, gc_block.values.l));
            }
            break;

        default:
            break;
    }

    if(plan_data.message)
        gc_output_message(plan_data.message);

    // [21. Program flow ]:
    // M0,M1,M2,M30,M60: Perform non-running program flow actions. During a program pause, the buffer may
    // refill and can only be resumed by the cycle start run-time command.
    if((gc_state.modal.program_flow = gc_block.modal.program_flow) || sys.flags.single_block) {

        protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on.

        if(gc_state.modal.program_flow == ProgramFlow_Return) {
            if(grbl.on_macro_return)
                grbl.on_macro_return();
            else if(grbl.on_program_completed)
                grbl.on_program_completed(gc_state.modal.program_flow, check_mode);
        } else if(gc_state.modal.program_flow == ProgramFlow_Paused || gc_block.modal.program_flow == ProgramFlow_OptionalStop || gc_block.modal.program_flow == ProgramFlow_CompletedM60 || sys.flags.single_block) {

            status_code_t status = grbl.on_program_paused ? grbl.on_program_paused(gc_state.modal.program_flow, check_mode) : Status_Unhandled;
            if(!(status == Status_Handled || status == Status_Unhandled)) {
                gc_state.modal.program_flow = ProgramFlow_Running; // Reset program flow.
                RETURN(status);
            }

            if(!check_mode) {
                if(gc_block.modal.program_flow == ProgramFlow_CompletedM60 && hal.pallet_shuttle)
                    hal.pallet_shuttle();

                if(status == Status_Unhandled) {
                    system_set_exec_state_flag(EXEC_FEED_HOLD); // Use feed hold for program pause.
                    protocol_execute_realtime();                // Execute suspend.
                }
            }
        } else { // == ProgramFlow_Completed
            // Upon program complete, only a subset of g-codes reset to certain defaults, according to
            // LinuxCNC's program end descriptions and testing. Only modal groups [G-code 1,2,3,5,7,12]
            // and [M-code 7,8,9] reset to [G1,G17,G90,G94,G40,G54,M5,M9,M48]. The remaining modal groups
            // [G-code 4,6,8,10,13,14,15] and [M-code 4,5,6] and the modal words [F,S,T,H] do not reset.

            if(!check_mode && gc_block.modal.program_flow == ProgramFlow_CompletedM30 && hal.pallet_shuttle)
                hal.pallet_shuttle();

            gc_state.file_run = false;
            gc_state.modal.motion = MotionMode_Linear;
            gc_block.modal.canned_cycle_active = false;
            gc_state.modal.plane_select = PlaneSelect_XY;
//            gc_state.modal.plane_select = settings.flags.lathe_mode ? PlaneSelect_ZX : PlaneSelect_XY;
            gc_state.modal.distance_incremental = false;
            gc_state.modal.feed_mode = FeedMode_UnitsPerMin;
// TODO: check           gc_state.distance_per_rev = 0.0f;

#if CUTTER_COMP_ENABLE
            gc_state.ccomp_off = false;
            gc_state.modal.cutter_comp.side = CComp_Off;
            cc_enable(&gc_state.modal.cutter_comp, plane, (coord_data_t *)gc_state.position);
#endif
            if(gc_state.modal.g5x_offset.id != CoordinateSystem_G54) {
                gc_state.modal.g5x_offset.id = CoordinateSystem_G54;
                report_add_realtime(Report_GWCO);
            }
            gc_state.modal.coolant = (coolant_state_t){0};
            gc_state.modal.override_ctrl.feed_rates_disable = Off;
            gc_state.modal.override_ctrl.spindle_rpm_disable = Off;
#if ENABLE_ACCELERATION_PROFILES
            gc_state.modal.acceleration_factor = gc_get_accel_factor(0);
#endif
            for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
                if(gc_state.modal.spindle[idx].hal) {
                    gc_state.modal.spindle[idx].css = NULL;
                    gc_state.modal.spindle[idx].state = (spindle_state_t){0};
                    gc_state.modal.spindle[idx].rpm_mode = SpindleSpeedMode_RPM; // NOTE: not compliant with linuxcnc (?)
                    gc_state.modal.spindle[idx].hal->param->state.override_disable = Off;
#if N_SYS_SPINDLE > 1
                    if(settings.flags.restore_overrides)
                        gc_state.modal.spindle[idx].hal->param->override_pct = DEFAULT_SPINDLE_RPM_OVERRIDE;
#endif
                }
            }
#if N_SYS_SPINDLE == 1
            if(settings.flags.restore_overrides)
                sspindle->hal->param->override_pct = DEFAULT_SPINDLE_RPM_OVERRIDE;
#endif
            if(settings.parking.flags.enabled)
                gc_state.modal.override_ctrl.parking_disable = settings.parking.flags.enable_override_control &&
                                                                settings.parking.flags.deactivate_upon_init;
            sys.override.control = gc_state.modal.override_ctrl;

            if(settings.flags.restore_overrides)
                plan_feed_override(DEFAULT_FEED_OVERRIDE, DEFAULT_RAPID_OVERRIDE);

            // Execute coordinate change and spindle/coolant stop.
            if(!check_mode) {

                if(!(settings_read_coord_data(gc_state.modal.g5x_offset.id, &gc_state.modal.g5x_offset.data))) {
                    gc_state.modal.program_flow = ProgramFlow_Running; // Reset program flow.
                    RETURN(Status_SettingReadFail);
                }

#if COMPATIBILITY_LEVEL <= 1
                if(!settings.flags.g92_is_volatile) {
                    coord_system_data_t g92_offset_stored;
                    if(settings_read_coord_data(CoordinateSystem_G92, &g92_offset_stored) && !isequal_position_vector(g92_offset_stored.coord.values, gc_state.g92_offset.coord.values))
                        settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_offset); // Save G92 offsets to non-volatile storage
                }
#endif

                system_flag_wco_change(); // Set to refresh immediately just in case something altered.

                spindle_all_off(false);
                hal.coolant.set_state(gc_state.modal.coolant);
                report_add_realtime(Report_Spindle); // Set to report change
                report_add_realtime(Report_Coolant); // immediately.
            }

            if(grbl.on_program_completed)
                grbl.on_program_completed(gc_state.modal.program_flow, check_mode);

            // Clear any pending output commands etc...
            gc_at_exit(hal.stream.state.m98_macro_prescan ? Status_Handled : Status_UserException);

#if NGC_PARAMETERS_ENABLE
            ngc_modal_state_invalidate();
#endif

            if(!check_mode || !settings.flags.m98_prescan_enable)
                grbl.report.feedback_message(Message_ProgramEnd);
        }
        gc_state.modal.program_flow = ProgramFlow_Running; // Reset program flow.
    }

#if NGC_EXPRESSIONS_ENABLE
    if(ngc_param_count) do {
        ngc_param_count--;
        ngc_param_set(ngc_params[ngc_param_count].id, ngc_params[ngc_param_count].value);
    } while(ngc_param_count);
#endif

    // TODO: % to denote start of program.

    return Status_OK;
}