Dijkstrov algoritem-Iskanje poti iz enega vozlišča C++

Pozdravljeni!

Prosil bi za pomoč pri implementaciji algoritma iskanje v širino "Dijkstrov algoritem". S pomočjo podanega psevdokoda sem napisal nekaj, žal pa si s psevdokodom ne morem pomagat, da bi nalogo rešil do konca.
Spodaj je koda, ki sem jo do zdaj napisal. Pod kodo pa je še testni graf s katerim se testira algoritem in psevdokod.
Prosil bi, če mi lahko kdo tole pomaga rešit. Hvala v naprej.

#include <iostream>
#include <fstream>
#include <queue>
#include <vector>
using namespace std;

int velikost;
int C[30][30];
int **matrika;

void beri(string f)
{
  fstream dat(f.c_str(), fstream::in);
  dat >> velikost;
  int i=0;
  int p, q, c;

  while(!dat.eof())
  {
    dat >> p >> q >> c;
	C[p-1][q-1] = c;
	C[q-1][p-1] = c;
    i++;
  }
  dat.close();
}

struct Vozlisce
{
	int ime;
    int predhodnik;
    int dolzina;
};

void Sortiranje(vector<Vozlisce>Q){	
	for(int i=0;i<Q.size()-1;i++){
		for(int j=0;j<Q.size()-1;j++){
			if(Q[j].dolzina > Q[j+1].dolzina){
				Vozlisce temp = Q[j];
				Q[j] = Q[j+1];
				Q[j+1] = temp;
			}
		}
	}
}

void ISKANJE_V_SIRINO(Vozlisce *G, int start){
	vector<Vozlisce> Q(velikost);
	
	for(int i=0;i<velikost;i++){
		G[i].dolzina=9999;
		G[i].predhodnik=-1;
	}
	G[start].dolzina=0;
	
	for(int i=0;i<velikost;i++){ //VSTAVI_VSA_VOZLISCA(Q, G.V)
		Q.push_back(i);
	}
	//sortiranje vrste Q zato, da imamo v njej shranjene vedno povezave od najmanjse proti vecji.
	Sortiranje(Q);

	while (!Q.empty()){
		int MinElement = 0;
		double MinVrednost = G[Q.at(0)].dolzina;
		Vozlisce u = Q.front(); //IZLOCI_NAJMANJSEGA(Q)

		for(int i=0;i<velikost;i++){
			if(C[u.ime][i]>0){ //je sosed
				MinElement =i;
				MinVrednost= C[u.ime][i];
			}
		}
		int u=Q.at(MinElement);
		Q.erase(Q.begin() + MinElement);

			double Razdalja= C[u.ime][i];

			if(Razdalja<G[start].dolzina && Razdalja >0){
				for(int i=0;i<velikost;i++){
					//poiščemo soseda in njegovo dolzino in če je manjša od najdene jo zamenjamo
					//sprememba se mora naredit v vrsti Q kot v Vozliscih

				}
				
				if(G[v].dolzina > G[u].dolzina + C[i][u]){
					G[v].dolzina= G[u].dolzina + C[i][u];
					G[v].predhodnik=u;

					}
				}
			}
		}         ///nisem preprican, sortiranje vrste Q naj bi se tukaj se enkrat klicalo
	}	
}

void IZPIS_POTI(Vozlisce *G, int start, int konec)
{
	if(konec==start)
    {
		cout<<"Dolzina poti do vozlisca Start: "<<start<<" je "<<G[start].dolzina<<endl<<endl;
	}
	else if (G[konec].predhodnik==-1)
    {
		cout<<"Med Vozliscem "<<start<<" in vozliscem"<<konec<<" Ni poti! "<<endl<<endl;
    }
    else{
			IZPIS_POTI(G, start, G[konec].predhodnik);
			cout<<"Dolzina: "<<G[konec].dolzina<<endl;
		}
    }


int main()
{
	//system( "color 90" );
    int izbira;
    Vozlisce *G;
    int start;
    int konec;
		
    do
    {
        cout<<"  Dijkstrov algoritem - izbira: "<<endl<<endl;
        cout<<"1 Nalozi graf "<<endl;
        cout<<"2 Zagon algoritma "<<endl;
        cout<<"3 Izpis najkrajse poti "<<endl;
        cout<<"4 Konec "<<endl;
        cout<<endl;

        cout<<"Vasa izbira: ";
        cin>>izbira;

        switch (izbira)
        {
            case 1:
            {
                beri("graf.txt");
		
                G = new Vozlisce[velikost];
                matrika = new int*[velikost];

                for(int x=0; x<velikost; x++)
                {
                    matrika[x] = new int[velikost];
                }
				cout<<"Branje Grafa je Koncano"<<endl<<endl;

            } ; break;
            case 2:
            {
				cout<<"Vnesite vozlisce Start: ";
				cin>>start;
				if(start<0){
					cout<<"Ponovno izberite opcijo 2 iz menija in vnesite primerne parametre "<<endl<<endl<<endl;
				}
				else{
					ISKANJE_V_SIRINO(G, start);
				}
            } break;
            case 3:
            {
				cout<<"Vnesi vozlisce Konec "<<endl;
				cin>>konec;
				if(konec<0 && konec>=velikost-1){
					cout<<"Ponovno izberite opcijo 3 iz menija in vnesite primerne parametre "<<endl<<endl<<endl;
				}
				else
				IZPIS_POTI(G, start, konec);
            } break;
            default:
            {
                delete [] G;
                for(int x=0; x<velikost; x++)
                {
                    delete [] matrika[x];
                }
                delete [] matrika;
            } break;
        }
    }
    while(izbira!=4);

    return 0;
}

Podan imamo tudi testni graf, ki ga shranim v graf.txt in ga berem.

8
1 2 3
1 3 6
1 8 1
2 3 2
2 8 2
3 4 1
4 5 6
4 6 3
4 7 4
5 6 3
6 7 2
7 8 10

Psevdokod algoritma:

procedure DIJKSTROV_ALGORITEM(G, s)
begin
for each vozlišče v ∈ G.V do
begin
d[v] := ∞
predhodnik[v] := NIL
end
d[s] := 0
VSTAVI_VSA_VOZLIŠČA(Q, G.V)
while not(PRAZNA(Q)) do
begin
u := IZLOČI_NAJMANJŠEGA(Q)
for each vozlišče v ∈ G.Sosedi(u) do
if d[v] > d[u] + G.P[(u, v)].utež then
begin
d[v] := d[u] + G.P[(u, v)].utež
predhodnik[v] := u
end
end
end