Fixes #641

Bug fixed
Wrote tests