обнаружил, что алгоритм на самом деле работает не правильно.
Алгоритм не может работать неправильно ;-)
Неправильно работает ваш код, которым вы пытались реализовать этот алгоритм :-(
P.S. Неужели вы всерьёз рассчитывали, что кто-то станет разгребать такую простыню как вы приводите?
Научитесь задавать вопросы более конкретно.
