Fix Horizontal and Vertical UBL optimization bug. (#6540)

* Fix Horizontal and Vertical optimization bug.

And drastically reduce the debug code foot print....

* Yeah Baby!

The debug print is going A - OK !

Marlin/ubl_motion.cpp

@@ -1,3 +1,4 @@
+
 /**
  * Marlin 3D Printer Firmware
  * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
@@ -167,26 +168,6 @@
 
       float z0 = z1 + (z2 - z1) * yratio;
 
-      /**
-       * Debug code to use non-optimized get_z_correction() and to do a sanity check
-       * that the correct value is being passed to planner.buffer_line()
-       */
-      /*
-        z_optimized = z0;
-        z0 = ubl.get_z_correction(end[X_AXIS], end[Y_AXIS]);
-        if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
-        debug_current_and_destination(PSTR("FINAL_MOVE: z_correction()"));
-        if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
-        if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN  ");
-        SERIAL_ECHOPAIR("  end[X_AXIS]=", end[X_AXIS]);
-        SERIAL_ECHOPAIR("  end[Y_AXIS]=", end[Y_AXIS]);
-        SERIAL_ECHOPAIR("  z0=", z0);
-        SERIAL_ECHOPAIR("  z_optimized=", z_optimized);
-        SERIAL_ECHOPAIR("  err=",fabs(z_optimized - z0));
-        SERIAL_EOL;
-        }
-      */
-
       z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
       /**
@@ -218,8 +199,8 @@
     const float dx = end[X_AXIS] - start[X_AXIS],
                 dy = end[Y_AXIS] - start[Y_AXIS];
 
-    const int left_flag = dx < 0.0 ? 1 : 0,
-              down_flag = dy < 0.0 ? 1 : 0;
+    const int left_flag = dx < 0.0 ? 1.0 : 0.0,
+              down_flag = dy < 0.0 ? 1.0 : 0.0;
 
     const float adx = left_flag ? -dx : dx,
                 ady = down_flag ? -dy : dy;
@@ -250,9 +231,8 @@
     const float m = dy / dx,
                 c = start[Y_AXIS] - m * start[X_AXIS];
 
-    const bool inf_normalized_flag = isinf(e_normalized_dist),
-               inf_m_flag = isinf(m);
-
+    const bool inf_normalized_flag=isinf(e_normalized_dist),
+               inf_m_flag=isinf(m);
     /**
      * This block handles vertical lines. These are lines that stay within the same
      * X Cell column. They do not need to be perfectly vertical. They just can
@@ -273,26 +253,6 @@
 
         float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi);
 
-        /**
-         * Debug code to use non-optimized get_z_correction() and to do a sanity check
-         * that the correct value is being passed to planner.buffer_line()
-         */
-        /*
-          z_optimized = z0;
-          z0 = ubl.get_z_correction(x, next_mesh_line_y);
-          if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
-            debug_current_and_destination(PSTR("VERTICAL z_correction()"));
-          if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
-            if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN  ");
-          SERIAL_ECHOPAIR("  x=", x);
-          SERIAL_ECHOPAIR("  next_mesh_line_y=", next_mesh_line_y);
-          SERIAL_ECHOPAIR("  z0=", z0);
-          SERIAL_ECHOPAIR("  z_optimized=", z_optimized);
-          SERIAL_ECHOPAIR("  err=",fabs(z_optimized-z0));
-          SERIAL_ECHO("\n");
-          }
-        */
-
         z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
@@ -314,13 +274,23 @@
          */
         if (y != start[Y_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = y - start[Y_AXIS];                               // we don't need to check if the extruder position
-            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a vertical move
+
+//          on_axis_distance = y - start[Y_AXIS];                               
+            on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
+
+//          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
+//          on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
+
+//          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
+//          on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
+
+
+            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
           else {
-            e_position = start[E_AXIS];
-            z_position = start[Z_AXIS];
+            e_position = end[E_AXIS];
+            z_position = end[Z_AXIS];
           }
 
           planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -354,30 +324,10 @@
       while (current_xi != cell_dest_xi + left_flag) {
         current_xi += dxi;
         const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi]))),
