Fixed compiler warnings and started on dijkstra
Fixed queue init....