trapq: Add trapq_find_move() function

Add support for finding a move by time in the trapezoidal velocity
move list.

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

2 files changed, 22 insertions, 0 deletions

diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index 5d5073c1..838fcdec 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -130,6 +130,27 @@ trapq_integrate(struct move *m, int axis, double start, double end)
     return res;
 }
 
+// Find a move associated with a given time
+struct move *
+trapq_find_move(struct move *m, double *ptime)
+{
+    double move_time = *ptime;
+    for (;;) {
+        if (unlikely(move_time < 0.)) {
+            // Check previous move in list
+            m = list_prev_entry(m, node);
+            move_time += m->move_t;
+        } else if (unlikely(move_time > m->move_t)) {
+            // Check next move in list
+            move_time -= m->move_t;
+            m = list_next_entry(m, node);
+        } else {
+            *ptime = move_time;
+            return m;
+        }
+    }
+}
+
 #define NEVER_TIME 9999999999999999.9
 
 // Allocate a new 'trapq' object
diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h
index fa55faaf..14da9ee9 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -33,6 +33,7 @@ void trapq_append(struct trapq *tq, double print_time
 double move_get_distance(struct move *m, double move_time);
 struct coord move_get_coord(struct move *m, double move_time);
 double trapq_integrate(struct move *m, int axis, double start, double end);
+struct move *trapq_find_move(struct move *m, double *ptime);
 struct trapq *trapq_alloc(void);
 void trapq_free(struct trapq *tq);
 void trapq_check_sentinels(struct trapq *tq);