Fix bug and added unit tests

Fixing issue #13