Fix planner block optimization

- Fixed the planner incorrectly avoiding optimization of the block following the active one.
- Added extra conditions to terminate planner early and avoid redundant computations.

Marlin/src/module/planner.cpp

 volatile uint8_t Planner::block_buffer_head,  // Index of the next block to be pushed
                  Planner::block_buffer_tail;  // Index of the busy block, if any
 uint16_t Planner::cleaning_buffer_counter;    // A counter to disable queuing of blocks
-uint8_t Planner::delay_before_delivering;     // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+uint8_t Planner::delay_before_delivering,     // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+        Planner::block_buffer_planned;        // Index of the optimally planned block
 
 float Planner::max_feedrate_mm_s[XYZE_N],   // Max speeds in mm per second
       Planner::axis_steps_per_mm[XYZE_N],
     bed_level_matrix.set_to_identity();
   #endif
   clear_block_buffer();
+  block_buffer_planned = 0;
   delay_before_delivering = 0;
 }
 
   if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
+/*                            PLANNER SPEED DEFINITION
+                                     +--------+   <- current->nominal_speed
+                                    /          \
+         current->entry_speed ->   +            \
+                                   |             + <- next->entry_speed (aka exit speed)
+                                   +-------------+
+                                       time -->
+
+  Recalculates the motion plan according to the following basic guidelines:
+
+    1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
+        (i.e. current->entry_speed) such that:
+      a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
+         neighboring blocks.
+      b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
+         with a maximum allowable deceleration over the block travel distance.
+      c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
+    2. Go over every block in chronological (forward) order and dial down junction speed values if
+      a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
+         acceleration over the block travel distance.
+
+  When these stages are complete, the planner will have maximized the velocity profiles throughout the all
+  of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
+  other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
+  are possible. If a new block is added to the buffer, the plan is recomputed according to the said
+  guidelines for a new optimal plan.
+
+  To increase computational efficiency of these guidelines, a set of planner block pointers have been
+  created to indicate stop-compute points for when the planner guidelines cannot logically make any further
+  changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
+  planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
+  bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
+  added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
+  them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
+  point) are all accelerating, they are all optimal and can not be altered by a new block added to the
+  planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
+  junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
+  used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
+  recomputed as stated in the general guidelines.
+
+  Planner buffer index mapping:
+  - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
+  - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
+      the buffer is full or empty. As described for standard ring buffers, this block is always empty.
+  - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
+      streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
+      planner buffer that don't change with the addition of a new block, as describe above. In addition,
+      this block can never be less than block_buffer_tail and will always be pushed forward and maintain
+      this requirement when encountered by the plan_discard_current_block() routine during a cycle.
+
+  NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
+  line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
+  enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
+  decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
+  becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
+  will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
+  motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
+  the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
+  for the planner to compute over. It also increases the number of computations the planner has to perform
+  to compute an optimal plan, so select carefully.
+*/
+
 // The kernel called by recalculate() when scanning the plan from last to first entry.
 void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
   if (current) {
         : MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters));
       if (current->entry_speed_sqr != new_entry_speed_sqr) {
         current->entry_speed_sqr = new_entry_speed_sqr;
+
+        // Need to recalculate the block speed
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
       }
     }
  * Once in reverse and once forward. This implements the reverse pass.
  */
 void Planner::reverse_pass() {
-  if (movesplanned() > 2) {
-    const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
-    uint8_t blocknr = prev_block_index(block_buffer_head);
+  // Initialize block index to the last block in the planner buffer.
+  uint8_t block_index = prev_block_index(block_buffer_head);
+
+  // Read the index of the last buffer planned block.
+  // The ISR may change it so get a stable local copy.
+  uint8_t planned_block_index = block_buffer_planned;
+
+  // If there was a race condition and block_buffer_planned was incremented
+  //  or was pointing at the head (queue empty) break loop now and avoid
+  //  planning already consumed blocks
+  if (planned_block_index == block_buffer_head) return;
+
+  // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
+  // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
+  // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
+  block_t *current;
+  const block_t *next = NULL;
+  while (block_index != planned_block_index) {
 
     // Perform the reverse pass
-    block_t *current, *next = NULL;
-    while (blocknr != endnr) {
-      // Perform the reverse pass - Only consider non sync blocks
-      current = &block_buffer[blocknr];
-      if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
-        reverse_pass_kernel(current, next);
-        next = current;
-      }
-      // Advance to the next
-      blocknr = prev_block_index(blocknr);
+    current = &block_buffer[block_index];
+
+    // Only consider non sync blocks
+    if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
+      reverse_pass_kernel(current, next);
+      next = current;
     }
+
+    // Advance to the next
+    block_index = prev_block_index(block_index);
   }
 }
 
 // The kernel called by recalculate() when scanning the plan from first to last entry.
