Fix G2/G3 P<circles> E and Z motion (#19797)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/src/gcode/bedlevel/G26.cpp

 #define G26_ERR true
 
 #if ENABLED(ARC_SUPPORT)
-  void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const uint8_t clockwise);
+  void plan_arc(const xyze_pos_t&, const ab_float_t&, const bool, const uint8_t);
 #endif
 
 constexpr float g26_e_axis_feedrate = 0.025;
 
         const feedRate_t old_feedrate = feedrate_mm_s;
         feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
-        plan_arc(endpoint, arc_offset, false);  // Draw a counter-clockwise arc
+        plan_arc(endpoint, arc_offset, false, 0);  // Draw a counter-clockwise arc
         feedrate_mm_s = old_feedrate;
         destination = current_position;
 

Marlin/src/gcode/motion/G2_G3.cpp

 void plan_arc(
   const xyze_pos_t &cart,   // Destination position
   const ab_float_t &offset, // Center of rotation relative to current_position
-  const uint8_t clockwise   // Clockwise?
+  const bool clockwise,     // Clockwise?
+  const uint8_t circles     // Take the scenic route
 ) {
   #if ENABLED(CNC_WORKSPACE_PLANES)
     AxisEnum p_axis, q_axis, l_axis;
               center_Q = current_position[q_axis] - rvec.b,
               rt_X = cart[p_axis] - center_P,
               rt_Y = cart[q_axis] - center_Q,
-              start_L = current_position[l_axis],
-              linear_travel = cart[l_axis] - start_L,
-              extruder_travel = cart.e - current_position.e;
+              start_L = current_position[l_axis];
 
   // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
   float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
   if (clockwise) angular_travel -= RADIANS(360);
 
   // Make a circle if the angular rotation is 0 and the target is current position
-  if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {
+  if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
     angular_travel = RADIANS(360);
     #ifdef MIN_ARC_SEGMENTS
       min_segments = MIN_ARC_SEGMENTS;
     #endif
   }
 
+  float linear_travel = cart[l_axis] - start_L,
+        extruder_travel = cart.e - current_position.e;
+
+  // If circling around...
+  if (ENABLED(ARC_P_CIRCLES) && circles) {
+    const float total_angular = angular_travel + circles * RADIANS(360),  // Total rotation with all circles and remainder
+              part_per_circle = RADIANS(360) / total_angular,             // Each circle's part of the total
+                 l_per_circle = linear_travel * part_per_circle,          // L movement per circle
+                 e_per_circle = extruder_travel * part_per_circle;        // E movement per circle
+    xyze_pos_t temp_position = current_position;                          // for plan_arc to compare to current_position
+    for (uint16_t n = circles; n--;) {
+      temp_position.e += e_per_circle;                                    // Destination E axis
+      temp_position[l_axis] += l_per_circle;                              // Destination L axis
+      plan_arc(temp_position, offset, clockwise, 0);                      // Plan a single whole circle
+    }
+    linear_travel = cart[l_axis] - current_position[l_axis];
+    extruder_travel = cart.e - current_position.e;
+  }
+
   const float flat_mm = radius * angular_travel,
               mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
   if (mm_of_travel < 0.001f) return;
               linear_per_segment = linear_travel / segments,
               extruder_per_segment = extruder_travel / segments,
               sq_theta_per_segment = sq(theta_per_segment),
-              sin_T = theta_per_segment - sq_theta_per_segment*theta_per_segment/6,
+              sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
               cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
 
   // Initialize the linear axis
 
       #if ENABLED(ARC_P_CIRCLES)
         // P indicates number of circles to do
-        int8_t circles_to_do = parser.byteval('P');
+        const int8_t circles_to_do = parser.byteval('P');
         if (!WITHIN(circles_to_do, 0, 100))
           SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
-
-        while (circles_to_do--)
-          plan_arc(current_position, arc_offset, clockwise);
+      #else
+        constexpr uint8_t circles_to_do = 0;
       #endif
 
       // Send the arc to the planner
-      plan_arc(destination, arc_offset, clockwise);
+      plan_arc(destination, arc_offset, clockwise, circles_to_do);
       reset_stepper_timeout();
     }
     else