Replaced old pathfinding code with new A* pathfinding