Solved insertion sort in C++

Signed-off-by: Ionut Radu <ihonius@gmail.com>