Fix and optimize code

This is a proposed fix for issues #16 and #17