Fix bug and add unit tests