[백준 5213] 과외맨 (BFS) (C/C++) (★★)

#include <stdio.h>
#include <iostream>
#include <queue>
#include <algorithm>
#include <vector>
#include <string.h>
using namespace std;
 
int N;
pair<int, int> Map[510][1020]; // 타일숫자, 타일번호 
int visited[510][1020];
int path[510*1020];
int tileNum;
int last; 
vector<int> trace;
 
int dx[4] = {-1, 1, 0, 0}; // 상하좌우 
int dy[4] = {0, 0, -1, 1}; // 상하좌우 
 
void BFS()
{
    queue<pair<int, int>> q;
    q.push({1, 1});
    q.push({1, 2});
    visited[1][1] = 1;
    visited[1][2] = 1;
    
    path[1] = 0; // 0번(초기화용 타일번호) -> 1번(시작 타일번호) 
    
    while(!q.empty())
    {
        int x = q.front().first;
        int y = q.front().second;
        q.pop();
        
 
        if(Map[x][y].second == tileNum)
        {
//            cout << visited[x][y] << endl;
            last = Map[x][y].second;
            return;
        }
        else
        {
            last = max(last, Map[x][y].second);
        }
        
        for(int i = 0; i < 4; i++)
        {
            int xpos = x+dx[i];
            int ypos = y+dy[i];
            
            if(visited[xpos][ypos] != 0 || Map[xpos][ypos].first == 0 || Map[xpos][ypos].second == 0)
            {
                continue;
            }
            
            // (타일 번호가 다르고) 타일의 숫자가 같은 경우
            if(Map[x][y].second != Map[xpos][ypos].second && Map[x][y].first == Map[xpos][ypos].first)
            {
                q.push({xpos, ypos});
                visited[xpos][ypos] = visited[x][y]+1;
                path[Map[xpos][ypos].second] = Map[x][y].second;
                
                // 타일 번호가 같은 타일 숫자 왼쪽, 오른쪽 확인 후 큐에 삽입 
                if(Map[xpos][ypos-1].second == Map[xpos][ypos].second)
                {
                    q.push({xpos, ypos-1});    
                    visited[xpos][ypos-1] = visited[xpos][ypos];
                }
                else if(Map[xpos][ypos].second == Map[xpos][ypos+1].second)
                {
                    q.push({xpos, ypos+1});    
                    visited[xpos][ypos+1] = visited[xpos][ypos];
                }
            }
        }
    }
}
 
int main(void)
{
//    freopen("B5213_input.txt", "r", stdin);
    
    cin >> N;
    
    int row = N;
    
    for(int k = 1; k <= row; k++)
    {
        // 홀수번째 row 
        if(k % 2 == 1)
        {
            for(int i = 1; i <= 2*N; i++)
            {
                if(i % 2 == 1)
                {
                    tileNum++;
                }
                
                int a;
                cin >> a;
                
                Map[k][i].first = a;
                Map[k][i].second = tileNum;
            }        
        }
        // 짝수번째 row
        else
        {
            for(int i = 2; i <= 2*N-1; i++)
            {
                if(i % 2 == 0)
                {
                    tileNum++;
                }
                
                int a;
                cin >> a;
                
                Map[k][i].first = a;
                Map[k][i].second = tileNum;
            }
        }
    }
 
    BFS();
    
    // 경로 구하는 과정 
    int idx = last;
    while(path[idx] != 0)
    {
        trace.push_back(idx);
        idx = path[idx];
    }
    trace.push_back(1);
    
    cout << trace.size() << endl;
    for(int i = trace.size()-1; i >= 0; i--)
    {
        cout << trace[i] << " ";
    }
        
    return 0;
}