import svgpathtools
import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import numpy as np


# Function to extract paths from an SVG file
def extract_svg_paths(svg_file):
    paths, *attributes = svgpathtools.svg2paths(svg_file)
    return paths


# Function to compute time-reparametrized velocities using toppra based on segment type
def compute_reparametrized_speeds(path, velocity_limit=100, acceleration_limit=500):
    sampled_points = []
    ss = []
    current_s = 0

    for seg in path:
        num_samples = 20 if isinstance(seg, svgpathtools.CubicBezier) else 10
        segment_points = [seg.point(t) for t in np.linspace(0, 1, num_samples)]
        sampled_points.extend(segment_points)
        ss.extend(np.linspace(current_s, current_s + 1, num_samples))
        current_s += 1

    waypoints = np.array([(p.real, p.imag) for p in sampled_points])
    ss = np.array(ss)
    velocity_limits = np.array([[-velocity_limit, velocity_limit]] * len(ss))
    acceleration_limits = np.array([[-acceleration_limit, acceleration_limit]] * len(ss))

    pc = ta.SplineInterpolator(ss, waypoints)
    constraints = [
        constraint.JointVelocityConstraint(velocity_limits),
        constraint.JointAccelerationConstraint(acceleration_limits)
    ]

    instance = algo.TOPPRA(constraints, pc, solver_wrapper="seidel")
    sd_sq = instance.compute_parameterization(0, 0)

    return list(sd_sq)


# Function to convert a path to G-code using computed speeds
def path_to_gcode(path, speeds):
    gcode = []
    gcode.append("G21 ; Set units to mm")
    gcode.append("G90 ; Absolute positioning")

    speed_index = 0
    for seg in path:
        num_samples = 20 if isinstance(seg, svgpathtools.CubicBezier) else 10
        for t in np.linspace(0, 1, num_samples):
            point = seg.point(t)
            speed = speeds[speed_index] if speed_index < len(speeds) else 1000
            gcode.append(f"G1 X{point.real:.3f} Y{point.imag:.3f} F{speed}")
            speed_index += 1
        gcode.append("M05 ; Stop spindle")

    return '\n'.join(gcode)


# Main function to process SVG to G-code
def svg_to_gcode(svg_file):
    paths = extract_svg_paths(svg_file)
    gcode_output = []

    for path in paths:
        speeds = compute_reparametrized_speeds(path)
        gcode_output.append(path_to_gcode(path, speeds))

    return '\n'.join(gcode_output)


# Example usage
# print(svg_to_gcode("drawing.svg"))