Clean up implementation

This is an up-to-date version of #8. This is more or less a rewrite with the logic chunked up into individual functions (that could eventually be tested independently). The second commit contains a fix for #7 that I've been using for the past days.