Working alg
Nicer looking but same thing
I did it