Fix for #337 with refactoring and more tests