Fix #217
Add test for #217
Code cleanup