Fix #4 and a bit of code clean up