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, [])

i want to convert this into cpp but only till 
this function
plot_bezier_curves(extract_bezier_curves(file_path)[0], extract_bezier_curves(file_path)[1], show_centres=True, threshold=100, size = 5)


i converted the full script in cpp here's it
please just do the initial plotting
#include <iostream>
#include <vector>
#include <cmath>
#include <tuple>
#include <string>
#include <algorithm>
#include <unordered_set>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <map>
#include <functional>

// TinyXML2 for SVG parsing
#include "tinyxml2.h"

// Eigen for matrix and vector operations
#include <Eigen/Dense>

// mlpack for DBSCAN
#include <mlpack/core.hpp>
#include <mlpack/methods/dbscan/dbscan.hpp>

using namespace std;
using namespace Eigen;
using namespace tinyxml2;
using namespace mlpack;

// Structure to hold a point
struct Point {
    double x;
    double y;

    bool operator<(const Point& other) const {
        return tie(x, y) < tie(other.x, other.y);
    }

    bool operator==(const Point& other) const {
        return abs(x - other.x) < 1e-6 && abs(y - other.y) < 1e-6;
    }
};

// Function to compute binomial coefficients
double binomial_coefficient(int n, int k) {
    if (k > n - k)
        k = n - k;
    double res = 1;
    for(int i = 0; i < k; ++i){
        res *= (n - i);
        res /= (i + 1);
    }
    return res;
}

// Bernstein Polynomial
double bernstein_poly(int i, int n, double t){
    return binomial_coefficient(n, i) * pow(t, n-i) * pow(1 - t, i);
}

// Function to evaluate a Cubic Bezier curve
vector<Point> evaluate_cubic_bezier(const vector<Point>& points, int n_points=100){
    vector<Point> curve;
    int n = 3; // Cubic
    for(int step=0; step <= n_points; ++step){
        double t = static_cast<double>(step) / n_points;
        double x = 0.0, y = 0.0;
        for(int i=0; i<=n; ++i){
            double bp = bernstein_poly(i, n, t);
            x += bp * points[i].x;
            y += bp * points[i].y;
        }
        curve.emplace_back(Point{ x, y });
    }
    return curve;
}

// Function to evaluate a Quadratic Bezier curve
vector<Point> evaluate_quadratic_bezier(const vector<Point>& points, int n_points=100){
    vector<Point> curve;
    int n = 2; // Quadratic
    for(int step=0; step <= n_points; ++step){
        double t = static_cast<double>(step) / n_points;
        double x = 0.0, y = 0.0;
        for(int i=0; i<=n; ++i){
            double bp = bernstein_poly(i, n, t);
            x += bp * points[i].x;
            y += bp * points[i].y;
        }
        curve.emplace_back(Point{ x, y });
    }
    return curve;
}

// Function to compute first and second derivatives for Cubic Bezier
tuple<vector<Point>, vector<Point>> compute_cubic_derivatives(const vector<Point>& points, int n_points=100){
    vector<Point> first_derivative;
    vector<Point> second_derivative;
    int n = 3; // Cubic

    // First derivative
    for(int step=0; step <= n_points; ++step){
        double t = static_cast<double>(step) / n_points;
        double dx = 0.0, dy = 0.0;
        for(int i=0; i < n; ++i){
            double bp = bernstein_poly(i, n-1, t);
            dx += bp * n * (points[i+1].x - points[i].x);
            dy += bp * n * (points[i+1].y - points[i].y);
        }
        first_derivative.emplace_back(Point{ dx, dy });
    }

    // Second derivative
    for(int step=0; step <= n_points; ++step){
        double t = static_cast<double>(step) / n_points;
        double d2x = 0.0, d2y = 0.0;
        for(int i=0; i < n-1; ++i){
            double bp = bernstein_poly(i, n-2, t);
            d2x += bp * n * (n-1) * (points[i+2].x - 2*points[i+1].x + points[i].x);
            d2y += bp * n * (n-1) * (points[i+2].y - 2*points[i+1].y + points[i].y);
        }
        second_derivative.emplace_back(Point{ d2x, d2y });
    }

    return make_tuple(first_derivative, second_derivative);
}

