trapq: Implement sentinel nodes on the trapq list

Use sentinels to make list traversal code simpler.  Also add in null
moves so that there are no time gaps in the list.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

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);