From 730a6d868bd2c23c7383a78ba9acec3ed3f7fdab Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sun, 8 Dec 2019 18:30:42 -0500
Subject: [PATCH] kin_extruder: Move integration code from trapq.c to
 kin_extruder.c

Move the code that calculates the definitive integral to the
kin_extruder.c file.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/chelper/kin_extruder.c | 65 +++++++++++++++++++++++++++++++++++
 klippy/chelper/trapq.c        | 65 -----------------------------------
 klippy/chelper/trapq.h        |  2 --
 3 files changed, 65 insertions(+), 67 deletions(-)

diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c
index 9617afe8..1540c776 100644
--- a/klippy/chelper/kin_extruder.c
+++ b/klippy/chelper/kin_extruder.c
@@ -12,6 +12,71 @@
 #include "pyhelper.h" // errorf
 #include "trapq.h" // move_get_distance
 
+// Helper code for integrating acceleration
+static double
+integrate_accel(struct move *m, double start, double end)
+{
+    double half_v = .5 * m->start_v, sixth_a = (1. / 3.) * m->half_accel;
+    double si = start * start * (half_v + sixth_a * start);
+    double ei = end * end * (half_v + sixth_a * end);
+    return ei - si;
+}
+
+// Calculate the definitive integral on part of a move
+static double
+move_integrate(struct move *m, int axis, double start, double end)
+{
+    if (start < 0.)
+        start = 0.;
+    if (end > m->move_t)
+        end = m->move_t;
+    double base = m->start_pos.axis[axis - 'x'] * (end - start);
+    double integral = integrate_accel(m, start, end);
+    return base + integral * m->axes_r.axis[axis - 'x'];
+}
+
+// Calculate the definitive integral for a cartesian axis
+static double
+trapq_integrate(struct move *m, int axis, double start, double end)
+{
+    double res = move_integrate(m, axis, start, end);
+    // Integrate over previous moves
+    struct move *prev = m;
+    while (unlikely(start < 0.)) {
+        prev = list_prev_entry(prev, node);
+        start += prev->move_t;
+        res += move_integrate(prev, axis, start, prev->move_t);
+    }
+    // Integrate over future moves
+    while (unlikely(end > m->move_t)) {
+        end -= m->move_t;
+        m = list_next_entry(m, node);
+        res += move_integrate(m, axis, 0., end);
+    }
+    return res;
+}
+
+// Find a move associated with a given time
+static 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;
+        }
+    }
+}
+
 struct extruder_stepper {
     struct stepper_kinematics sk;
     double pressure_advance_factor, half_smooth_time, inv_smooth_time;
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c
index 838fcdec..603ce9e2 100644
--- a/klippy/chelper/trapq.c
+++ b/klippy/chelper/trapq.c
@@ -86,71 +86,6 @@ move_get_coord(struct move *m, double move_time)
         .z = m->start_pos.z + m->axes_r.z * move_dist };
 }
 
-// Helper code for integrating acceleration
-static double
-integrate_accel(struct move *m, double start, double end)
-{
-    double half_v = .5 * m->start_v, sixth_a = (1. / 3.) * m->half_accel;
-    double si = start * start * (half_v + sixth_a * start);
-    double ei = end * end * (half_v + sixth_a * end);
-    return ei - si;
-}
-
-// Calculate the definitive integral on part of a move
-static double
-move_integrate(struct move *m, int axis, double start, double end)
-{
-    if (start < 0.)
-        start = 0.;
-    if (end > m->move_t)
-        end = m->move_t;
-    double base = m->start_pos.axis[axis - 'x'] * (end - start);
-    double integral = integrate_accel(m, start, end);
-    return base + integral * m->axes_r.axis[axis - 'x'];
-}
-
-// Calculate the definitive integral for a cartesian axis
-double
-trapq_integrate(struct move *m, int axis, double start, double end)
-{
-    double res = move_integrate(m, axis, start, end);
-    // Integrate over previous moves
-    struct move *prev = m;
-    while (unlikely(start < 0.)) {
-        prev = list_prev_entry(prev, node);
-        start += prev->move_t;
-        res += move_integrate(prev, axis, start, prev->move_t);
-    }
-    // Integrate over future moves
-    while (unlikely(end > m->move_t)) {
-        end -= m->move_t;
-        m = list_next_entry(m, node);
-        res += move_integrate(m, axis, 0., 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 14da9ee9..94590061 100644
--- a/klippy/chelper/trapq.h
+++ b/klippy/chelper/trapq.h
@@ -32,8 +32,6 @@ void trapq_append(struct trapq *tq, double print_time
                   , double start_v, double cruise_v, double accel);
 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);