Code cleanup & bug fix

This fixes issue #40