From cba7a285e4693fbca736f76df9ff407892676cdf Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Wed, 8 Oct 2025 15:45:00 -0400
Subject: [PATCH] trapq: Extend trapq_extract_old() to also check active moves

Support extracting moves from both the "live" trapq->moves as well as
the "history" from trapq->history storage.  Now that moves are flushed
separately from the lookahead queue, there is a good chance that the
current move being processed on the mcus will still be in the active
trapq list.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/chelper/list.h  |  5 +++++
 klippy/chelper/trapq.c | 36 ++++++++++++++++++++++++++----------
 2 files changed, 31 insertions(+), 10 deletions(-)

diff --git a/klippy/chelper/list.h b/klippy/chelper/list.h
index 12fe2b038..f38fca6cc 100644
--- a/klippy/chelper/list.h
+++ b/klippy/chelper/list.h
@@ -116,6 +116,11 @@ list_join_tail(struct list_head *add, struct list_head *h)
          ; &pos->member != &(head)->root                        \
          ; pos = list_next_entry(pos, member))
 
+#define list_for_each_entry_reverse(pos, head, member)          \
+    for (pos = list_last_entry((head), typeof(*pos), member)    \
+         ; &pos->member != &(head)->root                        \
+         ; pos = list_prev_entry(pos, member))
+
 #define list_for_each_entry_safe(pos, n, head, member)          \
     for (pos = list_first_entry((head), typeof(*pos), member)   \
           , n = list_next_entry(pos, member)                    \
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index c238a3818..a2a5f37f3 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -228,6 +228,22 @@ trapq_set_position(struct trapq *tq, double print_time
     list_add_head(&m->node, &tq->history);
 }
 
+// Copy the info in a 'struct move' to a 'struct pull_move'
+static void
+copy_pull_move(struct pull_move *p, struct move *m)
+{
+    p->print_time = m->print_time;
+    p->move_t = m->move_t;
+    p->start_v = m->start_v;
+    p->accel = 2. * m->half_accel;
+    p->start_x = m->start_pos.x;
+    p->start_y = m->start_pos.y;
+    p->start_z = m->start_pos.z;
+    p->x_r = m->axes_r.x;
+    p->y_r = m->axes_r.y;
+    p->z_r = m->axes_r.z;
+}
+
 // Return history of movement queue
 int __visible
 trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
@@ -235,21 +251,21 @@ trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
 {
     int res = 0;
     struct move *m;
+    list_for_each_entry_reverse(m, &tq->moves, node) {
+        if (start_time >= m->print_time + m->move_t || res >= max)
+            break;
+        if (end_time <= m->print_time || (!m->start_v && !m->half_accel))
+            continue;
+        copy_pull_move(p, m);
+        p++;
+        res++;
+    }
     list_for_each_entry(m, &tq->history, node) {
         if (start_time >= m->print_time + m->move_t || res >= max)
             break;
         if (end_time <= m->print_time)
             continue;
-        p->print_time = m->print_time;
-        p->move_t = m->move_t;
-        p->start_v = m->start_v;
-        p->accel = 2. * m->half_accel;
-        p->start_x = m->start_pos.x;
-        p->start_y = m->start_pos.y;
-        p->start_z = m->start_pos.z;
-        p->x_r = m->axes_r.x;
-        p->y_r = m->axes_r.y;
-        p->z_r = m->axes_r.z;
+        copy_pull_move(p, m);
         p++;
         res++;
     }