Removed some loops and replaces them with templated iterator + lambdas. This gave some speedup.
Replaced all suitable loops for iterate().