Fix #2 and add tests