Fixed heap sort, added test function for sorting algs