Clean up and optimize pathfinding