import numpy as np
from svgpathtools import svg2paths #type: ignore
from scipy.special import binom #type: ignore
from sklearn.cluster import DBSCAN #type: ignore
import matplotlib.pyplot as plt #type: ignore
import math
from math import atan2, degrees, sqrt
from itertools import combinations

file_path = 'Test_cases/occlusion2.svg'

def bernstein_poly(i, n, t):
    return binom(n, i) * (t**(n-i)) * ((1 - t)**i)

def bezier_curve(points, n_points=100):
    n = len(points) - 1
    t = np.linspace(0, 1, n_points)
    curve = np.zeros((n_points, 2))
    for i in range(n + 1):
        curve += np.outer(bernstein_poly(i, n, t), points[i])
    return curve

def bezier_derivatives(points, n_points=100):
    n = len(points) - 1
    t = np.linspace(0, 1, n_points)
    d_curve = np.zeros((n_points, 2))
    d2_curve = np.zeros((n_points, 2))

    for i in range(n):
        d_curve += np.outer(bernstein_poly(i, n-1, t), n * (points[i+1] - points[i]))

    for i in range(n-1):
        d2_curve += np.outer(bernstein_poly(i, n-2, t), n * (n-1) * (points[i+2] - 2*points[i+1] + points[i]))

    return d_curve, d2_curve

def extract_bezier_curves(svg_file):
    paths, _ = svg2paths(svg_file)

    cubic_bezier_points = []
    quadratic_bezier_points = []

    for path in paths:
        for segment in path:
            if segment.__class__.__name__ == 'CubicBezier':
                cubic_bezier_points.append([
                    (segment.start.real, segment.start.imag),
                    (segment.control1.real, segment.control1.imag),
                    (segment.control2.real, segment.control2.imag),
                    (segment.end.real, segment.end.imag)
                ])
            elif segment.__class__.__name__ == 'QuadraticBezier':
                quadratic_bezier_points.append([
                    (segment.start.real, segment.start.imag),
                    (segment.control.real, segment.control.imag),
                    (segment.end.real, segment.end.imag)
                ])

    cubic_bezier_points = np.array(cubic_bezier_points)
    quadratic_bezier_points = np.array(quadratic_bezier_points)
    return cubic_bezier_points, quadratic_bezier_points


def osculating_circle_center(points, threshold, n_points=100):
    curve = bezier_curve(points, n_points)
    d_curve, d2_curve = bezier_derivatives(points, n_points)

    curvature = (d_curve[:, 0] * d2_curve[:, 1] - d_curve[:, 1] * d2_curve[:, 0]) / (d_curve[:, 0]**2 + d_curve[:, 1]**2)**1.5
    radius_of_curvature = 1 / np.abs(curvature)

    normal = np.zeros_like(d_curve)
    normal[:, 0] = -d_curve[:, 1]
    normal[:, 1] = d_curve[:, 0]
    normal_direction = np.sign(curvature)
    normal *= normal_direction[:, np.newaxis]
    normal /= np.linalg.norm(normal, axis=1, keepdims=True)

    centers = curve + normal * radius_of_curvature[:, np.newaxis]

    distances = np.linalg.norm(centers - curve, axis=1)
    valid_centers = centers[distances < threshold]

    return valid_centers

def calculate_curvature(points, n_points=100):
    d_curve, d2_curve = bezier_derivatives(points, n_points)
    curvature = (d_curve[:, 0] * d2_curve[:, 1] - d_curve[:, 1] * d2_curve[:, 0]) / (d_curve[:, 0]**2 + d_curve[:, 1]**2)**1.5
    return curvature

def plot_bezier_curves(cubic_points, quadratic_points, threshold=1.0, show_centres = False, size = 10):
    plt.figure(figsize=(size, size))

    for points in cubic_points:
        curve = bezier_curve(points)
        centers = osculating_circle_center(points, threshold)
        color = np.random.rand(3,)  
        plt.plot(curve[:, 0], curve[:, 1], label='Cubic Bezier', color=color)
        if show_centres == True:
            plt.plot(centers[:, 0], centers[:, 1], 'x', label='Osculating Circle Centers', color=color)  # Osculating circle centers

    for points in quadratic_points:
        curve = bezier_curve(points)
        centers = osculating_circle_center(points, threshold)
        color = np.random.rand(3,) 
        plt.plot(curve[:, 0], curve[:, 1], label='Quadratic Bezier', color=color)
        plt.plot(points[:, 0], points[:, 1], 'bo--')
        if show_centres == True:
            plt.plot(centers[:, 0], centers[:, 1], 'x', label='Osculating Circle Centers', color=color)  # Osculating circle centers

    if(show_centres == True):
        plt.title('Bezier Curves with Osculating Circle Centres')
    else:
        plt.title('Bezier Curves')
    plt.xlabel('X')
    plt.ylabel('Y')
    plt.grid()
    plt.show()

plot_bezier_curves(extract_bezier_curves(file_path)[0], extract_bezier_curves(file_path)[1], size = 5)
plot_bezier_curves(extract_bezier_curves(file_path)[0], extract_bezier_curves(file_path)[1], show_centres=True, threshold=100, size = 5)

cubic_beziers = extract_bezier_curves(file_path)[0]
tolerance = 0.5

