если у вас такой сложный алгоритм работы то для начала «plc_prg» хорошо бы было сделать на «sfc» (что-то похожее на блок схему) там всю свою программу разобьёте на куски и правильно уложите в шаги Sfc-программы это заметно упростит алгоритм. внутренности шагов тоже можно сделать Sfc языком программирования. и в финале когда упрощать таким образом не получается начинаем всю программу разбивать на функции, функциональные блоки и просто программы. затем все повторяющиеся куски программы оформляем отдельными компонентами. и только после этого садится писать саму программу и последовательность выполнения тех или иных действий.
P. S. мне аналогичный подход очень помогает :-)