A* 第k短路

#include <cstdio>
#include <algorithm>
#include <queue>
#include <cstring>
#include <cctype>
using namespace std;
void read(int &num)
{
    char ch;
    while(!isdigit(ch=getchar()));
    for(num=ch-'0';isdigit(ch=getchar());num=num*10+ch-'0');
}
 
const int MAXN = 1005;
const int MAXM = 100005;
const int INF = 0x3f3f3f3f;
 
int n, m;
 
int fir[MAXN], to[MAXM], nxt[MAXM], wt[MAXM], cnt;
inline void Add(int u, int v, int w) { to[++cnt] = v; nxt[cnt] = fir[u]; fir[u] = cnt; wt[cnt] = w; }
 
int rfir[MAXN], rto[MAXM], rnxt[MAXM], rwt[MAXM], rcnt;
inline void rAdd(int ru, int rv, int rw) { rto[++rcnt] = rv; rnxt[rcnt] = rfir[ru]; rfir[ru] = rcnt; rwt[rcnt] = rw; }
 
int dis[MAXN]; int inq[MAXN];
inline bool SPFA(int T, int S)
{
    memset(dis, 0x3f, sizeof dis);
    queue<int> Q;
    dis[T] = 0; Q.push(T);
    while(!Q.empty())
    {
        int u = Q.front();
        if(inq[u] > n) return 0;
        for(int i = rfir[u]; i; i = rnxt[i])
            if(dis[rto[i]] > dis[u] + rwt[i])
            {
                dis[rto[i]] = dis[u] + rwt[i];
                if(!inq[rto[i]])
                    Q.push(rto[i]), inq[rto[i]] = 1;
            }
        Q.pop(); inq[u]--;
    }
    return dis[S] < INF;
}
 
struct node
{
    int x, g, f;
    node(){}
    node(int xx, int gg, int ff):x(xx), g(gg), f(ff){}
    bool operator <(const node &y)const
    {
        return y.f == f ? y.g < g : y.f < f;
    }
};
 
inline int K_th(int S, int T, int K)
{
    if(!SPFA(T, S)) return -1;
    if(S == T) K++;
    priority_queue<node>Q;
    Q.push(node(S, 0, dis[S]));
    int counter = 0;
    while(!Q.empty())
    {
        node u = Q.top(); Q.pop();
        if(u.x == T && ++counter == K) return u.g;
        for(int i = fir[u.x]; i; i = nxt[i])
            Q.push(node(to[i], u.g + wt[i], u.g + wt[i] +dis[to[i]]));
    }
    return -1;
}
 
int main()
{
    //freopen("data.in", "r", stdin);
    int x, y, z;
    read(n), read(m);
    for(int i = 1; i <= m; i++)
        read(x), read(y), read(z), Add(x, y, z), rAdd(y, x, z);
    read(x), read(y), read(z);
    printf("%d\n", K_th(x, y, z));
}