Dijkstra_only

`timescale 1ns/1ns

module dijkstra(
input clk_50,
input [15:0]s3,
input [15:0]s1,
input [15:0]s2
);

reg [6:0] i=0, j, k, l, adj, near, parnode;

reg [11:0] dist [0:36];
reg [6:0] parent [0:36];
reg visited[0:36];

integer length=0;    // length of path
reg [6:0] sp[0:36];     // Store path
reg [1:0] p_dc[0:36];       //Stores The turns

reg [11:0] cost[0:37][0:37];
reg [11:0] cost2[0:37][0:37];

parameter V=37;

function automatic [6:0]minNode;
    input [6:0]a;
    reg [6:0]minValue = 999;
   reg [6: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;
                //$display("Enter min");
        end
        end
    minNode = minNode1;
     //$display("Function Called %6d", minNode1);
    end
endfunction


initial begin
    for(i=0;i<V;i=i+1)
    begin
    for(j=0;j<V;j=j+1) begin

        if(i==j) begin
                cost2[i][j] = 0;
                //cost[i][j] = 0;
          end
        else begin
                cost2[i][j] = 999;
                //cost[i][j] = 999;
          end
        end
    end
cost2[0][1] = 3;
cost2[0][2] = 2;
cost2[0][8] = 1;

cost2[1][0] = 4;

cost2[2][0] = 1;
cost2[2][3] = 2;
cost2[2][4] = 3;

cost2[3][2] = 1;
cost2[3][6] = 3;
cost2[3][17] = 2;

cost2[4][2] = 4;
cost2[4][5] = 2;
cost2[4][11] = 3;

cost2[5][4] = 1;

cost2[6][3] = 4;
cost2[6][7] = 2;
cost2[6][14] = 3;

cost2[7][6] = 1;

cost2[8][0] = 4;
cost2[8][10] = 2;
cost2[8][33] = 3;

cost2[9][10] = 3;

cost2[10][8] = 1;
cost2[10][9] = 4;
cost2[10][11] = 2;

cost2[11][4] = 4;
cost2[11][10] = 1;
cost2[11][12] = 2;
cost2[11][18] = 3;

cost2[12][11] = 1;        //M
cost2[12][13] = 4;
cost2[12][14] = 2;
cost2[12][23] = 3;

cost2[13][12] = 3;

cost2[14][6] = 4;        //O
cost2[14][12] = 1;
cost2[14][15] = 2;

cost2[15][14] = 1;
cost2[15][16] = 4;
cost2[15][17] = 2;

cost2[16][15] = 3;       //Q

cost2[17][15] = 1;
cost2[17][3] = 4;
cost2[17][26] = 3;

cost2[18][19] = 2;
cost2[18][11] = 4;
cost2[18][21] = 3;       //S

cost2[19][18] = 1;

cost2[20][21] = 2;       //U

cost2[21][20] = 1;
cost2[21][22] = 2;
cost2[21][28] = 3;
cost2[21][18] = 4;

cost2[22][21] = 1;       //W

cost2[23][24] = 2;
cost2[23][12] = 4;
cost2[23][34] = 3;

cost2[24][22] = 1;

cost2[25][26] = 2;       //Z

cost2[26][25] = 1;
cost2[26][17] = 4;
cost2[26][34] = 3;       //AA

cost2[27][28] = 2;

cost2[28][27] = 1;
cost2[28][21] = 4;
cost2[28][29] = 2;
cost2[28][31] = 3;

cost2[29][28] = 1;       //AD

cost2[30][31] = 2;

cost2[31][30] = 1;       //AF
cost2[31][28] = 4;
cost2[31][32] = 2;
cost2[31][33] = 3;

cost2[32][31] = 1;       //AG

cost2[33][31] = 4;
cost2[33][8] = 1;
cost2[33][34] = 2;

cost2[34][35] = 2;
cost2[34][23] = 4;
cost2[34][33] = 1;

cost2[35][36] = 4;
cost2[35][26] = 2;
cost2[35][34] = 1;

cost2[36][35] = 3;

for(i=0;i<V;i=i+1)
    begin
    for(j=0;j<V;j=j+1) begin

        if(i==j) begin
                cost[i][j] = 0;
          end
        else begin
                cost[i][j] = 999;
          end
        end
    end
cost[0][1] = 100;               //A
cost[0][2] = 2;
cost[0][8] = 3;

cost[1][0] = 100;               //B

cost[2][0] = 2;                 //C
cost[2][3] = 3;
cost[2][4] = 2;

cost[3][2] = 3;                 //D
cost[3][6] = 2;
cost[3][17] = 4;

cost[4][2] = 2;         //E
cost[4][5] = 100;
cost[4][11] = 2;

cost[5][4] = 100;       //F

cost[6][3] = 2;         //G
cost[6][7] = 100;
cost[6][14] = 3;

cost[7][6] = 100;       //H

cost[8][0] = 3;         //I
cost[8][10] = 2;
cost[8][33] = 4;

cost[9][10] = 100;      //J

cost[10][8] = 2;        //K
cost[10][9] = 100;
cost[10][11] = 1;

cost[11][4] = 2;        //L
cost[11][10] = 1;
cost[11][12] = 3;
cost[11][18] = 1;

cost[12][11] = 3;           //M
cost[12][13] = 100;
cost[12][14] = 1;
cost[12][23] = 2;

cost[13][12] = 100;     //N

cost[14][6] = 3;            //O
cost[14][12] = 1;
cost[14][15] = 2;

cost[15][14] = 2;       //P
cost[15][16] = 100;
cost[15][17] = 1;

cost[16][15] = 100;         //Q

cost[17][15] = 1;       //R
cost[17][3] = 4;
cost[17][26] = 2;

cost[18][19] = 100;     //S
cost[18][11] = 1;
cost[18][21] = 1;

cost[19][18] = 100;     //T

cost[20][21] = 100;         //U

cost[21][20] = 100;     //V
cost[21][22] = 100;
cost[21][28] = 1;
cost[21][18] = 1;

cost[22][21] = 100;             //W

cost[23][24] = 100;     //X
cost[23][12] = 2;
cost[23][34] = 3;

cost[24][23] = 100;     //Y

cost[25][26] = 100;             //Z

cost[26][25] = 100;     //AA
cost[26][17] = 2;
cost[26][34] = 4;

cost[27][28] = 100;     //AB

cost[28][27] = 100;     //AC
cost[28][21] = 1;
cost[28][29] = 100;
cost[28][31] = 1;

cost[29][28] = 100;         //AD

cost[30][31] = 100;     //AE

cost[31][30] = 100;             //AF
cost[31][28] = 1;
cost[31][32] = 100;
cost[31][33] = 1;

cost[32][31] = 100;             //AG

cost[33][31] = 1;       //AH
cost[33][8] = 4;
cost[33][34] = 3;

cost[34][35] = 2;       //AI
cost[34][23] = 3;
cost[34][33] = 3;

cost[35][36] = 100;     //AJ
cost[35][26] = 4;
cost[35][34] = 2;

cost[36][35] = 100;     //AK

end
function automatic 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;
    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
      parnode = parent[enda];
      sp[length] = enda;
      for(i=0;i<V;i=i+1)
           begin
                if(parnode != start) begin
               //$display(" <- %6d || %6d",parnode,parent[parnode]);
                length = length + 1;
                sp[length] = parnode;
                parnode = parent[parnode];
                    end
            end
        length= length + 1;
        sp[length] = start;
      //$display("%6d", length);
        //direction
        i=0;
        for(j=35;j>0;j=j-1) begin
            //$display("%6d", sp[j]);

            if(j<length && j>0) begin
                $display("%6d", sp[j]);
                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;
                        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;
                        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;
                        i=i+1;
                    end

            end


        end

    dijkstra = 1;
end
endfunction

reg flag=0;
reg [6:0]nodeC=0;

reg [6:0] delay=0;

reg kl=0;

always @(posedge clk_50) begin
    //if(delay == 100) begin
    if(flag == 0) begin
        flag = dijkstra(33, 7);
        i=0;
        end
     /*for (kl=0;kl<36;kl=kl+1) begin
        case(p_dc[kl])
            0: begin
            $display("Straight");
            end

            1: begin
            $display("Left");
            end

            2: begin
            $display("Right");
            end
        endcase
      end*/

      if(kl==0 && s3>2000 && s1>2000 && s2>2000)
        begin
            /*case(p_dc[i])
            0: begin
            $display("Straight");
            end

            1: begin
            $display("Left");
            end

            2: begin
            $display("Right");
            end
        endcase*/
        if(p_dc[i] == 0) $display("Straight");

        if(p_dc[i] == 1) $display("Left");

        if(p_dc[i] == 2) $display("Right");
        i=i+1;
        kl=1;
        end

     else if(kl==1 && s1<2000 && s2>2000 && s3<2000) begin
        kl=0;
     end

 //end
 //else delay = delay + 1;
end

endmodule