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

file_path = '../problems/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,)  # Random color for each curve
        plt.plot(curve[:, 0], curve[:, 1], label='Cubic Bezier', color=color)
        #plt.plot(points[:, 0], points[:, 1], 'ro--')  # Control points
        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,)  # Random color for each curve
        plt.plot(curve[:, 0], curve[:, 1], label='Quadratic Bezier', color=color)
        plt.plot(points[:, 0], points[:, 1], 'bo--')  # Control points
        if show_centres == True:
            plt.plot(centers[:, 0], centers[:, 1], 'x', label='Osculating Circle Centers', color=color)  # Osculating circle centers

    #plt.legend()
    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):]

# 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)
# print(len(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, [])

explain this