#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <climits>
using namespace std;

struct Node {
    int x, y;
    int g, h, f;
    Node* parent;
    
    Node(int x, int y, int g = 0, int h = 0, Node* parent = nullptr) 
        : x(x), y(y), g(g), h(h), f(g + h), parent(parent) {}
    
    bool operator>(const Node& other) const {
        return f > other.f;
    }
};

class AStar {
private:
    vector<vector<int>> grid;
    int rows, cols;
    
    int heuristic(int x1, int y1, int x2, int y2) {
        return abs(x1 - x2) + abs(y1 - y2); // Manhattan distance
    }
    
    bool isValid(int x, int y) {
        return x >= 0 && x < rows && y >= 0 && y < cols && grid[x][y] == 0;
    }
    
    vector<pair<int, int>> getNeighbors(int x, int y) {
        vector<pair<int, int>> neighbors;
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};
        
        for (int i = 0; i < 4; i++) {
            int nx = x + dx[i];
            int ny = y + dy[i];
            if (isValid(nx, ny)) {
                neighbors.push_back({nx, ny});
            }
        }
        return neighbors;
    }
    
public:
    AStar(vector<vector<int>>& grid) : grid(grid) {
        rows = grid.size();
        cols = grid[0].size();
    }
    
    vector<pair<int, int>> findPath(int startX, int startY, int endX, int endY) {
        priority_queue<Node, vector<Node>, greater<Node>> openSet;
        vector<vector<bool>> inOpenSet(rows, vector<bool>(cols, false));
        vector<vector<bool>> inClosedSet(rows, vector<bool>(cols, false));
        vector<vector<Node*>> nodeMap(rows, vector<Node*>(cols, nullptr));
        
        Node* startNode = new Node(startX, startY, 0, 
                                  heuristic(startX, startY, endX, endY));
        openSet.push(*startNode);
        nodeMap[startX][startY] = startNode;
        inOpenSet[startX][startY] = true;
        
        while (!openSet.empty()) {
            Node current = openSet.top();
            openSet.pop();
            
            if (current.x == endX && current.y == endY) {
                // Reconstruct path
                vector<pair<int, int>> path;
                Node* node = nodeMap[current.x][current.y];
                while (node != nullptr) {
                    path.push_back({node->x, node->y});
                    node = node->parent;
                }
                reverse(path.begin(), path.end());
                return path;
            }
            
            inOpenSet[current.x][current.y] = false;
            inClosedSet[current.x][current.y] = true;
            
            for (auto& neighbor : getNeighbors(current.x, current.y)) {
                int nx = neighbor.first, ny = neighbor.second;
                
                if (inClosedSet[nx][ny]) continue;
                
                int tentativeG = current.g + 1;
                
                if (!inOpenSet[nx][ny]) {
                    Node* newNode = new Node(nx, ny, tentativeG,
                                           heuristic(nx, ny, endX, endY),
                                           nodeMap[current.x][current.y]);
                    openSet.push(*newNode);
                    nodeMap[nx][ny] = newNode;
                    inOpenSet[nx][ny] = true;
                } else if (tentativeG < nodeMap[nx][ny]->g) {
                    nodeMap[nx][ny]->g = tentativeG;
                    nodeMap[nx][ny]->f = tentativeG + nodeMap[nx][ny]->h;
                    nodeMap[nx][ny]->parent = nodeMap[current.x][current.y];
                }
            }
        }
        
        return {}; // No path found
    }
};

int main() {
    vector<vector<int>> grid = {
        {0, 0, 0, 1, 0},
        {0, 1, 0, 1, 0},
        {0, 1, 0, 0, 0},
        {0, 0, 0, 1, 0},
        {0, 0, 0, 0, 0}
    };
    
    AStar astar(grid);
    auto path = astar.findPath(0, 0, 4, 4);
    
    cout << "Grid (0 = free, 1 = obstacle):" << endl;
    for (auto& row : grid) {
        for (int cell : row) {
            cout << cell << " ";
        }
        cout << endl;
    }
    
    cout << "\nPath from (0,0) to (4,4):" << endl;
    for (auto& point : path) {
        cout << "(" << point.first << "," << point.second << ") ";
    }
    cout << endl;
    
    return 0;
}