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;
}