// Function to compute curvature
vector<double> compute_curvature(const vector<Point>& first_derivative, const vector<Point>& second_derivative){
    int n = first_derivative.size();
    vector<double> curvature(n, 0.0);
    for(int i=0; i<n; ++i){
        double dx = first_derivative[i].x;
        double dy = first_derivative[i].y;
        double ddx = second_derivative[i].x;
        double ddy = second_derivative[i].y;
        double numerator = dx * ddy - dy * ddx;
        double denominator = pow(dx*dx + dy*dy, 1.5);
        if(denominator != 0){
            curvature[i] = numerator / denominator;
        }
        else{
            curvature[i] = 0.0;
        }
    }
    return curvature;
}

// Function to parse SVG and extract cubic and quadratic beziers
pair<vector<vector<Point>>, vector<vector<Point>>> extract_bezier_curves(const string& svg_file){
    XMLDocument doc;
    XMLError eResult = doc.LoadFile(svg_file.c_str());
    if(eResult != XML_SUCCESS){
        cout << "Error loading SVG file: " << svg_file << endl;
        exit(1);
    }

    XMLElement* root = doc.FirstChildElement("svg");
    if(root == nullptr){
        cout << "No <svg> element found!" << endl;
        exit(1);
    }

    vector<vector<Point>> cubic_bezier_curves;
    vector<vector<Point>> quadratic_bezier_curves;

    // Recursive function to find all <path> elements
    function<void(XMLElement*)> find_paths = [&](XMLElement* element) {
        for(XMLElement* child = element->FirstChildElement(); child != nullptr; child = child->NextSiblingElement()){
            if(strcmp(child->Name(), "path") == 0){
                const char* d = child->Attribute("d");
                if(d == nullptr){
                    continue;
                }

                string path_data(d);
                cout << "Parsing path: " << path_data << endl; // Debugging Output
                char current_command = '\0';
                double x = 0.0, y = 0.0;
                double start_x = 0.0, start_y = 0.0; // For 'Z'/'z'

                size_t i = 0;
                while(i < path_data.length()){
                    if(isalpha(path_data[i])){
                        current_command = path_data[i];
                        i++;
                    }
                    else{
                        // Parse numbers
                        vector<double> nums;
                        while(i < path_data.length() && (isdigit(path_data[i]) || path_data[i]=='.' || path_data[i]=='-' || path_data[i]==',' || isspace(path_data[i]))){
                            if(path_data[i] == ',' || isspace(path_data[i])){
                                i++;
                                continue;
                            }
                            size_t start_num = i;
                            // Handle negative numbers and decimal points
                            if(path_data[i] == '-'){
                                i++;
                            }
                            while(i < path_data.length() && (isdigit(path_data[i]) || path_data[i]=='.')){
                                i++;
                            }
                            string num_str = path_data.substr(start_num, i - start_num);
                            try{
                                double num = stod(num_str);
                                nums.push_back(num);
                            }
                            catch(...){
                                break;
                            }
                        }

                        switch(current_command){
                            case 'M':
                            case 'm':{
                                for(size_t j = 0; j + 1 < nums.size(); j += 2){
                                    if(current_command == 'm'){
                                        x += nums[j];
                                        y += nums[j+1];
                                    }
                                    else{
                                        x = nums[j];
                                        y = nums[j+1];
                                    }
                                    start_x = x;
                                    start_y = y;
                                    // Subsequent coord pairs after 'M'/'m' are treated as 'L'/'l'
                                }
                                break;
                            }
                            case 'C':
                            case 'c':{
                                bool is_relative = (current_command == 'c');
                                for(size_t j = 0; j + 5 < nums.size(); j += 6){
                                    vector<Point> bezier;
                                    bezier.emplace_back(Point{ x, y });
                                    double ctrl1_x = is_relative ? x + nums[j] : nums[j];
                                    double ctrl1_y = is_relative ? y + nums[j+1] : nums[j+1];
                                    double ctrl2_x = is_relative ? x + nums[j+2] : nums[j+2];
                                    double ctrl2_y = is_relative ? y + nums[j+3] : nums[j+3];
                                    double end_x = is_relative ? x + nums[j+4] : nums[j+4];
                                    double end_y = is_relative ? y + nums[j+5] : nums[j+5];
                                    bezier.emplace_back(Point{ ctrl1_x, ctrl1_y });
                                    bezier.emplace_back(Point{ ctrl2_x, ctrl2_y });
                                    bezier.emplace_back(Point{ end_x, end_y });
                                    cubic_bezier_curves.emplace_back(bezier);
                                    x = end_x;
                                    y = end_y;
                                }
                                break;
                            }
                            case 'Q':
                            case 'q':{
                                bool is_relative = (current_command == 'q');
                                for(size_t j = 0; j + 3 < nums.size(); j += 4){
                                    vector<Point> bezier;
                                    bezier.emplace_back(Point{ x, y });
                                    double ctrl_x = is_relative ? x + nums[j] : nums[j];
                                    double ctrl_y = is_relative ? y + nums[j+1] : nums[j+1];
                                    double end_x = is_relative ? x + nums[j+2] : nums[j+2];
                                    double end_y = is_relative ? y + nums[j+3] : nums[j+3];
                                    bezier.emplace_back(Point{ ctrl_x, ctrl_y });
                                    bezier.emplace_back(Point{ end_x, end_y });
                                    quadratic_bezier_curves.emplace_back(bezier);
                                    x = end_x;
                                    y = end_y;
                                }
                                break;
                            }
                            case 'Z':
                            case 'z':{
                                x = start_x;
                                y = start_y;
                                break;
                            }
                            // Handle other commands like 'L', 'l' if necessary
                            default:
                                // For unsupported commands, skip
                                break;
                        }
                    }
                }
            }
            else{
                // Recursive call for nested elements
                find_paths(child);
            }
        }
    };

    // Start recursion from root
    find_paths(root);

    return make_pair(cubic_bezier_curves, quadratic_bezier_curves);
}

