Adding code to sort almost sorted array