Pascal/Turbo Pascal - Algoritmo Dijkstra

Hola:
He intentado implementar el algoritmo de Dijkstra, pero no funciona correctamente. ¿Podría ayudarme alguien, por favor?

program Project1;

{$APPTYPE CONSOLE}

uses
SysUtils;

type
matrix=array[1..5,1..5]of integer;
tedge=record
a,b:integer;
c:integer;
end;
matrix1=array[1..5]of integer;

var
edges:array[1..5] of tedge;
weight:matrix; {matrix for the weight of each edge}
weightmax,first,last:integer; {infinity}
distance:matrix1; {distances between the first a node and each vertex}
node:matrix1; {previous vertex list}

procedure inicialize (var paredges:array of tedge;var parweight:matrix;var parfirst,parlast:integer);
var
I,J:integer;

begin
paredges[1].a:=1;
paredges[1].b:=2;
paredges[1].c:=3;
paredges[2].a:=2;
paredges[2].b:=3;
paredges[2].c:=5;
paredges[3].a:=3;
paredges[3].b:=4;
paredges[3].c:=6;
paredges[4].a:=1;
paredges[4].b:=5;
paredges[4].c:=5;
paredges[5].a:=5;
paredges[5].b:=4;
paredges[5].c:=4;

weightmax:=1;

for I:=1 to 5 do weightmax:=weightmax + paredges[I].c;

for I:=1 to 5 do
for J:=1 to 5 do
parweight[I,J]:=weightmax;

parweight[1,2]:=3;
parweight[2,1]:=3;
parweight[2,3]:=5;
parweight[3,2]:=5;
parweight[3,4]:=6;
parweight[4,3]:=6;
parweight[4,5]:=4;
parweight[5,4]:=4;
parweight[1,5]:=5;
parweight[5,1]:=5;

writeln;
write('First node: ');
readln(parfirst);
write('Last node: ');
readln(parlast);
end;

procedure calculate(parweight:matrix; var pardistance, parnode:array of integer;weightmax,parfirst,parlast:integer);
var
I,K,S,J,dist,Dmin,Kmin:integer;
nodebis:array[1..5]of integer;

begin
for I:=1 to 5 do begin
parnode[I]:=0;
pardistance[I]:=weightmax;
end;

{for now, only the first node is in the list}
parnode[parfirst]:=parfirst;
pardistance[parfirst]:=0;
S:=1;
nodebis[1]:=parfirst;

repeat
{new i: minimun distance}
Kmin:=S;
I:=nodebis[S];
Dmin:=pardistance[I];

for K:=1 to S-1 do begin
J:=nodebis[K];
Dist:=pardistance[J];

if (Dist<Dmin) then begin
Kmin:=K;
I:=J;
Dmin:=Dist;
end;
end;

if (I<>last) then begin
nodebis[Kmin]:=nodebis[S];
S:=S-1;
{see other nodes around i}
for J:=1 to 5 do
if (pardistance[I] + parweight[I,J]<pardistance[J])then begin
if (pardistance[J]=weightmax) then begin
S:=S+1; {add j to the list}
nodebis[S]:=J;
end;
{update}
pardistance[J]:=pardistance[I]+parweight[I,J];
parnode[J]:=I;
end;
end;
until (I=parlast) or (S=0);
end;

procedure print(parnode:array of integer;parfirst,parlast:integer);
var
I,path:integer;
pathnode:array[1..5]of integer;

begin
path:=1;
pathnode[1]:=parlast;
I:=parlast;

while(I<>parfirst) do begin {print the nodes turning them}
I:=parnode[I];
path:=path+1;
pathnode[path]:=I;
end;

writeln;
writeln('The nodes of the shortest path: ');
For I:=path downto 1 do begin
write(pathnode[I]);
end;
end;

begin

inicialize(edges,weight,first,last);
calculate(weight,distance,node,weightmax,first,last);
print(node,first,last);
readln;
end.