%%%%%
% Hand in 2: tower of hanoi solved in a generic way (any number of disks)
% with prolog

:-use_module(library(lists)).

% start and goal states for 3 disks (In can be extended to any number of disks)


disknumber(3).

generate_ordered_list(0,[]).
generate_ordered_list(Limit,XS) :-
	Limit>0,LimitPred is Limit -1,generate_ordered_list(LimitPred,YS),append(YS,[Limit],XS).

ordered_disk_list(XS) :- disknumber(Y), generate_ordered_list(Y,XS).

start_state((XS,[],[])) :- ordered_disk_list(XS).

goal_state(([],[],XS)):- ordered_disk_list(XS).

% move the upper disk of the first peg to the second peg
next_child(([X|XS],YS,ZS),(XS,[X|YS],ZS)) :-
	not(unsafe((XS,[X|YS],ZS))).
% move the upper disk of the first peg to the third peg
next_child(([X|XS],YS,ZS),(XS,YS,[X|ZS])) :-
	not(unsafe((XS,YS,[X|ZS]))).

% move the upper disk of the second peg to the first peg
next_child((XS,[Y|YS],ZS),([Y|XS],YS,ZS)) :-
	not(unsafe(([Y|XS],YS,ZS))).
% move the upper disk of the second peg to the third peg
next_child((XS,[Y|YS],ZS),(XS,YS,[Y|ZS])) :-
	not(unsafe((XS,YS,[Y|ZS]))).

% move the upper disk of the third peg to the first peg
next_child((XS,YS,[Z|ZS]),([Z|XS],YS,ZS)) :-
	not(unsafe(([Z|XS],YS,ZS))).
% move the upper disk of the third peg to the second peg
next_child((XS,YS,[Z|ZS]),(XS,[Z|YS],ZS)) :-
	not(unsafe((XS,[Z|YS],ZS))).

% unsafe states
unsafe((X,Y,Z)) :- unsafe_peg(X);unsafe_peg(Y);unsafe_peg(Z).
% unsafe peg
unsafe_peg([X,Y|XS]) :- X>Y;unsafe_peg(XS).

% heuristic function
h((_,_,X),N) :- disknumber(D),sum(X,Y),N is (D*(D+1))/2-Y.
sum([],0). 
sum([X|XS],Y) :- sum(XS,Z),Y is Z+X.
	
not(X) :-
         call(user:X), !,
         fail.
not(_).

%%%%%%%%%%%%%%%
% Depth first
%
%
%

depth_first(Result) :-
	start_state(S),
	depth_first([S],[],Result).

depth_first([],_,_) :- fail.
depth_first([FirstOpen|_],Closed,Result) :- goal_state(FirstOpen),reverse([FirstOpen|Closed],Result).
depth_first([X|RestOpen],Closed,Result) :-
	bagof(Y,next_child(X,Y),Children),
	new_open(Children,RestOpen,Closed,NewOpen),
	depth_first(NewOpen,[X|Closed],Result).

new_open([],Open,_,Open).	
new_open([First|RestChildren],Open,Closed,NewOpen) :-
	not(member(First,Open)),
	not(member(First,Closed)),
	new_open([RestChildren],Open,Closed,[First|NewOpen]).
	   

%%%%%
% Best first sök algoritmen.
% Använder sig av heuristik (manhattan distance) för att rangordna olika stigar.
% Stigen med lägst heuristiskvärde evauleras expanderas.
% Detta sker till målet är nått.
%%%%%
:-use_module(library(lists)).

best_first(Result):-
	start_state(S),
	best_first([[S]],Result).
best_first([[X|Restofpath]|_],Result):-
	goal_state(X),
	reverse([X|Restofpath],Result).
best_first([Current_path|Paths],Result):-
	expand_paths(Current_path, Set_of_expansions),
	append(Paths,Set_of_expansions,Newpaths),
	sort_paths_based_on_h(Newpaths,Sortedpaths),
	best_first(Sortedpaths, Result).

expand_paths([X|Restofpath],Expanded_paths):-
	bagof(Y,next_child(X,Y),Children),
	expand_all(Children,[X|Restofpath],Expanded_paths).

expand_all([],_,[]). 
expand_all([Current|Children],Path,[[Current|Path]|Expanded_paths]):-
	not(member(Current, Path)),!,
	expand_all(Children, Path, Expanded_paths).
expand_all([_|Children],Path,List):-
	expand_all(Children,Path,List).

sort_paths_based_on_h(Newpaths,Sortedpaths):-
	sort_paths(Newpaths,[],[(Score,P)|Rest]),
	write(Score),write(' '),flush_output,
	remove_scores_from_paths([(Score,P)|Rest], Sortedpaths).

sort_paths([],Sorted, Sorted).
sort_paths([[State|Restofpath]|Newpaths],Alreadysorted,Sortedpaths):-
	h(State,Score),
	insert((Score,[State|Restofpath]),Alreadysorted,Sorted),
        sort_paths(Newpaths,Sorted,Sortedpaths).

insert((X,Y),[],[(X,Y)]).
insert((Score, Path), [(S, P)|Alreadysorted],[(Score,Path)|[(S,P)|Alreadysorted]]):-
	Score =< S.
insert(Newpair,[Pair|Alreadysorted],[Pair|Sorted]):-
           insert(Newpair,Alreadysorted,Sorted).

remove_scores_from_paths([],[]).
remove_scores_from_paths( [(_,P)| Pathswithscores],[P|Paths]):-
	remove_scores_from_paths(Pathswithscores, Paths).