module Dijkstra(input clk_50;)
reg[11:0] dist [0:36];
reg visited [0:36];
reg [6:0] length = 0;
reg [6:0] sp [36:0];
reg [1:0] p_dc [0:36];
reg [6:0] parent [0:36];
reg [5:0] V = 37;   //parameter V =37

reg [7:0] i,j,k;
reg [11:0] cost [0:36][0:36];

//shortest distance function

function [6:0]minNode;

input [6:0] a;
reg [11:0] minValue = 999; //   reg [6:0]minValue = 999;
reg [11:0] minNode1 = 0
begin
  for (k = 0; k <V ; k = k+1)
  begin
   if (dist[k] < minValue && visited[k] == 0)
   begin
        minValue = dist[k];
        minNode1 = k;
        visited[k] = 1;    //To stop back tracing
   end
  end
  minNode = minNode1;
end
endfunction

//add a cost matrix


reg [6:0] start1 = 33;
reg [6:0] end1 = 3;
reg [6:0] s6 = 22;
reg [6:0] dzn1 =7;

function dijkstra;
input [6:0] start;
input [6:0] enda;
begin
    $display("enter dijkstra");
    for ( i = 0; i < V ; i = i + 1)
    begin
        dist[i] = 999;
        parent[i] = i;
        visited[i] = 0;
        sp[i] = 0;
        p_dc[i] = -1;
    end

    dist[start] = 0
    for ( i = 0; i < V; i = i + 1)
    begin
        near = minNode(1);   /////
        visited[near] = 1;   /////

        for (adj = 0; adj < V ; adj = adj + 1)
        begin
            if (cost[near][adj] != 0 && dist [adj] > dist [near] + cost [near][adj])
            begin
                dist[adj] = dist[near] + cost[near][adj];
                parent[adj] = near;
            end
        end
    end

    length = length + 1 ;
    sp[length] = start;
    i = 0;   /////

    for ( j = 35; j > 0; j = j - 1)
    begin
        if (j<length && j>0)
        begin
             if(((cost2[sp[j]][sp[j+1]]==1) && (cost2[sp[j]][sp[j-1]] == 2)) || ((cost2[sp[j]][sp[j+1]]==2) && (cost2[sp[j]][sp[j-1]] == 1)) || ((cost2[sp[j]][sp[j+1]]==4) && (cost2[sp[j]][sp[j-1]] == 3)) || ((cost2[sp[j]][sp[j+1]]==3) &&(cost2[sp[j]][sp[j-1]] == 4)))
                    begin $display("Straight");
                        p_dc[i]=0;

                        $display("%6d",p_dc[i]);
                        i=i+1;
                    end
             if(((cost2[sp[j]][sp[j+1]]==1) && (cost2[sp[j]][sp[j-1]] == 3)) || ((cost2[sp[j]][sp[j+1]]==3) && (cost2[sp[j]][sp[j-1]] == 2)) || ((cost2[sp[j]][sp[j+1]]==2) && (cost2[sp[j]][sp[j-1]] == 4)) || ((cost2[sp[j]][sp[j+1]]==4) && (cost2[sp[j]][sp[j-1]] == 1)))
                    begin $display("Left");
                        p_dc[i]=1;

                        $display("%6d",p_dc[i]);
                        i=i+1;
                    end
                if(((cost2[sp[j]][sp[j+1]]==1) && (cost2[sp[j]][sp[j-1]] == 4)) || ((cost2[sp[j]][sp[j+1]]==4) && (cost2[sp[j]][sp[j-1]] == 2)) || ((cost2[sp[j]][sp[j+1]]==2) && (cost2[sp[j]][sp[j-1]] == 3)) || ((cost2[sp[j]][sp[j+1]]==3) && (cost2[sp[j]][sp[j-1]] == 1)))
                    begin $display("Right");
                        p_dc[i]=2;
                        $display("%6d",p_dc[i]);
                        i=i+1;
                    end
        end
    end

    dijkstra = 1


end
endfunction
reg flag_dijkstra = 0;
reg signed [6:0] count_dijkstra = -1;
if (flag_dijkstra == 0;)
begin
    flag_dijkstra = dijkstra(start1,end1)
end

if (flag_dijkstra == 1)
begin
     count_dijkstra = count_dijkstra+1;
    $display("%6d, %6d, %6d",p_dc[0], count_dijkstra, p_dc[count_dijkstra]);
    if(count_dijkstra==(length-1))
        begin
            start1 = end1;
            if(sp[0] == end1) begin
                end1=s6;
            end
            if(sp[0] == s6) begin
                end1=dzn1;
            end
            flag_dijkstra = 0;
        end

endmodule