USACO 2006 November Gold

#include <cstdio>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <cstdlib>
#include <cmath>
#include <utility>
#include <vector>
#include <queue>
#include <map>
#include <set>
#define max(x,y) ((x)>(y)?(x):(y))
#define min(x,y) ((x)>(y)?(y):(x))
#define INF 0x3f3f3f3f

using namespace std;

struct Edge
{
int y,w,ne;
}e[200005];

int x,y,w,n,m;
int be[5005],all;
int h[5005];
bool vis[5005];

struct Point
{
int x,g;
bool operator < (const Point T) const
{
return g+h[x]>T.g+h[T.x];
}
};

void add(int x, int y, int w)
{
e[all].y=y;
e[all].w=w;
e[all].ne=be[x];
be[x]=all++;
}

void SPFA(int s)
{
queue< int > q;
for(int i=0; i<=n; i++)
{
h[i]=INF;
vis[i]=0;
}
h[s]=0;
vis[s]=1;
while(!q.empty())
q.pop();
q.push(s);
while(!q.empty())
{
int u=q.front();
q.pop();
vis[u]=0;
for(int i=be[u]; i!=-1; i=e[i].ne)
{
int v=e[i].y;
if(h[v]>h[u]+e[i].w)
{
h[v]=h[u]+e[i].w;
if(!vis[v])
{
vis[v]=1;
q.push(v);
}
}
}
}
}

int Astar(int s, int t, int k)
{
SPFA(t);
priority_queue< Point > q;
while(!q.empty())
q.pop();
memset(vis,0,sizeof(vis));
Point cur,nxt;
cur.x=s;
cur.g=0;
q.push(cur);
while(!q.empty())
{
cur=q.top();
q.pop();
if(cur.x==t && !--k)
return cur.g;
for(int i=be[cur.x]; i!=-1; i=e[i].ne)
{
nxt.x=e[i].y;
nxt.g=cur.g+e[i].w;
q.push(nxt);
}
}
return -1;
}

void init()
{
all=0;
memset(be,-1,sizeof(be));
}

int main()
{
scanf("%d%d",&n,&m);
init();
for(int i=0; i<m; i++)
{
scanf("%d%d%d",&x,&y,&w);
add(x,y,w);
add(y,x,w);
}
printf("%d\n",Astar(1,n,2));
return 0;
}