From 543935828134831a3129247fcfcf3cb071048011 Mon Sep 17 00:00:00 2001
From: Roxy-3D <Roxy-3D@users.noreply.github.com>
Date: Thu, 19 Oct 2017 10:44:45 -0500
Subject: [PATCH] Basic UBL operations working on 32-bit platforms (#8024)

* 32-bit work for UBL

* Update FT i3-2020 reference file
---
 .../HAL/HAL_LPC1768/persistent_store_impl.cpp |   4 +-
 .../Folger Tech/i3-2020/Configuration.h       |   4 +-
 .../Folger Tech/i3-2020/Configuration_adv.h   |   2 +-
 .../bedlevel/ubl/G26_Mesh_Validation_Tool.cpp |   2 +-
 Marlin/src/feature/bedlevel/ubl/ubl.h         |   3 +-
 Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp   | 133 ++++++++++++------
 Marlin/src/gcode/bedlevel/ubl/M421.cpp        |   2 +-
 Marlin/src/gcode/calibrate/M48.cpp            |   8 +-
 Marlin/src/gcode/parser.h                     |   2 +-
 9 files changed, 101 insertions(+), 59 deletions(-)

diff --git a/Marlin/src/HAL/HAL_LPC1768/persistent_store_impl.cpp b/Marlin/src/HAL/HAL_LPC1768/persistent_store_impl.cpp
index 222cb5b08af..c8aaa0f315b 100644
--- a/Marlin/src/HAL/HAL_LPC1768/persistent_store_impl.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/persistent_store_impl.cpp
@@ -21,7 +21,7 @@ FIL eeprom_file;
 bool access_start() {
   UINT file_size = 0,
        bytes_written = 0;
-  const char eeprom_zero = 0xFF;
+  const char eeprom_erase_value = 0xFF;
   MSC_Aquire_Lock();
   if (f_mount(&fat_fs, "", 1)) {
     MSC_Release_Lock();
@@ -35,7 +35,7 @@ bool access_start() {
   if (res == FR_OK) {
     f_lseek(&eeprom_file, file_size);
     while (file_size <= E2END && res == FR_OK) {
-      res = f_write(&eeprom_file, &eeprom_zero, 1, &bytes_written);
+      res = f_write(&eeprom_file, &eeprom_erase_value, 1, &bytes_written);
       file_size++;
     }
   }
diff --git a/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration.h b/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration.h
index ca2eddf9b65..33068731902 100644
--- a/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration.h	
+++ b/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration.h	
@@ -125,7 +125,7 @@
 
 // Optional custom name for your RepStrap or other custom machine
 // Displayed in the LCD "Ready" message
-#define CUSTOM_MACHINE_NAME "FT-2020 v3"
+#define CUSTOM_MACHINE_NAME "FT-2020 v4"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -1677,7 +1677,7 @@
 // Servo deactivation
 //
 // With this option servos are powered only during movement, then turned off to prevent jitter.
-#define DEACTIVATE_SERVOS_AFTER_MOVE
+//#define DEACTIVATE_SERVOS_AFTER_MOVE
 
 /**
  * Filament Width Sensor
diff --git a/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration_adv.h b/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration_adv.h
index 1119045dcd2..2822cee7953 100644
--- a/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration_adv.h	
+++ b/Marlin/src/config/examples/Folger Tech/i3-2020/Configuration_adv.h	
@@ -1338,7 +1338,7 @@
  * For clients that use a fixed-width font (like OctoPrint), leave this set to 1.0.
  * Otherwise, adjust according to your client and font.
  */
-#define PROPORTIONAL_FONT_RATIO 1.5
+#define PROPORTIONAL_FONT_RATIO 2.2
 
 /**
  * Spend 28 bytes of SRAM to optimize the GCode parser
diff --git a/Marlin/src/feature/bedlevel/ubl/G26_Mesh_Validation_Tool.cpp b/Marlin/src/feature/bedlevel/ubl/G26_Mesh_Validation_Tool.cpp
index 19d1a1b942a..52b37e6a1bd 100644
--- a/Marlin/src/feature/bedlevel/ubl/G26_Mesh_Validation_Tool.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/G26_Mesh_Validation_Tool.cpp
@@ -143,7 +143,7 @@
 // Private functions
 
 static uint16_t circle_flags[16], horizontal_mesh_line_flags[16], vertical_mesh_line_flags[16];
-float g26_e_axis_feedrate = 0.020,
+float g26_e_axis_feedrate = 0.025,
       random_deviation = 0.0;
 
 static bool g26_retracted = false; // Track the retracted state of the nozzle so mismatched
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h
index 050a2dd6646..20333895f40 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl.h
+++ b/Marlin/src/feature/bedlevel/ubl/ubl.h
@@ -141,7 +141,8 @@ class unified_bed_leveling {
     static void save_ubl_active_state_and_disable();
     static void restore_ubl_active_state_and_leave();
     static void display_map(const int);
-    static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[16], bool);
+    static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[16]);
+    static mesh_index_pair find_furthest_invalid_mesh_point();
     static void reset();
     static void invalidate();
     static void set_all_mesh_points_to_value(const float);
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
index 74e26f8442b..f1eb6b7f001 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -333,7 +333,7 @@
       else {
         while (g29_repetition_cnt--) {
           if (cnt > 20) { cnt = 0; idle(); }
-          const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
+          const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL);
           if (location.x_index < 0) {
             // No more REACHABLE mesh points to invalidate, so we ASSUME the user
             // meant to invalidate the ENTIRE mesh, which cannot be done with
@@ -529,7 +529,7 @@
             }
             else {
               while (g29_repetition_cnt--) {  // this only populates reachable mesh points near
-                const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
+                const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL);
                 if (location.x_index < 0) {
                   // No more REACHABLE INVALID mesh points to populate, so we ASSUME
                   // user meant to populate ALL INVALID mesh points to value
@@ -744,6 +744,8 @@
       uint16_t max_iterations = GRID_MAX_POINTS;
 
       do {
+        if (do_ubl_mesh_map) display_map(g29_map_type);
+
         #if ENABLED(NEWPANEL)
           if (ubl_lcd_clicked()) {
             SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
@@ -757,7 +759,10 @@
           }
         #endif
 
-        location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, close_or_far);
+        if (close_or_far)
+          location = find_furthest_invalid_mesh_point();
+        else
+          location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL);
 
         if (location.x_index >= 0) {    // mesh point found and is reachable by probe
           const float rawx = mesh_index_to_xpos(location.x_index),
@@ -767,8 +772,6 @@
           z_values[location.x_index][location.y_index] = measured_z;
         }
 
-        if (do_ubl_mesh_map) display_map(g29_map_type);
-
       } while (location.x_index >= 0 && --max_iterations);
 
       STOW_PROBE();
@@ -962,7 +965,7 @@
 
       mesh_index_pair location;
       do {
-        location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_NOZZLE_AS_REFERENCE, NULL, false);
+        location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_NOZZLE_AS_REFERENCE, NULL);
         // It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
         if (location.x_index < 0 && location.y_index < 0) continue;
 
@@ -1289,7 +1292,7 @@
    */
   void unified_bed_leveling::g29_eeprom_dump() {
     unsigned char cccc;
-    uint16_t kkkk;
+    unsigned int  kkkk;  // Needs to be of unspecfied size to compile clean on all platforms
 
     SERIAL_ECHO_START();
     SERIAL_ECHOLNPGM("EEPROM Dump:");
@@ -1299,7 +1302,7 @@
       SERIAL_ECHOPGM(": ");
       for (uint16_t j = 0; j < 16; j++) {
         kkkk = i + j;
-        eeprom_read_block(&cccc, (void *)kkkk, 1);
+        eeprom_read_block(&cccc, (const void *) kkkk, sizeof(unsigned char));
         print_hex_byte(cccc);
         SERIAL_ECHO(' ');
       }
@@ -1345,18 +1348,84 @@
         z_values[x][y] -= tmp_z_values[x][y];
   }
 
-  mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, uint16_t bits[16], const bool far_flag) {
+
+  mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
+
+    bool found_a_NAN  = false;
+    bool found_a_real = false;
     mesh_index_pair out_mesh;
     out_mesh.x_index = out_mesh.y_index = -1;
+    out_mesh.distance = -99999.99;
+
+    for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
+      for (int8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
+
+        if ( isnan(z_values[i][j])) { // Check to see if this location holds an invalid mesh point
+
+          const float mx = mesh_index_to_xpos(i),
+                      my = mesh_index_to_ypos(j);
+
+          if ( !position_is_reachable_by_probe_raw_xy(mx, my))  // make sure the probe can get to the mesh point
+            continue;
+
+          found_a_NAN = true;
+
+          int8_t closest_x=-1, closest_y=-1;
+          float d1, d2 = 99999.9;
+          for (int8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
+            for (int8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
+              if (!isnan(z_values[k][l])) {
+                found_a_real = true;
+
+          // Add in a random weighting factor that scrambles the probing of the
+          // last half of the mesh (when every unprobed mesh point is one index
+          // from a probed location).
+
+                d1 = HYPOT(i - k, j - l) + (1.0 / ((millis() % 47) + 13));
+
+                if (d1 < d2) {    // found a closer distance from invalid mesh point at (i,j) to defined mesh point at (k,l)
+                  d2 = d1;       // found a closer location with
+                  closest_x = i;    // an assigned mesh point value
+                  closest_y = j;
+                }
+              }
+            }
+          }
+
+          //
+          // at this point d2 should have the closest defined mesh point to invalid mesh point (i,j)
+          //
+
+          if (found_a_real && (closest_x >= 0) && (d2 > out_mesh.distance)) {
+            out_mesh.distance = d2;         // found an invalid location with a greater distance
+            out_mesh.x_index = closest_x;   // to a defined mesh point
+            out_mesh.y_index = closest_y;
+          }
+        }
+      } // for j
+    } // for i
+
+    if (!found_a_real && found_a_NAN) {        // if the mesh is totally unpopulated, start the probing
+      out_mesh.x_index = GRID_MAX_POINTS_X / 2;
+      out_mesh.y_index = GRID_MAX_POINTS_Y / 2;
+      out_mesh.distance = 1.0;
+    }
+    return out_mesh;
+  }
+
+  mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, uint16_t bits[16]) {
+    mesh_index_pair out_mesh;
+    out_mesh.x_index = out_mesh.y_index = -1;
+    out_mesh.distance = -99999.9;
 
     // Get our reference position. Either the nozzle or probe location.
     const float px = RAW_X_POSITION(lx) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
                 py = RAW_Y_POSITION(ly) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
 
-    float best_so_far = far_flag ? -99999.99 : 99999.99;
+    float best_so_far = 99999.99;
 
-    for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
-      for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
+    for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
+      for (int8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
 
         if ( (type == INVALID && isnan(z_values[i][j]))  // Check to see if this location holds the right thing
           || (type == REAL && !isnan(z_values[i][j]))
@@ -1376,35 +1445,14 @@
             continue;
 
           // Reachable. Check if it's the best_so_far location to the nozzle.
-          // Add in a weighting factor that considers the current location of the nozzle.
 
           float distance = HYPOT(px - mx, py - my);
 
-          /**
-           * If doing the far_flag action, we want to be as far as possible
-           * from the starting point and from any other probed points. We
-           * want the next point spread out and filling in any blank spaces
-           * in the mesh. So we add in some of the distance to every probed
-           * point we can find.
-           */
-          if (far_flag) {
-            for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
-              for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
-                if (i != k && j != l && !isnan(z_values[k][l])) {
-                  //distance += pow((float) abs(i - k) * (MESH_X_DIST), 2) + pow((float) abs(j - l) * (MESH_Y_DIST), 2);  // working here
-                  distance += HYPOT(MESH_X_DIST, MESH_Y_DIST) / log(HYPOT((i - k) * (MESH_X_DIST) + .001, (j - l) * (MESH_Y_DIST)) + .001);
-                }
-              }
-            }
-          }
-          else
           // factor in the distance from the current location for the normal case
           // so the nozzle isn't running all over the bed.
-            distance += HYPOT(raw_x - mx, raw_y - my) * 0.1;
-
-          // if far_flag, look for farthest point
-          if (far_flag == (distance > best_so_far) && distance != best_so_far) {
-            best_so_far = distance;   // We found a closer/farther location with
+          distance += HYPOT(raw_x - mx, raw_y - my) * 0.1;
+          if (distance < best_so_far) {
+            best_so_far = distance;   // We found a closer location with
             out_mesh.x_index = i;     // the specified type of mesh value.
             out_mesh.y_index = j;
             out_mesh.distance = best_so_far;
@@ -1412,7 +1460,6 @@
         }
       } // for j
     } // for i
-
     return out_mesh;
   }
 
