From b6a676e0932d39599d86731536b72574e34f77c2 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Fri, 5 Nov 2021 14:44:25 +0100
Subject: [PATCH] AVR: Do not use __builtin_abs() for long types

In AVR __builtin_abs() breaks for non-base types.

Provide a generic function and use an overload when it is safe to use
instead.

This fixes the underlying step count calculation in PlanMove, thus
removing the need for the PlanLongMove work-around.
---
 src/cmath.h                    | 12 ++++++++----
 src/config/config.h            |  1 -
 src/logic/feed_to_bondtech.cpp |  2 +-
 src/logic/unload_to_finda.cpp  |  2 +-
 src/modules/motion.h           | 31 -------------------------------
 5 files changed, 10 insertions(+), 38 deletions(-)

diff --git a/src/cmath.h b/src/cmath.h
index 721f528..814a308 100644
--- a/src/cmath.h
+++ b/src/cmath.h
@@ -15,18 +15,22 @@ using std::min;
 #include <math.h>
 
 template <typename T>
-static inline const T min(T a, T b) {
+static inline const T min(const T a, const T b) {
     return a <= b ? a : b;
 }
 
 template <typename T>
-static inline const T max(T a, T b) {
+static inline const T max(const T a, const T b) {
     return a > b ? a : b;
 }
 
 template <typename T>
-static inline const T abs(T n) {
-    // Use builtin function when available
+static inline const T abs(const T n) {
+    return n < 0 ? -n : n;
+}
+
+static inline const int16_t abs(const int16_t n) {
+    // Use builtin function only when possible
     return __builtin_abs((n));
 }
 
diff --git a/src/config/config.h b/src/config/config.h
index 41fc026..afadc7a 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -53,7 +53,6 @@ static constexpr uint16_t maxStepFrequency = 40000;
 static constexpr uint16_t minStepRate = 120;
 
 /// Size for the motion planner block buffer size
-/// Beware of too low setting (esp. because of Motion::PlanLongMove)
 static constexpr uint8_t blockBufferSize = 4;
 
 /// Step timer frequency divider (F = F_CPU / divider)
diff --git a/src/logic/feed_to_bondtech.cpp b/src/logic/feed_to_bondtech.cpp
index 39b6450..c1d4d9d 100644
--- a/src/logic/feed_to_bondtech.cpp
+++ b/src/logic/feed_to_bondtech.cpp
@@ -27,7 +27,7 @@ bool FeedToBondtech::Step() {
             dbg_logic_fP(PSTR("Pulley start steps %u"), mm::motion.CurPosition(mm::Pulley));
             state = PushingFilamentToFSensor;
             mm::motion.InitAxis(mm::Pulley);
-            mm::motion.PlanLongMove<mm::Pulley>(config::defaultBowdenLength, config::pulleyFeedrate, config::pulleySlowFeedrate);
+            mm::motion.PlanMove<mm::Pulley>(config::defaultBowdenLength, config::pulleyFeedrate, config::pulleySlowFeedrate);
         }
         return false;
     case PushingFilamentToFSensor:
diff --git a/src/logic/unload_to_finda.cpp b/src/logic/unload_to_finda.cpp
index 6620a5f..a25a509 100644
--- a/src/logic/unload_to_finda.cpp
+++ b/src/logic/unload_to_finda.cpp
@@ -36,7 +36,7 @@ bool UnloadToFinda::Step() {
         if (mi::idler.Engaged()) {
             state = WaitingForFINDA;
             mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
-            mm::motion.PlanLongMove<mm::Pulley>(-config::defaultBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, config::pulleyFeedrate);
+            mm::motion.PlanMove<mm::Pulley>(-config::defaultBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, config::pulleyFeedrate);
         }
         return false;
     case WaitingForFINDA:
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 3448c92..ff78c7c 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -162,37 +162,6 @@ public:
             unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
     }
 
-    /// This function exists solely because of some mysterious overflow bug in planning/stepping
-    /// which occurrs when number of steps exceeds 32K. The bug doesn't even exhibit in unit tests :( .
-    /// Therefore it plans multiple segments in case the desired distance is longer than 32K steps.
-    /// So far this is only used for moving the Pulley...
-    template <Axis A, config::UnitBase B>
-    constexpr void PlanLongMove(config::Unit<long double, B, Lenght> delta,
-        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
-        auto steps = unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(delta);
-        if (steps.v >= 0) {
-            while (steps.v > 32767) {
-                PlanMove<A>(
-                    { 32767 },
-                    unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
-                    unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate)); // keep the end feedrate the same to continue with the next segment
-                steps.v -= 32767;
-            }
-        } else {
-            while (steps.v < -32767) {
-                PlanMove<A>(
-                    { -32767 },
-                    unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
-                    unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate)); // keep the end feedrate the same to continue with the next segment
-                steps.v += 32767;
-            }
-        }
-        PlanMove<A>( // last segment
-            steps,
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
-    }
-
     /// @returns head position of an axis (last enqueued position)
     /// @param axis axis affected
     pos_t Position(Axis axis) const;