Problema Autogrill

long long chiedi(long long p, std::set<long long> &autogrill) {
    long long min_val;
    long long final_val;
    long long val;
    if (autogrill.empty()){
        return -1;
    }
    set<long long>::iterator it = autogrill.upper_bound(p);  
    if (it != autogrill.end()){
        min_val = abs(p-*it);
        final_val = *it;
    }
    else {
        it = prev(it);
        min_val = abs(p-*it);
        final_val = *it;
    }
    it = prev(it);
    if (it != autogrill.begin()){
        val = abs(p-*it);
        if (val < min_val){
            min_val = abs(p-*it);
            final_val = *it;
        }
    }
    return final_val;
}

Controllo l’upper bound di p.
Se è diverso dalla fine del set calcolo la distanza dall’autogrill, in alternativa se è uguale alla fine del set mi sposto al precedente e calcolo la distanza dall’autogrill.
Per entrambi i casi mi sposto al precedente e se è diverso dall’inizio del set, calcolo la distanza dall’autogrill e se è minore aggiorno l’autogrill a cui andare.
Sempre 0/100.
Cosa mi manca da fare?