Как не путаться в своем коде.

Впихивание html в echo плохо выглядит и плохо читается. И да, комментарии излишне подробны.

Отделил html от php и причесал твои циклы. В результат получилось: https://pastebin.com/sgpmLCDT

Тут уже дело вкуса. Но лично мну табуляцией чаще пользуюсь, чо бы структуру было видно.

ЗЫ: А это чего, реклама?

для начала учись писать не <?php а просто <?
1 данная обработка уже давно настраивается на любом сервере
2 также большинство современных движков уже забили на полное написание
3 это уменьшает объем файла, а при большем объеме кода в связке с html даже очень значительно уменьшает
учись понимать по структуре а не по описанию, это поможет разбираться в чужих кодах, первая строка у тебя может начинаться с <?if... т. е. 3 строчки можно минусовать
описывать надо кратко... т. е. первое описание можно написать "категории и саб категории"
используй табуляцию чтобы видеть наглядно структуру.
echo используется в случае если в строке присутствует множество переменных, в других случаях лучше закрывать php, писать html и дальше опять открывать php. 1-2 переменные в строке html можно вывести через <?=
имея красивую структуры с табуляцией и понятные названия переменных можно читать код без комментариев, описывая в них только краткие и основные обработки например где for, там можно написать перед ним комментарий "начало категорий меню" а после него соответственно "конец..."