From 45c232b2c724136aa9ef54b2858712640baacb3d Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Mon, 19 Jul 2021 11:32:16 -0400
Subject: [PATCH] trapq: Keep history of recent trapq moves

Store trapq moves in a separate "history" list after each move is
nominally expired.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/chelper/__init__.py |  9 +++++++
 klippy/chelper/trapq.c     | 55 +++++++++++++++++++++++++++++++++++---
 klippy/chelper/trapq.h     | 11 +++++++-
 3 files changed, 71 insertions(+), 4 deletions(-)

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 09897927..97493355 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -68,6 +68,13 @@ defs_itersolve = """
 """
 
 defs_trapq = """
+    struct pull_move {
+        double print_time, move_t;
+        double start_v, accel;
+        double start_x, start_y, start_z;
+        double x_r, y_r, z_r;
+    };
+
     void trapq_append(struct trapq *tq, double print_time
         , double accel_t, double cruise_t, double decel_t
         , double start_pos_x, double start_pos_y, double start_pos_z
@@ -76,6 +83,8 @@ defs_trapq = """
     struct trapq *trapq_alloc(void);
     void trapq_free(struct trapq *tq);
     void trapq_finalize_moves(struct trapq *tq, double print_time);
+    int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
+        , double start_time, double end_time);
 """
 
 defs_kin_cartesian = """
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index 177db8d8..eaeb5949 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -1,6 +1,6 @@
 // Trapezoidal velocity movement queue
 //
-// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2018-2021  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
@@ -95,6 +95,7 @@ trapq_alloc(void)
     struct trapq *tq = malloc(sizeof(*tq));
     memset(tq, 0, sizeof(*tq));
     list_init(&tq->moves);
+    list_init(&tq->history);
     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);
@@ -111,6 +112,11 @@ trapq_free(struct trapq *tq)
         list_del(&m->node);
         free(m);
     }
+    while (!list_empty(&tq->history)) {
+        struct move *m = list_first_entry(&tq->history, struct move, node);
+        list_del(&m->node);
+        free(m);
+    }
     free(tq);
 }
 
@@ -157,21 +163,64 @@ trapq_add_move(struct trapq *tq, struct move *m)
     tail_sentinel->print_time = 0.;
 }
 
+#define HISTORY_EXPIRE (30.0)
+
 // Expire any moves older than `print_time` from the trapezoid velocity queue
 void __visible
 trapq_finalize_moves(struct trapq *tq, double print_time)
 {
     struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
     struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
+    // Move expired moves from main "moves" list to "history" list
     for (;;) {
         struct move *m = list_next_entry(head_sentinel, node);
         if (m == tail_sentinel) {
             tail_sentinel->print_time = NEVER_TIME;
-            return;
+            break;
         }
         if (m->print_time + m->move_t > print_time)
-            return;
+            break;
+        list_del(&m->node);
+        list_add_head(&m->node, &tq->history);
+    }
+    // Free old moves from history list
+    if (list_empty(&tq->history))
+        return;
+    struct move *latest = list_first_entry(&tq->history, struct move, node);
+    double expire_time = latest->print_time + latest->move_t - HISTORY_EXPIRE;
+    for (;;) {
+        struct move *m = list_last_entry(&tq->history, struct move, node);
+        if (m == latest || m->print_time + m->move_t > expire_time)
+            break;
         list_del(&m->node);
         free(m);
     }
 }
+
+// Return history of movement queue
+int __visible
+trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
+                  , double start_time, double end_time)
+{
+    int res = 0;
+    struct move *m;
+    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;
+        p++;
+        res++;
+    }
+    return res;
+}
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
index a930a2e1..154fe04d 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -21,7 +21,14 @@ struct move {
 };
 
 struct trapq {
-    struct list_head moves;
+    struct list_head moves, history;
+};
+
+struct pull_move {
+    double print_time, move_t;
+    double start_v, accel;
+    double start_x, start_y, start_z;
+    double x_r, y_r, z_r;
 };
 
 struct move *move_alloc(void);
@@ -37,5 +44,7 @@ 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_finalize_moves(struct trapq *tq, double print_time);
+int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
+                      , double start_time, double end_time);
 
 #endif // trapq.h