Add pickNextWaypoint function and introduce twoInts struct