Fix bug and add tests