POJ 1661 Help Jimmy

/*96655 ‘s source code for M
Memory: 8604 KB        Time: 63 MS
Language: G++        Result: Accepted
*/
#include<iostream>
#include<cstdio>
#include<cstdlib>
#include<cmath>
#include<algorithm>
#include<cstring>
#include<cstring>
#include<vector>
#include<queue>
#include<stack>
using namespace std;
short x[1006],y[1005],h[1005];
short xx[2005],hh[1005],xcnt,hcnt,d,ww,f;
int dp[1003][2003];
bool cmp(int a,int b)
{
    return a>b;
}
int main()
{
    int T;
    scanf("%d",&T);
    while(T--)
    {
        int n,bb,cc,m;
        scanf("%d%d%d%d",&n,&bb,&cc,&m);
        xcnt=hcnt=0;
        for(int i=1; i<=n; ++i)
        {
            scanf("%d%d%d",&x[i],&y[i],&h[i]);
            xx[xcnt++]=x[i];
            xx[xcnt++]=y[i];
            hh[hcnt++]=h[i];
        }
        xx[xcnt++]=bb;
        hh[hcnt++]=0;
        sort(xx,xx+xcnt);
        sort(hh,hh+hcnt,cmp);
        d=1;
        for(int i=1; i<xcnt; i++)
            if(xx[i]!=xx[i-1])xx[d++]=xx[i];
        xcnt=d,d=1;
        for(int i=1; i<hcnt; i++)
            if(hh[i]!=hh[i-1])hh[d++]=hh[i];
        hcnt=d;
        vector<int>a[hcnt+1];
        memset(dp,-1,sizeof(dp));
        int ans=0x3f3f3f3f;
        for(int i=0; i<xcnt; i++)
            dp[hcnt-1][i]=ans;
        for(int i=1; i<=n; i++)
        {
            int id;
            for(int j=0; j<hcnt; j++)
                if(h[i]==hh[j])
                {
                    id=j;
                    break;
                }
            int s=lower_bound(xx,xx+xcnt,x[i])-xx;
            int t=lower_bound(xx,xx+xcnt,y[i])-xx;
            a[id].push_back(s);
            a[id].push_back(t);
            for(int j=s; j<=t; ++j)
                dp[id][j]=ans;
        }
        f=lower_bound(xx,xx+xcnt,bb)-xx;
        for(int i=0; i<hcnt; i++)
        {
            if(dp[i][f]!=-1)
            {
                dp[i][f]=cc-hh[i];
                ww=i;
                break;
            }
        }
        for(int i=ww; i<hcnt-1; ++i)
        {
            for(int j=0; j<xcnt; j++)
            {
                if(dp[i][j]<ans&&dp[i][j]!=-1)
                {
                    int s,t;
                    for(int k=1;k<a[i].size();k+=2)
                    {
                        if(a[i][k-1]<=j&&a[i][k]>=j)
                        {
                            s=a[i][k-1];
                            t=a[i][k];
                            break;
                        }
                    }
                    int l=xx[j]-xx[s],r=xx[t]-xx[j];
                    dp[i][s]=min(dp[i][s],dp[i][j]+l);
                    dp[i][t]=min(dp[i][t],dp[i][j]+r);

                }
            }
            for(int j=0; j<a[i].size(); ++j)
            {
                int p=a[i][j];
                if(dp[i][p]!=-1&&dp[i][p]<ans)
                {
                    for(int k=i+1; k<hcnt&&hh[i]-hh[k]<=m; k++)
                        if(dp[k][p]!=-1)
                        {
                            dp[k][p]=min(dp[k][p],dp[i][p]+hh[i]-hh[k]);
                            break;
                        }
                }
            }
        }
        for(int i=0; i<xcnt; i++)
            ans=min(ans,dp[hcnt-1][i]);
        printf("%d\n",ans);
    }
    return 0;
}