Сырых Максим, запилил умный светофор

Кажется, даже работает
Вот тут перемудрил, имхо. Можно было сделать очередь. А вообще нужна функция-итератор