Correct major bug and add unit tests