From 0696dda4700589ccf88dfb4470180a556ff6b3a2 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Fri, 12 May 2017 01:05:11 -0500
Subject: [PATCH] Cleanups to UBL_DELTA

---
 Marlin/Conditionals_post.h          |  10 +-
 Marlin/G26_Mesh_Validation_Tool.cpp |  13 +--
 Marlin/Marlin_main.cpp              |  43 ++++----
 Marlin/SanityCheck.h                |  29 ++----
 Marlin/planner.cpp                  |  20 ++--
 Marlin/ubl_motion.cpp               | 149 +++++++++++++---------------
 6 files changed, 116 insertions(+), 148 deletions(-)

diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index 5280da81a..b303b65aa 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -731,15 +731,12 @@
    * Set granular options based on the specific type of leveling
    */
 
-  #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(DELTA)
-    #define UBL_DELTA
-  #endif
-
+  #define UBL_DELTA  (ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(DELTA))
   #define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT))
   #define ABL_GRID   (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
   #define HAS_ABL    (ABL_PLANAR || ABL_GRID || ENABLED(AUTO_BED_LEVELING_UBL))
   #define HAS_LEVELING          (HAS_ABL || ENABLED(MESH_BED_LEVELING))
-  #define PLANNER_LEVELING      (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING) || ENABLED(UBL_DELTA))
+  #define PLANNER_LEVELING      (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING) || UBL_DELTA)
   #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
   #if HAS_PROBING_PROCEDURE
     #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))
@@ -823,8 +820,7 @@
   /**
    * DELTA_SEGMENT_MIN_LENGTH for UBL_DELTA
    */
-
-  #if ENABLED(UBL_DELTA)
+  #if UBL_DELTA
     #ifndef DELTA_SEGMENT_MIN_LENGTH
       #if IS_SCARA
         #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp
index 4f231c2ae..0cf414735 100644
--- a/Marlin/G26_Mesh_Validation_Tool.cpp
+++ b/Marlin/G26_Mesh_Validation_Tool.cpp
@@ -278,8 +278,7 @@
 
         // If this mesh location is outside the printable_radius, skip it.
 
-        if ( ! position_is_reachable_raw_xy( circle_x, circle_y ))
-          continue;
+        if (!position_is_reachable_raw_xy(circle_x, circle_y)) continue;
 
         xi = location.x_index;  // Just to shrink the next few lines and make them easier to understand
         yi = location.y_index;
@@ -329,9 +328,7 @@
                 ye = circle_y + sin_table[tmp_div_30 + 1];
           #if IS_KINEMATIC
             // Check to make sure this segment is entirely on the bed, skip if not.
-            if (( ! position_is_reachable_raw_xy( x , y  )) ||
-                ( ! position_is_reachable_raw_xy( xe, ye )))
-              continue;
+            if (!position_is_reachable_raw_xy(x, y) || !position_is_reachable_raw_xy(xe, ye)) continue;
           #else                                              // not, we need to skip
             x  = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
             y  = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1);
@@ -459,8 +456,7 @@
               sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1);
               ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
 
