Fix #41 and small refactoring