/*
%W% %G%
*/

append([],L,L).
append([X|L1],L2,[X|L3]) :- append(L1,L2,L3).


insort([],[],_).
insort([X|L],M,O) :- insort(L,N,O), insortx(X,N,M,O).

insortx(X,[A|L],[A|M],O) :-
	P =.. [O,A,X],
	call(P),
	!,
	insortx(X,L,M,O).
insortx(X,L,[X|L],O).

findall(X,G,_) :-
  asserta(found(mark)),
  call(G),
  asserta(found(X)),
  fail.
findall(_,_,L) :- collect_found([],M), !, L = M.

collect_found(S,L) :- getnext(X), !, collect_found([X|S],L).
collect_found(L,L).

getnext(X) :- retract(found(X)), !, X \== mark.


pln(Oid,X,Y,D) :- G =.. [X,Oid,Y,D], call(G).


plugin(desc(Town,Dist),[desc(Town2,Dist2)|T],[desc(Town2,Dist2)|L]) :-
  Dist2 < Dist,
  !,
  plugin(desc(Town,Dist),T,L).
plugin(X,L,[X|L]).

get_len([],0).
get_len([H|T],S):- get_len(T,Tlen), S is Tlen + 1.

write_oids([]).
write_oids([H|T]):- write(H), nl, write_oids(T).


/* -------------Shortest Path-----------------*/

make_cname(V,Vname):-
	name(V,VL),
	name(Vname, [99|VL]), /* 99 is "c" */
	!.

get_cost(V,C,Oids):-
	make_cname(V, Vname),
	G =.. [Vname,C,Oids],
	(call(G) ; C = 10000000),
	!.

store_cost(V,New,Oids):-
	make_cname(V, Vname),
	G =.. [Vname,_,_],
	(retract(G) ; true),
	G2 =.. [Vname,New,Oids],
	assert(G2),
	!.

init_sgo(X,O):-
	store_cost(X,0,[]),
	assert(fu([desc(X,0)])).

sort_node(desc(A,CA),desc(B,CB)) :- CA =< CB.

good_neighbour(UCost,Oids,V,D,O):-
	get_cost(V,VCost,_),
	Sum is D + UCost, Sum < VCost,
	store_cost(V,Sum,[O|Oids]),
	/* write('========='), nl, listing(u),nl,write('========='),nl, */
	
	retract(fu(List)),
	plugin(desc(V,Sum),List,Sorted),
	/* get_len(Sorted,Len), write(Len), nl, */
	assert(fu(Sorted)),
	
	!.

sp_solve(E) :- 
	repeat,

	((retract(fu([desc(U,_)|Tail])), not(U=E)) ; (!,fail)),
	assert(fu(Tail)),

	get_cost(U,UCost,Oids),
	pln(O,U,V,D),
	not([O|_] = Oids), /* Don't go back... */
	good_neighbour(UCost,Oids,V,D,O),
	fail.

start_pln(O,X):-
	repeat,
	current_functor(_,Functor),
	functor(Functor,F,3),
	C =.. [F,O,V,D],
	call(C),
	atom(V),
	number(D),
	!,
	X = F,
	write(C), nl.

solve(A,B):- 
	start_pln(A,X), start_pln(B,Y), init_sgo(X,A),
	not(sp_solve(Y)), get_cost(Y,C,P), append(P,[A,B],Oids),
	write(Oids), nl,
	telling(F),tell(oids_out),write_oids(Oids),told,tell(F),
	write('Total distance: '), write(C).

test:- solve(428179,428080).

test2:- trace, solve(429336,429335).