-              if (( position_is_reachable_raw_xy( sx, sy )) &&
-                  ( position_is_reachable_raw_xy( ex, ey ))) {
+              if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) {
 
                 if (ubl.g26_debug_flag) {
                   SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
@@ -494,8 +490,7 @@
                 sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
                 ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
 
-                if (( position_is_reachable_raw_xy( sx, sy )) &&
-                    ( position_is_reachable_raw_xy( ex, ey ))) {
+                if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) {
 
                   if (ubl.g26_debug_flag) {
                     SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 64ad8b4de..05ce02c95 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2427,9 +2427,12 @@ static void clean_up_after_endstop_or_probe_move() {
 
     #elif ENABLED(AUTO_BED_LEVELING_UBL)
 
-      #if ENABLED(UBL_DELTA)
-        if (( ubl.state.active ) && ( ! enable )) {   // leveling from on to off
-          planner.unapply_leveling(current_position);
+      #if PLANNER_LEVELING
+        if (ubl.state.active != enable) {
+          if (!enable)   // leveling from on to off
+            planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+          else
+            planner.unapply_leveling(current_position);
         }
       #endif
 
@@ -11104,7 +11107,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
 
 #endif // AUTO_BED_LEVELING_BILINEAR
 
-#if IS_KINEMATIC && DISABLED(UBL_DELTA)
+#if IS_KINEMATIC && !UBL_DELTA
 
   /**
    * Prepare a linear move in a DELTA or SCARA setup.
@@ -11124,7 +11127,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
     }
 
     // Fail if attempting move outside printable radius
-    if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] )) return true;
+    if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true;
 
     // Get the cartesian distances moved in XYZE
     float difference[XYZE];
@@ -11225,7 +11228,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
     return false;
   }
 
-#else // !IS_KINEMATIC
+#else // !IS_KINEMATIC || UBL_DELTA
 
   /**
    * Prepare a linear move in a Cartesian setup.
@@ -11263,7 +11266,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
     return false;
   }
 
-#endif // !IS_KINEMATIC
+#endif // !IS_KINEMATIC || UBL_DELTA
 
 #if ENABLED(DUAL_X_CARRIAGE)
 
@@ -11375,21 +11378,21 @@ void prepare_move_to_destination() {
 
   #endif
 
-  #if IS_KINEMATIC
-    #if ENABLED(UBL_DELTA)
-      if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return;
+  if (
+    #if IS_KINEMATIC
+      #if UBL_DELTA
+        ubl_prepare_linear_move_to(destination, feedrate_mm_s)
+      #else
+        prepare_kinematic_move_to(destination)
+      #endif
+    #elif ENABLED(DUAL_X_CARRIAGE)
+      prepare_move_to_destination_dualx()
+    #elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
+      ubl_prepare_linear_move_to(destination, feedrate_mm_s)
     #else
-      if (prepare_kinematic_move_to(destination)) return;
+      prepare_move_to_destination_cartesian()
     #endif
-  #else
-    #if ENABLED(DUAL_X_CARRIAGE)
-      if (prepare_move_to_destination_dualx()) return;
-    #elif ENABLED(UBL_DELTA) // will work for CARTESIAN too (smaller segments follow mesh more closely)
-      if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return;
-    #else
-      if (prepare_move_to_destination_cartesian()) return;
-    #endif
-  #endif
+  ) return;
 
   set_current_to_destination();
 }
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index 006bf6829..3aa307298 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -248,11 +248,9 @@
 #if ENABLED(DELTA)
   #if DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG)
     #error "You probably want to use Max Endstops for DELTA!"
-  #endif
-  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(UBL_DELTA)
-    #error "ENABLE_LEVELING_FADE_HEIGHT for DELTA requires UBL_DELTA and AUTO_BED_LEVELING_UBL."
-  #endif
-  #if ABL_GRID
+  #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_DELTA
+    #error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL."
+  #elif ABL_GRID
     #if (GRID_MAX_POINTS_X & 1) == 0 || (GRID_MAX_POINTS_Y & 1) == 0
       #error "DELTA requires GRID_MAX_POINTS_X and GRID_MAX_POINTS_Y to be odd numbers."
     #elif GRID_MAX_POINTS_X < 3
@@ -431,20 +429,11 @@ static_assert(1 >= 0
  * Unified Bed Leveling
  */
 #if ENABLED(AUTO_BED_LEVELING_UBL)
-  #if IS_KINEMATIC
-    #if ENABLED(DELTA)
-      #if DISABLED(UBL_DELTA)
-        #error "AUTO_BED_LEVELING_UBL requires UBL_DELTA for DELTA printers."
-      #endif
-    #else // SCARA
-      #error "AUTO_BED_LEVELING_UBL not supported for SCARA printers."
-    #endif
-  #endif
-  #if DISABLED(NEWPANEL)
+  #if IS_SCARA
+    #error "AUTO_BED_LEVELING_UBL does not yet support SCARA printers."
+  #elif DISABLED(NEWPANEL)
     #error "AUTO_BED_LEVELING_UBL requires an LCD controller."
   #endif
-#elif ENABLED(UBL_DELTA)
-  #error "UBL_DELTA requires AUTO_BED_LEVELING_UBL."
 #endif
 
 /**
@@ -603,10 +592,8 @@ static_assert(1 >= 0
   /**
    * Delta and SCARA have limited bed leveling options
    */
-  #if IS_KINEMATIC
-    #if DISABLED(AUTO_BED_LEVELING_BILINEAR) && DISABLED(UBL_DELTA)
-      #error "Only AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL with UBL_DELTA support DELTA and SCARA bed leveling."
-    #endif
+  #if IS_SCARA && DISABLED(AUTO_BED_LEVELING_BILINEAR)
+    #error "Only AUTO_BED_LEVELING_BILINEAR currently supports SCARA bed leveling."
   #endif
 
   /**
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 8c641fc26..21a98a304 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -534,12 +534,12 @@ void Planner::check_axes_activity() {
    */
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
 
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA)  // probably should also be enabled for UBL without UBL_DELTA
+    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA  // probably should also be enabled for UBL without UBL_DELTA
       if (!ubl.state.active) return;
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
         // if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
         if ((planner.z_fade_height) && (planner.z_fade_height <= RAW_Z_POSITION(lz))) return;
-        lz += ubl.state.z_offset + ( ubl.get_z_correction(lx,ly) * ubl.fade_scaling_factor_for_z(lz));
+        lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly) * ubl.fade_scaling_factor_for_z(lz);
       #else // no fade
         lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly);
       #endif // FADE
@@ -598,13 +598,13 @@ void Planner::check_axes_activity() {
 
   void Planner::unapply_leveling(float logical[XYZ]) {
 
-    #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA)
+    #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA
 
-      if ( ubl.state.active ) {
+      if (ubl.state.active) {
 
-        float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]);
-        float z_ublmesh = ubl.get_z_correction(logical[X_AXIS],logical[Y_AXIS]);
-        float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
+        const float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]),
+                    z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
+              float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
 
         #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
 
@@ -616,9 +616,9 @@ void Planner::check_axes_activity() {
           //    so U(1-M/H)==L-O-M
           //    so U==(L-O-M)/(1-M/H) for U<H
 
-          if ( planner.z_fade_height ) {
-            float z_unfaded = z_unlevel / ( 1.0 - ( z_ublmesh * planner.inverse_z_fade_height ));
-            if ( z_unfaded < planner.z_fade_height )  // don't know until after compute
+          if (planner.z_fade_height) {
+            float z_unfaded = z_unlevel / (1.0 - z_ublmesh * planner.inverse_z_fade_height);
+            if (z_unfaded < planner.z_fade_height)  // don't know until after compute
               z_unlevel = z_unfaded;
           }
 
diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp
index aaca8d00d..1d59e31b9 100644
--- a/Marlin/ubl_motion.cpp
+++ b/Marlin/ubl_motion.cpp
@@ -474,20 +474,10 @@
     set_current_to_destination();
   }
 
-
-  #ifdef UBL_DELTA
-
-    #define COPY_XYZE( target, source ) { \
-                target[X_AXIS] = source[X_AXIS]; \
-                target[Y_AXIS] = source[Y_AXIS]; \
-                target[Z_AXIS] = source[Z_AXIS]; \
-                target[E_AXIS] = source[E_AXIS]; \
-            }
+  #if UBL_DELTA
 
     #if IS_SCARA // scale the feed rate from mm/s to degrees/s
-      static float scara_feed_factor;
-      static float scara_oldA;
-      static float scara_oldB;
+      static float scara_feed_factor, scara_oldA, scara_oldB;
     #endif
 
     // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, 
@@ -501,18 +491,18 @@
         float feedrate = fr_mm_s;
 
         #if IS_SCARA // scale the feed rate from mm/s to degrees/s
-          float adiff = abs(delta[A_AXIS] - scara_oldA);
-          float bdiff = abs(delta[B_AXIS] - scara_oldB);
+          float adiff = abs(delta[A_AXIS] - scara_oldA),
+                bdiff = abs(delta[B_AXIS] - scara_oldB);
           scara_oldA = delta[A_AXIS];
           scara_oldB = delta[B_AXIS];
           feedrate = max(adiff, bdiff) * scara_feed_factor;
         #endif
 
-        planner._buffer_line( delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder );
+        planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
 
       #else // cartesian
 
-        planner._buffer_line( ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder );
+        planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
 
       #endif
     }
@@ -525,7 +515,7 @@
 
     static bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
 
-      if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] ))  // fail if moving outside reachable boundary
+      if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS]))  // fail if moving outside reachable boundary
         return true; // did not move, so current_position still accurate
 
       const float difference[XYZE] = {    // cartesian distances moved in XYZE
@@ -533,21 +523,21 @@
                     ltarget[Y_AXIS] - current_position[Y_AXIS],
                     ltarget[Z_AXIS] - current_position[Z_AXIS],
                     ltarget[E_AXIS] - current_position[E_AXIS]
-                    };
+                  };
 
