Fixes #25 and added test