[2.0.x] tool change bad behaviours

This will solve 2 issues on tool change.
1) when no home is done ("no_move" is set) tool change should never move X & Y because... I don't know where I am...only Z should be adjusted to avoid bed collision if other nozzle is lower than current.
Since no move no bed level matrix should be applied
2) When SWITCHING_NOZZLE is enabled and system is homed there are two movements to go to destination, but first can move Z to older position and then crash on the bed

Marlin/src/module/tool_change.cpp

@@ -494,21 +494,25 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
 
         // Move to the "old position" (move the extruder into place)
         if (!no_move && IsRunning()) {
+          #if ENABLED(SWITCHING_NOZZLE)
+            if (z_raise != z_diff)
+              destination[Z_AXIS] += z_diff;  // Include the Z restore with the "move back"
+          #endif
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);
           #endif
-          prepare_move_to_destination();
+          // Move back to the original (or tweaked) position
+          do_blocking_move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS]);
         }
-
         #if ENABLED(SWITCHING_NOZZLE)
           // Move back down, if needed. (Including when the new tool is higher.)
-          if (z_raise != z_diff) {
+          else if (z_raise != z_diff) {
+            set_destination_from_current(); // Prevent any XY move
             destination[Z_AXIS] += z_diff;
             feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];
             prepare_move_to_destination();
           }
         #endif
-
       } // (tmp_extruder != active_extruder)
 
       stepper.synchronize();