-      float cartesian_xy_mm = sqrtf( sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) ); // total horizontal xy distance
+      const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]);        // total horizontal xy distance
 
       #if IS_KINEMATIC
-        float    seconds  = cartesian_xy_mm / feedrate;                                   // seconds to move xy distance at requested rate
-        uint16_t segments = lroundf( delta_segments_per_second * seconds );               // preferred number of segments for distance @ feedrate
-        uint16_t seglimit = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
-        NOMORE( segments, seglimit );                                                     // limit to minimum segment length (fewer segments)
+        const float seconds = cartesian_xy_mm / feedrate;                                 // seconds to move xy distance at requested rate
+        uint16_t segments = lroundf(delta_segments_per_second * seconds),                // preferred number of segments for distance @ feedrate
+                 seglimit = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
+        NOMORE(segments, seglimit);                                                     // limit to minimum segment length (fewer segments)
       #else
-        uint16_t segments = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
+        uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
       #endif
 
-      NOLESS( segments, 1 );                // must have at least one segment
-      float inv_segments = 1.0 / segments;  // divide once, multiply thereafter
+      NOLESS(segments, 1);                        // must have at least one segment
+      const float inv_segments = 1.0 / segments;  // divide once, multiply thereafter
 
       #if IS_SCARA // scale the feed rate from mm/s to degrees/s
         scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
