I have added some tests to illustrate it more
and an explanation with "what you might have started with and what you will end with after this routine" as a comment into the code.
The example with p5 < p6 was not working, I updated my patch.