@@ -1448,7 +1495,7 @@
       uint16_t not_done[16];
       memset(not_done, 0xFF, sizeof(not_done));
       do {
-        location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false);
+        location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done);
 
         if (location.x_index < 0) break; // stop when we can't find any more reachable points.
 
@@ -1572,16 +1619,10 @@
       info3 PROGMEM = { GRID_MAX_POINTS_X - 1, 0,  0, GRID_MAX_POINTS_Y,      true  };  // Right side of the mesh looking left
     static const smart_fill_info * const info[] PROGMEM = { &info0, &info1, &info2, &info3 };
 
-    // static const smart_fill_info info[] PROGMEM = {
-    //   { 0, GRID_MAX_POINTS_X,      0, GRID_MAX_POINTS_Y - 2,  false } PROGMEM,  // Bottom of the mesh looking up
-    //   { 0, GRID_MAX_POINTS_X,      GRID_MAX_POINTS_Y - 1, 0,  false } PROGMEM,  // Top of the mesh looking down
-    //   { 0, GRID_MAX_POINTS_X - 2,  0, GRID_MAX_POINTS_Y,      true  } PROGMEM,  // Left side of the mesh looking right
-    //   { GRID_MAX_POINTS_X - 1, 0,  0, GRID_MAX_POINTS_Y,      true  } PROGMEM   // Right side of the mesh looking left
-    // };
     for (uint8_t i = 0; i < COUNT(info); ++i) {
       const smart_fill_info *f = (smart_fill_info*)pgm_read_ptr(&info[i]);
-      const int8_t sx = pgm_read_word(&f->sx), sy = pgm_read_word(&f->sy),
-                   ex = pgm_read_word(&f->ex), ey = pgm_read_word(&f->ey);
+      const int8_t sx = pgm_read_byte(&f->sx), sy = pgm_read_byte(&f->sy),
+                   ex = pgm_read_byte(&f->ex), ey = pgm_read_byte(&f->ey);
       if (pgm_read_byte(&f->yfirst)) {
         const int8_t dir = ex > sx ? 1 : -1;
         for (uint8_t y = sy; y != ey; ++y)
diff --git a/Marlin/src/gcode/bedlevel/ubl/M421.cpp b/Marlin/src/gcode/bedlevel/ubl/M421.cpp
index 07ca280e11c..83c8cbbc397 100644
--- a/Marlin/src/gcode/bedlevel/ubl/M421.cpp
+++ b/Marlin/src/gcode/bedlevel/ubl/M421.cpp
@@ -49,7 +49,7 @@ void GcodeSuite::M421() {
              hasQ = !hasZ && parser.seen('Q');
 
   if (hasC) {
-    const mesh_index_pair location = ubl.find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, NULL, false);
+    const mesh_index_pair location = ubl.find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, NULL);
     ix = location.x_index;
     iy = location.y_index;
   }
diff --git a/Marlin/src/gcode/calibrate/M48.cpp b/Marlin/src/gcode/calibrate/M48.cpp
index a910f02ee6e..ced53d47eda 100644
--- a/Marlin/src/gcode/calibrate/M48.cpp
+++ b/Marlin/src/gcode/calibrate/M48.cpp
@@ -137,13 +137,13 @@ void GcodeSuite::M48() {
     for (uint8_t n = 0; n < n_samples; n++) {
       if (n_legs) {
         const int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
-        float angle = random(0.0, 360.0);
+        float angle = random(0, 360);
         const float radius = random(
           #if ENABLED(DELTA)
-            0.1250000000 * (DELTA_PROBEABLE_RADIUS),
-            0.3333333333 * (DELTA_PROBEABLE_RADIUS)
+            (int) (0.1250000000 * (DELTA_PROBEABLE_RADIUS)),
+            (int) (0.3333333333 * (DELTA_PROBEABLE_RADIUS))
           #else
-            5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE)
+            (int) 5.0, (int) (0.125 * min(X_BED_SIZE, Y_BED_SIZE))
           #endif
         );
 
diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
index e792c07a2e1..a8b7a54e8d8 100644
--- a/Marlin/src/gcode/parser.h
+++ b/Marlin/src/gcode/parser.h
@@ -294,7 +294,7 @@ public:
 
   // Provide simple value accessors with default option
   FORCE_INLINE static float    floatval(const char c, const float dval=0.0)   { return seenval(c) ? value_float()        : dval; }
-  FORCE_INLINE static bool     boolval(const char c)                          { return seenval(c) ? value_bool()      : seen(c); }
+  FORCE_INLINE static bool     boolval(const char c)                          { return seenval(c) ? value_bool()         : seen(c); }
   FORCE_INLINE static uint8_t  byteval(const char c, const uint8_t dval=0)    { return seenval(c) ? value_byte()         : dval; }
   FORCE_INLINE static int16_t  intval(const char c, const int16_t dval=0)     { return seenval(c) ? value_int()          : dval; }
   FORCE_INLINE static uint16_t ushortval(const char c, const uint16_t dval=0) { return seenval(c) ? value_ushort()       : dval; }