Reuse iterator handling code.

This change is probably controversial.
But I think we really should start reusing code and not implement everything again in every function.
This is my first c++ lambda function I ever wrote but I still think this may be a good use case.
What do you think? Better name for the function is definitely needed.