Optimized algorithm with SortedSet