line hatching -> svg & gcode

hatching.py

import cv2
import numpy as np
import cairo
import time
import serial  # Import serial first
import serial.tools.list_ports  # Then import list_ports
from tqdm import tqdm
import matplotlib.pyplot as plt  # Add this import at the top

pens = {
    'fineliner': {
        'width_mm': 0.8,  # Width in millimeters
        'color': [0, 0, 0, 0.5]
    }
}


def optimize_path_order(paths):
    """Optimize path order using a faster greedy approach"""
    if not paths:
        return []
    
    # Convert paths to numpy arrays for faster operations
    paths = [np.array(path) for path in paths]
    n_paths = len(paths)
    
    # Pre-calculate all start and end points
    starts = np.array([path[0] for path in paths])
    ends = np.array([path[-1] for path in paths])
    
    # Initialize result
    optimized = []
    used = np.zeros(n_paths, dtype=bool)
    current_pos = np.array([0, 0])
    
    # Pre-calculate all distances from origin
    origin_to_start = np.linalg.norm(starts - current_pos, axis=1)
    origin_to_end = np.linalg.norm(ends - current_pos, axis=1)
    
    # Get first path (closest to origin)
    first_idx = np.argmin(np.minimum(origin_to_start, origin_to_end))
    reverse = origin_to_end[first_idx] < origin_to_start[first_idx]
    
    optimized.append(paths[first_idx][::-1] if reverse else paths[first_idx])
    used[first_idx] = True
    current_pos = paths[first_idx][0] if reverse else paths[first_idx][-1]
    
    # Process remaining paths
    for _ in range(n_paths - 1):
        # Calculate distances to current position
        distances_to_start = np.linalg.norm(starts - current_pos, axis=1)
        distances_to_end = np.linalg.norm(ends - current_pos, axis=1)
        
        # Mask out used paths
        distances_to_start[used] = np.inf
        distances_to_end[used] = np.inf
        
        # Find closest path
        start_min = np.min(distances_to_start)
        end_min = np.min(distances_to_end)
        
        if start_min <= end_min:
            next_idx = np.argmin(distances_to_start)
            reverse = False
        else:
            next_idx = np.argmin(distances_to_end)
            reverse = True
        
        # Add path to result
        path = paths[next_idx]
        optimized.append(path[::-1] if reverse else path)
        used[next_idx] = True
        current_pos = path[0] if reverse else path[-1]
    
    return optimized

def send_to_plotter(commands, port, baud_rate):
    """Send GCode commands directly to plotter with buffer management"""
    try:
        # Open serial connection
        print(f"Attempting to connect to {port} at {baud_rate} baud...")
        plotter = serial.Serial(port, baud_rate, timeout=1)
        time.sleep(2)  # Wait for connection to establish
        
        print("Connected to plotter")
        
        # Track commands in flight
        commands_in_flight = []
        MAX_BUFFER_LENGTH = 4  # Maximum number of commands to buffer
        
        def read_response():
            """Read response with retries, handling welcome message"""
            while True:
                try:
                    response = plotter.readline().decode('ascii').strip()
                    if not response:
                        continue
                    if "DrawCore" in response:
                        print(f"Plotter: {response}")
                        continue
                    print(f"Received response: {response}")  # Debug: Print all responses
                    return response
                except UnicodeDecodeError:
                    print("Warning: UnicodeDecodeError while reading response")
                    continue
                    
        def command(cmd, wait_for_ok=False):
            """Send a command and optionally wait for acknowledgment"""
            if not cmd or cmd.startswith(';'):
                return True
                
            try:
                plotter.write(f"{cmd}\n".encode('ascii'))
                commands_in_flight.append(cmd)
                
                if wait_for_ok or len(commands_in_flight) >= MAX_BUFFER_LENGTH:
                    print(f"Waiting for response (buffer: {len(commands_in_flight)} commands)")  # Debug
                    while commands_in_flight:
                        response = read_response()
                        if response.startswith("ok"):
                            commands_in_flight.pop(0)
                        else:
                            print(f"\nUnexpected response: {response}")
                            print(f"  Command was: {commands_in_flight[0]}")
                            return False
                return True
                    
            except (serial.SerialException, IOError, RuntimeError, OSError) as err:
                if not cmd.strip().lower() == "rb":
                    print(f"\nFailed after command: {cmd}")
                    print(f"Error: {err}")
                return False
        
        # Count actual drawing commands for progress bar
        draw_commands = [cmd for cmd in commands if not cmd.startswith(';')]
        
        # Send main commands with progress bar
        with tqdm(total=len(draw_commands), desc="Plotting") as pbar:
            for cmd in commands:
                if cmd.startswith(';'):
                    if "Color group" in cmd:
                        pbar.write(f"\n{cmd}")  # Show color changes above progress bar
                    continue
                
                if cmd.strip().startswith('M0'):
                    # Wait for buffer to clear
                    while commands_in_flight:
                        if read_response().startswith("ok"):
                            commands_in_flight.pop(0)
                    
                    pbar.write("\nChange pen and press Enter to continue...")
                    input()
                    continue
                
                if not command(cmd):
                    raise Exception(f"Command failed")
                pbar.update(1)
        
        # Wait for remaining commands
        while commands_in_flight:
            if read_response().startswith("ok"):
                commands_in_flight.pop(0)
        
        plotter.close()
        print("\nPlotting complete!")
        
    except Exception as e:
        print(f"\nError: {e}")
        print("Available ports:")
        for port in list(serial.tools.list_ports.comports()):
            print(f"  {port.device}")

