itersolve: Simplify main itersolve_gen_steps_range() loop

Reorder the loop to try and make the code flow a little simpler.

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

1 files changed, 42 insertions, 46 deletions

diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 9dde391a..918c44d0 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -64,58 +64,54 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
     double start = move_start - m->print_time, end = move_end - m->print_time;
     struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
     double seek_time_delta = 0.000100;
-    int sdir = stepcompress_get_step_dir(sk->sc);
+    int sdir = !!stepcompress_get_step_dir(sk->sc);
     for (;;) {
-        // Determine if next step is in forward or reverse direction
-        double dist = high.position - last.position;
-        if (fabs(dist) < half_step) {
-        seek_new_high_range:
-            if (high.time >= end)
-                // At end of move
-                break;
-            // Need to increase next step search range
-            low = high;
-            do {
-                high.time = last.time + seek_time_delta;
-                seek_time_delta += seek_time_delta;
-            } while (unlikely(high.time <= low.time));
-            if (high.time > end)
-                high.time = end;
-            high.position = calc_position_cb(sk, m, high.time);
-            continue;
-        }
-        int next_sdir = dist > 0.;
-        if (unlikely(next_sdir != sdir)) {
-            // Direction change
-            if (fabs(dist) < half_step + .000000001)
-                // Only change direction if going past midway point
-                goto seek_new_high_range;
-            if (last.time >= low.time) {
-                // Must seek new low range to avoid re-finding previous time
-                if (high.time < last.time + .000000001)
-                    goto seek_new_high_range;
+        double diff = high.position - last.position, dist = sdir ? diff : -diff;
+        if (dist >= half_step) {
+            // Have valid upper bound - now find step
+            double target = last.position + (sdir ? half_step : -half_step);
+            struct timepos next = itersolve_find_step(sk, m, low, high, target);
+            // Add step at given time
+            int ret = stepcompress_append(sk->sc, sdir
+                                          , m->print_time, next.time);
+            if (ret)
+                return ret;
+            seek_time_delta = next.time - last.time;
+            if (seek_time_delta < .000000001)
+                seek_time_delta = .000000001;
+            last.position = target + (sdir ? half_step : -half_step);
+            last.time = next.time;
+            low = next;
+            if (low.time < high.time)
+                // The existing search range is still valid
+                continue;
+        } else if (unlikely(dist < -(half_step + .000000001))) {
+            // Found direction change
+            if (low.time > last.time) {
+                // Update direction and retry
+                sdir = !sdir;
+                continue;
+            }
+            // Must update range to avoid re-finding previous time
+            if (high.time > last.time + .000000001) {
+                // Reduce the high bound - it will become a better low bound
                 high.time = (last.time + high.time) * .5;
                 high.position = calc_position_cb(sk, m, high.time);
                 continue;
             }
-            sdir = next_sdir;
         }
-        // Find step
-        double target = last.position + (sdir ? half_step : -half_step);
-        struct timepos next = itersolve_find_step(sk, m, low, high, target);
-        // Add step at given time
-        int ret = stepcompress_append(sk->sc, sdir, m->print_time, next.time);
-        if (ret)
-            return ret;
-        seek_time_delta = next.time - last.time;
-        if (seek_time_delta < .000000001)
-            seek_time_delta = .000000001;
-        last.position = target + (sdir ? half_step : -half_step);
-        last.time = next.time;
-        low = next;
-        if (last.time >= high.time)
-            // The high range is no longer valid - recalculate it
-            goto seek_new_high_range;
+        // Need to increase the search range to find an upper bound
+        if (high.time >= end)
+            // At end of move
+            break;
+        low = high;
+        do {
+            high.time = last.time + seek_time_delta;
+            seek_time_delta += seek_time_delta;
+        } while (unlikely(high.time <= low.time));
+        if (high.time > end)
+            high.time = end;
+        high.position = calc_position_cb(sk, m, high.time);
     }
     sk->commanded_pos = last.position;
     if (sk->post_cb)