Add InsertionSort
Extract duplicate code