Fix for issue #4 + tests