merged and improved pathfinding code