I'm trying to get BFS working to find an optimal path on a grid system.
Currently, it works until the end node is found. But no path is sent out because I'm not sure how to do this. BFS How would I record the best node to take for the path?
Function for BFS algorithm (executed on button press):
public void BFS()
    {
        rq.Enqueue(start.X);
        cq.Enqueue(start.Y);
        gridmatrix[start.X, start.Y].VISITED = true;
        while (rq.Count() > 0)
        {
            int r = rq.Dequeue();
            int c = cq.Dequeue();
            if (r == end.X && c == end.Y)
            {
                endfound = true;
                break;
            }
            explore_neighbours(r,c);
            
            nodesleftinlayer--;
            if(nodesleftinlayer == 0)
            {
                nodesleftinlayer = nodesinnextlayer;
                nodesinnextlayer = 0;
                BFSMoveCount++;
            }
            if(endfound == true)
            {
                Console.WriteLine(BFSMoveCount);
               
            }
            else
            {
                Console.WriteLine(-1);
            }
            
           
        }
    }
Function for finding all adjacent nodes of the current one:
void explore_neighbours(int r, int c)
    {
 
        for (int i = 0; i < 4; i++) {
            int rr = r + dr[i]; //direction array
            int cc = c + dc[i];
            
            if(rr < 0 || cc < 0)
            {
                continue;
            }
            if(rr >= gridmatrix.GetLength(0) || cc >= gridmatrix.GetLength(1))
            {
                continue;
            }
            if(gridmatrix[rr,cc].VISITED == true)
            {
                continue;
            }
            if(gridmatrix[rr,cc].COLOUR == 1)
            {
                continue;
            }
            rq.Enqueue(rr);
            cq.Enqueue(cc);
            gridmatrix[rr, cc].VISITED = true;
            if(gridmatrix[rr, cc] == gridmatrix[end.X, end.Y])
            {
                endfound = true;
            }
            if(gridmatrix[rr,cc].COLOUR != 4 && gridmatrix[rr,cc].COLOUR != 1) //If grid tile has been checked
            {
                gridmatrix[rr, cc].COLOUR = 6; //set colour on the grid to pink
            }
            
            nodesinnextlayer++;
        }
    }
 
     
    