Algorytm Dijkstry

Witam mam z implementacją algorytmu Dijkstry w Visual Studio 2010, prawie ten sam kod działa w Dev ++, wiem że był juz podobny problem poruszany na forum ale rozwiązanie które pomogło w tamtym przypadku w moim nie zadziałało oto mój kod:

#include "StdAfx.h"
#include "Dijkstra.h"
#include <iostream>
#include "Sieć.h"
#include "stdafx.h"
#include "afxdialogex.h"

using namespace std;
	
const int INF = 2147483647;					 //duża wartość (w tym wypadku max dla typu int)

		struct TNode								 //struktura "węzeł" można zrobić taką klasę
			{
				 int node;							 // numer wierzchołka 
				 int weight;						 // waga danej krawędzi 
				 struct TNode * next;				 // następny element listy (wskaźnik na typ TNode)
			};

void Dijkstra::oblicz(Sieć now)
	{
	
		cout<<"podaj ile wezlow: "<<endl;
		
		
		int i,j,u,n,m,x,y,z,v0;						 //zmienne pomocnicze m.in. nr. pierwszego wierzchołka
		int *d = new int[now.ilosc_wezlow+1], *p = new int[now.ilosc_wezlow+1],//tworzenie dynamicznych tablic
			*kopiec = new int[now.ilosc_wezlow+1], *kc = new int[now.ilosc_wezlow+1];
		bool *QS= new bool[now.ilosc_wezlow+1];				 //tablica zawierające informację czy dany wierzchołek jest już sprawdzony
  													 //jeśli przyjmuje wartość TRUE to należy do zbioru już policzonego  "S"
  													 //jeśli false, to należy do zbioru nie policzonego "Q"
  					
		struct TNode **L= new struct TNode* [now.ilosc_wezlow],*pw;	 //tworzenie tablicy zawierającej N+1 wskaźników mogących wskazywać na elementy
  													 //typu TNode gdzie jest to typ strukturalny
  												     //L to lista sąsiedztwa
  								
// Inicjujemy struktury danych

		 for(i = 1; i <= now.ilosc_wezlow; i++)
			 {
				  d[i]  = INF;						 // koszty dojścia
				  p[i]  = 0;					     // poprzednik na ścieżce
				  QS[i] = false;					 // przynależność do Q (false) lub S (true)
				  L[i]  = NULL;					     // lista sąsiedztwa
				  kc[i] = kopiec[i] = i;			 // kopiec
			 }
		 n = 0;

// Odczytujemy dane wejściowe
		
		cout<<"podaj od ktorego zaczac: "<<endl;
		 v0 = now.od_ktorego;	
		 								 			 // odczytujemy numer wierzchołka startowego
		 m = now.ilosc_lukow;						 // odczytujemy ilość krawędzi
		 for(i = 1; i <= m; i++)
			{
				x = now.polaczenie_w_l[i].odczyt_w1();
				y = now.polaczenie_w_l[i].odczyt_w2();
				z = now.metryka_luku[i];			 // odczytujemy krawędź, tzn które wierzchołki są
				if(x > n)						     // połączone oraz jaką wagę ma ta krawędź
					n = x;
				if(y > n) 	
					n = y;
					
			    pw = new TNode;						 //utworzenie nowej struktury
		        pw->next = L[x];					 //przypisanie do aktualnie przetwaranego wierzchołka wartości tablicy L[od poprzedniego wierzchołka]
			    pw->node = y;						 //przypisanie do aktualnie przetwarzanego wierzchołka,wierzchołek podany jako kolejny
				pw->weight = z;						 //przypisanie wagi
				L[x] = pw;							 //element tablicy L[x] ma teraz wartość pw
			 }
		 cout << endl;

		 int hlen  = n;								 // ilość wierzchołków w kopcu
  
		 d[v0] = 0;									 // koszt dojścia dla v0 jest zerowy

// Odtwarzamy własność kopca

		 x = kopiec[1]; 
		 kopiec[1] = kopiec[v0];
		 kopiec[v0] = x;
		 kc[v0] = 1;
		 kc[1] = v0;
    
		 for(i = 1; i <= n; i++)
		  {
		      u = kopiec[1];
// Usuwamy łuk i odtwarzamy własność kopca
			  int parent;
    
			  kopiec[1] = kopiec[hlen]; hlen--;
			  kc[kopiec[1]] = parent = 1;
			  while(true)
			     {
				    int left = parent + parent;
				    int right = left + 1;
      
				    if(left > hlen) break;

					int dmin = d[kopiec[left]];
					int pmin = left;

				    if((right <= hlen) && (dmin > d[kopiec[right]]))
						 {
							  dmin = d[kopiec[right]];
							  pmin = right;
						 }
					 if (d[kopiec[parent]] <= dmin) break;
					 x = kopiec[parent]; 
					 kopiec[parent] = kopiec[pmin]; kopiec[pmin] = x;
					 kc[kopiec[parent]] = parent; 
					 kc[kopiec[pmin]] = pmin;
				     parent = pmin;
				    // h +=3;
				  }
    
// znaleziony wierzchołek przenosimy do S

			 QS[u] = true;
			
// Modyfikujemy odpowiednio wszystkich sąsiadów z Q
    
			 pw = L[u];
			 while(pw)
			    {
				    if(!QS[pw->node] && (d[pw->node] > d[u] + pw->weight))
						  {
							    d[pw->node] = d[u] + pw->weight;
								p[pw->node] = u;
								//h += 3;
// Po zmianie d[v] odtwarzamy własność kopca

								 int child = kc[pw->node];
								 while(child > 1)
									   {
											  parent = child / 2;
											  if(d[kopiec[parent]] <= d[kopiec[child]]) break;

											  x = kopiec[parent]; 
											  kopiec[parent] = kopiec[child];
											  kopiec[child] = x;
											  kc[kopiec[parent]] = parent;
											  kc[kopiec[child]] = child;
											  child = parent;
								//	h += 3;
									   }
						  }
						//  h += 3;
					pw = pw->next;
			   }
	}

// przenoszenie wyników do pliku

  int *stos= new int [now.ilosc_wezlow+1],ws;
  
  for(i = 1; i <= n; i++)
	  {
		 cout << i << ": ";
		 ws = 0; 
		 j = i;

		 while(j)									// Wierzchołki z końca ścieżki umieszczamy na stosie
			  {
				    stos[ws++] = j;
					j = p[j];
			  }

		 while(ws) cout << stos[--ws] << " ";	    // Wypisujemy kolejne wierzchołki ze szczytu stosu
		 cout << "#" << d[i] << endl;				// Wypisujemy koszt dojścia
	 }        

    char s[1];
  cin.getline(s,1);
  cin.getline(s,1);
 }

Podziel ten kod na funkcję, które jesteś w stanie przetestować i tym samym sprawdzić, która nie działa. Gdyby ktoś miał mi zapłacić za poprawienie tego kodu, bo "nie działa", to właśnie od tego bym zaczął.

Edit: Co najmniej każdy z tych bloków oznaczonych komentarzem powinien być oddzielną funkcją. Ogólnie funkcja powinna robić jedną rzecz, jak możesz wydzielić w niej nietrywialny fragment, który wykonuje czynność nie do końca związaną z samą funkcją (np. "Odtwarzamy własność kopca", "Usuwamy łuk i odtwarzamy własność kopca") to powinien on być osobną funkcją, wtedy w kodzie głównej funkcji zamiast piętnastu linijek masz np. OdtwarzamyWłasnośćKopca().