Queue depth gate: eliminate velocity spike at segment activation

Extend the RT queue depth gate to all non-EXACT segments, not just
id==0. When a segment is alone in the queue, its profile is computed
with v_exit=0 (no successor yet). Previously, RT would activate it
immediately, and the optimizer's active-segment skip prevented
correction when the successor arrived — causing a velocity spike at
the junction.

The gate holds activation until queue_len >= 2 or 20ms timeout,
giving the optimizer time to recompute with the correct exit velocity
while the segment is still inactive. EXACT segments (last of program)
skip the gate since their v_exit=0 is always correct.

Also removes investigation debug probes (FIX4_DBG, STUCK_FORCE).

Author: grandixximo

src/emc/motion_planning/motion_planning_9d.cc

@@ -4339,16 +4339,6 @@ int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimiza
         }
         if (is_first_profile && v_exit > 0.0) {
             v_exit = 0.0;
-            // Debug: log Fix 4 pessimistic override
-            {
-                static int fix4_dbg_count = 0;
-                if (fix4_dbg_count < 30) {
-                    fix4_dbg_count++;
-                    rtapi_print_msg(RTAPI_MSG_ERR,
-                        "FIX4_DBG[id=%d]: first_profile v_exit forced 0 (was %.3f) type=%d target=%.3f\n",
-                        tc->id, atomicLoadDouble(&tc->shared_9d.final_vel), tc->motion_type, tc->target);
-                }
-            }
         }
 
         // prev_exit_vel is updated AFTER Ruckig compute (below) to use
@@ -4449,8 +4439,6 @@ int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimiza
                     prev_exit_feed_scale = feed_scale;
                     prev_exit_vel_known = true;
 
-
-
                     // One-step backtrack: fix backward reachability gap
                     // Skip active segment — rewriting triggers STOPWATCH_RESET
                     // with velocity discontinuity worse than the chain gap.

src/emc/tp/tp.c

@@ -4557,11 +4557,6 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc,
 
         if (stuck_cycles > 10) {  // Stuck for 10ms
             // Force to target
-            printf("STUCK_FORCE seg=%d progress=%.6f target=%.6f dx=%.6f "
-                   "vel=%.6f elapsed=%.6f dur=%.6f pos_base=%.6f\n",
-                   tc->id, tc->progress, tc->target, dx,
-                   tc->currentvel, tc->elapsed_time,
-                   tc->shared_9d.profile.duration, tc->position_base);
             dx = 0.0;
             tc->progress = tc->target;
             stuck_cycles = 0;
@@ -5024,7 +5019,7 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp,
     //Run with full cycle time
     tc_debug_print("Normal cycle\n");
     tc->cycle_time = tp->cycleTime;
-    
+
     int mode = 0;
     tpUpdateCycle(tp, tc, nexttc, &mode);
 
@@ -5133,24 +5128,35 @@ int tpRunCycle(TP_STRUCT * const tp, long period)
             profile_wait_count = 0;
         }
 
-        /* Only delay on the FIRST segment of a program (id == 0).
-         * Without the id check, this also triggers on the LAST segment
-         * (which also has progress=0, queue_len=1, nexttc=NULL) causing
-         * a 100-cycle delay before the final move executes. */
-        if (tc->id == 0 && tc->progress < TP_POS_EPSILON && queue_len < 2 && nexttc == NULL) {
-            static int startup_wait_count = 0;
-            startup_wait_count++;
+        /* Queue depth gate: wait for at least 2 segments before activating.
+         * When a segment is alone in the queue (queue_len < 2), its profile
+         * may have been computed with v_exit=0 (no successor/blend yet).
+         * The gate holds RT until the interpreter adds the next segment,
+         * giving the optimizer a chance to recompute with the correct
+         * exit velocity.  The segment is NOT active during the gate,
+         * so the optimizer's active-segment skip doesn't prevent correction.
+         *
+         * Skip the gate for EXACT segments (last segment of program) —
+         * their v_exit=0 is always correct, no correction needed. */
+        if (tc->progress < TP_POS_EPSILON && queue_len < 2 && nexttc == NULL
+                && tc->term_cond != TC_TERM_COND_EXACT) {
+            static int gate_seg_id = -1;
+            static int gate_wait_count = 0;
+            /* Reset counter for each new segment */
+            if (tc->id != gate_seg_id) {
+                gate_seg_id = tc->id;
+                gate_wait_count = 0;
+            }
+            gate_wait_count++;
 
-            /* Wait up to 100 cycles (100ms) for more segments */
-            if (startup_wait_count < 100) {
+            /* Wait up to 20 cycles (20ms at 1kHz) for a successor */
+            if (gate_wait_count < 20) {
                 return TP_ERR_WAITING;
             }
-            startup_wait_count = 0;  // Reset for next program
+            gate_wait_count = 0;
         }
     }
 
-    /* Removed verbose segment debug - now proven to work */
-
     tc_debug_print("-------------------\n");
 
     /* Apply pre-computed 9D optimizer velocities if planner_type == 2