Optimizations, bug fixes, correct benchmark code