From 263eeffd71b962c093c8ff7cfe277c18731c598b Mon Sep 17 00:00:00 2001
From: Markus Lampert <markus@bibi.ca>
Date: Mon, 21 Feb 2022 23:44:14 -0800
Subject: [PATCH] Fixed lead in/out with elevator location.

---
 src/Mod/Path/PathScripts/PathThreadMilling.py | 18 ++++--
 .../Path/PathTests/TestPathThreadMilling.py   | 56 +++++++++++++++++++
 2 files changed, 68 insertions(+), 6 deletions(-)

diff --git a/src/Mod/Path/PathScripts/PathThreadMilling.py b/src/Mod/Path/PathScripts/PathThreadMilling.py
index 61e7c4a5c6..b4ba56bf3c 100644
--- a/src/Mod/Path/PathScripts/PathThreadMilling.py
+++ b/src/Mod/Path/PathScripts/PathThreadMilling.py
@@ -176,7 +176,7 @@ def threadCommands(center, cmd, zStart, zFinal, pitch, radius, leadInOut, elevat
 
     if PathGeom.isRoughly(z, thread.zFinal):
         x = center.x
-        y = yMin if 0 == (i & 0x01) else yMax
+        y = yMin if (i & 0x01) else yMax
     else:
         n = math.fabs(thread.zFinal - thread.zStart) / thread.hPitch
         k = n - int(n)
@@ -191,20 +191,26 @@ def threadCommands(center, cmd, zStart, zFinal, pitch, radius, leadInOut, elevat
         comment(path, 'finish-thread')
 
     a = math.atan2(y - center.y, x - center.x)
-    dx = math.cos(a) * elevator
-    dy = math.sin(a) * elevator
+    dx = math.cos(a) * (radius - elevator)
+    dy = math.sin(a) * (radius - elevator)
+    PathLog.debug('')
+    PathLog.debug("a={}: dx={:.2f}, dy={:.2f}".format(a / math.pi * 180, dx, dy))
+
+    elevatorX = x - dx
+    elevatorY = y - dy
+    PathLog.debug("({:.2f}, {:.2f}) -> ({:.2f}, {:.2f})".format(x, y, elevatorX, elevatorY))
 
     if leadInOut:
         comment(path, 'lead-out')
         path.append(
             Path.Command(
                 thread.cmd,
-                {"X": center.x + dx, "Y": center.y + dy, "I": dx / 2, "J": dy / 2},
+                {"X": elevatorX, "Y": elevatorY, "I": -dx / 2, "J": -dy / 2},
             )
         )
         comment(path, 'lead-out')
-
-    path.append(Path.Command("G1", {"X": center.x + dx, "Y": center.y - dy}))
+    else:
+        path.append(Path.Command("G1", {"X": elevatorX, "Y": elevatorY}))
 
     return path
 
diff --git a/src/Mod/Path/PathTests/TestPathThreadMilling.py b/src/Mod/Path/PathTests/TestPathThreadMilling.py
index db15f2a628..880dc49bc7 100644
--- a/src/Mod/Path/PathTests/TestPathThreadMilling.py
+++ b/src/Mod/Path/PathTests/TestPathThreadMilling.py
@@ -206,3 +206,59 @@ class TestPathThreadMilling(PathTestBase):
                 ]
         self.assertEqual([p.toGCode() for p in path], gcode)
 
+    def test50(self):
+        '''Verify lead in/out commands for a single thread'''
+
+        center      = FreeCAD.Vector()
+        cmd         = 'G2'
+        zStart      = 0
+        zFinal      = 1
+        pitch       = 1
+        radius      = 3
+        leadInOut   = True
+        elevator    = 2
+
+        path = PathThreadMilling.threadCommands(center, cmd, zStart, zFinal, pitch, radius, leadInOut, elevator)
+
+        gcode = [
+                'G0 X0.000000 Y2.000000',
+                'G0 Z0.000000', 
+                '(------- lead-in -------)',
+                'G2 J0.500000 Y3.000000',
+                '(------- lead-in -------)',
+                'G2 J-3.000000 Y-3.000000 Z0.500000', 
+                'G2 J3.000000 Y3.000000 Z1.000000', 
+                '(------- lead-out -------)',
+                'G2 I0.000000 J-0.500000 X0.000000 Y2.000000', 
+                '(------- lead-out -------)',
+                ]
+        self.assertEqual([p.toGCode() for p in path], gcode)
+
+    def test51(self):
+        '''Verify lead in/out commands for one and a half threads'''
+
+        center      = FreeCAD.Vector()
+        cmd         = 'G2'
+        zStart      = 0
+        zFinal      = 1.5
+        pitch       = 1
+        radius      = 3
+        leadInOut   = True
+        elevator    = 2
+
+        path = PathThreadMilling.threadCommands(center, cmd, zStart, zFinal, pitch, radius, leadInOut, elevator)
+
+        gcode = [
+                'G0 X0.000000 Y2.000000',
+                'G0 Z0.000000', 
+                '(------- lead-in -------)',
+                'G2 J0.500000 Y3.000000',
+                '(------- lead-in -------)',
+                'G2 J-3.000000 Y-3.000000 Z0.500000', 
+                'G2 J3.000000 Y3.000000 Z1.000000', 
+                'G2 J-3.000000 Y-3.000000 Z1.500000', 
+                '(------- lead-out -------)',
+                'G2 I0.000000 J0.500000 X0.000000 Y-2.000000', 
+                '(------- lead-out -------)',
+                ]
+        self.assertEqual([p.toGCode() for p in path], gcode)