Algorithm written and tested

It seems to work. But see notes.