SCU 4445 Right turn

#include<bits/stdc++.h>
using namespace std;

const int INF = 0x7FFFFFFF;
const int mod = 1e9 + 7;
const int N = 5e6 + 10;
const int M = 1e4 + 1;
const double eps = 1e-10;
int T,n,m;

struct P
{
    int x,y;
    P(int X=0,int Y=0)
    {
        x=X;
        y=Y;
    }
}p[2000],q[2000];

bool cmp1(P a,P b)
{
    if(a.x!=b.x) return a.x<b.x;
    return a.y<b.y;
}

bool cmp2(P a,P b)
{
    if(a.y!=b.y) return a.y<b.y;
    return a.x<b.x;
}

int nowx,nowy,nowd;

int main()
{
    while(~scanf("%d",&n))
    {
        for(int i=1;i<=n;i++)
        {
            scanf("%d%d",&p[i].x,&p[i].y);
            q[i].x=p[i].x;
            q[i].y=p[i].y;
        }

        sort(p+1,p+1+n,cmp1);
        sort(q+1,q+1+n,cmp2);

        nowx=nowy=0; nowd=0;

        int ans=0;

        while(1)
        {
            if(nowd==0)
            {
                int k = lower_bound(q+1,q+n+1,P(nowx,nowy),cmp2)-q;
                if(k==n+1||q[k].y!=nowy)
                {
                    printf("%d\n",ans);
                    break;
                }
                else ans++,nowx = q[k].x-1,nowd=(nowd+1)%4;
            }
            else if(nowd==2)
            {
                int k = lower_bound(q+1,q+n+1,P(nowx,nowy),cmp2)-q-1;
                if(k==0||q[k].y!=nowy)
                {
                    printf("%d\n",ans);
                    break;
                }
                else ans++,nowx = q[k].x+1,nowd=(nowd+1)%4;
            }

            else if(nowd==1)
            {
                int k = lower_bound(p+1,p+n+1,P(nowx,nowy),cmp1)-p-1;
                if(k==0||p[k].x!=nowx)
                {
                    printf("%d\n",ans);
                    break;
                }
                else ans++,nowy = p[k].y+1,nowd=(nowd+1)%4;
            }

            else
            {
                int k = lower_bound(p+1,p+n+1,P(nowx,nowy),cmp1)-p;
                if(k==n+1||p[k].x!=nowx)
                {
                    printf("%d\n",ans);
                    break;
                }
                else ans++,nowy = p[k].y-1,nowd=(nowd+1)%4;
            }

            if(ans>4*n)
            {
                printf("-1\n");
                break;
            }
        }
    }
    return 0;
}