Initial, non working commit

This is the start of my algorithm. The code currently doesn’t work.