Exercise 4.9 question 3 done.

Optimised and removed a lot of un-needed code. Might still need to use Dijkstra’s Algorithm