Вот здесь точно нужен рефакторинг, есть идеи?

А зачем вам рефакторинг? Цель какая?

Визуализация алгоритмов стандартной библиотеки C++

В контексте не константный вызывается и там, где записи не происходит.