// Function to compute tangent slopes at start and end points of cubic beziers
map<string, vector<double>> compute_tangent_slopes(const vector<vector<Point>>& cubic_beziers){
    map<string, vector<double>> slope_dict;

    // Helper lambda to convert point to string (rounded)
    auto point_to_string = [](const Point& p) -> string{
        // Round to 4 decimal places
        stringstream ss;
        ss << fixed << setprecision(4) << p.x << "," << p.y;
        return ss.str();
    };

    // Initialize slope_dict
    for(const auto& bezier : cubic_beziers){
        string start = point_to_string(bezier[0]);
        string end = point_to_string(bezier[3]);
        // Initialize empty vectors
        if(slope_dict.find(start) == slope_dict.end()){
            slope_dict[start] = vector<double>();
        }
        if(slope_dict.find(end) == slope_dict.end()){
            slope_dict[end] = vector<double>();
        }
    }

    // Compute slopes
    for(const auto& bezier : cubic_beziers){
        // Compute derivatives at start and end
        // derivative_start = 3*(P1 - P0)
        double dx_start = 3 * (bezier[1].x - bezier[0].x);
        double dy_start = 3 * (bezier[1].y - bezier[0].y);
        // derivative_end = 3*(P3 - P2)
        double dx_end = 3 * (bezier[3].x - bezier[2].x);
        double dy_end = 3 * (bezier[3].y - bezier[2].y);

        // Compute slopes
        double slope_start = (dx_start != 0) ? (dy_start / dx_start) : INFINITY;
        double slope_end = (dx_end != 0) ? (dy_end / dx_end) : INFINITY;

        // Add to dict
        string start = point_to_string(bezier[0]);
        string end = point_to_string(bezier[3]);
        slope_dict[start].push_back(slope_start);
        slope_dict[end].push_back(slope_end);
    }

    return slope_dict;
}

// Helper function to check if two slopes are almost equal within a threshold (degrees)
bool almost_equal(double slope1, double slope2, double threshold_deg){
    if(isinf(slope1) && isinf(slope2)){
        return true;
    }
    if(isinf(slope1) || isinf(slope2)){
        return false;
    }
    double angle1 = atan(slope1);
    double angle2 = atan(slope2);
    double diff = abs(angle1 - angle2);
    return diff <= (threshold_deg * M_PI / 180.0);
}

// Function to remove almost equal elements from a list of slopes
vector<double> remove_almost_equal_elements(const vector<double>& slopes, double threshold_deg=5.0){
    vector<double> filtered;
    vector<bool> to_remove(slopes.size(), false);
    for(int i=0; i<slopes.size(); ++i){
        if(to_remove[i]) continue;
        for(int j=i+1; j<slopes.size(); ++j){
            if(almost_equal(slopes[i], slopes[j], threshold_deg)){
                to_remove[j] = true;
            }
        }
        filtered.push_back(slopes[i]);
    }
    return filtered;
}

