Fix #152. Improve code and test