Dijkstra + A Star Search working