Fix bugs, add unit tests, add benchmark.