Add benchmark and unit tests