Add tests and benchmark