// Function to identify corner points
map<string, double> identify_corners(map<string, vector<double>>& slope_dict){
    map<string, double> corner_points;
    for(auto& [point, slopes] : slope_dict){
        // Remove almost equal slopes
        vector<double> unique_slopes = remove_almost_equal_elements(slopes, 5.0);
        if(unique_slopes.size() == 1){
            corner_points[point] = unique_slopes[0];
        }
    }
    return corner_points;
}

// Function to check if two points are on the same bezier curve
bool on_same_bezier_curve(const string& p1, const string& p2, const vector<vector<Point>>& beziers){
    for(const auto& bezier : beziers){
        bool found_p1 = false, found_p2 = false;
        for(const auto& p : bezier){
            // Convert point to string (rounded)
            stringstream ss;
            ss << fixed << setprecision(4) << p.x << "," << p.y;
            string curr = ss.str();
            if(curr == p1){
                found_p1 = true;
            }
            if(curr == p2){
                found_p2 = true;
            }
            if(found_p1 && found_p2){
                return true;
            }
        }
    }
    return false;
}

// Function to find intersection point of two lines defined by (x1, y1, m1) and (x2, y2, m2)
pair<double, double> find_intersection(double x1, double y1, double m1, double x2, double y2, double m2){
    if(m1 == m2){
        throw invalid_argument("Lines are parallel.");
    }
    double x = (m1 * x1 - m2 * x2 + y2 - y1) / (m1 - m2);
    double y = m1 * (x - x1) + y1;
    return make_pair(x, y);
}

// Function to create filler cubic bezier curve between two points with given slopes
vector<Point> make_filler_curve(double x1, double y1, double m1, double x2, double y2, double m2){
    pair<double, double> intersect;
    try{
        intersect = find_intersection(x1, y1, m1, x2, y2, m2);
    }
    catch(const invalid_argument& e){
        // Parallel lines, cannot find intersection. Return a straight line with duplicated control points
        return { Point{ x1, y1 }, Point{ (x1 + x2)/2, (y1 + y2)/2 }, Point{ (x1 + x2)/2, (y1 + y2)/2 }, Point{ x2, y2 } };
    }

    double cx1 = (x1 + intersect.first) / 2.0;
    double cy1 = (y1 + intersect.second) / 2.0;
    double cx2 = (x2 + intersect.first) / 2.0;
    double cy2 = (y2 + intersect.second) / 2.0;

    return { Point{ x1, y1 }, Point{ cx1, cy1 }, Point{ cx2, cy2 }, Point{ x2, y2 } };
}

// Function to write curves to file for plotting
void write_curves_to_file(const vector<vector<Point>>& cubic_beziers, const vector<vector<Point>>& quadratic_beziers, const vector<vector<Point>>& filler_beziers, const string& filename){
    ofstream file(filename);
    if(!file.is_open()){
        cout << "Error opening file for writing: " << filename << endl;
        return;
    }

    // Write cubic beziers
    for(const auto& bezier : cubic_beziers){
        for(const auto& p : bezier){
            file << p.x << " " << p.y << "\n";
        }
        file << "\n"; // Separator
    }

    // Write quadratic beziers
    for(const auto& bezier : quadratic_beziers){
        for(const auto& p : bezier){
            file << p.x << " " << p.y << "\n";
        }
        file << "\n"; // Separator
    }

    // Write filler beziers
    for(const auto& bezier : filler_beziers){
        for(const auto& p : bezier){
            file << p.x << " " << p.y << "\n";
        }
        file << "\n"; // Separator
    }

    file.close();
}

