Finished AStar Algorithm Broken