Untitled

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_set>
#include <cmath>
#include <unordered_map>

struct Node {
    int x, y;
    double g, h;
    Node* parent;

    Node(int x, int y, double g = 0.0, double h = 0.0, Node* parent = nullptr)
        : x(x), y(y), g(g), h(h), parent(parent) {}

    double f() const { return g + h; }
};

struct Compare {
    bool operator()(const Node* a, const Node* b) const {
        return a->f() > b->f();
    }
};

double heuristic(int x1, int y1, int x2, int y2) {
    return std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}

std::vector<Node*> getNeighbors(Node* node, const std::vector<std::vector<int>>& grid) {
    std::vector<Node*> neighbors;
    std::vector<std::pair<int, int>> directions = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}};
    int rows = grid.size(), cols = grid[0].size();

    for (const auto& dir : directions) {
        int newX = node->x + dir.first, newY = node->y + dir.second;
        if (newX >= 0 && newX < rows && newY >= 0 && newY < cols && grid[newX][newY] == 0) {
            neighbors.push_back(new Node(newX, newY));
        }
    }

    return neighbors;
}

std::vector<Node*> reconstructPath(Node* node) {
    std::vector<Node*> path;
    while (node) {
        path.push_back(node);
        node = node->parent;
    }
    std::reverse(path.begin(), path.end());
    return path;
}

std::vector<Node*> aStar(const std::vector<std::vector<int>>& grid, const Node& start, const Node& goal) {
    std::priority_queue<Node*, std::vector<Node*>, Compare> openList;
    std::unordered_set<int> closedSet;
    std::unordered_map<int, Node*> allNodes;

    auto hashNode = [](const Node* node) {
        return node->x * 1000 + node->y;
    };

    Node* startNode = new Node(start.x, start.y, 0.0, heuristic(start.x, start.y, goal.x, goal.y));
    openList.push(startNode);
    allNodes[hashNode(startNode)] = startNode;

    while (!openList.empty()) {
        Node* current = openList.top();
        openList.pop();

        if (current->x == goal.x && current->y == goal.y) {
            return reconstructPath(current);
        }

        closedSet.insert(hashNode(current));

        for (Node* neighbor : getNeighbors(current, grid)) {
            int neighborHash = hashNode(neighbor);

            if (closedSet.find(neighborHash) != closedSet.end()) {
                delete neighbor;
                continue;
            }

            double tentativeG = current->g + heuristic(current->x, current->y, neighbor->x, neighbor->y);

            if (allNodes.find(neighborHash) == allNodes.end() || tentativeG < neighbor->g) {
                neighbor->g = tentativeG;
                neighbor->h = heuristic(neighbor->x, neighbor->y, goal.x, goal.y);
                neighbor->parent = current;

                if (allNodes.find(neighborHash) == allNodes.end()) {
                    openList.push(neighbor);
                    allNodes[neighborHash] = neighbor;
                }
            } else {
                delete neighbor;
            }
        }
    }

    return {};
}

int main() {
    std::vector<std::vector<int>> grid = {
        {0, 1, 0, 0, 0},
        {0, 1, 0, 1, 0},
        {0, 0, 0, 1, 0},
        {0, 1, 1, 1, 0},
        {0, 0, 0, 0, 0}
    };

    Node start(0, 0);
    Node goal(4, 4);

    std::vector<Node*> path = aStar(grid, start, goal);

    if (!path.empty()) {
        std::cout << "Path found:\n";
        for (const Node* node : path) {
            std::cout << "(" << node->x << ", " << node->y << ") ";
        }
        std::cout << std::endl;
    } else {
        std::cout << "No path found." << std::endl;
    }

    return 0;
}