Traveling Salesman Problem Solver

void prim_mst(std::vector<Coordinate_MST>& coordinates, double& weight_tot) {
    coordinates[0].distance = 0; // Start with first vertex

    std::priority_queue<std::pair<double, size_t>,
                        std::vector<std::pair<double, size_t>>,
                        Compare> min_heap;

    min_heap.emplace(0.0, 0); // Push the starting vertex

    while (!min_heap.empty()) {
        auto [curr_dist, curr_idx] = min_heap.top();
        min_heap.pop();

        if (coordinates[curr_idx].visited) continue;

        coordinates[curr_idx].visited = true;
        weight_tot += sqrt(curr_dist); // Accumulate MST weight (sqrt if using squared distance)

        // Try to update distances to unvisited neighbors
        for (size_t i = 0; i < coordinates.size(); i++) {
            if (!coordinates[i].visited) {
                double weight = euclidean_distance_MST(coordinates[curr_idx], coordinates[i]);
                if (coordinates[i].distance > weight && travel(coordinates[curr_idx], coordinates[i])) {
                    coordinates[i].distance = weight;
                    coordinates[i].prev = static_cast<long>(curr_idx);
                    min_heap.emplace(weight, i);
                }
            }
        }
    }
}

std::vector<size_t> arbitrary_insertion_tsp(std::vector<Coordinate_FAST>& coords, double& weight_tot) {
    std::vector<size_t> path;
    path.reserve(coords.size() + 1); // Reserve space to avoid reallocations

    // Start path with nodes 0 → 1
    path.push_back(0);
    weight_tot += euclidean_distance_FAST(coords[0], coords[1]);
    path.push_back(1);

    // Iteratively insert remaining nodes at the position with least increase in total distance
    for (size_t i = 2; i < coords.size(); ++i) {
        const auto& curr = coords[i];
        size_t best_pos = 0;
        double best_dist = std::numeric_limits<double>::max();

        // Try all insertion positions between existing path points
        for (size_t j = 0; j < path.size() - 1; ++j) {
            double dist1 = euclidean_distance_FAST(coords[path[j]], curr);
            double dist2 = euclidean_distance_FAST(curr, coords[path[j + 1]]);
            double dist3 = euclidean_distance_FAST(coords[path[j]], coords[path[j + 1]]);
            double delta = dist1 + dist2 - dist3; // Insertion cost

            if (delta < best_dist) {
                best_dist = delta;
                best_pos = j + 1;
            }
        }

        path.insert(std::next(path.begin(), best_pos), i);
        weight_tot += best_dist;
    }

    // Close the loop by returning to the start
    weight_tot += euclidean_distance_FAST(coords[path.back()], coords[path[0]]);
    path.push_back(path[0]);

    return path;
}

class OptTSP {
public:
    OptTSP(const std::vector<Point>& pts) : points(pts), V(pts.size()) {
        best_cost = std::numeric_limits<double>::max();
        path.resize(V);
        for (size_t i = 0; i < V; ++i) path[i] = i; // Initial path: [0, 1, 2, ..., V-1]
        compute_dist_matrix(); // Fill distance matrix
        estimate_upper_bound(); // Initial guess (linear path)
    }

    void solve() {
        branch_and_bound(1); // Start branch and bound from second position (first fixed at 0)
    }

    double get_cost() const { return best_cost; }
    const std::vector<size_t>& get_path() const { return best_path; }

private:
    std::vector<Point> points;
    std::vector<size_t> path, best_path;
    std::vector<std::vector<double>> dist_matrix;
    double best_cost;
    size_t V;

    // Precompute pairwise distances
    void compute_dist_matrix() {
        dist_matrix.resize(V, std::vector<double>(V));
        for (size_t i = 0; i < V; ++i)
            for (size_t j = 0; j < V; ++j)
                dist_matrix[i][j] = (i == j) ? 0 : dist(points[i], points[j]);
    }

    // Use naive path as an initial upper bound (e.g., 0 → 1 → ... → n-1 → 0)
    void estimate_upper_bound() {
        best_cost = 0;
        for (size_t i = 0; i < V - 1; ++i)
            best_cost += dist_matrix[i][i + 1];
        best_cost += dist_matrix[V - 1][0];
    }

    // Prune using MST + closest connections heuristic
    bool promising(size_t perm_len) {
        double partial = 0;
        for (size_t i = 0; i < perm_len - 1; ++i)
            partial += dist_matrix[path[i]][path[i + 1]];

        double mst_weight = mst(perm_len); // MST of unvisited
        double connect_start = min_edge(path[0], perm_len); // From start to unvisited
        double connect_end = min_edge(path[perm_len - 1], perm_len); // From current end to unvisited

        return partial + mst_weight + connect_start + connect_end < best_cost;
    }

    // Find shortest edge from a fixed node to unvisited nodes
    double min_edge(size_t from, size_t start) {
        double min_w = std::numeric_limits<double>::max();
        for (size_t i = start; i < V; ++i)
            min_w = std::min(min_w, dist_matrix[from][path[i]]);
        return min_w;
    }

    // Compute MST weight for unvisited nodes using Prim's algorithm
    double mst(size_t start) {
        std::vector<double> key(V, std::numeric_limits<double>::max());
        std::vector<bool> in_mst(V, false);
        key[path[start]] = 0;
        double total = 0;

        for (size_t count = start; count < V; ++count) {
            double min_k = std::numeric_limits<double>::max();
            size_t u = -1;

            // Select the minimum weight unvisited vertex
            for (size_t i = start; i < V; ++i) {
                if (!in_mst[path[i]] && key[path[i]] < min_k) {
                    min_k = key[path[i]];
                    u = path[i];
                }
            }

            in_mst[u] = true;
            total += key[u];

            // Update keys
            for (size_t i = start; i < V; ++i) {
                size_t v = path[i];
                if (!in_mst[v] && dist_matrix[u][v] < key[v])
                    key[v] = dist_matrix[u][v];
            }
        }
        return total;
    }

    // Recursive backtracking with branch-and-bound pruning
    void branch_and_bound(size_t perm_len) {
        if (perm_len == V) {
            // Full path constructed; compute tour cost
            double total = 0;
            for (size_t i = 0; i < V - 1; ++i)
                total += dist_matrix[path[i]][path[i + 1]];
            total += dist_matrix[path[V - 1]][path[0]]; // Return to start

            // Update best solution if better
            if (total < best_cost) {
                best_cost = total;
                best_path = path;
            }
            return;
        }

        if (!promising(perm_len)) return; // Prune

        // Generate permutations recursively
        for (size_t i = perm_len; i < V; ++i) {
            std::swap(path[perm_len], path[i]);
            branch_and_bound(perm_len + 1);
            std::swap(path[perm_len], path[i]); // Backtrack
        }
    }
};