:- use_module(library(heaps)).
:- use_module(library(lists)).

astar :- statistics(runtime,_),init(S), astar(S), statistics(runtime,[_,T]), print(T),print(' ms'),nl.

astar(S) :- empty_heap(EH),
   add_states_to_heap([st(0,S,[S])],EH,StartHeap),
   nl,print('Starting A* search'),nl, 
   asearch(StartHeap,1).

asearch(Heap,NrOfSteps) :- 
  trace_info(Heap),
  get_from_heap(Heap,_Heuristic,st(G,State,Path),NewHeap),
  print(NrOfSteps),print(' ==> '), print(st(G,State)),nl,
   (goal(State) , print('GOAL'(State)),nl, reverse(Path,RealPath),
                   print('PATH: '), print(RealPath),nl,flush_output
     ;
    (findall(st(G1,SuccState,[SuccState|Path]),
	         (s(State,Weight,SuccState),G1 is G+Weight),SList),
	% print(succs(SList)),nl,
     add_states_to_heap(SList,NewHeap,UpdatedHeap),
     S1 is NrOfSteps+1,
     asearch(UpdatedHeap,S1)
    )
   ).

add_states_to_heap([],H,H).
add_states_to_heap([st(G,State,Path)|T],Heap,UpdHeap) :-
   (h(State,SH) -> true ; print(err(State)),nl),  % compute heuristic function
   eval_function(G,SH,F),  % Compute weight
   add_to_heap(Heap,F,st(G,State,Path),H2),
   add_states_to_heap(T,H2,UpdHeap).

eval_function(G_SoFar,H_EstDist,F) :- F is G_SoFar+H_EstDist. % A*
%eval_function(_G_SoFar,H_EstDist,F) :- F =H_EstDist. % Greedy
%eval_function(G_SoFar,_,F) :- F=G_SoFar. % Dijkstra

% print tracing info and display graph about current state
:- dynamic print_trace_info/0.
print_trace_info.
silent :- retractall(print_trace_info).
trace_info(_) :- \+ print_trace_info, !.
trace_info(Heap) :- heap_to_list(Heap,List),
 % portray_heap(Heap), nl,
  print(List),nl,
  print_nodes(List), read(_).

/* To Do:
  how to get BF, DF, Dijkstra, Greedy Search
  add loop checking to astar
*/


/* 

 BF: G1 is G+1,  H = 0
 Dijkstra G1 is G+Weight, H=0
 DF: G1 = G-1,  H = 0
 Greedy: G=0, H = h
*/

% The Problem from Norvig's slides on A*:

init(arad).

sd(oradea,71,zerind).
sd(oradea,151,sibiu).
sd(arad,75,zerind).
sd(arad,118,timisoara).
sd(arad,140,sibiu).
sd(fagaras,99,sibiu).
sd(rimnicu_vilcea,80,sibiu).
sd(bucharest,211,fagaras).
sd(bucharest,101,pitesti).
sd(bucharest,90,giurgiu).
sd(craiova,138,pitesti).
sd(craiova,120,dobreta).
sd(craiova,146,rimnicu_vilcea).
sd(dobreta,75,mehadia).
sd(lugoj,70,mehadia).
sd(lugoj,111,timasoara).
sd(pitesti,97, rimnicu_vilcea).

sd(eforie,86,hirsova).
sd(iasi,87,neamt).
sd(bucharest,85,urziceni).
sd(hirsova,98,urziceni).
sd(iasi,92,vaslui).
sd(urziceni,142,vaslui).

s(X,S,Y) :- sd(X,S,Y).
s(X,S,Y) :- sd(Y,S,X).
%s(X,W,Y) :- X @>Y, s(Y,W,X).

h(arad,366).
h(bucharest,0).
h(craiova,160).
h(dobreta,242).
h(eforie,161). %%
h(fagaras,178).
h(giurgiu,77).
h(hirsova,151). %%
h(iasi,226).%%
h(lugoj,244).
h(mehadia,241).
h(neamt,234). %%
h(oradea,380).
h(pitesti,98).
h(rimnicu_vilcea,193).
h(sibiu,253).
h(timisoara,329).
h(urziceni,80). %%
h(vaslui,199). %%
h(zerind,374). 

goal(bucharest).

eight :- silent,
 ['/Users/leuschel/svn_root/stups_teaching/stups-1/beispiele_vorlesung/2009/search/astar_8.pl'].

/* ---------------------------------------------------------------------- */   

% output graph + heap as dot graph
print_nodes :- empty_heap(H), print_nodes(H).

% current top node is marked green; other nodes on heap are marked red
print_nodes(Heap) :- print_graph_header(prolog),
  findall(X,s(X,_,_),L), sort(L,SL), 
  member(Y,SL), 
  (member(_-st(_,Y,_),Heap) -> 
    (Heap=[_-st(_,Y,_)|_] -> print_filled_node(Y,green)
     ; print_filled_node(Y,red))
    ; print_node(Y,blue)),fail.
print_nodes(_) :- sd(X,W,Y), print(X), print(' -> '), print(Y),
    print(' [color = steelblue, label="'),
	print(W), print('"];'),nl,fail.
print_nodes(_) :- print_graph_footer.

/* ---------------------------------------------------------------------- */   

:- use_module(library(system3)).

print_graph_header(Type) :- tell('~/Desktop/tmp2/out.dot'),
    print('digraph '), print(Type), print(' {'),nl,
   % print('graph [orientation=landscape, page="8.5, 11",ratio=fill,size="7.5,10"];'),nl,
    print(' graph [page="8.5, 11",ratio=fill,size="7.5,10"];'),
    nl.

print_graph_footer :-  print('}'),nl, told,
	print('Done'),nl,
%%	system('dot -Tps ~/Desktop/tmp/out.dot >~/Desktop/tmp/out.ps'),
%%	system('open  ~/Desktop/tmp/out.ps').
	system('open  ~/Desktop/tmp2/out.dot').


print_node(ID,Col) :-
	print(ID), print(' [shape=ellipse, color="'),
	print(Col), print('", label="'),
	print(ID), print('"];'),nl.
print_filled_node(ID,Col) :-
	print(ID), print(' [shape=ellipse, style=filled,color="'),
	print(Col), print('", label="'),
	print(ID), print('"];'),nl.