fixed a few bugs with AStar and implemented findClosest (Dijkstra)