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>

1 files changed, 45 insertions, 3 deletions

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