스타트 택시 C++

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

19238번: 스타트 택시

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

using namespace std;

int map[21][21];
struct MOVE { int x,y; };
MOVE mv[4] = { {-1,0}, {0,-1}, {0,1}, {1,0} };
int n, m, oil;
pair<int, int> loc;
vector <pair<pair<int,int>,pair<int, int>>> psg;
int current;
int visited[21][21];

//dir == 0 : 손님까지, dir == 1 : 목적지까지
int bfs(int i,int dir) {

	memset(visited, -1, sizeof(visited));

	int start_x,start_y,dest_x, dest_y;
	if (dir == 0) {
		dest_x = psg[i].first.first;
		dest_y = psg[i].first.second;
	}
	else {
		dest_x = psg[i].second.first;
		dest_y = psg[i].second.second;
	}

	start_x = loc.first;
	start_y = loc.second;

	if (start_x == dest_x && start_y == dest_y)
		return 0;

	queue <pair<pair<int, int>,int>> q;
	visited[start_x][start_y] = 1;
	q.push(make_pair(make_pair(start_x, start_y),0));

	while (!q.empty()) {
		int ii = q.front().first.first;
		int jj = q.front().first.second;
		int count = q.front().second;
		q.pop();

		for (int k = 0; k < 4; k++) {
			int movei = ii + mv[k].x;
			int movej = jj + mv[k].y;
			
			if (movei == dest_x && movej == dest_y) {
				count += 1;
				return count;
			}

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

void ChoosePsg() {
	int distance=1000;

	for (int i = 0; i < psg.size(); i++) {
		int temp = bfs(i,0);

		if (temp == -1 && distance == 1000) {
			current = -1;
			continue;
		}
		else if (temp == -1 && distance != 1000)
			continue;

		if (i == 0) {
			distance = temp;
			current = i;
		}
		else {
			if (temp == distance) {
				if (psg[current].first > psg[i].first) {
					distance = temp;
					current = i;
				}
				else if (psg[current].first == psg[i].first && psg[current].second > psg[i].second) {
					distance = temp;
					current = i;
				}
			}
			else if (temp < distance) {
				distance = temp;
				current = i;
			}
		}
	}
	if (current != -1) {
		loc = psg[current].first;
		oil -= distance;
	}
}

int MoveToLoc() {
	int distance = bfs(current,1);
	if (distance == -1)
		return -1;
	oil -= distance;

	return distance;
}

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

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

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

	for (int i = 0; i < m; i++) {
		int startx, starty, endx, endy;
		cin >> startx >> starty >> endx >> endy;
		psg.push_back(make_pair(make_pair(startx, starty), make_pair(endx, endy)));
	}

	while (!psg.empty()) {
		ChoosePsg();
		if (oil <= 0  || current ==-1) {
			cout << -1 << endl;
			return 0;
		}
		int waste = MoveToLoc();
		if (waste == -1) {
			cout << -1 << endl;
			return 0;
		}
		loc = psg[current].second;
		psg.erase(psg.begin() + current);
		if (oil < 0 ) {
			cout << -1 << endl;
			return 0;
		}
		oil += waste * 2;
	}
	cout << oil;
	return 0;
}