int main(int argc, char* argv[]){
    if(argc < 2){
        cout << "Usage: " << argv[0] << " <svg_file>" << endl;
        return 1;
    }

    string file_path = argv[1];

    // 1. Extract Bezier Curves from SVG
    auto bezier_curves = extract_bezier_curves(file_path);
    vector<vector<Point>> cubic_beziers = bezier_curves.first;
    vector<vector<Point>> quadratic_beziers = bezier_curves.second;

    cout << "Extracted " << cubic_beziers.size() << " cubic beziers and " << quadratic_beziers.size() << " quadratic beziers." << endl;

    if(cubic_beziers.empty() && quadratic_beziers.empty()){
        cout << "No Bézier curves found in the SVG." << endl;
        return 1;
    }

    // 2. Flat Lists for Clustering
    vector<Point> startpoints, endpoints;
    for(const auto& bezier : cubic_beziers){
        startpoints.emplace_back(bezier[0]);
        endpoints.emplace_back(bezier[3]);
    }

    // Combine all points
    vector<Point> all_points;
    all_points.reserve(startpoints.size() + endpoints.size());
    all_points.insert(all_points.end(), startpoints.begin(), startpoints.end());
    all_points.insert(all_points.end(), endpoints.begin(), endpoints.end());

    // 3. Prepare Data for mlpack DBSCAN
    arma::mat data(2, all_points.size());
    for (size_t i = 0; i < all_points.size(); ++i) {
        data(0, i) = all_points[i].x;
        data(1, i) = all_points[i].y;
    }

    // 4. Perform DBSCAN Clustering (Use the data created above)
    double eps = 5.0; // Adjust based on your SVG scale
    int minPts = 2;

    // Correct usage of KDTree and DefaultPointSelection 
    using KDTree = mlpack::KDTree<mlpack::EuclideanDistance>; // Define the KDTree alias inside main
    auto dbs = DBSCAN<KDTree, mlpack::tree::DefaultPointSelection<arma::Row<size_t>>>(eps, minPts); // Create DBSCAN instance

    arma::Row<size_t> assignments;
    dbs.Cluster(data, assignments);

    // 5. Replace Points in Clusters with Centroids
    map<size_t, vector<Point>> clusters;

    for (size_t i = 0; i < assignments.n_elem; ++i) {
        if (assignments[i] == -1) {  // Check for noise label -1
            continue;
        }
        clusters[assignments[i]].emplace_back(all_points[i]);
    }

    // Compute centroids
    for(auto& [cluster_id, points] : clusters){
        double sum_x = 0.0, sum_y = 0.0;
        for(const auto& p : points){
            sum_x += p.x;
            sum_y += p.y;
        }
        double centroid_x = sum_x / points.size();
        double centroid_y = sum_y / points.size();
        for(auto& p : clusters[cluster_id]){
            p.x = centroid_x;
            p.y = centroid_y;
        }
    }

    // Update startpoints and endpoints with clustered points
    for (int i = 0; i < startpoints.size(); ++i) {
        if (assignments[i] != -1) {
            if (clusters.find(assignments[i]) != clusters.end()) {
                startpoints[i] = clusters[assignments[i]][0];
            }
        }
    }

    // Update endpoints with clustered points
    for (int i = 0; i < endpoints.size(); ++i) {
        // Calculate the index in assignments for the endpoint
        size_t endpoint_index = i + startpoints.size(); 
        if (assignments[endpoint_index] != -1) {
            if (clusters.find(assignments[endpoint_index]) != clusters.end()) {
                endpoints[i] = clusters[assignments[endpoint_index]][0];
            }
        }
    }

    // Update cubic_beziers with clustered points
    for(int i=0; i < cubic_beziers.size(); ++i){
        cubic_beziers[i][0] = startpoints[i];
        cubic_beziers[i][3] = endpoints[i];
    }

    // 6. Remove Duplicate Curves
    // Function to get unique curves based on start and end points
    auto get_unique_curves = [](const vector<vector<Point>>& curves) -> vector<vector<Point>>{
        vector<vector<Point>> unique_curves;
        unordered_set<string> seen_endpoints;

        // Helper lambda to convert point to string
        auto point_to_string = [](const Point& p) -> string{
            stringstream ss;
            ss << fixed << setprecision(4) << p.x << "," << p.y;
            return ss.str();
        };

        for(const auto& curve : curves){
            string start = point_to_string(curve[0]);
            string end = point_to_string(curve[3]);
            string key1 = start + "-" + end;
            string key2 = end + "-" + start;
            if(seen_endpoints.find(key1) == seen_endpoints.end() && seen_endpoints.find(key2) == seen_endpoints.end()){
                unique_curves.push_back(curve);
                seen_endpoints.insert(key1);
            }
        }
        return unique_curves;
    };

    cubic_beziers = get_unique_curves(cubic_beziers);
    cout << "After removing duplicates, " << cubic_beziers.size() << " cubic beziers remain." << endl;

    if(cubic_beziers.empty()){
        cout << "No unique cubic Bézier curves found after removing duplicates." << endl;
        return 1;
    }

    // 7. Compute Tangent Slopes
    map<string, vector<double>> slope_dict = compute_tangent_slopes(cubic_beziers);

    // 8. Identify Corner Points
    map<string, double> corner_points = identify_corners(slope_dict);
    cout << "Identified " << corner_points.size() << " corner points." << endl;

    if(corner_points.empty()){
        cout << "No corner points identified." << endl;
    }

    // 9. Pair Corner Points
    vector<tuple<string, string, double, double>> pairs; // p1, p2, angle, distance
    for(auto it1 = corner_points.begin(); it1 != corner_points.end(); ++it1){
        for(auto it2 = next(it1); it2 != corner_points.end(); ++it2){
            string p1 = it1->first;
            string p2 = it2->first;
            // Check if on same bezier curve
            if(on_same_bezier_curve(p1, p2, cubic_beziers)){
                continue;
            }
            // Compute angle
            double angle = 0.0;
            if(isinf(it1->second) && isinf(it2->second)){
                angle = 0.0;
            }
            else if(isinf(it1->second) || isinf(it2->second)){
                angle = 90.0;
            }
            else{
                double angle_rad = atan(abs((it2->second - it1->second) / (1 + it1->second * it2->second)));
                angle = abs((angle_rad * 180.0) / M_PI);
            }
            // Compute distance
            double distance = 0.0;
            double x1_val, y1_val, x2_val, y2_val;
            sscanf(p1.c_str(), "%lf,%lf", &x1_val, &y1_val);
            sscanf(p2.c_str(), "%lf,%lf", &x2_val, &y2_val);
            distance = sqrt(pow(x2_val - x1_val, 2) + pow(y2_val - y1_val, 2));
            pairs.emplace_back(make_tuple(p1, p2, angle, distance));
        }
    }

    // Sort pairs by distance and then by angle
    sort(pairs.begin(), pairs.end(), [&](const tuple<string, string, double, double>& a, const tuple<string, string, double, double>& b) -> bool{
        if(get<3>(a) != get<3>(b))
            return get<3>(a) < get<3>(b);
        return get<2>(a) < get<2>(b);
    });

    // Select top 4 non-overlapping pairs
    vector<pair<string, string>> result_pairs;
    unordered_set<string> used_points;
    for(const auto& pair : pairs){
        string p1 = get<0>(pair);
        string p2 = get<1>(pair);
        if(used_points.find(p1) == used_points.end() && used_points.find(p2) == used_points.end()){
            result_pairs.emplace_back(make_pair(p1, p2));
            used_points.insert(p1);
            used_points.insert(p2);
            if(result_pairs.size() == 4){
                break;
            }
        }
    }

    cout << "Selected " << result_pairs.size() << " pairs for filler curves." << endl;

    // 10. Generate Filler Curves
    vector<vector<Point>> filler_curves;
    for(const auto& pair : result_pairs){
        // Parse points
        double x1, y1, x2, y2;
        double m1, m2;
        sscanf(pair.first.c_str(), "%lf,%lf", &x1, &y1);
        sscanf(pair.second.c_str(), "%lf,%lf", &x2, &y2);
        m1 = corner_points[pair.first];
        m2 = corner_points[pair.second];
        // Create filler curve
        vector<Point> filler = make_filler_curve(x1, y1, m1, x2, y2, m2);
        filler_curves.emplace_back(filler);
    }

    // 11. Combine All Curves
    vector<vector<Point>> final_curves = cubic_beziers;
    final_curves.insert(final_curves.end(), filler_curves.begin(), filler_curves.end());

    // 12. Write Curves to File for Plotting
    write_curves_to_file(final_curves, quadratic_beziers, filler_curves, "processed_curves.dat");
    cout << "Curves written to processed_curves.dat for plotting." << endl;

    // Inform the user to use GNUplot for visualization
    cout << "To visualize the curves, you can use GNUplot with the following commands:" << endl;
    cout << "gnuplot> set title 'Processed Bézier Curves'" << endl;
    cout << "gnuplot> set xlabel 'X'" << endl;
    cout << "gnuplot> set ylabel 'Y'" << endl;
    cout << "gnuplot> plot 'processed_curves.dat' with lines title 'Bézier Curves'" << endl;

    return 0;
}