FINALIZED gate: prevent segment activation before boundaries known

Replace the old profile-valid + queue-depth gates with a single
FINALIZED gate that checks optimization_state >= TC_PLAN_FINALIZED
before allowing RT to activate any segment.

- Forward pass only stamps FINALIZED when exit boundary conditions
  are known: EXACT segments (vf=0 always correct), segments with a
  successor in queue, or tail segments when queue is sealed.
- SKIP path re-stamps FINALIZED after backward pass knocks state
  back to SMOOTHED.
- queue_sealed flag in TP_STRUCT: set by tpFlushCompressor_9D at
  sync points (dwell, mode change, program end), cleared by
  tpAddLine_9D/tpAddCircle_9D when new motion arrives. Lets the
  optimizer finalize the tail segment immediately instead of
  waiting for a successor that will never come.
- 200ms safety-net timeout for cases not covered by the seal
  (first segment after tool change, program start).
- Cleanup: removed debug probes (GATE_DBG, ACTIVATE_DBG, QUEUE_DBG,
  OPT_DBG, SEAL_DBG, XING_DBG, FWD_VF_DBG), stale active-segment
  rewrite, pessimistic first-profile hack.

Author: grandixximo

src/emc/motion_planning/motion_planning_9d.cc

@@ -115,7 +115,7 @@ static TC_STRUCT * tcqItem_user(TC_QUEUE_STRUCT * const tcq, int n)
 // Replan flag: set whenever the queue changes (segment added, feed change,
 // merge) so that replanForward() runs on the next tick.  Cleared when a
 // full forward pass completes without budget expiry.
-static bool g_needs_replan = false;
+bool g_needs_replan = false;
 
 /**
  * @brief Userspace version of tcqPut for planner_type 2
@@ -2352,6 +2352,18 @@ static int replanForward(TP_STRUCT *tp, double v0_override, double budget_sec)
             if (entry_ok && feed_ok && exit_ok) {
                 // Profile is clean — propagate its actual exit and continue
                 fo_skip_count++;
+                // Re-stamp FINALIZED if backward pass knocked it back to SMOOTHED
+                bool boundaries_known_skip = (tc->term_cond == TC_TERM_COND_EXACT)
+                                           || (i + 1 < queue_len)
+                                           || (tp->queue_sealed && i == queue_len - 1);
+                if (boundaries_known_skip) {
+                    int cur_state = __atomic_load_n(
+                        (int*)&tc->shared_9d.optimization_state, __ATOMIC_ACQUIRE);
+                    if (cur_state < TC_PLAN_FINALIZED) {
+                        atomicStoreInt((int*)&tc->shared_9d.optimization_state,
+                                       TC_PLAN_FINALIZED);
+                    }
+                }
                 if (tc->term_cond == TC_TERM_COND_TANGENT) {
                     double pu = profileExitVelUnscaled(&tc->shared_9d.profile);
                     prev_exit_vel = pu * tc->shared_9d.profile.computed_feed_scale;
@@ -2377,19 +2389,6 @@ static int replanForward(TP_STRUCT *tp, double v0_override, double budget_sec)
             continue;
         }
 
-        // --- Fix 4: pessimistic first profile ---
-        // On first profile (no prior valid profile), force exit=0 so RT always
-        // gets a safe profile.  Raises on subsequent calls once the backward pass
-        // has converged.  Exception: bezier blends have deterministic exits.
-        bool is_first_profile = !tc->shared_9d.profile.valid;
-        if (is_first_profile && scaled_v_exit > 0.0) {
-            TC_STRUCT *next_seg = (i + 1 < queue_len) ? tcqItem_user(queue, i + 1) : NULL;
-            bool next_is_blend = (next_seg && next_seg->motion_type == TC_BEZIER);
-            if (tc->motion_type != TC_BEZIER && !next_is_blend) {
-                scaled_v_exit = 0.0;
-                desired_fvel  = 0.0;
-            }
-        }
 
         // --- Feed hold: write zero-velocity stopped profile ---
         // When feed is effectively 0 (feed hold), skip Ruckig (can't traverse
@@ -2515,7 +2514,18 @@ static int replanForward(TP_STRUCT *tp, double v0_override, double budget_sec)
                 fo_dirty_count++;
                 tc->shared_9d.profile.dbg_src = 2;  // forward pass
                 tc->shared_9d.profile.dbg_v0_req = scaled_v_entry;
-                atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED);
+                // Only finalize when exit boundary conditions are known.
+                // STOP segments may be promoted to TANGENT when the next
+                // segment creates a blend, so require a successor in queue.
+                // EXACT (last segment of program) always has correct vf=0.
+                // queue_sealed: interpreter at sync point, no successor coming.
+                bool has_successor = (i + 1 < queue_len);
+                bool sealed_tail = (tp->queue_sealed && i == queue_len - 1);
+                bool boundaries_known = (tc->term_cond == TC_TERM_COND_EXACT)
+                                      || has_successor || sealed_tail;
+                if (boundaries_known) {
+                    atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED);
+                }
                 if (tc->term_cond == TC_TERM_COND_TANGENT) {
                     double pu = profileExitVelUnscaled(&tc->shared_9d.profile);
                     prev_exit_vel = pu * tc->shared_9d.profile.computed_feed_scale;
@@ -4053,8 +4063,11 @@ extern "C" int tpOptimizePlannedMotions_9D(TP_STRUCT * const tp, int /*optimizat
     if (g_feed_mgr.committed_feed < 0.001 && g_feed_mgr.planning.isOpen())
         return 0;
 
-    // Run unlimited replan — processes all segments needing recomputation.
-    (void)replanForward(tp, -1.0, 0.0 /* unlimited */);
+    // Budget-limited replan: don't block the interpreter for the entire queue.
+    // The servo cycle (checkFeedOverride → g_needs_replan) picks up remaining
+    // segments incrementally.  2ms is enough to process the tail segments
+    // (new segment + neighbors) while clean segments SKIP in ~50ns each.
+    (void)replanForward(tp, -1.0, 0.002);
 
     return 0;
 }
