Как найду свободное время напишу, выложу. Там ведь на русском тоже можно выкладывать?

Кстати, забыл ещё сказать, что в результате распутывания узла вполне может случится так, что в новом пути появятся узлы более низкого порядка чем был он. Так что надо откатываться назад и проверять/распутывать узлы более низких порядков заново.