diff --git a/uCNC/src/cnc.c b/uCNC/src/cnc.c
index a7c702d30..40a42ad48 100644
--- a/uCNC/src/cnc.c
+++ b/uCNC/src/cnc.c
@@ -47,7 +47,7 @@ typedef struct
 	volatile int8_t alarm;
 } cnc_state_t;
 
-static bool lock_itp = false;
+static bool cnc_lock_itp = false;
 static cnc_state_t cnc_state;
 bool cnc_status_report_lock;
 
@@ -274,11 +274,11 @@ bool cnc_dotasks(void)
 	}
 #endif
 
-	if (!lock_itp)
+	if (!cnc_lock_itp)
 	{
-		lock_itp = true;
+		cnc_lock_itp = true;
 		itp_run();
-		lock_itp = false;
+		cnc_lock_itp = false;
 	}
 
 	return !cnc_get_exec_state(EXEC_KILL);
@@ -310,7 +310,7 @@ void cnc_store_motion(void)
 		cnc_clear_exec_state(EXEC_HOLD);
 	}
 
-	lock_itp = false;
+	cnc_lock_itp = false;
 #endif
 }
 
@@ -343,7 +343,7 @@ void cnc_restore_motion(void)
 	{
 		cnc_clear_exec_state(EXEC_HOLD);
 	}
-	lock_itp = false;
+	cnc_lock_itp = false;
 #endif
 }
 
@@ -595,13 +595,13 @@ void cnc_clear_exec_state(uint8_t statemask)
 	CLEARFLAG(cnc_state.exec_state, statemask);
 }
 
-void cnc_delay_ms(uint32_t miliseconds)
+void cnc_delay_ms(uint32_t milliseconds)
 {
-	uint32_t t_start = mcu_millis();
-	while ((mcu_millis() - t_start) < miliseconds)
+	milliseconds += mcu_millis();
+	do
 	{
 		cnc_dotasks();
-	}
+	} while (mcu_millis() < milliseconds);
 }
 
 void cnc_reset(void)
diff --git a/uCNC/src/core/interpolator.c b/uCNC/src/core/interpolator.c
index 523de65dd..1dc8a7c4c 100644
--- a/uCNC/src/core/interpolator.c
+++ b/uCNC/src/core/interpolator.c
@@ -475,6 +475,7 @@ void itp_run(void)
 			if (accel_until == remaining_steps)
 			{
 				itp_cur_plan_block->entry_feed_sqr = junction_speed_sqr;
+				current_speed = fast_flt_sqrt(junction_speed_sqr);
 			}
 
 			if (junction_speed_sqr > exit_speed_sqr)
@@ -553,12 +554,6 @@ void itp_run(void)
 			sgm->flags = ITP_UPDATE_ISR | ITP_DEACCEL;
 		}
 
-		// update speed at the end of segment
-		if (speed_change)
-		{
-			itp_cur_plan_block->entry_feed_sqr = MAX(0, fast_flt_pow2((current_speed + speed_change)));
-		}
-
 		/*
 			common calculations for all three profiles (accel, constant and deaccel)
 		*/
@@ -571,6 +566,11 @@ void itp_run(void)
 			partial_distance += current_speed * integrator;
 			// computes how many steps it will perform at this speed and frame window
 			segm_steps = (uint16_t)floorf(partial_distance);
