Fixing waypoints ArrayList