Изменил алгоритм на верный, всё считает правильно, класс маин сделал похожим на ООП, но, по хорошему, надо оттуда всё выносить в отдельный класс Solver.