@@ -560,57 +550,53 @@
                     difference[Y_AXIS] * inv_segments,
                     difference[Z_AXIS] * inv_segments,
                     difference[E_AXIS] * inv_segments 
-                    };
+                  };
 
       // Note that E segment distance could vary slightly as z mesh height
       // changes for each segment, but small enough to ignore.
 
-      bool above_fade_height = false;
-      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        if (( planner.z_fade_height != 0 ) && 
-            ( planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS]) )) {
-          above_fade_height = true;
-          }
-      #endif
+      const bool above_fade_height = (
+        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+          planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
+        #else
+          false
+        #endif
+      );
 
       // Only compute leveling per segment if ubl active and target below z_fade_height.
 
-      if (( ! ubl.state.active ) || ( above_fade_height )) {   // no mesh leveling
+      if (!ubl.state.active || above_fade_height) {   // no mesh leveling
 
         const float z_offset = ubl.state.active ? ubl.state.z_offset : 0.0;
 
-        float seg_dest[XYZE];                     // per-segment destination,
-        COPY_XYZE( seg_dest, current_position );  // starting from current position
+        float seg_dest[XYZE];              // per-segment destination,
+        COPY(seg_dest, current_position);  // starting from current position
 
         while (--segments) {
           LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
           float ztemp = seg_dest[Z_AXIS];
           seg_dest[Z_AXIS] += z_offset;
-          ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
+          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
           seg_dest[Z_AXIS] = ztemp;
         }
 
         // Since repeated adding segment_distance accumulates small errors, final move to exact destination.
-        COPY_XYZE( seg_dest, ltarget );
+        COPY(seg_dest, ltarget);
         seg_dest[Z_AXIS] += z_offset;
-        ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
+        ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
         return false; // moved but did not set_current_to_destination();
       }
 
       // Otherwise perform per-segment leveling
 
-      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        float fade_scaling_factor = ubl.fade_scaling_factor_for_z(ltarget[Z_AXIS]);
-      #endif
-
       float seg_dest[XYZE];  // per-segment destination, initialize to first segment
       LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i];
 
       const float& dx_seg = segment_distance[X_AXIS];  // alias for clarity
       const float& dy_seg = segment_distance[Y_AXIS];
 