-void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) {
+void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index) {
   if (previous) {
     // If the previous block is an acceleration block, too short to complete the full speed
     // change, adjust the entry speed accordingly. Entry speeds have already been reset,
     // maximized, and reverse-planned. If nominal length is set, max junction speed is
     // guaranteed to be reached. No need to recheck.
-    if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
-      if (previous->entry_speed_sqr < current->entry_speed_sqr) {
-        // Compute the maximum allowable speed
-        const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
-        // If true, current block is full-acceleration
-        if (current->entry_speed_sqr > new_entry_speed_sqr) {
-          // Always <= max_entry_speed_sqr. Backward pass sets this.
-          current->entry_speed_sqr = new_entry_speed_sqr;
-          SBI(current->flag, BLOCK_BIT_RECALCULATE);
-        }
+    if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) &&
+      previous->entry_speed_sqr < current->entry_speed_sqr) {
+
+      // Compute the maximum allowable speed
+      const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
+
+      // If true, current block is full-acceleration and we can move the planned pointer forward.
+      if (new_entry_speed_sqr < current->entry_speed_sqr) {
+
+        // Always <= max_entry_speed_sqr. Backward pass sets this.
+        current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
+
+        // Set optimal plan pointer.
+        block_buffer_planned = block_index;
+
+        // And mark we need to recompute the trapezoidal shape
+        SBI(current->flag, BLOCK_BIT_RECALCULATE);
       }
     }
+
+    // Any block set at its maximum entry speed also creates an optimal plan up to this
+    // point in the buffer. When the plan is bracketed by either the beginning of the
+    // buffer and a maximum entry speed or two maximum entry speeds, every block in between
+    // cannot logically be further improved. Hence, we don't have to recompute them anymore.
+    if (current->entry_speed_sqr == current->max_entry_speed_sqr)
+      block_buffer_planned = block_index;
   }
 }
 
  * Once in reverse and once forward. This implements the forward pass.
  */
 void Planner::forward_pass() {
-  const uint8_t endnr = block_buffer_head;
-  uint8_t blocknr = block_buffer_tail;
-
-  // Perform the forward pass
-  block_t *current, *previous = NULL;
-  while (blocknr != endnr) {
-    // Perform the forward pass - Only consider non-sync blocks
-    current = &block_buffer[blocknr];
+
+  // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
+  // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
+
+  // Begin at buffer planned pointer. Note that block_buffer_planned can be modified
+  //  by the stepper ISR,  so read it ONCE. It it guaranteed that block_buffer_planned
+  //  will never lead head, so the loop is safe to execute. Also note that the forward
+  //  pass will never modify the values at the tail.
+  uint8_t block_index = block_buffer_planned;
+
+  block_t *current;
+  const block_t * previous = NULL;
+  while (block_index != block_buffer_head) {
+
+    // Perform the forward pass
+    current = &block_buffer[block_index];
+
+    // Skip SYNC blocks
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
-      forward_pass_kernel(previous, current);
+      forward_pass_kernel(previous, current, block_index);
       previous = current;
     }
     // Advance to the previous
-    blocknr = next_block_index(blocknr);
+    block_index = next_block_index(block_index);
   }
 }
 
  * recalculate() after updating the blocks.
  */
 void Planner::recalculate_trapezoids() {
+  // The tail may be changed by the ISR so get a local copy.
   uint8_t block_index = block_buffer_tail;
 
   // As there could be a sync block in the head of the queue, and the next loop must not
   }
 }
 
