Обновленный Рутисхаузер. Переписал код более кратко и понятно. Единственное, не оттестировал до конца. Если кто-то найдет тест на котором программа дает неправильный ответ -- поправлю и буду признателен этому субъекту изучения информатики.
Там makefile внутри, если у кого-то стоит Unix-like операционная система, то он может скомпилировать программу одной командой "make" в директории с makefile'ом