startpoints = cubic_beziers[:, 0, :]
endpoints = cubic_beziers[:, -1, :]

all_points = np.vstack((startpoints, endpoints))

db = DBSCAN(eps=tolerance, min_samples=2).fit(all_points)

labels = db.labels_

for label in set(labels):
    if label != -1:
        indices = np.where(labels == label)[0]
        centroid = np.mean(all_points[indices], axis=0)
        all_points[indices] = centroid

cubic_beziers[:, 0, :] = all_points[:len(cubic_beziers)]
cubic_beziers[:, -1, :] = all_points[len(cubic_beziers):]


def get_unique_curves(curves):
    unique_curves = []
    seen_endpoints = set()

    for curve in curves:
        start, end = tuple(curve[0]), tuple(curve[-1])
        if (start, end) not in seen_endpoints and (end, start) not in seen_endpoints:
            unique_curves.append(curve)
            seen_endpoints.add((start, end))

    return np.array(unique_curves)


cubic_beziers = get_unique_curves(cubic_beziers)

def bezier_tangent_equations(P0, P1, P2, P3):
    derivative_start = 3 * (P1 - P0)
    derivative_end = 3 * (P3 - P2)

    slope_start = derivative_start[1] / derivative_start[0] if derivative_start[0] != 0 else np.inf
    slope_end = derivative_end[1] / derivative_end[0] if derivative_end[0] != 0 else np.inf

    return slope_start, slope_end

slope_dict = {}
for cubic_bezier in cubic_beziers:
    start_point = cubic_bezier[0]
    end_point = cubic_bezier[-1]
    slope_dict[tuple(np.round(start_point, decimals=4))] = []
    slope_dict[tuple(np.round(end_point, decimals=4))] = []

for cubic_bezier in cubic_beziers:
    start_point = cubic_bezier[0]
    end_point = cubic_bezier[-1]
    slope_start, slope_end = bezier_tangent_equations(cubic_bezier[0], cubic_bezier[1], cubic_bezier[2], cubic_bezier[3])
    slope_dict[tuple(np.round(start_point, decimals=4))].append(slope_start)
    slope_dict[tuple(np.round(end_point, decimals=4))].append(slope_end)

def almost_equal(a, b, threshold):
    a_rad = math.atan(a)
    b_rad = math.atan(b)
    return abs(a_rad - b_rad) <= math.radians(threshold)

def remove_almost_equal_elements(lst, epsilon=0.1):
    n = len(lst)
    maparray = [False] * n
    for i in range(n):
        for j in range(i + 1, n):
            if almost_equal(lst[i], lst[j], 5):
                maparray[i] = True
                maparray[j] = True
    return [lst[i] for i in range(n) if not maparray[i]]

for point in slope_dict:
    slope_dict[point] = remove_almost_equal_elements(slope_dict[point], 0.1)

corner_points = {}
for point in slope_dict:
    if len(slope_dict[point]) == 1:
        corner_points[point] = slope_dict[point][0]

bezier_curves = np.round(cubic_beziers, decimals=3)
def on_same_bezier_curve(p1, p2, haha):
    for curve in haha:
        if any(np.all(curve == p1, axis=1)) and any(np.all(curve == p2, axis=1)):
            return True
    return False

def angle_between_slopes(slope1, slope2):
    angle = math.atan(abs((slope2 - slope1) / (1 + slope1 * slope2)))
    return math.degrees(angle)

def distance_between_points(p1, p2):
    return math.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2)

pairs = []
for (p1, s1), (p2, s2) in combinations(corner_points.items(), 2):
    if not on_same_bezier_curve(p1, p2, bezier_curves):
        angle = angle_between_slopes(s1, s2)
        distance = distance_between_points(p1, p2)
        pairs.append((p1, p2, angle, distance))

sorted_pairs = sorted(pairs, key=lambda x: (x[3], x[2]))

result_pairs = []
for pair in sorted_pairs:
    p1, p2 = pair[0], pair[1]
    if (p1 not in [p for pair in result_pairs for p in pair]) and (p2 not in [p for pair in result_pairs for p in pair]):
        result_pairs.append((p1, p2))
        if len(result_pairs) == 4:
            break

def find_intersection(x1, y1, m1, x2, y2, m2):
    if m1 == m2:
        raise ValueError("Lines are parallel.")
    x_intersect = (m1 * x1 - m2 * x2 + y2 - y1) / (m1 - m2)
    y_intersect = m1 * (x_intersect - x1) + y1
    return x_intersect, y_intersect

def make_filler_curve(x1, y1, m1, x2, y2, m2):
    x_int, y_int = find_intersection(x1, y1, m1, x2, y2, m2)
    cx1, cy1 = (x1 + x_int) / 2, (y1 + y_int) / 2
    cx2, cy2 = (x2 + x_int) / 2, (y2 + y_int) / 2
    return [(x1, y1), (cx1, cy1), (cx2, cy2), (x2, y2)]

control_points = [make_filler_curve(p1[0], p1[1], corner_points[p1], p2[0], p2[1], corner_points[p2])
                  for p1, p2 in result_pairs]
final_curves = np.vstack((cubic_beziers, np.array(control_points)))

plot_bezier_curves(final_curves, [])



convert this into c++