PAT (Advanced Level) 1067. Sort with Swap(0,*) (25)

#include<cstdio>
#include<cstring>
#include<cmath>
#include<vector>
#include<map>
#include<stack>
#include<queue>
#include<string>
#include<algorithm>
using namespace std;

int n;
int a[100000+10];
queue<int>Q;
int pos[100000+10];

int main()
{
scanf("%d",&n);
for(int i=0;i<n;i++) scanf("%d",&a[i]);
for(int i=0;i<n;i++) pos[a[i]]=i;
for(int i=0;i<n;i++)
{
if(a[i]==0) continue;
if(a[i]!=i) Q.push(a[i]);
}

int ans=0;
while(1)
{
if(a[0]==0)
{
while(!Q.empty())
{
int head=Q.front(); Q.pop();
if(a[head]==head) continue;
else
{
ans++;
int pos1=pos[0];
int pos2=pos[head];
swap(a[pos1],a[pos2]);
swap(pos[head],pos[0]);
break;
}
}
if(Q.empty()) break;
}
else
{
ans++;
int pos1=pos[0];
int pos2=pos[pos[0]];
int num1=0;
int num2=pos[0];
swap(a[pos1],a[pos2]);
swap(pos[num1],pos[num2]);
}
}
printf("%d\n",ans);
return 0;
}