"""
=============================================================================
MACRO: CNC Slot Skeletonizer (Centerlines Only)
=============================================================================
DESCRIPTION:
This macro is specifically designed for CAM/CNC routing where the router bit 
diameter matches the slot width. It reads a DXF-style slot sketch, calculates
the mathematical centerlines, and produces a sketch containing ONLY the 
toolpaths (centerlines).

WHAT IT DOES:
1. Safely duplicates the selected sketch.
2. Mathematically calculates true Arcs from faceted DXF micro-segments.
3. Calculates intersections for Straight, L-shape, and T-shape junctions. 
4. Merges collinear centerline segments.
5. ADVANCED CAM OPTIMIZATION: Groups connected lines into continuous chains so
   the tool doesn't lift unnecessarily. Starts the job at the origin (0,0) and
   takes the shortest rapid paths between cut groups.
6. WIPES THE SKETCH COMPLETELY CLEAN of all original geometry.
7. Draws ONLY the pure skeleton centerlines.
=============================================================================
"""

import FreeCAD as App
import FreeCADGui as Gui
import Part
import math

try:
    from PySide6.QtWidgets import QMessageBox
except ImportError:
    try:
        from PySide2.QtWidgets import QMessageBox
    except ImportError:
        from PySide.QtGui import QMessageBox

# =================================================================
# CONFIGURATION
MAX_SEGMENT_LENGTH = 0.5  
# =================================================================

def get_circumcenter(p1, p2, p3):
    temp = p2.x**2 + p2.y**2
    bc = (p1.x**2 + p1.y**2 - temp) / 2.0
    cd = (temp - p3.x**2 - p3.y**2) / 2.0
    det = (p1.x - p2.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p2.y)
    
    if abs(det) < 1e-6: return None, None 
    
    cx = (bc * (p2.y - p3.y) - cd * (p1.y - p2.y)) / det
    cy = ((p1.x - p2.x) * cd - (p2.x - p3.x) * bc) / det
    center = App.Vector(cx, cy, 0)
    radius = (p1 - center).Length 
    return center, radius

def prepare_working_sketch():
    doc = App.ActiveDocument
    if not doc: return None
        
    target = None
    edit_view = Gui.ActiveDocument.getInEdit()
    if edit_view and edit_view.Object.isDerivedFrom("Sketcher::SketchObject"):
        target = edit_view.Object
        Gui.ActiveDocument.resetEdit() 
        
    if not target:
        sel = Gui.Selection.getSelection()
        if sel and sel[0].isDerivedFrom("Sketcher::SketchObject"):
            target = sel[0]
            
    if not target: return None

    new_sketch = doc.copyObject(target, False)
    new_sketch.Label = target.Label + "_Toolpath"
    
    if hasattr(target, "ViewObject") and target.ViewObject:
        target.ViewObject.Visibility = False
    if hasattr(new_sketch, "ViewObject") and new_sketch.ViewObject:
        new_sketch.ViewObject.Visibility = True
        
    doc.recompute()
    return new_sketch

def merge_collinear_lines(lines):
    merged = True
    while merged:
        merged = False
        for i in range(len(lines)):
            for j in range(i + 1, len(lines)):
                l1 = lines[i]
                l2 = lines[j]
                
                shared_pt, other1, other2 = None, None, None
                tol = 1e-3
                
                if (l1[0] - l2[0]).Length < tol: shared_pt, other1, other2 = l1[0], l1[1], l2[1]
                elif (l1[0] - l2[1]).Length < tol: shared_pt, other1, other2 = l1[0], l1[1], l2[0]
                elif (l1[1] - l2[0]).Length < tol: shared_pt, other1, other2 = l1[1], l1[0], l2[1]
                elif (l1[1] - l2[1]).Length < tol: shared_pt, other1, other2 = l1[1], l1[0], l2[0]
                
                if shared_pt:
                    v1 = (other1 - shared_pt)
                    v2 = (other2 - shared_pt)
                    if v1.Length > tol and v2.Length > tol:
                        v1.normalize()
                        v2.normalize()
                        if abs(v1.dot(v2) + 1.0) < 1e-4: 
                            lines.pop(j)
                            lines.pop(i)
                            lines.append((other1, other2))
                            merged = True
                            break
            if merged: break
    return lines

