[WIP] New code for finding paths in TPT

I rewrote the dijkstra code and added an implementation of the path finding algorithm proposed by Metzner and discussed in issue #242.