Mission Elevation: Fix the issue with non sequential waypoint IDs

Also some refactoring to make the code cleaner to read