AStar is now a working Djikstra. Will add heuristics later.