A* Arama

#define V 6
#define INF 1000000
 
/* h[v] = straight-line estimate from v to the goal */
int aStar(int graph[V][V], int h[V], int src, int goal) {
    int g[V], f[V], closed[V] = {0};
    for (int i = 0; i < V; i++) { g[i] = INF; f[i] = INF; }
    g[src] = 0;
    f[src] = h[src];
 
    for (int count = 0; count < V; count++) {
        int u = -1;
        for (int v = 0; v < V; v++)
            if (!closed[v] && (u == -1 || f[v] < f[u]))
                u = v;
        if (u == goal) return g[goal];
        closed[u] = 1;
 
        for (int v = 0; v < V; v++)
            if (graph[u][v] && g[u] + graph[u][v] < g[v]) {
                g[v] = g[u] + graph[u][v];
                f[v] = g[v] + h[v];
            }
    }
    return -1;
}