Add AStar pathfinding and slight refactor