Insert a sorted range into another sorted range.

sorted_insert.cpp

class SortedInsert
{
public:
	
	template<typename Iterator1, typename Iterator2>
	void operator()(Iterator1 first_begin,
					Iterator1 first_end,
					Iterator2 second_begin,
					Iterator2 second_end)
	{
		while (first_begin != first_end)
		{
			auto insertion_point = _upper_bound(second_begin,
												second_end,
												*first_begin);
		
			// Handle begin = end - 1 and begin == end
			
			auto first_stop = _upper_bound(first_begin,
										   first_end,
										   *insertion_point);
			
			std::size_t distance = std::distance(first_begin, first_stop);
			
			second_end = _move(insertion_point, second_end, distance);
			
			std::copy(first_begin, first_stop, insertion_point);
			
			first_begin = first_stop;
		}
	}
	
private:
	
	template<typename Iterator, typename T>
	Iterator _upper_bound(Iterator begin, Iterator end, const T& value)
	{
		if (begin == end) return end;
		
		auto middle = begin;
		
		std::advance(middle, std::distance(begin, end)/2);
		
		if (value < *middle) return _upper_bound(begin, middle, value);
		
		else return _upper_bound(++middle, end, value);
	}
	
	template<typename Iterator>
	Iterator _move(Iterator begin, Iterator end, std::size_t distance)
	{
		auto destination = end--;
		
		std::advance(destination, distance);
		
		auto last = destination--;
		
		for (--begin; end != begin; --end, --destination)
		{
			*destination = *end;
		}
		
		return last;
	}
};

template<typename Iterator1, typename Iterator2>
void sorted_insert(Iterator1 first_begin,
				   Iterator1 first_end,
				   Iterator2 second_begin,
				   Iterator2 second_end)
{
	// Enable ADL
	using std::swap;
	
	std::size_t first = std::distance(first_begin, first_end);
	std::size_t second = std::distance(second_begin, second_end);
	
	if (second < first)
	{
		swap(first_begin, second_begin);
		swap(first_end, second_end);
	}
	
	SortedInsert insert;
	
	return insert(first_begin, first_end, second_begin, second_end);
}