Improved/simplified the getBestWaypoint() method