Хотя ваш второй шаг верен, я думаю, что проблема в том, что вы делаете слишком много операций.
Вы выполняете как BFS, так и DFS, что будет очень дорогостоящим.Вместо этого я бы порекомендовал использовать один из нескольких различных методов обхода, которые позволят минимизировать вычислительные затраты.
Это общая проблема для поиска кратчайшего пути, и одним из популярных решений является алгоритм Дейкстры.Вот статья, которая излагает эту тему.https://www.geeksforgeeks.org/dijkstras-shortest-path-algorithm-greedy-algo-7/
Короче говоря, что этот алгоритм делает, он берет начальную точку A, и затем он генерирует минимальное остовное дерево, пока точка B не будет достигнута, тогда есть единственный путь, по которому A может получитьк B, и это самый короткий путь.
И этот, и ваш алгоритм оба работают в O (nlogn), но на практике это решение можно рассматривать как запуск одной BFS вместо BFS иДФС.