PAT 1067 Sort with Swap(0,*)

#include <cstdio>
#include <cstdlib>
#include <vector>

using namespace std;

int swap(vector<int> &numpos, int va, int vb) {
    //printf("swap(%d at %d, %d at %d)\n", va, numpos[va], vb, numpos[vb]);
    int pa = numpos[va];
    int pb = numpos[vb];
    numpos[va] = pb;
    numpos[vb] = pa;
}

int pick_first_mismatch_value(vector<int> &numpos, int start) {
    int i = start;
    int len = numpos.size();
    while (i < len) {
        if (numpos[i] != i) {
            return i;
        
        }
        i++;
    }
    return -1;
}

int main() {
    int N;
    scanf("%d", &N);
    
    vector<int> numpos(N);
    
    int value;
    for (int i=0; i<N; i++) {
        scanf("%d", &value);
        numpos[value] = i;
    }
    
    int step = 0;
    int b = 0;
    for (;;) {
        if (numpos[0] == 0) {
            b = pick_first_mismatch_value(numpos, b);
            if (b <= 0) {
                break;
            }
            swap(numpos, 0, b);
            step++;
            continue;
        }
        swap(numpos, 0, numpos[0]);

        step++;
    }
    
    printf("%d", step);
    
    return 0;
}