Added test code and tweaked sort algorithm