-/**
- * Recalculate the motion plan according to the following algorithm:
- *
- *   1. Go over every block in reverse order...
- *
- *      Calculate a junction speed reduction (block_t.entry_factor) so:
- *
- *      a. The junction jerk is within the set limit, and
- *
- *      b. No speed reduction within one block requires faster
- *         deceleration than the one, true constant acceleration.
- *
- *   2. Go over every block in chronological order...
- *
- *      Dial down junction speed reduction values if:
- *      a. The speed increase within one block would require faster
- *         acceleration than the one, true constant acceleration.
- *
- * After that, all blocks will have an entry_factor allowing all speed changes to
- * be performed using only the one, true constant acceleration, and where no junction
- * jerk is jerkier than the set limit, Jerky. Finally it will:
- *
- *   3. Recalculate "trapezoids" for all blocks.
- */
 void Planner::recalculate() {
-  reverse_pass();
-  forward_pass();
+  // Initialize block index to the last block in the planner buffer.
+  const uint8_t block_index = prev_block_index(block_buffer_head);
+  // If there is just one block, no planning can be done. Avoid it!
+  if (block_index != block_buffer_planned) {
+    reverse_pass();
+    forward_pass();
+  }
   recalculate_trapezoids();
 }
 
 #endif // PLANNER_LEVELING
 
 void Planner::quick_stop() {
+
   // Remove all the queued blocks. Note that this function is NOT
   // called from the Stepper ISR, so we must consider tail as readonly!
-  // that is why we set head to tail!
-  block_buffer_head = block_buffer_tail;
+  // that is why we set head to tail - But there is a race condition that
+  // must be handled: The tail could change between the read and the assignment
+  // so this must be enclosed in a critical section
+
+  const bool was_enabled = STEPPER_ISR_ENABLED();
+  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+  // Drop all queue entries
+  block_buffer_planned = block_buffer_head = block_buffer_tail;
 
   // Restart the block delay for the first movement - As the queue was
   // forced to empty, there's no risk the ISR will touch this.
   // Make sure to drop any attempt of queuing moves for at least 1 second
   cleaning_buffer_counter = 1000;
 
+  // Reenable Stepper ISR
+  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+
   // And stop the stepper ISR
   stepper.quick_stop();
 }

Marlin/src/module/planner.h

     static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
                             block_buffer_tail;      // Index of the busy block, if any
     static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
-    static uint8_t delay_before_delivering;         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+    static uint8_t delay_before_delivering,         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+                   block_buffer_planned;            // Index of the optimally planned block
+                   
 
     #if ENABLED(DISTINCT_E_FACTORS)
       static uint8_t last_extruder;                 // Respond to extruder change
         block_t * const block = &block_buffer[block_buffer_tail];
 
         // No trapezoid calculated? Don't execute yet.
-        if ( TEST(block->flag, BLOCK_BIT_RECALCULATE)
-          || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].flag, BLOCK_BIT_RECALCULATE))
-        ) return NULL;
+        if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL;
 
         #if ENABLED(ULTRA_LCD)
           block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
         SBI(block->flag, BLOCK_BIT_BUSY);
         return block;
       }
-      else {
-        // The queue became empty
-        #if ENABLED(ULTRA_LCD)
-          clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
-        #endif
-        return NULL;
-      }
+
+      // The queue became empty
+      #if ENABLED(ULTRA_LCD)
+        clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
+      #endif
+
+      return NULL;
     }
 
     /**
      * NB: There MUST be a current block to call this function!!
      */
     FORCE_INLINE static void discard_current_block() {
-      block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
+      if (has_blocks_queued()) { // Discard non-empty buffer.
+        uint8_t block_index = next_block_index( block_buffer_tail );
+
+        // Push block_buffer_planned pointer, if encountered.
+        if (!has_blocks_queued()) block_buffer_planned = block_index;
+
+        block_buffer_tail = block_index;
+      }
     }
 
     #if ENABLED(ULTRA_LCD)
     /**
      * Get the index of the next / previous block in the ring buffer
      */
-    static constexpr int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); }
-    static constexpr int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
+    static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
+    static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
 
     /**
      * Calculate the distance (not time) it takes to accelerate
     static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
 
     static void reverse_pass_kernel(block_t* const current, const block_t * const next);
-    static void forward_pass_kernel(const block_t * const previous, block_t* const current);
+    static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);
 
     static void reverse_pass();
     static void forward_pass();