Problem p02557 (C++)

Buggy submission (status: Runtime Error):
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cmath>
#include <complex>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <numeric>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <stdio.h>
#include <string>
#include <time.h>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
#define all(a) a.begin(), a.end()
#define rep(i, n) for (ll i = 0; i < (n); i++)
#define pb push_back
#define debug(x) cerr << __LINE__ << ' ' << #x << ':' << x << '\n'
#pragma GCC optimize("O3")
#pragma GCC optimize("unroll-loops")
using namespace std;
typedef long long ll;
typedef unsigned int uint;
typedef unsigned long long ull;
typedef long double ld;
typedef pair<ll, ll> P;
typedef complex<ld> com;
template <class T> using pque = priority_queue<T, vector<T>, greater<T>>;
constexpr int inf = 1000000010;
constexpr ll INF = 1000000000000000010;
constexpr int mod1e9 = 1000000007;
constexpr int mod998 = 998244353;
constexpr ld eps = 1e-12;
constexpr ld pi = 3.141592653589793238;
constexpr ll ten(int n) { return n ? 10 * ten(n - 1) : 1; };
int dx[] = {1, 0, -1, 0, 1, 1, -1, -1};
int dy[] = {0, 1, 0, -1, 1, -1, 1, -1};
void fail() {
  cout << "-1\n";
  exit(0);
}
template <class T, class U> inline bool chmax(T &a, const U &b) {
  if (a < b) {
    a = b;
    return true;
  }
  return false;
}
template <class T, class U> inline bool chmin(T &a, const U &b) {
  if (a > b) {
    a = b;
    return true;
  }
  return false;
}
template <class T> istream &operator>>(istream &s, vector<T> &v) {
  for (auto &e : v)
    s >> e;
  return s;
}
template <class T> ostream &operator<<(ostream &s, const vector<T> &v) {
  for (auto &e : v)
    s << e << ' ';
  return s;
}

struct fastio {
  fastio() {
    cin.tie(0);
    cout.tie(0);
    ios::sync_with_stdio(false);
    cout << fixed << setprecision(20);
    cerr << fixed << setprecision(20);
  }
} fastio_;

