Problem p02809 (C++)

Buggy submission (status: Runtime Error):
#include <bits/stdc++.h>
#define ll long long
#define uint unsigned int
#define ull unsigned long long
#define inf 1010000000
#define infll 1010000000000000000ll
#define vi vector<int>
#define vll vector<ll>
#define pb push_back
#define pii pair<int, int>
#define pll pair<ll, ll>
#define pdd pair<double, double>
#define mpr make_pair
#define fi first
#define se second
#define pq priority_queue<int>
#define pqll priority_queue<ll>
#define up(x, y) (x < (y) ? x = (y) : 0)
#define dn(x, y) (x > (y) ? x = (y) : 0)
#define ad(x, y) (x = (x + (y)) % mod)
#define cbn(x, y) ((ll)fac[x] * inv[y] % mod * inv[(x) - (y)] % mod)
#define mod 1000000007
#define N 100009
using namespace std;

int n, cnt, sz, a[N], vis[N], bo[N], path[N], blg[N], bg[N], dgr[N];
set<int> S;
set<pii> T;
pq Q;
vi ans[N], f, g, b[N];
int read();
ll readll();
void dfs(int x, int para) {
  if (bo[x] == para)
    return;
  vis[x] = 1;
  bo[x] = para;
  S.insert(x);
  for (int p : b[x]) {
    // if (min(u,v)==min(x,p) && max(u,v)==max(x,p)) continue;
    dfs(p, para);
  }
}
bool work(int k, int len, int last, int rst, int j) {
  if (k > len) {
    ans[cnt].clear();
    int i;
    for (i = 1; i <= len; i++)
      ans[cnt].pb(path[i]);
    return 1;
  }
  if ((*T.rbegin()).fi == S.size() - 1) {
    //	cerr<<"!\n";
    int x = (*T.rbegin()).se;
    //	cerr<<x<<'\n';
    if (x == a[last])
      return 0;
    path[k] = x;
    S.erase(x);
    T.erase(mpr(dgr[x], x));
    bool flag = 0;
    if (S.find(a[x]) != S.end()) {
      flag = 1;
      T.erase(mpr(dgr[a[x]]--, a[x]));
      T.insert(mpr(dgr[a[x]], a[x]));
    }
    if (work(k + 1, len, x, 0, j))
      return 1;
    if (flag) {
      T.erase(mpr(dgr[a[x]]++, a[x]));
      T.insert(mpr(dgr[a[x]], a[x]));
    }
    return 0;
  }
  for (auto it = S.begin(); it != S.end();)
    if ((*it) != a[last]) {
      path[k] = *it;
      S.erase(it);
      int x = *it;
      T.erase(mpr(dgr[x], x));
      bool flag = 0;
      if (S.find(a[x]) != S.end()) {
        flag = 1;
        T.erase(mpr(dgr[a[x]]--, a[x]));
        T.insert(mpr(dgr[a[x]], a[x]));
      }
      if (work(k + 1, len, *it, rst, j))
        return 1;
      S.insert(path[k]);
      it = S.upper_bound(path[k]);
      x = path[k];
      T.insert(mpr(dgr[x], x));
      if (flag) {
        T.erase(mpr(dgr[a[x]]++, a[x]));
        T.insert(mpr(dgr[a[x]], a[x]));
      }
    } else
      it++;
  return 0;
}
int main() {
  scanf("%d", &n);
  if (n == 2) {
    puts("-1");
    return 0;
  }
  int i, j;
  for (i = 1; i <= n; i++) {
    scanf("%d", &a[i]);
    b[a[i]].pb(i);
  }
  cnt = 1;
  for (i = 1; i <= n; i++)
    S.insert(i);
  for (int p : S)
    dgr[a[p]]++;
  T.clear();
  for (int p : S)
    T.insert(mpr(dgr[p], p));
  work(1, S.size(), 0, 0, j);
  for (int p : ans[1])
    printf("%d ", p);
  puts("");
  return 0;
}

int read() {
  int x = 0;
  char ch = getchar();
  bool flag = 0;
  while (ch < '0' || ch > '9') {
    if (ch == '-')
      flag = 1;
    ch = getchar();
  }
  while (ch >= '0' && ch <= '9') {
    x = x * 10 + ch - '0';
    ch = getchar();
  }
  return flag ? -x : x;
}
ll readll() {
  ll x = 0;
  char ch = getchar();
  bool flag = 0;
  while (ch < '0' || ch > '9') {
    if (ch == '-')
      flag = 1;
    ch = getchar();
  }
  while (ch >= '0' && ch <= '9') {
    x = x * 10 + ch - '0';
    ch = getchar();
  }
  return flag ? -x : x;
}

