shortest path algorithm implemented for Grue, still a bit sloppy but functional