This separates the Dijkstra implementation into its own generic function. Whilst doing this we can simplify the code and also save yet more memory! Still don't get correct results for part 2.
