Index: uspace/lib/cpp/include/impl/vector.hpp
===================================================================
--- uspace/lib/cpp/include/impl/vector.hpp	(revision f041811a48cd504e9b94b83be3db2419a075fe93)
+++ uspace/lib/cpp/include/impl/vector.hpp	(revision 56521a2271bc8e132ff12f978afc62426fb5efb4)
@@ -392,82 +392,45 @@
             iterator emplace(const_iterator position, Args&&... args)
             {
-                auto idx = shift_right_(position, 1);
-                allocator_.construct(data_ + idx, std::forward<Args>(args)...);
-
-                return begin() + idx;
+                auto pos = const_cast<iterator>(position);
+
+                shift_(pos, 1);
+                allocator_.construct(pos, std::forward<Args>(args)...);
+
+                return pos;
             }
 
             iterator insert(const_iterator position, const value_type& x)
             {
-                /**
-                 * Note: The reason that all insert functions are done in this weird
-                 *       way with an auxiliary vector instead of just shifting
-                 *       the elements is that we need to provide strong exception
-                 *       guarantee - in the case of exception our vector needs to
-                 *       stay in its pre-instert state and this swap method guarantees
-                 *       that.
-                 * TODO: Avoid reallocation if it still fits!
-                 */
-                size_type idx = static_cast<size_type>(position - cbegin());
-
-                vector tmp{};
-                tmp.resize_without_copy_(max(size_ + 1, capacity_));
-
-                // Copy before insertion index.
-                tmp.copy_(0, begin(), begin() + idx);
-
-                // Insertion.
-                tmp.data_[idx] = x;
-
-                // Copy after insertion index.
-                tmp.copy_(idx + 1, begin() + idx + 1, end());
-                tmp.size_ = size_ + 1;
-                swap(tmp);
-
-                return begin() + idx;
+                auto pos = const_cast<iterator>(position);
+
+                shift_(pos, 1);
+                *pos = x;
+
+                ++size_;
+                return pos;
             }
 
             iterator insert(const_iterator position, value_type&& x)
             {
-                size_type idx = static_cast<size_type>(position - cbegin());
-
-                vector tmp{};
-                tmp.resize_without_copy_(max(size_ + 1, capacity_));
-
-                // Copy before insertion index.
-                tmp.copy_(0, begin(), begin() + idx);
-
-                // Insertion.
-                tmp.data_[idx] = std::forward<value_type>(x);
-
-                // Copy after insertion index.
-                tmp.copy_(idx + 1, begin() + idx + 1, end());
-                tmp.size_ = size_ + 1;
-                swap(tmp);
-
-                return begin() + idx;
+                auto pos = const_cast<iterator>(position);
+
+                shift_(pos, 1);
+                *pos = forward<value_type>(x);
+
+                ++size_;
+                return pos;
             }
 
             iterator insert(const_iterator position, size_type count, const value_type& x)
             {
-                size_type idx = static_cast<size_type>(position - cbegin());
-
-                vector tmp{};
-                tmp.resize_without_copy_(max(size_ + 1, capacity_));
-
-                // Copy before insertion index.
-                tmp.copy_(0, begin(), begin() + idx);
-
-                // Insertion.
-                auto tmp_idx = idx;
-                for (size_type i = 0; i < count; ++i, ++tmp_idx)
-                    tmp.data_[tmp_idx] = x;
-
-                // Copy after insertion index.
-                tmp.copy_(tmp_idx, begin() + idx, end());
-                tmp.size_ = size_ + count;
-                swap(tmp);
-
-                return begin() + idx;
+                auto pos = const_cast<iterator>(position);
+
+                shift_(pos, count);
+                auto copy_target = pos;
+                for (size_type i = 0; i < count; ++i)
+                    *copy_target++ = x;
+
+                size_ += count;
+                return pos;
             }
 
@@ -476,16 +439,23 @@
                             InputIterator last)
             {
-                size_type idx = static_cast<size_type>(position - cbegin());
-                size_type count = static_cast<size_type>(last - first);
-
-                return insert_(idx, count, first, last);
+                auto pos = const_cast<iterator>(position);
+                auto count = static_cast<size_type>(last - first);
+
+                shift_(pos, count);
+                std::copy(first, last, pos);
+
+                size_ += count;
+                return pos;
             }
 
             iterator insert(const_iterator position, initializer_list<T> init)
             {
-                size_type idx = static_cast<size_type>(position - cbegin());
-                size_type count = init.size();
-
-                return insert_(idx, count, init.begin(), init.end());
+                auto pos = const_cast<iterator>(position);
+
+                shift_(pos, init.size());
+                std::copy(init.begin(), init.end(), pos);
+
+                size_ += init.size();
+                return pos;
             }
 
@@ -580,29 +550,27 @@
             }
 
-            template<class Iterator>
-            void copy_(size_type idx, Iterator first, Iterator last)
-            {
-                for (size_type i = idx; first != last; ++i, ++first)
-                    data_[i] = *first;
-            }
-
-            template<class Iterator>
-            iterator insert_(size_type idx, size_type count, Iterator first, Iterator last)
-            {
-                vector tmp{};
-                tmp.resize_without_copy_(max(size_ + count, capacity_));
-
-                // Copy before insertion index.
-                tmp.copy_(0, begin(), begin() + idx);
-
-                // Insertion.
-                tmp.copy_(idx, first, last);
-
-                // Copy after insertion index.
-                tmp.copy_(idx + count, begin() + idx, end());
-                tmp.size_ = size_ + count;
-                swap(tmp);
-
-                return begin() + idx;
+            void shift_(iterator position, size_type count)
+            {
+                if (size_ + count < capacity_)
+                    std::copy_backwards(pos, end(), end() + count);
+                else
+                {
+                    auto start_idx = static_cast<size_type>(position - begin());
+                    auto end_idx = start_idx + count;
+                    auto new_size = size_ + count;
+
+                    // Auxiliary vector for easier swap.
+                    vector tmp{};
+                    tmp.resize_without_copy_(max(new_size, capacity_));
+                    tmp.size_ = new_size;
+
+                    // Copy before insertion index.
+                    std::copy(tmp.begin(), tmp.begin() + start_idx, begin());
+
+                    // Copy after insertion index.
+                    std::copy(tmp.begin() + end_idx, tmp.end(), begin() + start_idx);
+
+                    swap(tmp);
+                }
             }
     };