Its .... nicer?

Added more comments and moved to algorithm to a separate function to
make it easier to read and code the other one.