Algorytm najkrótszej ścieżki BFS

Z algorytmu poniżej chciałbym wyciągnąć dokładną najkrótszą drogę gdzie bedzie reprezentowany kazdy punkt przez (x,y) tak jak struktura point a nie tylko jej długość. Próbowałem zrobić kopie "q" w kazdy miejsce gdzie jest q robiłem te same operacje a później chciałem zwrócić w miejsce gdzie zwracam "curr.dist" niestety wyskoczyło sporo błędów więc wydaje mi się to złym rozwiązaniem.Za wszelką pomoc jestem wdzięczny.

struct Point
{
    int x;
    int y;
};
 
// An Data Structure for queue used in BFS
struct queueNode
{
    Point pt;  // The cordinates of a cell
    int dist;  // cell's distance of from the source
};
 
// check whether given cell (row, col) is a valid
// cell or not.
bool isValid(int row, int col)
{
    // return true if row number and column number
    // is in range
    return (row >= 0) && (row < ROW) &&
           (col >= 0) && (col < COL);
}
 
// These arrays are used to get row and column
// numbers of 4 neighbours of a given cell
int rowNum[] = {-1, 0, 0, 1};
int colNum[] = {0, -1, 1, 0};
 
// function to find the shortest path between
// a given source cell to a destination cell.
int BFS(int mat[][COL], Point src, Point dest)
{
    // check source and destination cell
    // of the matrix have value 1
    if (!mat[src.x][src.y] || !mat[dest.x][dest.y])
        return INT_MAX;
 
    bool visited[ROW][COL];
    memset(visited, false, sizeof visited);
     
    // Mark the source cell as visited
    visited[src.x][src.y] = true;
 
    // Create a queue for BFS
    queue<queueNode> q;
     
    // distance of source cell is 0
    queueNode s = {src, 0};
    q.push(s);  // Enqueue source cell
 
    // Do a BFS starting from source cell
    while (!q.empty())
    {
        queueNode curr = q.front();
        Point pt = curr.pt;
 
        // If we have reached the destination cell,
        // we are done
        if (pt.x == dest.x && pt.y == dest.y)
            return curr.dist;

        // Otherwise dequeue the front cell in the queue
        // and enqueue its adjacent cells
        q.pop();
 
        for (int i = 0; i < 4; i++)
        {
            int row = pt.x + rowNum[i];
            int col = pt.y + colNum[i];
             
            // if adjacent cell is valid, has path and
            // not visited yet, enqueue it.
            if (isValid(row, col) && mat[row][col] && 
               !visited[row][col])
            {
                // mark cell as visited and enqueue it
                visited[row][col] = true;
                queueNode Adjcell = { {row, col},
                                      curr.dist + 1 };
                q.push(Adjcell);
            }
        }
    }
 
    //return -1 if destination cannot be reached
    return INT_MAX;
}