Fix for #5
Some code cleanup