+			// update speed at the end of segment
+			if (speed_change)
+			{
+				itp_cur_plan_block->entry_feed_sqr = fast_flt_pow2(current_speed);
+			}
 		}
 		else
 		{
diff --git a/uCNC/src/core/parser.c b/uCNC/src/core/parser.c
index e0854fc8d..5e369d5d2 100644
--- a/uCNC/src/core/parser.c
+++ b/uCNC/src/core/parser.c
@@ -184,7 +184,11 @@ void parser_get_modes(uint8_t *modalgroups, uint16_t *feed, uint16_t *spindle, u
 	*spindle = (uint16_t)ABS(parser_state.spindle);
 	*coolant = parser_state.groups.coolant;
 	modalgroups[9] = (parser_state.groups.coolant == M9) ? 9 : MIN(parser_state.groups.coolant + 6, 8);
+#if TOOL_COUNT > 1
 	modalgroups[11] = parser_state.tool_index;
+#else
+	modalgroups[11] = 1;
+#endif
 #else
 	modalgroups[8] = 5;
 	modalgroups[9] = 9;
@@ -1052,7 +1056,7 @@ static uint8_t parser_validate_command(parser_state_t *new_state, parser_words_t
 
 // RS274NGC v3 - 3.7 Other Input Codes
 // Words S and T must be positive
-#if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 	if (words->s < 0 || words->t < 0)
 	{
 		return STATUS_NEGATIVE_VALUE;
@@ -1180,6 +1184,7 @@ uint8_t parser_exec_command(parser_state_t *new_state, parser_words_t *words, pa
 		new_state->spindle = (uint16_t)trunc(words->s);
 	}
 
+#if TOOL_COUNT > 1
 	// 5. select tool
 	if (CHECKFLAG(cmd->words, GCODE_WORD_T))
 	{
@@ -1197,6 +1202,7 @@ uint8_t parser_exec_command(parser_state_t *new_state, parser_words_t *words, pa
 		tool_change(words->t);
 		new_state->tool_index = new_state->groups.tool_change;
 	}
+#endif
 
 	// 7. spindle on/rev/off (M3/M4/M5)
 	block_data.spindle = new_state->spindle;
@@ -2293,9 +2299,11 @@ static uint8_t parser_mcode_word(uint8_t code, uint8_t mantissa, parser_state_t
 		code = (code == 5) ? M5 : code - 2;
 		new_state->groups.spindle_turning = code;
 		break;
+#if TOOL_COUNT > 1
 	case 6:
 		new_group |= GCODE_GROUP_TOOLCHANGE;
 		break;
+#endif
 #if ASSERT_PIN(COOLANT_MIST)
 	case 7:
 #endif
@@ -2568,7 +2576,9 @@ void parser_reset(bool stopgroup_only)
 	parser_state.groups.coolant = M9;		  // M9
 	parser_state.groups.spindle_turning = M5; // M5
 	parser_state.groups.tool_change = 1;
+#if TOOL_COUNT > 1
 	parser_state.tool_index = g_settings.default_tool;
+#endif
 	parser_state.groups.path_mode = G61;
 #endif
 	parser_state.groups.motion = G1;											   // G1
diff --git a/uCNC/src/core/parser.h b/uCNC/src/core/parser.h
index 08b0fc599..28aafa3af 100644
--- a/uCNC/src/core/parser.h
+++ b/uCNC/src/core/parser.h
@@ -285,7 +285,9 @@ extern "C"
 		parser_groups_t groups;
 		float feedrate;
 #if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 		uint8_t tool_index;
+#endif
 		uint16_t spindle;
 #endif
 #ifdef GCODE_PROCESS_LINE_NUMBERS
diff --git a/uCNC/src/hal/tools/tool.c b/uCNC/src/hal/tools/tool.c
index 3bfbf0d74..80724821d 100644
--- a/uCNC/src/hal/tools/tool.c
+++ b/uCNC/src/hal/tools/tool.c
@@ -82,13 +82,21 @@ void tool_init(void)
 #ifdef FORCE_GLOBALS_TO_0
 	memset(&tool_current, 0, sizeof(tool_t));
 #endif
+#if TOOL_COUNT > 1
 	tool_change(g_settings.default_tool);
+#else
+	memcpy(&tool_current, &TOOL1, sizeof(tool_t));
+	if (tool_current.startup_code)
+	{
+		tool_current.startup_code();
+	}
+#endif
 #endif
 }
 
 void tool_change(uint8_t tool)
 {
-#if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 	tool_stop();
 	if (tool_current.shutdown_code)
 	{
diff --git a/uCNC/src/interface/protocol.c b/uCNC/src/interface/protocol.c
index 203fe7269..f5b538253 100644
--- a/uCNC/src/interface/protocol.c
+++ b/uCNC/src/interface/protocol.c
@@ -670,7 +670,9 @@ void protocol_send_cnc_settings(void)
 #endif
 
 #if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 	protocol_send_gcode_setting_line_int(80, g_settings.default_tool);
+#endif
 	for (uint8_t i = 0; i < TOOL_COUNT; i++)
 	{
 		protocol_send_gcode_setting_line_flt(81 + i, g_settings.tool_length_offset[i]);
diff --git a/uCNC/src/interface/settings.c b/uCNC/src/interface/settings.c
index ac78ae880..b3aa00ceb 100644
--- a/uCNC/src/interface/settings.c
+++ b/uCNC/src/interface/settings.c
@@ -122,7 +122,9 @@ const settings_t __rom__ default_settings =
 		.acceleration = DEFAULT_ACCEL_PER_AXIS,
 		.max_distance = DEFAULT_MAX_DIST_PER_AXIS,
 #if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 		.default_tool = DEFAULT_STARTUP_TOOL,
+#endif
 		.tool_length_offset = DEFAULT_ARRAY(TOOL_COUNT, 0),
 #endif
 #if (KINEMATIC == KINEMATIC_LINEAR_DELTA)
@@ -493,7 +495,7 @@ uint8_t settings_change(setting_offset_t id, float value)
 			break;
 #endif
 #endif
-#if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 		case 80:
 			g_settings.default_tool = CLAMP(0, value8, (uint8_t)TOOL_COUNT);
 			break;
diff --git a/uCNC/src/interface/settings.h b/uCNC/src/interface/settings.h
index 3cd9149f0..dd8bcbb35 100644
--- a/uCNC/src/interface/settings.h
+++ b/uCNC/src/interface/settings.h
@@ -72,7 +72,9 @@ extern "C"
 		float acceleration[STEPPER_COUNT];
 		float max_distance[AXIS_COUNT];
 #if TOOL_COUNT > 0
+#if TOOL_COUNT > 1
 		uint8_t default_tool;
+#endif
 		float tool_length_offset[TOOL_COUNT];
 #endif
 #if (KINEMATIC == KINEMATIC_LINEAR_DELTA)