Basic path finding in graph

Inefficient but working I think