Add benchmarks and refactor some