- // if there's only one block in the queue, we fall through both while loops and this ends up in current
- // so we must set it here, or perform conditionals further down. this is easier
- next = current;
-
- while ((block_index != oldest) && (current->recalculate_flag))
+ /*
+ * a newly added block is decel limited
+ *
+ * we find its max entry speed given its exit speed
+ *
+ * for each block, walking backwards in the queue:
+ *
+ * if max entry speed == current entry speed
+ * then we can set recalculate to false, since clearly adding another block didn't allow us to enter faster
+ * and thus we don't need to check entry speed for this block any more
+ *
+ * once we find an accel limited block, we must find the max exit speed and walk the queue forwards
+ *
+ * for each block, walking forwards in the queue:
+ *
+ * given the exit speed of the previous block and our own max entry speed
+ * we can tell if we're accel or decel limited (or coasting)
+ *
+ * if prev_exit > max_entry
+ * then we're still decel limited. update previous trapezoid with our max entry for prev exit
+ * if max_entry >= prev_exit
+ * then we're accel limited. set recalculate to false, work out max exit speed
+ *
+ * finally, work out trapezoid for the final (and newest) block.
+ */
+
+ /*
+ * Step 1:
+ * For each block, given the exit speed and acceleration, find the maximum entry speed
+ */
+
+ float entry_speed = minimum_planner_speed;
+
+ block_index = queue.head_i;
+ current = queue.item_ref(block_index);
+
+ if (!queue.is_empty())