-                    y = m * next_mesh_line_x + c;   // Calculate X at the next Y mesh line
+                    y = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
 
         float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
 
-        /**
-         * Debug code to use non-optimized get_z_correction() and to do a sanity check
-         * that the correct value is being passed to planner.buffer_line()
-         */
-        /*
-          z_optimized = z0;
-          z0 = ubl.get_z_correction(next_mesh_line_x, y);
-          if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
-            debug_current_and_destination(PSTR("HORIZONTAL z_correction()"));
-          if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
-            if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN  ");
-          SERIAL_ECHOPAIR("  next_mesh_line_x=", next_mesh_line_x);
-          SERIAL_ECHOPAIR("  y=", y);
-          SERIAL_ECHOPAIR("  z0=", z0);
-          SERIAL_ECHOPAIR("  z_optimized=", z_optimized);
-          SERIAL_ECHOPAIR("  err=",fabs(z_optimized-z0));
-          SERIAL_ECHO("\n");
-          }
-        */
-
         z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
@@ -399,13 +349,19 @@
          */
         if (x != start[X_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = x - start[X_AXIS];                               // we don't need to check if the extruder position
+
+//          on_axis_distance = x - start[X_AXIS];
+            on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
+
+//          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
+//          on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
+
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
           else {
-            e_position = start[E_AXIS];
-            z_position = start[Z_AXIS];
+            e_position = end[E_AXIS];
+            z_position = end[Z_AXIS];
           }
 
           planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -453,27 +409,6 @@
         //
         float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi);
 
-        /**
-         * Debug code to use non-optimized get_z_correction() and to do a sanity check
-         * that the correct value is being passed to planner.buffer_line()
-         */
-        /*
-          z_optimized = z0;
-          z0 = ubl.get_z_correction(x, next_mesh_line_y);
-          if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
-            debug_current_and_destination(PSTR("General_1: z_correction()"));
-            if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
-            if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN  "); {
-              SERIAL_ECHOPAIR("  x=", x);
-            }
-            SERIAL_ECHOPAIR("  next_mesh_line_y=", next_mesh_line_y);
-            SERIAL_ECHOPAIR("  z0=", z0);
-            SERIAL_ECHOPAIR("  z_optimized=", z_optimized);
-            SERIAL_ECHOPAIR("  err=",fabs(z_optimized-z0));
-            SERIAL_ECHO("\n");
-          }
-        */
-
         z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
@@ -491,8 +426,8 @@
           z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
         }
         else {
-          e_position = start[E_AXIS];
-          z_position = start[Z_AXIS];
+          e_position = end[E_AXIS];
+          z_position = end[Z_AXIS];
         }
         planner.buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
         current_yi += dyi;
@@ -504,26 +439,6 @@
         //
         float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag);
 
-        /**
-         * Debug code to use non-optimized get_z_correction() and to do a sanity check
-         * that the correct value is being passed to planner.buffer_line()
-         */
-        /*
-          z_optimized = z0;
-          z0 = ubl.get_z_correction(next_mesh_line_x, y);
-          if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
-          debug_current_and_destination(PSTR("General_2: z_correction()"));
-          if (isnan(z0)) SERIAL_ECHO(" z0==NAN  ");
-          if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN  ");
-          SERIAL_ECHOPAIR("  next_mesh_line_x=", next_mesh_line_x);
-          SERIAL_ECHOPAIR("  y=", y);
-          SERIAL_ECHOPAIR("  z0=", z0);
-          SERIAL_ECHOPAIR("  z_optimized=", z_optimized);
-          SERIAL_ECHOPAIR("  err=",fabs(z_optimized-z0));
-          SERIAL_ECHO("\n");
-          }
-        */
-
         z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
 
         /**
@@ -541,8 +456,8 @@
           z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
         }
         else {
-          e_position = start[E_AXIS];
-          z_position = start[Z_AXIS];
+          e_position = end[E_AXIS];
+          z_position = end[Z_AXIS];
         }
 
         planner.buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);