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


# Function to extract and subdivide paths into continuous subpaths
def extract_svg_paths(svg_file):
    paths, attributes = svgpathtools.svg2paths(svg_file)
    continuous_paths = []

    for path in paths:
        subpath = []
        last_point = None
        for seg in path:
            if last_point is not None and seg.start != last_point:
                continuous_paths.append(svgpathtools.Path(*subpath))
                subpath = []
            subpath.append(seg)
            last_point = seg.end
        if subpath:
            continuous_paths.append(svgpathtools.Path(*subpath))

    return continuous_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.0

    for k, seg in enumerate(path):
        num_samples = 20 if isinstance(seg, svgpathtools.CubicBezier) else 10
        n = len(path)
        if k < n - 1:
            segment_points = np.array([[seg.point(t).real, seg.point(t).imag] for t in np.linspace(0, 1, num_samples)][:-1])
            sampled_points.append(segment_points)
            ss.extend(list(np.linspace(current_s, current_s + 1, num_samples))[:-1])
            current_s += 1
        else:
            segment_points = np.array([[seg.point(t).real, seg.point(t).imag] for t in np.linspace(0, 1, num_samples)])
            sampled_points.append(segment_points)
            ss.extend(np.linspace(current_s, current_s + 1, num_samples))
            current_s += 1

    sampled_points = np.vstack(sampled_points)
    ss = np.array(ss)  # Ensure correct shape
    velocity_limits = np.array([[-velocity_limit, velocity_limit]] * sampled_points.shape[1])
    acceleration_limits = np.array([[-acceleration_limit, acceleration_limit]] * sampled_points.shape[1])

    pc = ta.SplineInterpolator(ss, sampled_points)
    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(np.sqrt(sd_sq[1]))



def path_to_gcode(path, speeds):
    gcode = []
    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

    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 = ["G21 ; Set units to mm", "G90 ; Absolute positioning"]

    for path in paths:
        p_start = path.point(0)
        gcode_output.append(f"G0 X{p_start.real:.3f} Y{p_start.imag:.3f}")
        speeds = compute_reparametrized_speeds(path)
        gcode_output.append(path_to_gcode(path, speeds))

    return '\n'.join(gcode_output)

print(svg_to_gcode("clock.svg"))

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