#include <iostream>
#include <vector>
#include <cmath>
#include <tuple>
#include <string>
#include <algorithm>
#include <unordered_set>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <map>

// 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.push_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.push_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.push_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.push_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 compute osculating circle centers
vector<Point> compute_osculating_centers(const vector<Point>& curve, const vector<double>& curvature, double threshold=1.0){
    int n = curve.size();
    vector<Point> centers;
    for(int i=0; i<n; ++i){
        if(abs(curvature[i]) < 1e-6){
            continue; // Avoid division by zero
        }
        double radius = 1.0 / abs(curvature[i]);
        // Compute normal vector
        double dx = 0.0, dy = 0.0;
        // Approximate tangent vector
        if(i < n-1){
            dx = curve[i+1].x - curve[i].x;
            dy = curve[i+1].y - curve[i].y;
        }
        else{
            dx = curve[i].x - curve[i-1].x;
            dy = curve[i].y - curve[i-1].y;
        }
        // Normalize
        double length = sqrt(dx*dx + dy*dy);
        if(length == 0){
            continue;
        }
        dx /= length;
        dy /= length;
        // Normal vector
        double nx = -dy;
        double ny = dx;
        // Determine direction based on curvature sign
        if(curvature[i] < 0){
            nx *= -1;
            ny *= -1;
        }
        // Compute center
        double cx = curve[i].x + nx * radius;
        double cy = curve[i].y + ny * radius;
        // Check threshold
        double distance = sqrt((cx - curve[i].x)*(cx - curve[i].x) + (cy - curve[i].y)*(cy - curve[i].y));
        if(distance < threshold){
            centers.push_back(Point{ cx, cy });
        }
    }
    return centers;
}

// 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_beziers;
    vector<vector<Point>> quadratic_beziers;

    // Iterate through all <path> elements
    for(XMLElement* path = root->FirstChildElement("path"); path != nullptr; path = path->NextSiblingElement("path")){
        const char* d = path->Attribute("d");
        if(d == nullptr){
            continue;
        }

        string path_data(d);
        // Simple parser for 'C' (cubic) and 'Q' (quadratic) commands
        // Note: This is a simplified parser and may not handle all SVG path syntax
        char current_command = '\0';
        vector<Point> points;
        vector<Point> current_points;

        double x = 0.0, y = 0.0;

        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 = i;
                    // Handle potential 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, i - start);
                    try{
                        double num = stod(num_str);
                        nums.push_back(num);
                    }
                    catch(...){
                        // Handle potential conversion errors
                        break;
                    }
                }

                if(current_command == 'C' || current_command == 'c'){
                    // Cubic Bezier: C x1 y1 x2 y2 x y
                    // Assuming absolute coordinates ('C'), as handling relative coordinates ('c') requires additional logic
                    for(size_t j = 0; j + 5 < nums.size(); j += 6){
                        vector<Point> bezier;
                        bezier.push_back(Point{ x, y });
                        bezier.push_back(Point{ nums[j], nums[j+1] });
                        bezier.push_back(Point{ nums[j+2], nums[j+3] });
                        bezier.push_back(Point{ nums[j+4], nums[j+5] });
                        cubic_beziers.push_back(bezier);
                        x = nums[j+4];
                        y = nums[j+5];
                    }
                }
                else if(current_command == 'Q' || current_command == 'q'){
                    // Quadratic Bezier: Q x1 y1 x y
                    // Assuming absolute coordinates ('Q')
                    for(size_t j = 0; j + 3 < nums.size(); j += 4){
                        vector<Point> bezier;
                        bezier.push_back(Point{ x, y });
                        bezier.push_back(Point{ nums[j], nums[j+1] });
                        bezier.push_back(Point{ nums[j+2], nums[j+3] });
                        quadratic_beziers.push_back(bezier);
                        x = nums[j+2];
                        y = nums[j+3];
                    }
                }
                // You can handle other commands (like 'M', 'L', etc.) if needed
            }
        }
    }

    return make_pair(cubic_beziers, quadratic_beziers);
}

// 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);
        if(unique_slopes.size() == 1){
            corner_points[point] = unique_slopes[0];
        }
    }
    return corner_points;
}

// Function to compute angle between two slopes
double angle_between_slopes(double slope1, double slope2){
    if(isinf(slope1) && isinf(slope2)){
        return 0.0;
    }
    if(isinf(slope1) || isinf(slope2)){
        return 90.0;
    }
    double angle_rad = atan(abs((slope2 - slope1) / (1 + slope1 * slope2)));
    return abs((angle_rad * 180.0) / M_PI);
}

// Function to compute distance between two points (given as strings)
double distance_between_points(const string& p1_str, const string& p2_str){
    double x1, y1, x2, y2;
    sscanf(p1_str.c_str(), "%lf,%lf", &x1, &y1);
    sscanf(p2_str.c_str(), "%lf,%lf", &x2, &y2);
    return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

// 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(){
    string file_path = "../problems/occlusion2.svg"; // Update the path

    // 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;

    // 2. Flat Lists for Clustering
    vector<Point> startpoints, endpoints;
    for(const auto& bezier : cubic_beziers){
        startpoints.push_back(bezier[0]);
        endpoints.push_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 = 0.5;
    int minPts = 2;
    DBSCAN<> dbs(eps, minPts);

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


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

    // Infer the noise label (important for mlpack 3 compatibility):
    for (size_t i = 0; i < assignments.n_elem; ++i) {
        if (assignments[i] < 0) { 
            noiseLabel = assignments[i];
            break;
        }
    }

    for (size_t i = 0; i < assignments.n_elem; ++i) {
        if (assignments[i] == noiseLabel) {
            continue;
        }
        clusters[assignments[i]].push_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){
        size_t label = assignments[i];
        if(label != noiseLabel){ 
            startpoints[i] = clusters[label][0];
        }
    }
    for(int i=0; i < endpoints.size(); ++i){
        size_t label = assignments[i + startpoints.size()];
        if(label != noiseLabel){
            endpoints[i] = clusters[label][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;

    // 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;

    // 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 = angle_between_slopes(it1->second, it2->second);
            // Compute distance
            double distance = distance_between_points(p1, p2);
            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.push_back(filler);
    }

    // Combine all curves
    vector<vector<Point>> final_curves = cubic_beziers;
    final_curves.insert(final_curves.end(), filler_curves.begin(), filler_curves.end());

    // 11. 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;
}

this is the final code i made removing the issues