Use upper_bound instead of find_if

The vector is already sorted so it turns every insertion O(log n) instead
of O(n)