스타트 택시 C++

https://www.acmicpc.net/problem/19238

19238번: 스타트 택시

#include <iostream>
#include <cstring>
#include <vector>
#include <queue>

using namespace std;
const int MAX = 25;
int n, m, oil;
int map[MAX][MAX];
struct MOVE { int x, y; };
MOVE mv[4] = { {1,0}, {-1,0}, {0,1}, {0,-1} };
pair<int, int> loc;
struct INFO { int si, sj, ei, ej; };
vector<INFO> psg;
pair<int,pair<int, int>> sl;
pair<int, int> dest;
int tmp_oil;

void bfs() {
	int visited[MAX][MAX];
	memset(visited, -1, sizeof(visited));

	visited[loc.first][loc.second] = 1;
	queue<pair<int,pair<int,int>>> q;
	q.push(make_pair(0, loc));

	while (!q.empty()) {
		pair<int, int> l = q.front().second;
		int count = q.front().first;
		q.pop();

		if (map[l.first][l.second] == 2) {
			if (count < sl.first) {
				sl = make_pair(count, l);
			}
			else if (count == sl.first) {
				if (l.first < sl.second.first) {
					sl = make_pair(count, l);
				}
				else if (l.first == sl.second.first) {
					if (l.second < sl.second.second) {
						sl = make_pair(count, l);
					}
				}
			}
			continue;
		}

		for (int k = 0; k < 4; k++) {
			int movei = l.first + mv[k].x;
			int movej = l.second + mv[k].y;

			if (movei >= 0 && movei < n && movej >= 0 && movej < n) {
				if (map[movei][movej] != 1 && visited[movei][movej] == -1) {
					visited[movei][movej] = 1;
					q.push(make_pair(count + 1, make_pair(movei, movej)));
				}
			}
		}
	}
}

void FindPassenger() {
	sl = make_pair(987654321,make_pair(MAX,MAX));
	bfs();
}

void bfs2() {
	int visited[MAX][MAX];
	memset(visited, -1, sizeof(visited));

	visited[sl.second.first][sl.second.second] = 1;
	queue<pair<int, pair<int, int>>> q;
	q.push(make_pair(0, make_pair(sl.second.first,sl.second.second)));

	while (!q.empty()) {
		pair<int, int> cur = q.front().second;
		int count = q.front().first;
		q.pop();

		if (cur == dest) {
			tmp_oil = count;
			oil -= count;
			return;
		}

		for (int k = 0; k < 4; k++) {
			int movei = cur.first + mv[k].x;
			int movej = cur.second + mv[k].y;

			if (movei >= 0 && movei < n && movej >= 0 && movej < n) {
				if (map[movei][movej] != 1 && visited[movei][movej] == -1) {
					visited[movei][movej] = 1;
					q.push(make_pair(count+1, make_pair(movei, movej)));
				}
			}
		}
	}
}

void MovePassenger() {
	tmp_oil = 9876554321;
	int index;
	dest = make_pair(-1, -1);
	for (int i = 0; i < psg.size(); i++) {
		if (psg[i].si == sl.second.first && psg[i].sj == sl.second.second) {
			dest = make_pair(psg[i].ei, psg[i].ej);
			index = i;
			break;
		}
	}
	bfs2();
	if (tmp_oil != 987654321) {
		map[sl.second.first][sl.second.second] = 0;
		psg.erase(psg.begin() + index);
	}
}

int main(void) {
	cin >> n >> m >> oil;

	for (int i = 0; i < n; i++) {
		for (int j = 0; j < n; j++) {
			cin >> map[i][j];
		}
	}

	int i, j;
	cin >> i >> j;
	loc = make_pair(i-1, j-1);

	for (int i = 0; i < m; i++) {
		int si, sj, ei, ej;
		cin >> si >> sj >> ei >> ej;
		psg.push_back({ si-1,sj-1,ei-1,ej-1 });
		map[si-1][sj-1] = 2;
	}

	while (oil > 0 && psg.size()>0) {
		FindPassenger();
		if (sl.first == 987654321) {
			break;
		}

		oil -= sl.first;
		if (oil <= 0)	break;

		MovePassenger();
		if (tmp_oil == 987654321)	break;

		if (oil < 0)	break;
		oil += (tmp_oil * 2);
		loc = dest;
	}

	if (psg.size() == 0)	cout << oil << endl;
	else {
		cout << -1 << endl;
	}
}