module LSA(
    output [7:0]m1,  //motor1                                            PIN_D3
    output [7:0]m2,  //motor2                                            PIN_C3
    input [11:0]s1,  //12-bit output of ch. 5 (parallel)
    input [11:0]s2,  //12-bit output of ch. 6 (parallel)
    input [11:0]s3,  //12-bit output of ch. 7 (parallel)
    input clk_50,    //50 MHz clock
    input reset,
	 output Led1,    //Led used to indicate position of bot i.e. node or line
	 output Led2,
	 output Led3,
	 output HL1,        // to control uart
	 output [2:0] id
    );

reg signed[7:0]error = 0;
reg signed[7:0]difference = 0;
reg signed[7:0]correction = 0;
reg signed[7:0]cumulative_error = 0;
reg signed[7:0]preverror = 0;
reg [3:0]nodecount = -4'd1;     //No. of nodes bot has traversed, initially set to -1
//reg [3:0]flag=0;
reg flag = 0;   

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

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

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


reg led1=0;
reg led2=0;
reg led3=0;

parameter V = 37;

reg [7:0] i,j;

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

// Function to find out the shortest distance
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
//


//Storing Values of the cost matrix
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;
end

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 */
cost[0][1] = 100;               //A
cost[0][2] = 15;
cost[0][8] = 15;

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

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

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

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

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] = 15;			//I
cost[8][10] = 15;
cost[8][33] = 15;

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

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

cost[11][4] = 15;		//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] = 15;
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

reg [7:0]odc =50;     //optimum duty cycle
reg [7:0]mo1 = 50;    // pwm to motor1, initially set to 50
reg [7:0]mo2 = 50;    // pwm to motor2, initially set to 50
reg [7:0]ml1 = 50;    // pwm to motor1 when it is on line
reg [7:0]ml2 = 50; 	  // pwm to motor2 when it is on line
reg [7:0]mn1 = 50;    // pwm to motor1 when it is on node
reg [7:0]mn2 = 50;    // pwm to motor2 when it is on node
reg [11:0]thr = 2000; //Condition to differentiate between white and black surface
reg [7:0]r_COUNT = 0; //counter
reg clk_1 = 0;        //1 MHz Clock

reg [2:0] id1 =0;        //to control uart
reg hl;


always @(posedge clk_50) begin   //Scaling down of 50MHz clock to 1 MHz

    if(reset == 0)
     begin
        r_COUNT <= 0;
     end

   if (r_COUNT == 49 )begin             //Check if count reaches 49
      r_COUNT <= 0;                     //Reset counter if it reaches 49
   end else begin
      r_COUNT <= r_COUNT + 1;           //increment counter by 1 till 49
   end
    clk_1 <= (r_COUNT < 25);
end
reg [6:0] start1 = 33;
reg [6:0] end1 = 3;
reg [6:0] s6 = 22;
reg [6:0] dzn1 =7;

//dijkstra function
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;
		  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
      parnode = parent[enda];
      sp[length] = enda;
      for(i=0;i<V;i=i+1)
           begin
				if(parnode != start) begin
               
                length = length + 1;
                sp[length] = parnode;
					 $display(" <- %6d ", sp[length]);
                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;
						
						$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 [6:0] count_dijkstra =-1;
always @(posedge clk_1) begin    

//Call dijkstra
	if(flag_dijkstra == 0) begin
		flag_dijkstra = dijkstra(start1, 3);
		
		end
		
// Line Following
    if(reset == 0)
     begin
        nodecount =-4'd1 ;         //Initially node count set to -1
     end
	
	 led1 <= (s1>thr);               //Led lights up when s1 has a greater value than 2000
	 led2 <= (s2>thr);	   			//Led lights up when s2 has a greater value than 2000
	 led3 <= (s3>thr);			   	//Led lights up when s3 has a greater value than 2000

    error = (s1>thr) - (s3>thr);    //Relative error between sensors s1 and s3: values range from -1 to 1  P

    cumulative_error <= cumulative_error + error;  //Adds up the error to give a cumulative error  I
	 
    if (cumulative_error > 10)      //Condition to reset the value of cumulative error to 10 if it crosses 10  
    begin
        cumulative_error <= 10;
    end

     if (cumulative_error < -10)    //Condition to reset the value of cumulative error to -10 if it crosses -10  
    begin
        cumulative_error <= -10;
    end
	 
	 if (s1<thr && s2>thr && s3<thr) //Cumulative error resets to zero when the bot is on line i.e WBW
	 begin
			cumulative_error <= 0;   
			hl=0;
	 end
	 
     difference <= error-preverror;   //forms the differential part D
     correction <= ((10*error) + cumulative_error + (2*difference)); // kp = 10, ki = 1, kd =2
	  
	  preverror <= error;  // Stores value of current error to previous error so that it can be used in next loop cycle
	  
	  
     ml1 <= odc - correction;  // PID tuning for motor 1
     ml2 <= odc + correction;  // PID tuning for motor 2

     if (ml1>70)      //Resetting value of ml1 to 70 if it crosses 70
     begin
         ml1 <= 70;
     end
     if (ml2>70)      //Resetting value of ml2 to 70 if it crosses 70
     begin
         ml2 <= 70;
     end
     if (ml1<30)       //Resetting value of ml1 to 30 if it becomes less than 30
     begin
         ml1 <= 30;
     end
     if (ml2<30)       //Resetting value of ml2 to 30 if it becomes less than 30
     begin
         ml2 <= 30;
     end  
	if (s1 < thr && s2 > thr && s3 < thr) //ready for next node, flag resets to zero on line : WBW
	begin
        flag <= 0; 
    end 
	 /*else begin
        flag <= 1;
    end*/
 
    // Only detect node once.
	if (s1>thr && s2>thr && s3>thr && flag == 0 && flag_dijkstra==1)  //detect node 
	begin		  
		  hl = 1;
		  id1 = 4;
		  flag <= 1;
		  //$display("%7d " ,i);
		  //nodecount = p_dc[i];
		  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
		  
		  
		if(p_dc[count_dijkstra] == 0)
		begin  
			//$display("straight");	
			mn1 <= odc;
			mn2 <= odc;
		end
		if(p_dc[count_dijkstra] == 1)
		begin 
			//$display("left");	
			mn1 <= 5;
			mn2 <= 60;
		end
		if(p_dc[count_dijkstra] == 2)
		begin    
			//$display("right");	
			mn1 <= 60;
			mn2 <= 5;
		end
    end 
		
			
	/*if(flag == 1)    //Detection of node and applying pwm accordingly
	begin 
	
	if(p_dc[i] == 0)
	begin  
   $display("straight");	
		mn1 <= odc;
      mn2 <= odc;
	end
	if(p_dc[i] == 1)
	begin 
	$display("left");	
		mn1 <= 5;
      mn2 <= 60;
	end
	if(p_dc[i] == 2)
	begin    
	$display("right");	
		mn1 <= 60;
      mn2 <= 5;
	end
	
	end*/
	
	if(flag == 0)    //assigning values of ml1 and ml2 (line condition) to mo1 and mo2 respectively
	begin
		mo1 <= ml1;
		mo2 <= ml2;
	end else
	begin            //assigning values of mn1 and mn2 (node condition) to mo1 and mo2 respectively
		mo1 <= mn1;
		mo2 <= mn2;
	end
end
		
assign Led1 = led1; 
assign Led2 = led2;
assign Led3 = led3;

assign id = id1;
assign HL1 = hl;
assign m1 = mo1;
assign m2 = mo2;

endmodule