You could try to implement the Dijkstra Algorithm to find the shortest path in a set of nodes. The easiest way might be to save the distances between the nodes in a two-dimensional array: e.g. distance[3][5] = 70 for a distance of 70 between the nodes 3 and 5
