Fix for #77

The first commit is just some cleanup (that helped me understand the code).
The second commit is the proposed fix.