Algorytm 1 działający
(bałagan w nagłówkach i include'ach, do ustalenia i ogarnięcia)