General code cleanup

Improve queue and lock thread safety.
Removed unused variables, private methods, and code.
Replace iterators with for each constructs.
Simplify a few bits of logic.
Remove null checks where things cannot be null.
Add NPE protection in a few spots.