def optimize_toolpath_order(lines):
    if not lines: return []
    
    # Phase 1: Build continuous chains (Polylines)
    unvisited = lines.copy()
    chains = []
    tol = 1e-3
    
    while unvisited:
        current_chain = [unvisited.pop(0)]
        growing = True
        
        while growing:
            growing = False
            chain_start = current_chain[0][0]
            chain_end = current_chain[-1][1]
            
            for i, line in enumerate(unvisited):
                l_start, l_end = line[0], line[1]
                
                if (l_start - chain_end).Length < tol:
                    current_chain.append(unvisited.pop(i))
                    growing = True; break
                elif (l_end - chain_end).Length < tol:
                    current_chain.append((l_end, l_start)) # Flip
                    unvisited.pop(i)
                    growing = True; break
                elif (l_end - chain_start).Length < tol:
                    current_chain.insert(0, unvisited.pop(i))
                    growing = True; break
                elif (l_start - chain_start).Length < tol:
                    current_chain.insert(0, (l_end, l_start)) # Flip
                    unvisited.pop(i)
                    growing = True; break
                    
        chains.append(current_chain)
        
    # Phase 2: Traveling Salesperson between chains
    unvisited_chains = chains.copy()
    optimized_lines = []
    
    # Start with the chain closest to global Origin (0,0,0)
    start_idx = 0
    best_origin_dist = float('inf')
    for i, chain in enumerate(unvisited_chains):
        d1 = chain[0][0].Length
        d2 = chain[-1][1].Length
        if min(d1, d2) < best_origin_dist:
            best_origin_dist = min(d1, d2)
            start_idx = i
            
    current_chain = unvisited_chains.pop(start_idx)
    # Ensure it starts at the point closer to origin
    if current_chain[-1][1].Length < current_chain[0][0].Length:
        current_chain.reverse()
        current_chain = [(l[1], l[0]) for l in current_chain]
        
    optimized_lines.extend(current_chain)
    current_pos = current_chain[-1][1] # Position after cutting first chain
    
    while unvisited_chains:
        best_dist = float('inf')
        best_idx = -1
        reverse_chain = False
        
        for i, chain in enumerate(unvisited_chains):
            c_start = chain[0][0]
            c_end = chain[-1][1]
            
            dist_to_start = (c_start - current_pos).Length
            dist_to_end = (c_end - current_pos).Length
            
            if dist_to_start < best_dist:
                best_dist = dist_to_start
                best_idx = i
                reverse_chain = False
            
            if dist_to_end < best_dist:
                best_dist = dist_to_end
                best_idx = i
                reverse_chain = True
                
        next_chain = unvisited_chains.pop(best_idx)
        
        if reverse_chain:
            next_chain.reverse()
            next_chain = [(l[1], l[0]) for l in next_chain]
            
        optimized_lines.extend(next_chain)
        current_pos = next_chain[-1][1]
        
    return optimized_lines

def show_message(title, message):
    msg = QMessageBox()
    msg.setIcon(QMessageBox.Information)
    msg.setWindowTitle(title)
    msg.setText(message)
    try:
        msg.exec() 
    except AttributeError:
        msg.exec_()