@@ -4101,6 +4114,7 @@ extern "C" int tpClearPlanning_9D(TP_STRUCT * const tp)
     // causing non-deterministic behavior between identical runs.
     g_feed_mgr.reset();
     g_needs_replan = false;
+    tp->queue_sealed = 0;
 
     return 0;
 }

src/emc/motion_planning/motion_planning_9d_userspace.cc

@@ -44,6 +44,9 @@ extern "C" {
     extern emcmot_config_t *emcmotConfig;
 }
 
+// Replan flag (defined in motion_planning_9d.cc)
+extern bool g_needs_replan;
+
 // ── Segment Compressor ──────────────────────────────────────────────────────
 // Buffers consecutive near-collinear G1 segments and emits a single compressed
 // line within G64 P tolerance.  Eliminates artificial kink velocities from
@@ -236,7 +239,16 @@ static int compressorFlush(TP_STRUCT *tp)
  */
 extern "C" int tpFlushCompressor_9D(TP_STRUCT *tp)
 {
-    return compressorFlush(tp);
+    int result = compressorFlush(tp);
+
+    // Seal the queue: interpreter is at a sync point (tool change, dwell,
+    // mode change, program end).  No more motion segments are coming until
+    // the next tpAddLine_9D/tpAddCircle_9D call clears this flag.
+    // This lets the optimizer finalize the tail segment immediately.
+    tp->queue_sealed = 1;
+    g_needs_replan = true;
+
+    return result;
 }
 
 /**
@@ -1337,6 +1349,9 @@ extern "C" int tpAddLine_9D(
         return -1;
     }
 
+    // New motion arriving — unseal the queue.
+    tp->queue_sealed = 0;
+
     // First segment of a new program: reset userspace planning state.
     // tpClear() runs in RT context where tpClearPlanning_9D is unavailable,
     // so we detect program-start here by an empty queue.
@@ -1585,6 +1600,9 @@ extern "C" int tpAddCircle_9D(
         return -1;
     }
 
+    // New motion arriving — unseal the queue.
+    tp->queue_sealed = 0;
+
     // Flush any buffered compressed segments before adding a circle
     if (g_compressor.active) {
         compressorFlush(tp);

src/emc/tp/tp.c

@@ -563,6 +563,7 @@ int tpClear(TP_STRUCT * const tp)
     tp->aborting = 0;
     tp->pausing = 0;
     tp->abort_profiles_written = 0;
+    tp->queue_sealed = 0;
     tp->reverse_run = 0;
     tp->synchronized = 0;
     tp->uu_per_rev = 0.0;
@@ -4868,7 +4869,6 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
                 tc->currentjerk = cj;
             }
 
-
         } else {
             tc->progress = tcGetTarget(tc, tp->reverse_run);
         }
@@ -5252,59 +5252,41 @@ int tpRunCycle(TP_STRUCT * const tp, long period)
      * Solution: Wait for Ruckig profile validity AND queue depth before starting.
      */
     if (GET_TRAJ_PLANNER_TYPE() == 2) {
-        int queue_len = tcqLen(&tp->queue);
-
-        /* Wait for Ruckig profile to be computed before starting ANY segment.
-         * This prevents trapezoidal→Ruckig switch mid-execution which causes
-         * position discontinuity in cycles 2-8 after motion start. */
-        if (!__atomic_load_n(&tc->shared_9d.profile.valid, __ATOMIC_ACQUIRE) && tc->progress < TP_POS_EPSILON) {
-            /* Abort bypass: don't block abort behind profile wait */
-            if (tp->aborting) {
-                goto past_gates;
-            }
-            static int profile_wait_count = 0;
-            profile_wait_count++;
-
-            /* Wait up to 200 cycles (200ms at 1kHz) for profile */
-            if (profile_wait_count < 200) {
-                return TP_ERR_WAITING;
-            }
-            /* Timeout - userspace too slow, will use trapezoidal for entire segment */
-            rtapi_print_msg(RTAPI_MSG_WARN,
-                "Ruckig profile timeout seg=%d, using trapezoidal fallback\n", tc->id);
-            profile_wait_count = 0;
-        }
-
-        /* 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.
+        /* Boundary-condition gate: wait for the optimizer to finalize the
+         * profile before activating any segment.  TC_PLAN_FINALIZED is only
+         * set when the forward pass has computed a profile with known exit
+         * boundary conditions — either vf=0 for EXACT/STOP segments, or
+         * with a valid successor present for TANGENT segments.
          *
-         * 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) {
-            /* Abort bypass: don't block abort behind queue gate */
-            if (tp->aborting) {
-                goto past_gates;
-            }
-            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 20 cycles (20ms at 1kHz) for a successor */
-            if (gate_wait_count < 20) {
-                return TP_ERR_WAITING;
+         * This single gate replaces both the old profile-valid gate and the
+         * queue-depth gate.  It ensures RT never executes a profile that was
+         * computed before the planner knew what comes next. */
+        if (tc->progress < TP_POS_EPSILON && !tc->active) {
+            static int finalized_seg_id = -1;
+            static int finalized_wait_count = 0;
+            int opt_state = __atomic_load_n(
+                (int*)&tc->shared_9d.optimization_state, __ATOMIC_SEQ_CST);
+            if (opt_state < TC_PLAN_FINALIZED) {
+                if (tp->aborting) {
+                    goto past_gates;
+                }
+                if (tc->id != finalized_seg_id) {
+                    finalized_seg_id = tc->id;
+                    finalized_wait_count = 0;
+                }
+                finalized_wait_count++;
+
+                /* Safety-net timeout: should never fire in normal operation.
+                 * If it does, something prevented the optimizer from
+                 * finalizing this segment (successor never arrived, etc.) */
+                int timeout = 200;
+                if (finalized_wait_count < timeout) {
+                    return TP_ERR_WAITING;
+                }
+                finalized_wait_count = 0;
+            } else {
+                finalized_wait_count = 0;
             }
-            gate_wait_count = 0;
         }
 past_gates:
     }

src/emc/tp/tp_types.h

@@ -151,6 +151,14 @@ typedef struct TP_STRUCT {
     // Actions queued via tpSetSegmentAction() before the next motion segment
     segment_actions_t pending_actions;
 
+    // Queue sealed flag (planner_type 2).
+    // Set by tpFlushCompressor_9D() when the interpreter reaches a sync point
+    // (tool change, dwell, mode change, program end) — signals "no more motion
+    // segments coming."  Cleared by tpAddLine_9D / tpAddCircle_9D when new
+    // motion arrives.  Allows the optimizer to finalize the tail segment
+    // immediately instead of waiting for a successor that will never come.
+    int queue_sealed;
+
 } TP_STRUCT;