Fix some code duplication issues on this branch. Add minor optimizations to the new code.