It works!

Sloppy, but it finds a shortest path!