import svgpathtools
import math
from typing import List, Optional

class SVGToGCodeConverter:
    """
    General SVG to GCode converter, parametrized with the available functions.
    """

    def __init__(self, supported_g_functions: List[str]):
        """Initialize the converter with the supported G-functions.

        Args:
            supported_g_functions (List[str]): List of supported G-code functions (e.g., ["G1", "G2", "G3", "G4", "G5", "G6", "G7"]).
        """
        self.supported_g_functions = supported_g_functions
        self._warned_about_g5 = False

    def point_to_gcode(self, x: float, y: float, feedrate: Optional[float] = None) -> str:
        gcode = f"G1 X{x:.4f} Y{y:.4f}"
        if feedrate is not None:
            gcode += f" F{feedrate}"
        return gcode

    @staticmethod
    def move_to_gcode(x: float, y: float) -> str:
        return f"G0 X{x:.4f} Y{y:.4f}"

    def line_to_gcode(self, start: complex, end: complex) -> str:
        return self.point_to_gcode(end.real, end.imag)

    def arc_to_gcode(self, start: complex, end: complex, center: complex, clockwise: bool) -> str:
        if "G2" not in self.supported_g_functions and "G3" not in self.supported_g_functions:
            raise NotImplementedError("Arc support requires G2/G3 functions.")
        i_offset = center.real - start.real
        j_offset = center.imag - start.imag
        g_command = "G2" if clockwise else "G3"
        return f"{g_command} X{end.real:.4f} Y{end.imag:.4f} I{i_offset:.4f} J{j_offset:.4f}"

    def bezier_to_gcode(self, start: complex, control1: complex, control2: complex, end: complex, steps: int = 20) -> List[str]:
        if "G5" in self.supported_g_functions:
            return [
                f"G5 X{end.real:.4f} Y{end.imag:.4f} I{control1.real:.4f} J{control1.imag:.4f} P{control2.real:.4f} Q{control2.imag:.4f}"
            ]
        else:
            if not self._warned_about_g5:
                print("Warning: G5 is not supported. Approximating Bézier curve with linear segments.")
                self._warned_about_g5 = True
            gcode_lines = []
            for t in [i / steps for i in range(1, steps + 1)]:
                x = (1 - t)**3 * start.real + 3 * (1 - t)**2 * t * control1.real + 3 * (1 - t) * t**2 * control2.real + t**3 * end.real
                y = (1 - t)**3 * start.imag + 3 * (1 - t)**2 * t * control1.imag + 3 * (1 - t) * t**2 * control2.imag + t**3 * end.imag
                gcode_lines.append(self.point_to_gcode(x, y))
            return gcode_lines

    # Following two are not canonical, I don't know where I found those lol
    def ellipse_to_gcode(self, start: complex, end: complex, center: complex, rx: float, ry: float, rotation: float, clockwise: bool) -> str:
        if "G7" not in self.supported_g_functions:
            raise NotImplementedError("Ellipse support requires G7 function.")
        i_offset = center.real - start.real
        j_offset = center.imag - start.imag
        g_command = "G7"  # Assuming G7 is used for ellipses
        return f"{g_command} X{end.real:.4f} Y{end.imag:.4f} I{i_offset:.4f} J{j_offset:.4f} R1={rx:.4f} R2={ry:.4f} ROT={rotation:.4f}"

    def parabola_to_gcode(self, start: complex, vertex: complex, end: complex) -> str:
        if "G6" not in self.supported_g_functions:
            raise NotImplementedError("Parabola support requires G6 function.")
        g_command = "G6"  # Assuming G6 is used for parabolas
        return f"{g_command} X{end.real:.4f} Y{end.imag:.4f} VERTEX_X{vertex.real:.4f} VERTEX_Y{vertex.imag:.4f}"

    def wait_time_gcode(self, seconds: float) -> str:
        if "G4" not in self.supported_g_functions:
            raise NotImplementedError("Wait time support requires G4 function.")
        return f"G4 P{seconds:.3f}"

    def parse_svg_to_gcode(self, svg_path: svgpathtools.Path) -> List[str]:
        gcode = []

        for segment in svg_path:
            if isinstance(segment, svgpathtools.Line):
                gcode.append(self.line_to_gcode(segment.start, segment.end))

            elif isinstance(segment, svgpathtools.CubicBezier):
                gcode.extend(self.bezier_to_gcode(segment.start, segment.control1, segment.control2, segment.end))

            elif isinstance(segment, svgpathtools.Arc):
                center = segment.center
                if "G7" in self.supported_g_functions:
                    rx, ry = segment.radius.real, segment.radius.imag
                    rotation = segment.rotation
                    gcode.append(self.ellipse_to_gcode(segment.start, segment.end, center, rx, ry, rotation, segment.sweep_flag == 0))
                else:
                    gcode.append(self.arc_to_gcode(segment.start, segment.end, center, segment.sweep_flag == 0))

            elif hasattr(segment, "vertex") and "G6" in self.supported_g_functions:
                gcode.append(self.parabola_to_gcode(segment.start, segment.vertex, segment.end))

            else:
                raise ValueError(f"Unsupported path segment: {segment}")

        return gcode

    def svg_to_gcode(self, file_path: str, output_path: str) -> None:
        """Convert an SVG file to G-code.

        Args:
            file_path (str): Path to the input SVG file.
            output_path (str): Path to save the output G-code file.
        """
        paths, attributes = svgpathtools.svg2paths(file_path)

        gcode = ["G21 ; Set units to mm", "G90 ; Absolute positioning"]  # G-code header

        for path in paths:
            if path:
                start_point = path[0].start
                gcode.append(self.move_to_gcode(start_point.real, start_point.imag))
                gcode.extend(self.parse_svg_to_gcode(path))

        with open(output_path, "w") as gcode_file:
            gcode_file.write("\n".join(gcode))
        print(f"G-code saved to {output_path}")

# Example usage:
# converter = SVGToGCodeConverter(["G1", "G2", "G3", "G5", "G6", "G7"])
# converter.svg_to_gcode("example.svg", "output.gcode")