sucessoresF(G:EAtual:Cam, V, Sucessores) :-
    findall(F:H:G1:ENovo:[Acao|Cam],
        (acao(Acao,EAtual,ENovo, G2),
            not(member(ENovo,V)),
            h(ENovo, H), % calcula estimativa
            G1 is G + G2, % acumula custo da acao
            F is H + G1), % soma a estimativa ao custo acumulado
        Sucessores).

buscaH :-
    inicial(Estado), % obtém estado inicial
    buscaH([_:_:0:Estado:[]],[], P:G), % inicia busca A*
    format('~nPlano: ~w',[P]), % lista de acoes
    format('~nCusto: ~w~n~n', [G]).

buscaH([_:_:Custo:Estado:CaminhoInvertido|_],_,Caminho:Custo) :-
    meta(Estado), !, % se atingiu meta, usa cut ! para calcelar backtracking
    reverse(CaminhoInvertido, Caminho).

buscaH([Futuro:H:Custo:Estado:Caminho|PossiveisAntes], Visitados, P):-
    format('Estado atual: ~w~n', Estado),
    format('Custo atual: ~w~n', Custo),
    format('Custo futuro: ~w~n', Futuro),
    format('Custo estimado: ~w~n', H),
    sucessoresF(Custo:Estado:Caminho, Visitados, Sucessores),
    format('Sucessores: ~w~n', [Sucessores]),
    format('Visitados: ~w~n', [Visitados]),
    insereOrdenado(Sucessores, PossiveisAntes, Possiveis), % menor primeiro
    insereOrdenado(Sucessores, PossiveisAntes, Possiveis), % menor primeiro
    union([Estado], Visitados, NovoVisitados), % Visitados não são tentados de novo
    format('Possiveis: ~w~n~n', [Possiveis]),
    buscaH(Possiveis, NovoVisitados, P). % recursão

acao(vai(P, Q), P, Q, D):-
    via(P, Q, D).

acao(vai(Q, P), Q, P, D):-
    via(P, Q, D).

via(udia, arag, 37).
via(udia, ituiu, 137).
via(udia, prta, 84 ).
via(udia, uba, 105 ).
via(ituiu, capin, 36).
via(ituiu, gurin, 72).
via(gurin, cverde, 63 ).
%via(prta, cverde, 71).
via(uba, cflor, 74).
via(cflor, cverde, 134).
via(cflor, prta, 78 ).

h(Estado, H) :-
    meta(Meta),
    Estados = [udia, arag, ituiu, uba, capin, cflor, gurin, prta, cverde],
    D = [[  0, 30,120, 90,130, 90,150, 80,140],%udia
     [ 30,  0,130,120,140,120,170,100,150],%arag
     [120,130,  0,170, 30,120, 40, 60, 60],%ituiu
     [ 90,120,170,  0, 20, 60,190,100,150],%uba
     [130,140, 30, 20,  0,150, 60, 90, 90],%capin
     [ 90,120,120, 60,150,  0,130, 50, 90],%cflor
     [150,170, 40,190, 60,130,  0, 80, 40],%gurin
     [ 80,100, 60,100, 90, 50, 80,  0, 60],%prta
     [140,150, 60,150, 90, 90, 40, 60,  0]%cverde
    ],
    nth0(X, Estados, Estado),
    nth0(Y, Estados, Meta),
    nth0(X, D, Xlinha),
    nth0(Y, Xlinha, H),!.

insereOrdenado(NovasOpcoes, Caminhos, NovosCaminhos) :-
    append(NovasOpcoes, Caminhos, OpcoesCaminhos),
    sort(OpcoesCaminhos, NovosCaminhos), !.


  

