3 files changed, 55 insertions, 17 deletions

diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 5eafd2a7..84fd5f71 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -144,14 +144,12 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
 {
     double last_flush_time = sk->last_flush_time;
     sk->last_flush_time = flush_time;
-    if (!sk->tq || list_empty(&sk->tq->moves))
+    if (!sk->tq)
         return 0;
+    trapq_check_sentinels(sk->tq);
     struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
-    while (last_flush_time >= m->print_time + m->move_t) {
-        if (list_is_last(&m->node, &sk->tq->moves))
-            return 0;
+    while (last_flush_time >= m->print_time + m->move_t)
         m = list_next_entry(m, node);
-    }
     for (;;) {
         double start = m->print_time, end = start + m->move_t;
         if (start < last_flush_time)
@@ -166,7 +164,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
                 return ret;
         }
         last_flush_time = end;
-        if (list_is_last(&m->node, &sk->tq->moves))
+        if (flush_time <= m->print_time + m->move_t)
             return 0;
         m = list_next_entry(m, node);
     }
@@ -176,22 +174,19 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
 double __visible
 itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
 {
-    if (!sk->tq || list_empty(&sk->tq->moves))
+    if (!sk->tq)
         return 0.;
+    trapq_check_sentinels(sk->tq);
     struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
-    while (sk->last_flush_time >= m->print_time + m->move_t) {
-        if (list_is_last(&m->node, &sk->tq->moves))
-            return 0.;
+    while (sk->last_flush_time >= m->print_time + m->move_t)
         m = list_next_entry(m, node);
-    }
-    while (m->print_time < flush_time) {
+    for (;;) {
         if (check_active(sk, m))
             return m->print_time;
-        if (list_is_last(&m->node, &sk->tq->moves))
+        if (flush_time <= m->print_time + m->move_t)
             return 0.;
         m = list_next_entry(m, node);
     }
-    return 0.;
 }
 
 void __visible
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index ed6535dc..603ce9e2 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -86,6 +86,8 @@ move_get_coord(struct move *m, double move_time)
         .z = m->start_pos.z + m->axes_r.z * move_dist };
 }
 
+#define NEVER_TIME 9999999999999999.9
+
 // Allocate a new 'trapq' object
 struct trapq * __visible
 trapq_alloc(void)
@@ -93,6 +95,10 @@ trapq_alloc(void)
     struct trapq *tq = malloc(sizeof(*tq));
     memset(tq, 0, sizeof(*tq));
     list_init(&tq->moves);
+    struct move *head_sentinel = move_alloc(), *tail_sentinel = move_alloc();
+    tail_sentinel->print_time = tail_sentinel->move_t = NEVER_TIME;
+    list_add_head(&head_sentinel->node, &tq->moves);
+    list_add_tail(&tail_sentinel->node, &tq->moves);
     return tq;
 }
 
@@ -108,19 +114,55 @@ trapq_free(struct trapq *tq)
     free(tq);
 }
 
+// Update the list sentinels
+void
+trapq_check_sentinels(struct trapq *tq)
+{
+    struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    if (tail_sentinel->print_time)
+        // Already up to date
+        return;
+    struct move *m = list_prev_entry(tail_sentinel, node);
+    struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
+    if (m == head_sentinel) {
+        // No moves at all on this list
+        tail_sentinel->print_time = NEVER_TIME;
+        return;
+    }
+    tail_sentinel->print_time = m->print_time + m->move_t;
+    tail_sentinel->start_pos = move_get_coord(m, m->move_t);
+}
+
 // Add a move to the trapezoid velocity queue
 void
 trapq_add_move(struct trapq *tq, struct move *m)
 {
-    list_add_tail(&m->node, &tq->moves);
+    struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    struct move *prev = list_prev_entry(tail_sentinel, node);
+    if (prev->print_time + prev->move_t < m->print_time) {
+        // Add a null move to fill time gap
+        struct move *null_move = move_alloc();
+        null_move->start_pos = m->start_pos;
+        null_move->print_time = prev->print_time + prev->move_t;
+        null_move->move_t = m->print_time - null_move->print_time;
+        list_add_before(&null_move->node, &tail_sentinel->node);
+    }
+    list_add_before(&m->node, &tail_sentinel->node);
+    tail_sentinel->print_time = 0.;
 }
 
 // Free any moves older than `print_time` from the trapezoid velocity queue
 void __visible
 trapq_free_moves(struct trapq *tq, double print_time)
 {
-    while (!list_empty(&tq->moves)) {
-        struct move *m = list_first_entry(&tq->moves, struct move, node);
+    struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
+    struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    for (;;) {
+        struct move *m = list_next_entry(head_sentinel, node);
+        if (m == tail_sentinel) {
+            tail_sentinel->print_time = NEVER_TIME;
+            return;
+        }
         if (m->print_time + m->move_t > print_time)
             return;
         list_del(&m->node);
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
index ec762621..4572ef1a 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -29,6 +29,7 @@ double move_get_distance(struct move *m, double move_time);
 struct coord move_get_coord(struct move *m, double move_time);
 struct trapq *trapq_alloc(void);
 void trapq_free(struct trapq *tq);
+void trapq_check_sentinels(struct trapq *tq);
 void trapq_add_move(struct trapq *tq, struct move *m);
 void trapq_free_moves(struct trapq *tq, double print_time);