Add algorithm and test for next_permutation