Added benchmarking code and optimized a bit.