Aiuto con Soste in autostrada (autogrill)

Aiuto
6 
#include <set>

using namespace std;

set<int> AG;

void inizia() {
    return;
}

void apri(long long p) {
    AG.insert(p);
    return;
}

void chiudi(long long p) {
    AG.erase(p);
    return;
}

long long chiedi(long long p) {
    if(AG.empty())
        return -1;
    auto NearestPos = AG.lower_bound(p);
    if (NearestPos == AG.end())
        return *AG.rbegin();
    if (NearestPos == AG.begin())
        return *NearestPos;
    if(AG.size() == 1) 
        return *NearestPos;
    if (*NearestPos == p)
        return *NearestPos;
    
    long long leftNearest = *prev(NearestPos);
    long long rightNearest = *NearestPos;
    if (p - leftNearest >= rightNearest - p)
        return rightNearest;
    return leftNearest;

}

Usando AG.rbegin() Va molto meglio ma ancora non li risolve tutti
Non capisco proprio cosa altro potrei fare
Grazie ancora per l’aiuto