Example implementation of shortest path algorithm