From c9e536cf872c1fb2acce1bdb3da0d63f79093be8 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 3 Jul 2016 18:06:54 -0700
Subject: [PATCH] Additional patches for gcode_T position change

---
 Marlin/Marlin_main.cpp | 48 ++++++++++++++++++++++++------------------
 1 file changed, 27 insertions(+), 21 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index fc5df9443..a1a7a80b7 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -6497,6 +6497,13 @@ inline void gcode_T(uint8_t tmp_extruder) {
     return;
   }
 
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) {
+      SERIAL_ECHOLNPGM(">>> gcode_T");
+      DEBUG_POS("BEFORE", current_position);
+    }
+  #endif
+
   #if HOTENDS > 1
 
     float old_feedrate = feedrate;
@@ -6579,11 +6586,6 @@ inline void gcode_T(uint8_t tmp_extruder) {
         // Z software endstop. But this is technically correct (and
         // there is no viable alternative).
         //
-        float xydiff[2] = {
-          hotend_offset[X_AXIS][tmp_extruder] - hotend_offset[X_AXIS][active_extruder],
-          hotend_offset[Y_AXIS][tmp_extruder] - hotend_offset[Y_AXIS][active_extruder]
-        };
-
         #if ENABLED(AUTO_BED_LEVELING_FEATURE)
           // Offset extruder, make sure to apply the bed level rotation matrix
           vector_3 tmp_offset_vec = vector_3(hotend_offset[X_AXIS][tmp_extruder],
@@ -6596,31 +6598,29 @@ inline void gcode_T(uint8_t tmp_extruder) {
 
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(LEVELING)) {
-              SERIAL_ECHOLNPGM(">>> gcode_T");
               tmp_offset_vec.debug("tmp_offset_vec");
               act_offset_vec.debug("act_offset_vec");
               offset_vec.debug("offset_vec (BEFORE)");
-              DEBUG_POS("BEFORE rotation", current_position);
             }
           #endif
 
           offset_vec.apply_rotation(planner.bed_level_matrix.transpose(planner.bed_level_matrix));
 
-          // Adjust the current position
-          current_position[X_AXIS] += offset_vec.x;
-          current_position[Y_AXIS] += offset_vec.y;
-          current_position[Z_AXIS] += offset_vec.z;
-
           #if ENABLED(DEBUG_LEVELING_FEATURE)
-            if (DEBUGGING(LEVELING)) {
-              offset_vec.debug("offset_vec (AFTER)");
-              DEBUG_POS("AFTER rotation", current_position);
-              SERIAL_ECHOLNPGM("<<< gcode_T");
-            }
+            if (DEBUGGING(LEVELING)) offset_vec.debug("offset_vec (AFTER)");
           #endif
 
+          // Adjustments to the current position
+          float xydiff[2] = { offset_vec.x, offset_vec.y };
+          current_position[Z_AXIS] += offset_vec.z;
+
         #else // !AUTO_BED_LEVELING_FEATURE
 
+          float xydiff[2] = {
+            hotend_offset[X_AXIS][tmp_extruder] - hotend_offset[X_AXIS][active_extruder],
+            hotend_offset[Y_AXIS][tmp_extruder] - hotend_offset[Y_AXIS][active_extruder]
+          };
+
           #if ENABLED(MESH_BED_LEVELING)
 
             if (mbl.active()) {
@@ -6631,12 +6631,11 @@ inline void gcode_T(uint8_t tmp_extruder) {
 
           #endif // MESH_BED_LEVELING
 
-          // The newly-selected extruder XY is actually at...
-          current_position[X_AXIS] += xydiff[X_AXIS];
-          current_position[Y_AXIS] += xydiff[Y_AXIS];
-
         #endif // !AUTO_BED_LEVELING_FEATURE
 
+        // The newly-selected extruder XY is actually at...
+        current_position[X_AXIS] += xydiff[X_AXIS];
+        current_position[Y_AXIS] += xydiff[Y_AXIS];
         for (uint8_t i = X_AXIS; i <= Y_AXIS; i++) {
           position_shift[i] += xydiff[i];
           update_software_endstops((AxisEnum)i);
@@ -6670,6 +6669,13 @@ inline void gcode_T(uint8_t tmp_extruder) {
 
   #endif
 
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_POS("AFTER", current_position);
+      SERIAL_ECHOLNPGM("<<< gcode_T");
+    }
+  #endif
+
   SERIAL_ECHO_START;
   SERIAL_ECHOPGM(MSG_ACTIVE_EXTRUDER);
   SERIAL_PROTOCOLLN((int)active_extruder);