Optimized insertion sort