refactored Astar and pathfinding, still some work to do