-      float rx = RAW_X_POSITION(seg_dest[X_AXIS]);  // assume raw vs logical coordinates shifted but not scaled.
-      float ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
+      float rx = RAW_X_POSITION(seg_dest[X_AXIS]),  // assume raw vs logical coordinates shifted but not scaled.
+            ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
 
       do {  // for each mesh cell encountered during the move
 
@@ -621,74 +607,75 @@
         // in top of loop and again re-find same adjacent cell and use it, just less efficient
         // for mesh inset area.
 
-        int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
-               cell_xi = constrain( cell_xi, 0, (GRID_MAX_POINTS_X) - 1 );
+        int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
+               cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
 
-        int8_t cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
-               cell_yi = constrain( cell_yi, 0, (GRID_MAX_POINTS_Y) - 1 );
+        cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
+        cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
 
         // float x0 = (UBL_MESH_MIN_X) + ((MESH_X_DIST) * cell_xi );         // lower left cell corner
         // float y0 = (UBL_MESH_MIN_Y) + ((MESH_Y_DIST) * cell_yi );         // lower left cell corner
         // float x1 = x0 + MESH_X_DIST;                                      // upper right cell corner
         // float y1 = y0 + MESH_Y_DIST;                                      // upper right cell corner
 
-        float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi  ]));  // 64 byte table lookup avoids mul+add
-        float y0 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi  ]));  // 64 byte table lookup avoids mul+add
-        float x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+1]));  // 64 byte table lookup avoids mul+add
-        float y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1]));  // 64 byte table lookup avoids mul+add
+        const float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi  ])),  // 64 byte table lookup avoids mul+add
+                    y0 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi  ])),  // 64 byte table lookup avoids mul+add
+                    x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+1])),  // 64 byte table lookup avoids mul+add
+                    y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1])),  // 64 byte table lookup avoids mul+add
 
-        float cx = rx - x0;   // cell-relative x
-        float cy = ry - y0;   // cell-relative y
+                    cx = rx - x0,   // cell-relative x
+                    cy = ry - y0;   // cell-relative y
 
-        float z_x0y0 = ubl.z_values[cell_xi  ][cell_yi  ];  // z at lower left corner
-        float z_x1y0 = ubl.z_values[cell_xi+1][cell_yi  ];  // z at upper left corner
-        float z_x0y1 = ubl.z_values[cell_xi  ][cell_yi+1];  // z at lower right corner
-        float z_x1y1 = ubl.z_values[cell_xi+1][cell_yi+1];  // z at upper right corner
+        float z_x0y0 = ubl.z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
+              z_x1y0 = ubl.z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
+              z_x0y1 = ubl.z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
+              z_x1y1 = ubl.z_values[cell_xi+1][cell_yi+1];  // z at upper right corner
 
