Fixed the c++ insertion sort