Implement Dijkstra function from pseudocode