def process_sketch():
    sketch = prepare_working_sketch()
    if not sketch:
        App.Console.PrintError("Could not find a sketch. Select one in the tree view and run.\n")
        return

    geo = sketch.Geometry
    
    # --- STEP 1: IDENTIFY MICRO-SEGMENTS TO INFER ARC DATA ---
    graph = {}
    pt_dict = {}
    def get_pt_key(pt): return (round(pt.x, 3), round(pt.y, 3))
    
    for i, g in enumerate(geo):
        if isinstance(g, Part.LineSegment) and not sketch.GeometryFacadeList[i].Construction:
            if (g.EndPoint - g.StartPoint).Length < MAX_SEGMENT_LENGTH:
                k1 = get_pt_key(g.StartPoint)
                k2 = get_pt_key(g.EndPoint)
                
                if k1 not in graph: graph[k1] = []
                if k2 not in graph: graph[k2] = []
                
                graph[k1].append((k2, i))
                graph[k2].append((k1, i))
                
                pt_dict[k1] = g.StartPoint
                pt_dict[k2] = g.EndPoint

    paths = []
    visited_edges = set()
    
    for node, edges in graph.items():
        if len(edges) == 1: 
            first_edge = edges[0][1]
            if first_edge in visited_edges: continue
                
            path_nodes = [node]
            curr = node; prev = None
            
            while True:
                neighbors = graph[curr]
                next_node = None; next_idx = None
                for n, idx in neighbors:
                    if n != prev:
                        next_node = n; next_idx = idx
                        break
                if next_node is None: break 
                path_nodes.append(next_node)
                visited_edges.add(next_idx)
                if len(graph[next_node]) > 2: break 
                prev = curr; curr = next_node
                
            if len(path_nodes) >= 3:
                paths.append(path_nodes)

    # --- STEP 2: CALCULATE TRUE ARC CENTERS & DIRECTIONS ---
    arc_data = [] 
    
    for path_nodes in paths:
        p1 = pt_dict[path_nodes[0]] 
        p2 = pt_dict[path_nodes[len(path_nodes)//2]] 
        p3 = pt_dict[path_nodes[-1]] 
        
        center, radius = get_circumcenter(p1, p2, p3)
        if center:
            v1 = p1 - center
            v3 = p3 - center
            if v1.Length > 1e-4 and v3.Length > 1e-4:
                dot = max(-1.0, min(1.0, v1.dot(v3) / (v1.Length * v3.Length)))
                angle = math.acos(dot)
                if angle > 2.0:
                    chord = p3 - p1
                    perp = App.Vector(-chord.y, chord.x, 0)
                    if perp.Length > 1e-6:
                        perp.normalize()
                        if perp.dot(center - p2) < 0: perp = -perp
                        arc_data.append((center, perp))

    # Existing native Arcs
    for i, g in enumerate(geo):
        if isinstance(g, Part.ArcOfCircle) and not sketch.GeometryFacadeList[i].Construction:
            p1 = g.StartPoint
            p3 = g.EndPoint
            center = g.Center
            v1 = p1 - center
            v3 = p3 - center
            if v1.Length > 1e-4 and v3.Length > 1e-4:
                dot = max(-1.0, min(1.0, v1.dot(v3) / (v1.Length * v3.Length)))
                angle = math.acos(dot)
                if angle > 2.0:
                    chord = p3 - p1
                    perp = App.Vector(-chord.y, chord.x, 0)
                    if perp.Length > 1e-6:
                        perp.normalize()
                        mid_u = (g.FirstParameter + g.LastParameter) / 2.0
                        p2 = g.value(mid_u)
                        if perp.dot(center - p2) < 0: perp = -perp
                        arc_data.append((center, perp))

    # --- STEP 3: WIPE THE SKETCH CLEAN ---
    for i in range(sketch.ConstraintCount - 1, -1, -1):
        sketch.delConstraint(i)
    for i in range(sketch.GeometryCount - 1, -1, -1):
        sketch.delGeometry(i)

    # --- STEP 4: CALCULATE TOOLPATHS ---
    raw_lines = []

    for i, (c1, dir1) in enumerate(arc_data):
        min_t = float('inf')
        best_stop = None
        
        for j, (c2, dir2) in enumerate(arc_data):
            if i == j: continue
            cross = dir1.x * dir2.y - dir1.y * dir2.x
            
            if abs(cross) < 1e-4:
                vec = c2 - c1
                dist_cross = vec.x * dir1.y - vec.y * dir1.x
                if abs(dist_cross) < 1e-2: 
                    t1 = vec.dot(dir1)
                    t2 = (-vec).dot(dir2)
                    if t1 > 1e-2 and t2 > 1e-2: 
                        if t1 < min_t:
                            min_t = t1
                            best_stop = c2
            else:
                dx = c2.x - c1.x
                dy = c2.y - c1.y
                t1 = (dx * dir2.y - dy * dir2.x) / cross
                t2 = (dx * dir1.y - dy * dir1.x) / cross
                if t1 > 1e-2 and t2 > -1e-2: 
                    if t1 < min_t:
                        min_t = t1
                        best_stop = c1 + dir1 * t1
                        
        if best_stop is not None:
            if (c1 - best_stop).Length > 1e-3:
                pts = sorted([(round(c1.x, 3), round(c1.y, 3)), (round(best_stop.x, 3), round(best_stop.y, 3))])
                pA = App.Vector(pts[0][0], pts[0][1], 0)
                pB = App.Vector(pts[1][0], pts[1][1], 0)
                if not any((l[0]-pA).Length < 1e-3 and (l[1]-pB).Length < 1e-3 for l in raw_lines):
                    raw_lines.append((pA, pB))

    # --- STEP 5: MERGE AND OPTIMIZE PATHS FOR CNC ---
    merged_lines = merge_collinear_lines(raw_lines)
    optimized_lines = optimize_toolpath_order(merged_lines)

    # Draw the final optimized lines sequentially
    for pA, pB in optimized_lines:
        sketch.addGeometry(Part.LineSegment(pA, pB), False)

    App.ActiveDocument.recompute()
    App.Console.PrintMessage(f"Skeletonizer successful: Created '{sketch.Label}'.\n")
    show_message("CAM Toolpath Complete", f"Successfully generated continuous, optimized CNC toolpaths in:\n\n{sketch.Label}")

# Run it
process_sketch()
