From c9eb1d6ab74c03539af0421c14ae21df27ef06ed Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Fri, 31 Mar 2017 01:28:07 -0500
Subject: [PATCH] Make MBL a static class, use lookup for index-to-point
 conversion

---
 Marlin/Marlin_main.cpp       |  8 ++--
 Marlin/mesh_bed_leveling.cpp | 15 +++++++-
 Marlin/mesh_bed_leveling.h   | 74 ++++++++++++++++--------------------
 Marlin/ultralcd.cpp          |  2 +-
 4 files changed, 52 insertions(+), 47 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index e23207e6ab..d77c1b6451 100755
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -652,7 +652,7 @@ static bool send_ok[BUFSIZE];
 #endif
 
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
-  static MarlinBusyState busy_state = NOT_BUSY;
+  MarlinBusyState busy_state = NOT_BUSY;
   static millis_t next_busy_signal_ms = 0;
   uint8_t host_keepalive_interval = DEFAULT_KEEPALIVE_INTERVAL;
 #else
@@ -3839,7 +3839,7 @@ inline void gcode_G28() {
         // If there's another point to sample, move there with optional lift.
         if (probe_index < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
           mbl.zigzag(probe_index, px, py);
-          _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
+          _mbl_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]);
 
           #if HAS_SOFTWARE_ENDSTOPS
             // Disable software endstops to allow manual adjustment
@@ -9649,14 +9649,14 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
     const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
     if (cx2 != cx1 && TEST(x_splits, gcx)) {
       COPY(end, destination);
-      destination[X_AXIS] = LOGICAL_X_POSITION(mbl.get_probe_x(gcx));
+      destination[X_AXIS] = LOGICAL_X_POSITION(mbl.index_to_xpos[gcx]);
       normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
       destination[Y_AXIS] = MBL_SEGMENT_END(Y);
       CBI(x_splits, gcx);
     }
     else if (cy2 != cy1 && TEST(y_splits, gcy)) {
       COPY(end, destination);
-      destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.get_probe_y(gcy));
+      destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.index_to_ypos[gcy]);
       normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
       destination[X_AXIS] = MBL_SEGMENT_END(X);
       CBI(y_splits, gcy);
diff --git a/Marlin/mesh_bed_leveling.cpp b/Marlin/mesh_bed_leveling.cpp
index fa45198c6f..8809b0754e 100644
--- a/Marlin/mesh_bed_leveling.cpp
+++ b/Marlin/mesh_bed_leveling.cpp
@@ -26,7 +26,20 @@
 
   mesh_bed_leveling mbl;
 
-  mesh_bed_leveling::mesh_bed_leveling() { reset(); }
+  uint8_t mesh_bed_leveling::status;
+
+  float mesh_bed_leveling::z_offset,
+        mesh_bed_leveling::z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS],
+        mesh_bed_leveling::index_to_xpos[MESH_NUM_X_POINTS],
+        mesh_bed_leveling::index_to_ypos[MESH_NUM_Y_POINTS];
+
+  mesh_bed_leveling::mesh_bed_leveling() {
+    for (uint8_t i = 0; i < MESH_NUM_X_POINTS; ++i)
+      index_to_xpos[i] = MESH_MIN_X + i * (MESH_X_DIST);
+    for (uint8_t i = 0; i < MESH_NUM_Y_POINTS; ++i)
+      index_to_ypos[i] = MESH_MIN_Y + i * (MESH_Y_DIST);
+    reset();
+  }
 
   void mesh_bed_leveling::reset() {
     status = MBL_STATUS_NONE;
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
index fd0d1d7b1a..6a683627b8 100644
--- a/Marlin/mesh_bed_leveling.h
+++ b/Marlin/mesh_bed_leveling.h
@@ -40,91 +40,83 @@
     MBL_STATUS_REACTIVATE_BIT = 2
   };
 
-  #define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X))/(MESH_NUM_X_POINTS - 1))
-  #define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y))/(MESH_NUM_Y_POINTS - 1))
+  #define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X)) / (MESH_NUM_X_POINTS - 1))
+  #define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y)) / (MESH_NUM_Y_POINTS - 1))
 
   class mesh_bed_leveling {
   public:
-    uint8_t status; // Has Mesh and Is Active bits
-    float z_offset;
-    float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
+    static uint8_t status; // Has Mesh and Is Active bits
+    static float z_offset,
+                 z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS],
+                 index_to_xpos[MESH_NUM_X_POINTS],
+                 index_to_ypos[MESH_NUM_Y_POINTS];
 
     mesh_bed_leveling();
 
-    void reset();
+    static void reset();
 