def prepare_image_for_a3(image_color, px_to_mm=0.3, padding_mm=10):
    """
    Resize and crop images to fit A3 paper (297x420mm) with padding
    """
    # A3 dimensions in mm
    a3_width_mm = 297
    a3_height_mm = 420
    
    # Calculate available space in mm (subtracting padding from both sides)
    available_width_mm = a3_width_mm - (2 * padding_mm)
    available_height_mm = a3_height_mm - (2 * padding_mm)
    
    # Convert to pixels
    target_width_px = int(available_width_mm / px_to_mm)
    target_height_px = int(available_height_mm / px_to_mm)
    
    # Calculate scale to fit minimum side
    current_height, current_width = image_color.shape[:2]
    width_scale = target_width_px / current_width
    height_scale = target_height_px / current_height
    scale = max(width_scale, height_scale)  # Use max to fit to minimum side
    
    # Calculate new dimensions
    new_width = int(current_width * scale)
    new_height = int(current_height * scale)
    
    # Resize both images
    image_color_resized = cv2.resize(image_color, (new_width, new_height))
    
    # Calculate crop amounts
    crop_x = max(0, (new_width - target_width_px) // 2)
    crop_y = max(0, (new_height - target_height_px) // 2)
    
    # Crop from center
    image_color_cropped = image_color_resized[
        crop_y:crop_y + target_height_px,
        crop_x:crop_x + target_width_px
    ]
    
    print(f"Original size: {current_width}x{current_height}px")
    print(f"Resized to: {new_width}x{new_height}px")
    print(f"Cropped to: {target_width_px}x{target_height_px}px")
    print(f"Will print as: {target_width_px * px_to_mm:.1f}x{target_height_px * px_to_mm:.1f}mm")
    
    return image_color_cropped, available_height_mm

def create_lightness_mask(image_color, threshold=200):
    """
    Convert color image to grayscale and create a mask of dark pixels
    """
    # Convert BGR to grayscale
    gray = cv2.cvtColor(image_color, cv2.COLOR_BGR2GRAY)
    
    # Create mask where pixels are darker than threshold
    mask = gray < threshold
    
    return mask

def apply_perpendicular_offset(start_point, end_point, angle_degrees, start_max_offset=1, end_max_offset=1.5):
    """
    Shift start and end points perpendicular to line direction,
    with configurable max offsets
    """
    # Calculate perpendicular direction
    angle_rad = np.radians(angle_degrees)
    # Perpendicular vector (-sin, cos) to (cos, sin)
    dx = -np.sin(angle_rad)
    dy = np.cos(angle_rad)
    
    # Generate random offsets from 0 to max
    start_offset = np.random.uniform(0, start_max_offset)
    end_offset = np.random.uniform(0, end_max_offset)
    
    # Apply offsets
    shifted_start = start_point + np.array([dx * start_offset, dy * start_offset])
    shifted_end = end_point + np.array([dx * end_offset, dy * end_offset])
    
    return shifted_start.astype(np.int32), shifted_end.astype(np.int32)

def calculate_rotated_bounds(width, height, angle_degrees):
    """
    Calculate the starting and ending coordinates needed to cover the entire image
    with lines at the given angle
    """
    angle_rad = np.radians(angle_degrees)
    
    # For lines at angle θ:
    # - To cover top edge: start at x = -h/tan(θ)
    # - To cover bottom edge: end at x = w + h/tan(θ)
    # - For steep angles, also consider width projection
    
    if angle_degrees == 90:
        # Special case: vertical lines
        return 0, width, 0, height
    
    if angle_degrees == 0:
        # Special case: horizontal lines
        return 0, width, 0, height
    
    # Calculate how far back we need to start
    tan_angle = np.tan(angle_rad)
    
    # Calculate x-coordinates where lines need to start/end
    # to cover all corners of the image
    if angle_degrees < 90:
        # For angles 0° to 90°
        x_start = min(0, -height / tan_angle)
        x_end = max(width, width + height / tan_angle)
    else:
        # For angles > 90° (if needed)
        x_start = min(0, width - height / tan_angle)
        x_end = max(width, -height / tan_angle)
    
    # Y bounds are simple - we always start at bottom and go to top
    y_start = 0
    y_end = height
    
    return x_start, x_end, y_start, y_end

def create_diagonal_paths(mask, spacing=5, angle_degrees=45):
    """
    Create diagonal paths across the masked area using the rotated bounding box approach.
    Returns simple line segments at fixed spacing.
    """
    height, width = mask.shape
    print(f"\nStarting path creation for {angle_degrees}° lines...")
    
    # Calculate rotated bounding box
    angle_rad = np.radians(angle_degrees)
    cos_a = abs(np.cos(angle_rad))
    sin_a = abs(np.sin(angle_rad))
    
    # Calculate dimensions of bounding box
    bound_w = width * cos_a + height * sin_a
    bound_h = width * sin_a + height * cos_a
    
    # Create rotated bounding box corners (centered at origin)
    bound_corners = np.array([
        [-bound_w/2, -bound_h/2],  # Top-left
        [bound_w/2, -bound_h/2],   # Top-right
        [bound_w/2, bound_h/2],    # Bottom-right
        [-bound_w/2, bound_h/2]    # Bottom-left
    ])
    
    # Rotation matrices
    rotation_matrix = np.array([
        [np.cos(angle_rad), -np.sin(angle_rad)],
        [np.sin(angle_rad), np.cos(angle_rad)]
    ])
    rotated_corners = bound_corners @ rotation_matrix
    
    # Translate corners to image center
    image_center = np.array([width/2, height/2])
    rotated_corners = rotated_corners + image_center
    
    # Get vectors for line generation
    edge_vector = rotated_corners[1] - rotated_corners[0]  # Top edge vector
    edge_length = np.sqrt(np.sum(edge_vector**2))
    edge_unit = edge_vector / edge_length
    
    # Perpendicular vector going DOWN
    perp_vector = np.array([-edge_unit[1], edge_unit[0]])
    
    # Generate starting points along top edge with fixed spacing
    start_points = []
    current_dist = 0
    while current_dist < edge_length:
        start_point = rotated_corners[0] + edge_unit * current_dist
        start_points.append(start_point)
        current_dist += spacing
    
    print(f"Generated {len(start_points)} starting points")
    
    # Generate line segments
    line_segments = []
    for start_point in start_points:
        # Create end point by moving down perpendicular vector
        line_length = max(width, height) * 2
        end_point = start_point + perp_vector * line_length
        
        # Sample points along the line
        num_samples = int(line_length * 2)
        points = np.linspace(start_point, end_point, num_samples)
        
        current_start = None
        last_valid_point = None
        
        for point in points:
            x, y = int(point[0]), int(point[1])
            
            if 0 <= x < width and 0 <= y < height:
                if mask[y, x]:
                    if current_start is None:
                        current_start = np.array([x, y])
                    last_valid_point = np.array([x, y])
                elif current_start is not None:
                    # Line segment ends
                    line_segments.append((current_start, last_valid_point))
                    current_start = None
            elif current_start is not None:
                # Line exits image bounds
                line_segments.append((current_start, last_valid_point))
                current_start = None
    
    print(f"Generated {len(line_segments)} lines at {angle_degrees}°")
    return line_segments

def evaluate_cubic_bezier(p0, p1, p2, p3, num_points=50):
    """
    Evaluate a cubic bezier curve and return points along it.
    
    Args:
        p0: Start point
        p1, p2: Control points
        p3: End point
        num_points: Number of points to generate along curve
        
    Returns:
        Array of points along the bezier curve
    """
    points = []
    for t in np.linspace(0, 1, num_points):
        # Cubic Bezier formula:
        # B(t) = (1-t)³P₀ + 3(1-t)²tP₁ + 3(1-t)t²P₂ + t³P₃
        t2 = t * t
        t3 = t2 * t
        mt = 1 - t
        mt2 = mt * mt
        mt3 = mt2 * mt
        
        x = mt3 * p0[0] + 3 * mt2 * t * p1[0] + 3 * mt * t2 * p2[0] + t3 * p3[0]
        y = mt3 * p0[1] + 3 * mt2 * t * p1[1] + 3 * mt * t2 * p2[1] + t3 * p3[1]
        
        points.append(np.array([x, y]))
    
    return np.array(points)

def convert_to_bezier_curves(paths, max_control_offset=4.0, end_bias=0.7, points_per_curve=20):
    """
    Convert line segments into bezier curves and return points along the curves.
    
    Args:
        paths: List of (start, end) line segments
        max_control_offset: Maximum perpendicular offset for control points
        end_bias: How much to bias the curve towards the end (0-1)
        points_per_curve: Number of points to generate per curve
    
    Returns:
        List of point arrays, each representing a bezier curve
    """
    bezier_curves = []
    
    for start, end in paths:
        # Convert to float arrays
        start = start.astype(float)
        end = end.astype(float)
        
        # Calculate line direction and length
        direction = end - start
        length = np.sqrt(np.sum(direction**2))
        if length < 1e-6:
            continue
            
        # Normalize direction vector
        direction = direction / length
        
        # Calculate perpendicular vector
        perp = np.array([-direction[1], direction[0]])
        
        # Generate random offsets for control points
        # First control point has minimal offset
        offset1 = np.random.uniform(-max_control_offset * 0.2, max_control_offset * 0.2)
        # Second control point has full offset biased by end_bias
        offset2 = np.random.uniform(-max_control_offset, max_control_offset) * (1 + end_bias)
        
        # Calculate control points positions
        t1 = 0.33
        t2 = 0.67 + (0.33 * end_bias)  # Move second control point closer to end
        
        # Calculate control points
        control1 = start + direction * (length * t1) + perp * offset1
        control2 = start + direction * (length * t2) + perp * offset2
        
        # Generate points along the bezier curve
        curve_points = evaluate_cubic_bezier(
            start, control1, control2, end, 
            num_points=points_per_curve
        )
        
        # Convert to integer coordinates
        bezier_curves.append(curve_points.astype(np.int32))
    
    return bezier_curves

def trim_line_ends(paths, min_line_length=100, max_trim=10):
    """
    Randomly trim the ends of lines that are longer than min_line_length.
    
    Args:
        paths: List of (start, end) line segments
        min_line_length: Only trim lines longer than this
        max_trim: Maximum number of pixels to trim from each end
        
    Returns:
        List of trimmed line segments
    """
    trimmed_paths = []
    
    for start, end in paths:
        # Convert to float arrays for precise calculations
        start = start.astype(float)
        end = end.astype(float)
        
        # Calculate line length
        direction = end - start
        length = np.sqrt(np.sum(direction**2))
        
        if length < min_line_length:
            # Line too short, keep as is
            trimmed_paths.append((start.astype(np.int32), end.astype(np.int32)))
            continue
        
        # Normalize direction vector
        direction = direction / length
        
        # Generate random trim amounts for each end
        start_trim = np.random.uniform(0, max_trim)
        end_trim = np.random.uniform(0, max_trim)
        
        # Make sure we don't trim more than the line length
        total_trim = start_trim + end_trim
        if total_trim > length * 0.8:  # Don't trim more than 80% of the line
            scale = (length * 0.8) / total_trim
            start_trim *= scale
            end_trim *= scale
        
        # Calculate new endpoints
        new_start = start + direction * start_trim
        new_end = end - direction * end_trim
        
        # Add trimmed line
        trimmed_paths.append((new_start.astype(np.int32), new_end.astype(np.int32)))
    
    return trimmed_paths

def render_paths_with_cairo(path_groups, width, height, output_filename="output.png", scale_factor=0.3):
    """
    Render curves from multiple path groups using Cairo
    
    Args:
        path_groups: List of tuples (pen_id, paths), where paths is a list of point arrays
        width, height: Image dimensions
        output_filename: Output file path
    """
    surface = cairo.ImageSurface(cairo.FORMAT_ARGB32, width, height)
    ctx = cairo.Context(surface)
    
    ctx.set_antialias(cairo.ANTIALIAS_BEST)
    ctx.set_source_rgb(1, 1, 1)
    ctx.paint()
    
    # Draw each group
    for pen_id, curves in path_groups:
        # Get pen settings from pens dictionary
        pen = pens[pen_id]
        # Convert mm to pixels
        width_px = pen['width_mm'] / scale_factor
        ctx.set_line_width(width_px)
        ctx.set_source_rgba(*pen['color'])
        
        # Draw each curve in the group
        for curve_points in curves:
            if len(curve_points) < 2:
                continue
            
            ctx.move_to(curve_points[0][0], curve_points[0][1])
            for point in curve_points[1:]:
                ctx.line_to(point[0], point[1])
            ctx.stroke()
    
    surface.write_to_png(output_filename)
    print(f"Saved to {output_filename}")

def apply_perpendicular_jitter(paths, max_jitter=2.0):
    """
    Apply small random offsets perpendicular to each line segment.
    
    Args:
        paths: List of (start, end) line segments
        max_jitter: Maximum perpendicular offset in pixels
        
    Returns:
        List of jittered line segments
    """
    jittered_paths = []
    
    for start, end in paths:
        # Convert to float arrays for precise calculations
        start = start.astype(float)
        end = end.astype(float)
        
        # Calculate line direction vector
        direction = end - start
        length = np.sqrt(np.sum(direction**2))
        if length < 1e-6:  # Skip if points are too close
            continue
            
        # Normalize direction vector
        direction = direction / length
        
        # Calculate perpendicular vector (-dy, dx)
        perp = np.array([-direction[1], direction[0]])
        
        # Generate random offset
        offset = np.random.uniform(-max_jitter, max_jitter)
        
        # Apply offset to both points
        offset_vector = perp * offset
        new_start = start + offset_vector
        new_end = end + offset_vector
        
        # Convert back to integer coordinates
        jittered_paths.append((new_start.astype(np.int32), new_end.astype(np.int32)))
    
    return jittered_paths

def break_paths_into_segments(paths, min_length=150, max_length=300, min_gap=15, max_gap=30):
    """
    Break long paths into shorter segments with gaps between them.
    
    Args:
        paths: List of (start, end) line segments
        min_length: Minimum length of each segment
        max_length: Maximum length of each segment
        min_gap: Minimum gap between segments
        max_gap: Maximum gap between segments
    
    Returns:
        List of shorter line segments
    """
    segmented_paths = []
    
    for start, end in paths:
        # Convert to float arrays for precise calculations
        start = start.astype(float)
        end = end.astype(float)
        
        # Calculate total line length
        direction = end - start
        total_length = np.sqrt(np.sum(direction**2))
        
        if total_length < min_length:
            # Line is already shorter than minimum length, keep as is
            segmented_paths.append((start.astype(np.int32), end.astype(np.int32)))
            continue
            
        # Normalize direction vector
        direction = direction / total_length
        
        # Start from the beginning of the line
        current_pos = 0
        current_point = start
        
        while current_pos < total_length:
            # Determine length of this segment
            segment_length = np.random.randint(min_length, max_length + 1)
            segment_length = min(segment_length, total_length - current_pos)
            
            # Calculate segment end point
            segment_end = current_point + direction * segment_length
            
            # Add segment to list
            segmented_paths.append((
                current_point.astype(np.int32),
                segment_end.astype(np.int32)
            ))
            
            # Move position past this segment plus a gap
            gap = np.random.randint(min_gap, max_gap + 1)
            current_pos += segment_length + gap
            current_point = current_point + direction * (segment_length + gap)
            
            # Stop if we've gone past the end
            if current_pos >= total_length:
                break
    
    return segmented_paths


def generate_gcode(path_groups, penUp, penDown, scale_factor, available_height_mm):
    """
    Generate GCode for all path groups.
    Coordinates are converted to match plotter's coordinate system where:
    - (0,0) is at bottom left
    - X increases to the right
    - Y increases upward
    """
    commands = []
    
    # Track bounds
    min_x = float('inf')
    max_x = float('-inf')
    min_y = float('inf')
    max_y = float('-inf')
    
    # Process each group
    for group_idx, (pen_id, paths) in enumerate(path_groups):
        # Add color change comment and pause
        if group_idx > 0:
            commands.append(f"; Color group {group_idx + 1}: Change to pen {pen_id}")
            commands.append("M0")  # Pause for pen change
        else:
            commands.append(f"; Color group {group_idx + 1}: pen {pen_id}")
        
        # Optimize path order
        optimized_paths = optimize_path_order(paths)
        
        # a3 paper
        img_height = available_height_mm
        
        # Process each path
        for path in optimized_paths:
            # Convert path points from pixels to mm
            # Convert from top-left origin to bottom-left origin
            points_mm = []
            for point in path:
                x_mm = point[0] * scale_factor  # X stays the same
                y_mm = img_height - (point[1] * scale_factor)  # Y is flipped from bottom
                points_mm.append((x_mm, y_mm))
                
                # Track bounds
                min_x = min(min_x, x_mm)
                max_x = max(max_x, x_mm)
                min_y = min(min_y, y_mm)
                max_y = max(max_y, y_mm)
            
            # Move to start with pen up
            x, y = points_mm[0]
            commands.extend([
                f"G0 Z{penUp}",  # Lift pen
                f"G0 X{x:.3f} Y{y:.3f}"  # Move to start
            ])
            
            # Lower pen
            commands.append(f"G1 Z{penDown}")
            
            # Draw the path
            for x, y in points_mm[1:]:
                commands.append(f"G1 X{x:.3f} Y{y:.3f}")
    
    # Finish with pen up and return to origin
    commands.extend([
        f"G0 Z{penUp}",
        "G0 X0 Y0"
    ])
    
    # Report bounds
    print(f"\nGCode bounds (mm):")
    print(f"X: {min_x:.1f} to {max_x:.1f} (width: {max_x - min_x:.1f})")
    print(f"Y: {min_y:.1f} to {max_y:.1f} (height: {max_y - min_y:.1f})")
    
    return commands

def smooth_paths(paths, tolerance=0.5):
    """
    Smooth paths by removing points that don't significantly change the path direction.
    Uses Ramer-Douglas-Peucker algorithm.
    
    Args:
        paths: List of point arrays
        tolerance: Maximum distance a point can deviate from simplified line
    
    Returns:
        List of simplified point arrays
    """
    def rdp(points, epsilon):
        """Ramer-Douglas-Peucker algorithm"""
        if len(points) <= 2:
            return points
            
        # Find point with max distance from line between start and end
        line_start = points[0]
        line_end = points[-1]
        
        # Calculate distances from points to line
        max_dist = 0
        max_idx = 0
        
        for i in range(1, len(points) - 1):
            dist = np.abs(np.cross(line_end - line_start, points[i] - line_start)) / np.linalg.norm(line_end - line_start)
            if dist > max_dist:
                max_dist = dist
                max_idx = i
        
        # If max distance is greater than epsilon, recursively simplify
        if max_dist > epsilon:
            left = rdp(points[:max_idx + 1], epsilon)
            right = rdp(points[max_idx:], epsilon)
            return np.vstack((left[:-1], right))
        else:
            return np.vstack((points[0], points[-1]))
    
    smoothed = []
    for path in paths:
        if len(path) > 2:
            smoothed.append(rdp(path, tolerance))
        else:
            smoothed.append(path)
    
    return smoothed

def export_to_svg(path_groups, width, height, filename="output.svg"):
    """
    Export path groups to SVG file.
    
    Args:
        path_groups: List of (pen_id, paths) tuples
        width, height: Image dimensions in pixels
        filename: Output SVG filename
    """
    # SVG header with white background
    svg = [
        f'<?xml version="1.0" encoding="UTF-8"?>',
        f'<svg width="{width}px" height="{height}px" viewBox="0 0 {width} {height}"',
        '     xmlns="http://www.w3.org/2000/svg"',
        '     style="background-color: white;">',
    ]
    
    # Draw each group
    for pen_id, paths in path_groups:
        # Get pen color from pens dictionary
        if pen_id in pens:
            color = pens[pen_id]['color']
            # Convert color from 0-1 to hex
            hex_color = '#{:02x}{:02x}{:02x}'.format(
                int(color[0] * 255),
                int(color[1] * 255),
                int(color[2] * 255)
            )
            opacity = color[3]
        else:
            hex_color = '#000000'
            opacity = 0.5
        
        # Start group for this pen
        svg.append(f'    <g id="pen_{pen_id}" opacity="{opacity}">')
        
        # Create paths in this group
        for path in paths:
            if len(path) < 2:
                continue
            
            # Move to first point
            path_data = f'M {path[0][0]:.3f} {path[0][1]:.3f}'
            
            # Line to remaining points
            for point in path[1:]:
                path_data += f' L {point[0]:.3f} {point[1]:.3f}'
            
            # Add path element with style
            svg.append(
                f'        <path d="{path_data}" fill="none" stroke="{hex_color}" '
                f'stroke-width="{pens[pen_id]["width_mm"]}"/>'
            )
        
        # Close group
        svg.append('    </g>')
    
    # Close SVG
    svg.append('</svg>')
    
    # Write to file
    with open(filename, 'w') as f:
        f.write('\n'.join(svg))
    
    print(f"Saved SVG to {filename}")

def main():
    scale_factor = 0.3
    penUp = 0
    penDown = 5
    
    print("\nInitializing test sequence...")
    
    setup_commands = [
        "G21",  # Set units to mm
        "G90",  # Absolute positioning
        "G0 F12000",  # Set rapid move speed
        "G1 F6000",  # Set linear move speed
        f"G92 X0 Y0 Z0"  # Set current position as origin
    ]

    # we're plotting on A3 with a 10px border, so lets move by 10px and reset the origin
    position_commands = [
        'G0 X10 Y10',  # Move to start position
        f"G92 X0 Y0 Z0"  # Set current position as origin
    ]
   

    # Load image
    image_color = cv2.imread("ComfyUI_temp_qvgts_00005_.png", cv2.IMREAD_COLOR)
    
    # Resize image to fit A3 with padding
    image_color, available_height_mm = prepare_image_for_a3(image_color, scale_factor)
    height, width = image_color.shape[:2]
    
    # Create all masks
    dark_mask = create_lightness_mask(image_color, threshold=155)
    darker_mask = create_lightness_mask(image_color, threshold=100)
    darkest_mask = create_lightness_mask(image_color, threshold=65)
    dark_detail_mask = create_lightness_mask(image_color, threshold=30)
    darkest_detail_mask = create_lightness_mask(image_color, threshold=15)
    
    # Create paths using original masks
    paths1 = create_diagonal_paths(dark_mask, spacing=10, angle_degrees=45)
    paths2 = create_diagonal_paths(darker_mask, spacing=7, angle_degrees=75)
    paths3 = create_diagonal_paths(darkest_mask, spacing=5, angle_degrees=15)
    paths4 = create_diagonal_paths(dark_detail_mask, spacing=4, angle_degrees=60)
    paths5 = create_diagonal_paths(darkest_detail_mask, spacing=3, angle_degrees=30)
    
    # Process all paths
    hatching_paths = paths1 + paths2 + paths3 + paths4 + paths5
    hatching_paths = break_paths_into_segments(hatching_paths, 
                                        min_length=100, max_length=200,
                                        min_gap=15, max_gap=30)
    
    hatching_paths = apply_perpendicular_jitter(hatching_paths, max_jitter=1)
    hatching_paths = trim_line_ends(hatching_paths, min_line_length=100, max_trim=10)
    hatching_paths = convert_to_bezier_curves(hatching_paths, max_control_offset=4.0, end_bias=0.7, points_per_curve=20)
    hatching_paths = smooth_paths(hatching_paths, tolerance=0.5)
    
    # Create path groups
    path_groups = [("fineliner", hatching_paths)]
    
    # Render all groups
    render_paths_with_cairo(path_groups, width, height, 
                          "combined_layers.png",
                          scale_factor=scale_factor)
    
    export_to_svg(path_groups, width, height, "preview.svg")


    gcode = generate_gcode(path_groups, penUp, penDown, scale_factor, available_height_mm)
    
    # Show summary and ask for confirmation
    print("\nReady to plot:")
    print(f"- Number of path groups: {len(path_groups)}")
    print(f"- Total number of commands: {len(gcode)}")
    print(f"- Using pen heights: Up={penUp}mm, Down={penDown}mm")
    print(f"- Scale factor: {scale_factor}mm per pixel")
    
    response = input("\nSend to plotter? (y/n): ")
    if response.lower() == 'y':
        send_to_plotter(setup_commands + position_commands + gcode, "COM4", 115200)
    else:
        print("Plotting cancelled")
    

if __name__ == "__main__":
    main()