Fixed submission:
#include <bits/stdc++.h>
#define ll long long
#define uint unsigned int
#define ull unsigned long long
#define inf 1010000000
#define infll 1010000000000000000ll
#define vi vector<int>
#define vll vector<ll>
#define pb push_back
#define pii pair<int, int>
#define pll pair<ll, ll>
#define pdd pair<double, double>
#define mpr make_pair
#define fi first
#define se second
#define pq priority_queue<int>
#define pqll priority_queue<ll>
#define up(x, y) (x < (y) ? x = (y) : 0)
#define dn(x, y) (x > (y) ? x = (y) : 0)
#define ad(x, y) (x = (x + (y)) % mod)
#define cbn(x, y) ((ll)fac[x] * inv[y] % mod * inv[(x) - (y)] % mod)
#define mod 1000000007
#define N 100009
using namespace std;

int n, cnt, sz, a[N], vis[N], bo[N], path[N], blg[N], bg[N], dgr[N];
set<int> S;
set<pii> T;
pq Q;
vi ans[N], f, g, b[N];
int read();
ll readll();
void dfs(int x, int para) {
  if (bo[x] == para)
    return;
  vis[x] = 1;
  bo[x] = para;
  S.insert(x);
  for (int p : b[x]) {
    // if (min(u,v)==min(x,p) && max(u,v)==max(x,p)) continue;
    dfs(p, para);
  }
}
bool work(int k, int len, int last, int rst, int j) {
  if (k > len) {
    ans[cnt].clear();
    int i;
    for (i = 1; i <= len; i++)
      ans[cnt].pb(path[i]);
    return 1;
  }
  if ((*T.rbegin()).fi == S.size() - 1) {
    //	cerr<<"!\n";
    int x = (*T.rbegin()).se;
    //	cerr<<x<<'\n';
    if (x == a[last])
      return 0;
    path[k] = x;
    S.erase(x);
    T.erase(mpr(dgr[x], x));
    bool flag = 0;
    if (S.find(a[x]) != S.end()) {
      flag = 1;
      T.erase(mpr(dgr[a[x]]--, a[x]));
      T.insert(mpr(dgr[a[x]], a[x]));
    }
    if (work(k + 1, len, x, 0, j))
      return 1;
    if (flag) {
      T.erase(mpr(dgr[a[x]]++, a[x]));
      T.insert(mpr(dgr[a[x]], a[x]));
    }
    S.insert(x);
    T.insert(mpr(dgr[x], x));
    return 0;
  }
  for (auto it = S.begin(); it != S.end();)
    if ((*it) != a[last]) {
      path[k] = *it;
      S.erase(it);
      int x = *it;
      T.erase(mpr(dgr[x], x));
      bool flag = 0;
      if (S.find(a[x]) != S.end()) {
        flag = 1;
        T.erase(mpr(dgr[a[x]]--, a[x]));
        T.insert(mpr(dgr[a[x]], a[x]));
      }
      if (work(k + 1, len, *it, rst, j))
        return 1;
      S.insert(path[k]);
      it = S.upper_bound(path[k]);
      x = path[k];
      T.insert(mpr(dgr[x], x));
      if (flag) {
        T.erase(mpr(dgr[a[x]]++, a[x]));
        T.insert(mpr(dgr[a[x]], a[x]));
      }
    } else
      it++;
  return 0;
}
int main() {
  scanf("%d", &n);
  if (n == 2) {
    puts("-1");
    return 0;
  }
  int i, j;
  for (i = 1; i <= n; i++) {
    scanf("%d", &a[i]);
    b[a[i]].pb(i);
  }
  cnt = 1;
  for (i = 1; i <= n; i++)
    S.insert(i);
  for (int p : S)
    dgr[a[p]]++;
  T.clear();
  for (int p : S)
    T.insert(mpr(dgr[p], p));
  work(1, S.size(), 0, 0, j);
  for (int p : ans[1])
    printf("%d ", p);
  puts("");
  return 0;
}

int read() {
  int x = 0;
  char ch = getchar();
  bool flag = 0;
  while (ch < '0' || ch > '9') {
    if (ch == '-')
      flag = 1;
    ch = getchar();
  }
  while (ch >= '0' && ch <= '9') {
    x = x * 10 + ch - '0';
    ch = getchar();
  }
  return flag ? -x : x;
}
ll readll() {
  ll x = 0;
  char ch = getchar();
  bool flag = 0;
  while (ch < '0' || ch > '9') {
    if (ch == '-')
      flag = 1;
    ch = getchar();
  }
  while (ch >= '0' && ch <= '9') {
    x = x * 10 + ch - '0';
    ch = getchar();
  }
  return flag ? -x : x;
}