Problem p03444 (C++)

Buggy submission (status: Runtime Error):
#include <bits/stdc++.h>
using namespace std;

const int N = 4005;
int n, a[N], par[N];
int pos[N];
int dep[N];
bool vis[N];
bool del[N];
set<int> G[N];
vector<int> queries;

void query(int u) {
  if (!u)
    return;
  queries.push_back(u);
  int cur = a[u];
  a[u] = a[0];
  while (true) {
    u = par[u];
    if (u == -1)
      break;
    int new_cur = a[u];
    a[u] = cur;
    cur = new_cur;
  }
}

int main() {
  ios_base::sync_with_stdio(false);
  cin.tie(0);
  cin >> n;
  par[0] = -1;
  for (int i = 1; i < n; ++i) {
    cin >> par[i], G[par[i]].insert(i), dep[i] = dep[par[i]] + 1;
  }
  for (int i = 0; i < n; ++i)
    cin >> a[i];
  for (int i = 0; i < n; ++i)
    pos[i] = -1;

  vector<vector<int>> useless;
  while (true) {
    useless.clear();
    // find useless chains
    queue<int> q;
    for (int i = 0; i < n; ++i)
      if (!del[i] && G[i].size() == 0) {
        pos[i] = useless.size();
        useless.push_back(vector<int>(1, i));
        q.push(i);
      }
    if (useless.empty())
      break;

    while (!q.empty()) {
      int u = q.front();
      q.pop();
      if (u == 0)
        break;
      int p = par[u];
      if (G[p].size() == 1) { // p has only one child u
        pos[p] = pos[u];
        useless[pos[p]].push_back(p);
        q.push(p);
      }
    }

    // for (auto &vec : useless) {
    // 	for (auto &u : vec) cerr << u << ' '; cerr << endl;
    // }

    // solve
    int nleave = 0;
    for (auto &vec : useless)
      nleave += vec.size(); // vec: from bottom to top

    int ndone = 0;
    while (ndone < nleave) {
      if (pos[a[0]] != -1) { // in useless chain
        int curpos = pos[a[0]];
        int where = 0;
        for (int i = 0; i < useless[curpos].size(); ++i) {
          int u = useless[curpos][i];
          if (!vis[a[u]])
            continue;
          if (a[u] > a[0])
            ++where;
          else
            break; // a[u] < a[0]
        }
        vis[a[0]] = true;
        query(useless[curpos][where]);
        // for (int i = 0; i < n; ++i) cerr << a[i] << ' '; cerr << endl;
        ++ndone;
      } else {
        int u = 0;
        for (int i = 0; i < n; ++i)
          if (!del[i] && !vis[a[i]] && pos[a[i]] != -1 && dep[i] > dep[u])
            u = i;
        query(u);
      }
    }

    // erase
    for (auto &vec : useless) {
      for (auto &u : vec) {
        G[par[u]].erase(u);
        del[u] = true;
      }
    }
  }

  cout << queries.size() << endl;
  for (int u : queries)
    cout << u << endl;
}

Fixed submission:
#include <bits/stdc++.h>
using namespace std;

const int N = 4005;
int n, a[N], par[N];
int pos[N];
int dep[N];
bool vis[N];
bool del[N];
set<int> G[N];
vector<int> queries;

void query(int u) {
  if (!u)
    return;
  queries.push_back(u);
  int cur = a[u];
  a[u] = a[0];
  while (true) {
    u = par[u];
    if (u == -1)
      break;
    int new_cur = a[u];
    a[u] = cur;
    cur = new_cur;
  }
}

int main() {
  ios_base::sync_with_stdio(false);
  cin.tie(0);
  cin >> n;
  par[0] = -1;
  for (int i = 1; i < n; ++i) {
    cin >> par[i], G[par[i]].insert(i), dep[i] = dep[par[i]] + 1;
  }
  for (int i = 0; i < n; ++i)
    cin >> a[i];
  for (int i = 0; i < n; ++i)
    pos[i] = -1;

  vector<vector<int>> useless;
  while (true) {
    useless.clear();
    // find useless chains
    queue<int> q;
    for (int i = 0; i < n; ++i)
      if (!del[i] && G[i].size() == 0) {
        pos[i] = useless.size();
        useless.push_back(vector<int>(1, i));
        q.push(i);
      }
    if (useless.empty())
      break;

    while (!q.empty()) {
      int u = q.front();
      q.pop();
      if (u == 0)
        break;
      int p = par[u];
      if (G[p].size() == 1) { // p has only one child u
        pos[p] = pos[u];
        useless[pos[p]].push_back(p);
        q.push(p);
      }
    }

    // for (auto &vec : useless) {
    // 	for (auto &u : vec) cerr << u << ' '; cerr << endl;
    // }

    // solve
    int nleave = 0;
    for (auto &vec : useless)
      nleave += vec.size(); // vec: from bottom to top

    int ndone = 0;
    while (ndone < nleave) {
      if (pos[a[0]] != -1) { // in useless chain
        int curpos = pos[a[0]];
        int where = 0;
        for (int i = 0; i < useless[curpos].size(); ++i) {
          int u = useless[curpos][i];
          if (!vis[a[u]])
            continue;
          if (a[u] > a[0])
            ++where;
          else
            break; // a[u] < a[0]
        }
        vis[a[0]] = true;
        query(useless[curpos][where]);
        // for (int i = 0; i < n; ++i) cerr << a[i] << ' '; cerr << endl;
        ++ndone;
      } else {
        int u = 0;
        for (int i = 0; i < n; ++i)
          if (!del[i] && !vis[a[i]] && pos[a[i]] != -1 && dep[i] > dep[u])
            u = i;
        query(u);
      }
    }

    // erase
    for (auto &vec : useless) {
      for (auto &u : vec) {
        if (u != 0)
          G[par[u]].erase(u);
        del[u] = true;
      }
    }
  }

  cout << queries.size() << endl;
  for (int u : queries)
    cout << u << endl;
}