From ae676490c9b145b05ee1e85fbacb5d335b734385 Mon Sep 17 00:00:00 2001
From: Brian <bgort@users.noreply.github.com>
Date: Thu, 11 May 2017 12:10:38 -0400
Subject: [PATCH] M421:  Add 'adjust closest point' capability - Split M421
 into separate versions for bilinear and ubl - Fix minor issue in G26

---
 Marlin/G26_Mesh_Validation_Tool.cpp |  4 +-
 Marlin/Marlin_main.cpp              | 72 ++++++++++++++++++++++-------
 Marlin/ubl.h                        |  3 ++
 Marlin/ubl_G29.cpp                  |  3 --
 4 files changed, 61 insertions(+), 21 deletions(-)

diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp
index 56a2e4950..896eb4b8b 100644
--- a/Marlin/G26_Mesh_Validation_Tool.cpp
+++ b/Marlin/G26_Mesh_Validation_Tool.cpp
@@ -740,12 +740,14 @@
     }
 
     if (code_seen('R')) {
-      g26_repeats = code_has_value() ? code_value_int() - 1 : 999;
+      g26_repeats = code_has_value() ? code_value_int() : 999;
 
       if (g26_repeats <= 0) {
         SERIAL_PROTOCOLLNPGM("?(R)epeat value not plausible; must be greater than 0.");
         return UBL_ERR;
       }
+
+      g26_repeats--;
     }
 
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 63ae0a083..7eb417db7 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -8477,7 +8477,7 @@ void quickstop_stepper() {
     }
   }
 
-#elif ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL)
+#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
 
   /**
    * M421: Set a single Mesh Bed Leveling Z coordinate
@@ -8490,8 +8490,8 @@ void quickstop_stepper() {
     int8_t px = 0, py = 0;
     float z = 0;
     bool hasI, hasJ, hasZ, hasQ;
-    if ((hasI = code_seen('I'))) px = code_value_linear_units();
-    if ((hasJ = code_seen('J'))) py = code_value_linear_units();
+    if ((hasI = code_seen('I'))) px = code_value_int();
+    if ((hasJ = code_seen('J'))) py = code_value_int();
     if ((hasZ = code_seen('Z'))) z = code_value_linear_units();
     if ((hasQ = code_seen('Q'))) z = code_value_linear_units();
 
@@ -8503,23 +8503,15 @@ void quickstop_stepper() {
 
     if (WITHIN(px, 0, GRID_MAX_POINTS_X - 1) && WITHIN(py, 0, GRID_MAX_POINTS_Y - 1)) {
       if (hasZ) { // doing an absolute mesh value
-        #if ENABLED(AUTO_BED_LEVELING_UBL)
-          ubl.z_values[px][py] = z;
-        #else
-          z_values[px][py] = z;
-          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
-            bed_level_virt_interpolate();
-          #endif
+        z_values[px][py] = z;
+        #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+          bed_level_virt_interpolate();
         #endif
       } 
       else { // doing an offset of a mesh value
-        #if ENABLED(AUTO_BED_LEVELING_UBL)
-          ubl.z_values[px][py] += z;
-        #else
-          z_values[px][py] += z;
-          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
-            bed_level_virt_interpolate();
-          #endif
+        z_values[px][py] += z;
+        #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+          bed_level_virt_interpolate();
         #endif
       }
     }
@@ -8528,6 +8520,52 @@ void quickstop_stepper() {
       SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
     }
   }
+
+#elif ENABLED(AUTO_BED_LEVELING_UBL)
+
+  /**
+   * M421: Set a single Mesh Bed Leveling Z coordinate
+   *
+   *   M421 I<xindex> J<yindex> Z<linear>
+   *   or
+   *   M421 I<xindex> J<yindex> Q<offset>
+   */
+
+  //todo:  change multiple points simultaneously?
+
+  inline void gcode_M421() {
+    int8_t px = 0, py = 0;
+    float z = 0;
+    bool hasI, hasJ, hasZ, hasQ, hasC;
+    if ((hasI = code_seen('I'))) px = code_value_int();
+    if ((hasJ = code_seen('J'))) py = code_value_int();
+    if ((hasZ = code_seen('Z'))) z = code_value_linear_units();
+    if ((hasQ = code_seen('Q'))) z = code_value_linear_units();
+    hasC = code_seen('C');
+
+    if ( (!(hasI && hasJ) && !hasC) || (hasQ && hasZ) || (!hasQ && !hasZ)) {
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
+      return;
+    }
+
+    if (hasC) { // get closest position
+      const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, NULL, false);
+      px = location.x_index;
+      py = location.y_index;
+    }
+
+    if (WITHIN(px, 0, GRID_MAX_POINTS_X - 1) && WITHIN(py, 0, GRID_MAX_POINTS_Y - 1)) {
+      if (hasZ) // doing an absolute mesh value
+        ubl.z_values[px][py] = z;
+      else // doing an offset of a mesh value
+        ubl.z_values[px][py] += z;
+    }
+    else { // bad indexes were specified for the mesh point
+      SERIAL_ERROR_START;
+      SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
+    }
+  }
 #endif
 
 #if HAS_M206_COMMAND
diff --git a/Marlin/ubl.h b/Marlin/ubl.h
index b33b5d21f..4ae4c0504 100644
--- a/Marlin/ubl.h
+++ b/Marlin/ubl.h
@@ -35,6 +35,9 @@
   #define UBL_OK false
   #define UBL_ERR true
 
+  #define USE_NOZZLE_AS_REFERENCE 0
+  #define USE_PROBE_AS_REFERENCE 1
+
   typedef struct {
     int8_t x_index, y_index;
     float distance; // When populated, the distance from the search location
diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp
index cee20f174..82efb2fa0 100644
--- a/Marlin/ubl_G29.cpp
+++ b/Marlin/ubl_G29.cpp
@@ -311,9 +311,6 @@
    *   we now have the functionality and features of all three systems combined.
    */
 
-  #define USE_NOZZLE_AS_REFERENCE 0
-  #define USE_PROBE_AS_REFERENCE 1
-
   // The simple parameter flags and values are 'static' so parameter parsing can be in a support routine.
   static int g29_verbose_level, phase_value, repetition_cnt,
              storage_slot = 0, map_type, grid_size;