Implement shortest path algorithm