int main() {
  int n;
  cin >> n;
  vector<int> a(n), b(n), ca(n), cb(n), cnt(n);
  multiset<int> sta, stb;
  rep(i, n) {
    cin >> a[i];
    a[i]--;
    ca[a[i]]++;
    cnt[a[i]]++;
    sta.insert(a[i]);
  }
  rep(i, n) {
    cin >> b[i];
    b[i]--;
    cb[b[i]]++;
    cnt[b[i]]++;
    stb.insert(b[i]);
  }
  multiset<P, greater<P>> st;
  rep(i, n) st.insert({cnt[i], i});
  if ((*st.begin()).first > n) {
    cout << "No\n";
    return 0;
  }
  vector<vector<int>> match(n);
  for (int i = n; i >= 1; i--) {
    P p = *st.begin();
    st.erase(st.find(p));
    P q = *st.begin();
    st.erase(st.find(q));
    if (p.first == i && q.first == i) {
      match[p.second].pb(q.second);
      sta.erase(sta.find(p.second));
      stb.erase(stb.find(q.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[p.second]--;
      cb[q.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    } else if (p.first == i && ca[p.second] != 0) {
      int v = *stb.begin();
      if (v == p.second)
        v = *stb.rbegin();
      match[p.second].pb(v);
      st.insert(q);
      q = {cnt[v], v};
      st.erase(st.find(q));
      sta.erase(sta.find(p.second));
      stb.erase(stb.find(q.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[p.second]--;
      cb[q.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    } else if (p.first == i && cb[p.second] != 0) {
      int v = *sta.begin();
      if (v == p.second)
        v = *sta.rbegin();
      match[v].pb(p.second);
      st.insert(q);
      q = {cnt[v], v};
      st.erase(st.find(q));
      sta.erase(sta.find(q.second));
      stb.erase(stb.find(p.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[q.second]--;
      cb[p.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    } else {
      int v = *sta.begin();
      int w = *stb.begin();
      if (v == w)
        w = *stb.rbegin();
      st.insert(p);
      st.insert(q);
      p = {cnt[v], v};
      q = {cnt[w], w};
      st.erase(st.find(p));
      st.erase(st.find(q));
      match[p.second].pb(q.second);
      sta.erase(sta.find(p.second));
      stb.erase(stb.find(q.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[p.second]--;
      cb[q.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    }
  }
  cout << "Yes\n";
  rep(i, n) {
    for (int j : match[i]) {
      cout << j + 1 << ' ';
    }
  }
  cout << '\n';
}

Fixed submission:
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cmath>
#include <complex>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <numeric>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <stdio.h>
#include <string>
#include <time.h>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
#define all(a) a.begin(), a.end()
#define rep(i, n) for (ll i = 0; i < (n); i++)
#define pb push_back
#define debug(x) cerr << __LINE__ << ' ' << #x << ':' << x << '\n'
#pragma GCC optimize("O3")
#pragma GCC optimize("unroll-loops")
using namespace std;
typedef long long ll;
typedef unsigned int uint;
typedef unsigned long long ull;
typedef long double ld;
typedef pair<ll, ll> P;
typedef complex<ld> com;
template <class T> using pque = priority_queue<T, vector<T>, greater<T>>;
constexpr int inf = 1000000010;
constexpr ll INF = 1000000000000000010;
constexpr int mod1e9 = 1000000007;
constexpr int mod998 = 998244353;
constexpr ld eps = 1e-12;
constexpr ld pi = 3.141592653589793238;
constexpr ll ten(int n) { return n ? 10 * ten(n - 1) : 1; };
int dx[] = {1, 0, -1, 0, 1, 1, -1, -1};
int dy[] = {0, 1, 0, -1, 1, -1, 1, -1};
void fail() {
  cout << "-1\n";
  exit(0);
}
template <class T, class U> inline bool chmax(T &a, const U &b) {
  if (a < b) {
    a = b;
    return true;
  }
  return false;
}
template <class T, class U> inline bool chmin(T &a, const U &b) {
  if (a > b) {
    a = b;
    return true;
  }
  return false;
}
template <class T> istream &operator>>(istream &s, vector<T> &v) {
  for (auto &e : v)
    s >> e;
  return s;
}
template <class T> ostream &operator<<(ostream &s, const vector<T> &v) {
  for (auto &e : v)
    s << e << ' ';
  return s;
}

struct fastio {
  fastio() {
    cin.tie(0);
    cout.tie(0);
    ios::sync_with_stdio(false);
    cout << fixed << setprecision(20);
    cerr << fixed << setprecision(20);
  }
} fastio_;

int main() {
  int n;
  cin >> n;
  vector<int> a(n), b(n), ca(n), cb(n), cnt(n);
  multiset<int> sta, stb;
  rep(i, n) {
    cin >> a[i];
    a[i]--;
    ca[a[i]]++;
    cnt[a[i]]++;
    sta.insert(a[i]);
  }
  rep(i, n) {
    cin >> b[i];
    b[i]--;
    cb[b[i]]++;
    cnt[b[i]]++;
    stb.insert(b[i]);
  }
  multiset<P, greater<P>> st;
  rep(i, n) st.insert({cnt[i], i});
  if ((*st.begin()).first > n) {
    cout << "No\n";
    return 0;
  }
  vector<vector<int>> match(n);
  for (int i = n; i >= 1; i--) {
    P p = *st.begin();
    st.erase(st.find(p));
    P q = *st.begin();
    st.erase(st.find(q));
    if (p.first == i && q.first == i) {
      if (ca[p.second] == 0)
        swap(p, q);
      match[p.second].pb(q.second);
      sta.erase(sta.find(p.second));
      stb.erase(stb.find(q.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[p.second]--;
      cb[q.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    } else if (p.first == i && ca[p.second] != 0) {
      int v = *stb.begin();
      if (v == p.second)
        v = *stb.rbegin();
      match[p.second].pb(v);
      st.insert(q);
      q = {cnt[v], v};
      st.erase(st.find(q));
      sta.erase(sta.find(p.second));
      stb.erase(stb.find(q.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[p.second]--;
      cb[q.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    } else if (p.first == i && cb[p.second] != 0) {
      int v = *sta.begin();
      if (v == p.second)
        v = *sta.rbegin();
      match[v].pb(p.second);
      st.insert(q);
      q = {cnt[v], v};
      st.erase(st.find(q));
      sta.erase(sta.find(q.second));
      stb.erase(stb.find(p.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[q.second]--;
      cb[p.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    } else {
      int v = *sta.begin();
      int w = *stb.begin();
      if (v == w)
        w = *stb.rbegin();
      st.insert(p);
      st.insert(q);
      p = {cnt[v], v};
      q = {cnt[w], w};
      st.erase(st.find(p));
      st.erase(st.find(q));
      match[p.second].pb(q.second);
      sta.erase(sta.find(p.second));
      stb.erase(stb.find(q.second));
      cnt[p.second]--;
      cnt[q.second]--;
      ca[p.second]--;
      cb[q.second]--;
      p.first--;
      q.first--;
      st.insert(p);
      st.insert(q);
    }
  }
  cout << "Yes\n";
  rep(i, n) {
    for (int j : match[i]) {
      cout << j + 1 << ' ';
    }
  }
  cout << '\n';
}