Tidied up a bit

Works great, quite fast, probably faster than Haskell implementation on
my machine. Hotspot really helps after an iteration or two.