-    static FORCE_INLINE float get_probe_x(const int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; }
-    static FORCE_INLINE float get_probe_y(const int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
-    void set_z(const int8_t px, const int8_t py, const float &z) { z_values[py][px] = z; }
+    static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[py][px] = z; }
 
-    bool active() const                 { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
-    void set_active(const bool onOff)   { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); }
-    bool has_mesh() const               { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
-    void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); }
-    bool reactivate()                   { bool b = TEST(status, MBL_STATUS_REACTIVATE_BIT); CBI(status, MBL_STATUS_REACTIVATE_BIT); return b; }
-    void set_reactivate(const bool onOff) { onOff ? SBI(status, MBL_STATUS_REACTIVATE_BIT) : CBI(status, MBL_STATUS_REACTIVATE_BIT); }
+    static bool active()                       { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
+    static void set_active(const bool onOff)   { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); }
+    static bool has_mesh()                     { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
+    static void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); }
+    static bool reactivate()                   { bool b = TEST(status, MBL_STATUS_REACTIVATE_BIT); CBI(status, MBL_STATUS_REACTIVATE_BIT); return b; }
+    static void set_reactivate(const bool onOff) { onOff ? SBI(status, MBL_STATUS_REACTIVATE_BIT) : CBI(status, MBL_STATUS_REACTIVATE_BIT); }
 
-    inline void zigzag(const int8_t index, int8_t &px, int8_t &py) const {
+    static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) {
       px = index % (MESH_NUM_X_POINTS);
       py = index / (MESH_NUM_X_POINTS);
       if (py & 1) px = (MESH_NUM_X_POINTS - 1) - px; // Zig zag
     }
 
-    void set_zigzag_z(const int8_t index, const float &z) {
+    static void set_zigzag_z(const int8_t index, const float &z) {
       int8_t px, py;
       zigzag(index, px, py);
       set_z(px, py, z);
     }
 
-    int8_t cell_index_x(const float &x) const {
+    static int8_t cell_index_x(const float &x) {
       int8_t cx = (x - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
       return constrain(cx, 0, (MESH_NUM_X_POINTS) - 2);
     }
 
-    int8_t cell_index_y(const float &y) const {
+    static int8_t cell_index_y(const float &y) {
       int8_t cy = (y - (MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
       return constrain(cy, 0, (MESH_NUM_Y_POINTS) - 2);
     }
 
-    int8_t probe_index_x(const float &x) const {
+    static int8_t probe_index_x(const float &x) {
       int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST));
       return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1;
     }
 
-    int8_t probe_index_y(const float &y) const {
+    static int8_t probe_index_y(const float &y) {
       int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST));
       return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1;
     }
 
-    float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) const {
+    static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
       const float delta_z = (z2 - z1) / (a2 - a1);
       const float delta_a = a0 - a1;
       return z1 + delta_a * delta_z;
     }
 
-    float get_z(const float &x0, const float &y0
+    static float get_z(const float &x0, const float &y0
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
         , const float &factor
       #endif
-    ) const {
-      int8_t cx = cell_index_x(x0),
-             cy = cell_index_y(y0);
-      if (cx < 0 || cy < 0) return z_offset;
-      float z1 = calc_z0(x0,
-                         get_probe_x(cx), z_values[cy][cx],
-                         get_probe_x(cx + 1), z_values[cy][cx + 1]);
-      float z2 = calc_z0(x0,
-                         get_probe_x(cx), z_values[cy + 1][cx],
-                         get_probe_x(cx + 1), z_values[cy + 1][cx + 1]);
-      float z0 = calc_z0(y0,
-                         get_probe_y(cy), z1,
-                         get_probe_y(cy + 1), z2);
+    ) {
+      const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0);
+      const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cy][cx], index_to_xpos[cx + 1], z_values[cy][cx + 1]),
+                  z2 = calc_z0(x0, index_to_xpos[cx], z_values[cy + 1][cx], index_to_xpos[cx + 1], z_values[cy + 1][cx + 1]),
+                  z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2);
 
-      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        return z0 * factor + z_offset;
-      #else
-        return z0 + z_offset;
-      #endif
+      return z_offset + z0
+        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+          * factor
+        #endif
+      ;
     }
   };
 
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index f2ff7d4f19..02cb3e239b 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1437,7 +1437,7 @@ KeepDrawing:
       // _manual_probe_xy runs the menu loop until the move is done
       int8_t px, py;
       mbl.zigzag(manual_probe_index, px, py);
-      _manual_probe_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
+      _manual_probe_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]);
 
       // After the blocking function returns, change menus
       lcd_goto_screen(_lcd_level_bed_get_z);