Adde a c++ dijkstra implementation