-        if ( isnan( z_x0y0 )) z_x0y0 = 0;     // ideally activating ubl.state.active (G29 A) 
-        if ( isnan( z_x1y0 )) z_x1y0 = 0;     //   should refuse if any invalid mesh points
-        if ( isnan( z_x0y1 )) z_x0y1 = 0;     //   in order to avoid isnan tests per cell,
-        if ( isnan( z_x1y1 )) z_x1y1 = 0;     //   thus guessing zero for undefined points
+        if (isnan(z_x0y0)) z_x0y0 = 0;              // ideally activating ubl.state.active (G29 A) 
+        if (isnan(z_x1y0)) z_x1y0 = 0;              //   should refuse if any invalid mesh points
+        if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
+        if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
 
-        float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0/MESH_X_DIST);   // z slope per x along y0 (lower left to lower right)
-        float z_xmy1 = (z_x1y1 - z_x0y1) * (1.0/MESH_X_DIST);   // z slope per x along y1 (upper left to upper right)
+        const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
+                    z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)
 
-        float z_cxy0 = z_x0y0 + z_xmy0 * cx;        // z height along y0 at cx
-        float z_cxy1 = z_x0y1 + z_xmy1 * cx;        // z height along y1 at cx
-        float z_cxyd = z_cxy1 - z_cxy0;             // z height difference along cx from y0 to y1
+              float z_cxy0 = z_x0y0 + z_xmy0 * cx;          // z height along y0 at cx
 
-        float z_cxym = z_cxyd * (1.0/MESH_Y_DIST);  // z slope per y along cx from y0 to y1
-        float z_cxcy = z_cxy0 + z_cxym * cy;        // z height along cx at cy
+        const float z_cxy1 = z_x0y1 + z_xmy1 * cx,          // z height along y1 at cx
+                    z_cxyd = z_cxy1 - z_cxy0;               // z height difference along cx from y0 to y1
+
+              float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)),  // z slope per y along cx from y0 to y1
+                    z_cxcy = z_cxy0 + z_cxym * cy;          // z height along cx at cy
 
         // As subsequent segments step through this cell, the z_cxy0 intercept will change
         // and the z_cxym slope will change, both as a function of cx within the cell, and
         // each change by a constant for fixed segment lengths.
 
-        float z_sxy0 = z_xmy0 * dx_seg;                                   // per-segment adjustment to z_cxy0
-        float z_sxym = ( z_xmy1 - z_xmy0 ) * (1.0/MESH_Y_DIST) * dx_seg;  // per-segment adjustment to z_cxym
+        const float z_sxy0 = z_xmy0 * dx_seg,                                     // per-segment adjustment to z_cxy0
+                    z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg;  // per-segment adjustment to z_cxym
 
         do {  // for all segments within this mesh cell
 
           z_cxcy += ubl.state.z_offset;
 
-          if ( --segments == 0 ) {          // this is last segment, use ltarget for exact
-            COPY_XYZE( seg_dest, ltarget );
+          if (--segments == 0) {          // this is last segment, use ltarget for exact
+            COPY(seg_dest, ltarget);
             seg_dest[Z_AXIS] += z_cxcy;
-            ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
+            ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
             return false;   // did not set_current_to_destination()
           }
 
-          float z_orig = seg_dest[Z_AXIS];    // remember the pre-leveled segment z value
-          seg_dest[Z_AXIS] = z_orig + z_cxcy; // adjust segment z height per mesh leveling
-          ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
-          seg_dest[Z_AXIS] = z_orig;          // restore pre-leveled z before incrementing
+          const float z_orig = seg_dest[Z_AXIS];  // remember the pre-leveled segment z value
+          seg_dest[Z_AXIS] = z_orig + z_cxcy;     // adjust segment z height per mesh leveling
+          ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
+          seg_dest[Z_AXIS] = z_orig;              // restore pre-leveled z before incrementing
 
           LOOP_XYZE(i) seg_dest[i] += segment_distance[i];  // adjust seg_dest for next segment
 
           cx += dx_seg;
           cy += dy_seg;
 
-          if ( !WITHIN(cx,0,MESH_X_DIST) || !WITHIN(cy,0,MESH_Y_DIST)) {  // done within this cell, break to next
+          if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) {  // done within this cell, break to next
             rx = RAW_X_POSITION(seg_dest[X_AXIS]);
             ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
             break;