Dijkstra and returning path works perfectly :)