:- dynamic obj/2.
:- dynamic carries/1.

:- table blocked/1.
:- table gap/2.
:- table alley/1.
:- table pcp/1.
:- table rtl/1.
:- table rbl/1.
:- table rtr/1.
:- table rbr/1.
:- table rect/1.
:- table contains/2.
:- table room/1.
:- table v/1.
:- table e/2.
:- table distToAgent/2.
:- table nextOnPath/1.

blocked(XY) :- obj(wall(_),XY).
blocked(XY) :- obj(lava,XY). 
gap((X,Y2),horizontal) :- 
	blocked((X,Y1)), 
	Y2 is Y1+1, \+ blocked((X,Y2)), 
	Y3 is Y2+1, blocked((X,Y3)).
gap((X2,Y),vertical) :- 
	blocked((X1,Y)), 
	X2 is X1+1, \+ blocked((X2,Y)), 
	X3 is X2+1, blocked((X3,Y)).

alley((X,Y)) :- 
	gap((X,Y),horizontal), X1 is X+1, gap((X1,Y), horizontal). 
alley((X,Y)) :- 
	gap((X,Y),horizontal), X1 is X-1, gap((X1,Y), horizontal). 
alley((X,Y)) :- 
	gap((X,Y),vertical), Y1 is Y+1, gap((X,Y1), vertical). 
alley((X,Y)) :- 
	gap((X,Y),vertical), Y1 is Y-1, gap((X,Y1), vertical). 

pcp(U) :- blocked(U).
pcp(U) :- gap(U,_), \+ alley(U).

rtl((X,Y)) :- 
	pcp((X,Y)), X1 is X+1, pcp((X1,Y)), Y1 is Y+1, pcp((X,Y1)).
rbl((X,Y)) :- 
	pcp((X,Y)), X1 is X+1, pcp((X1,Y)), Y1 is Y-1, pcp((X,Y1)).
rtr((X,Y)) :- 
	pcp((X,Y)), X1 is X-1, pcp((X1,Y)), Y1 is Y+1, pcp((X,Y1)).
rbr((X,Y)) :- 
	pcp((X,Y)), X1 is X-1, pcp((X1,Y)), Y1 is Y-1, pcp((X,Y1)).

rect([[X1,Y1], [X2,Y2]]) :- 
	rtl((X1,Y1)), rbl((X1,Y2)), 
	rtr((X2,Y1)), rbr((X2,Y2)), 
	X2-X1>0, Y2-Y1>0.
contains([[X11,Y11],[X12,Y12]],[[X21,Y21],[X22,Y22]]) :- 
	rect([[X11,Y11],[X12,Y12]]), rect([[X21,Y21],[X22,Y22]]), 
	\+ [[X11,Y11],[X12,Y12]]=[[X21,Y21],[X22,Y22]], 
	X21>=X11, Y21>=Y11, X12>=X22, Y12>=Y22.
room(R) :- rect(R), \+ contains(R,_).

v(U) :- obj(door(_,_), U).
v(U) :- obj(goal, U).
v(U) :- obj(agent(_), U).
v(U) :- obj(key(_), U).
v(U) :- obj(ball(C), U), \+ C=blue.
v(U) :- gap(U,_), \+ alley(U), \+ obj(door(_,_), U).

inRoom((X,Y),[[X1,Y1],[X2,Y2]]) :- X>=X1, Y>=Y1, X2>=X,Y2>=Y.
e(U,V) :- v(U), v(V), \+ U=V, room(R), inRoom(U,R), inRoom(V,R).

% Use breath-first search to find the shortest path
bfs([[XY|Path]|_],[XY|Path]) :- obj(goal,XY).
bfs([[Current|Path]|Queue],PathToGoal) :-
	 findall(Next,e(Current,Next),ChildNodes),
	 nodesSortedByDist(ChildNodes,SortedChildNodes),
	 childPaths([Current|Path],SortedChildNodes,ChildPaths),
	 removeLockedDoors(ChildPaths,FilteredChildPaths),
	 append(Queue,FilteredChildPaths,NewQueue),
	 bfs(NewQueue,PathToGoal).
childPaths(_,[],[]).
childPaths(Path,[Next|Other],[[Next|Path]|NewPaths]) :-
		childPaths(Path,Other,NewPaths).

% Filter out locked doors and with no fitting key on the path.
keysOnPath([],[]).
keysOnPath([U|Us],[key(Color)|Keys]) :- 
	obj(key(Color),U), keysOnPath(Us,Keys).
keysOnPath([U|Us],Keys) :- 
	\+ obj(key(_),U), keysOnPath(Us,Keys).
removeLockedDoors([],[]).
removeLockedDoors([[U|Us]|RestOfPath],[[U|Us]|Filtered]) :-
	\+ obj(door(_,locked),U),
	removeLockedDoors(RestOfPath,Filtered).
removeLockedDoors([[U|Us]|RestOfPath],[[U|Us]|Filtered]) :-
	obj(door(Color,locked),U),
	carries(key(Color)),
	removeLockedDoors(RestOfPath,Filtered).
removeLockedDoors([[U|Us]|RestOfPath],[[U|Us]|Filtered]) :-
	obj(door(Color,locked),U),
	\+ carries(key(_)),
	keysOnPath(Us,Keys),
	member(key(Color),Keys),
	removeLockedDoors(RestOfPath,Filtered).
removeLockedDoors([[U|Us]|RestOfPath], Filtered) :-
	obj(door(Color,locked), U),
	\+ carries(key(Color)),
	keysOnPath(Us, Keys),
	\+ member(key(Color),Keys),
	removeLockedDoors(RestOfPath,Filtered).

% Sort children by their distance to the agent
dist((X1,Y1),(X2,Y2),D) :- D is abs(X2-X1)+abs(Y2-Y1).
distToAgent(U,D) :- obj(agent(_),V),dist(U,V,D).
distList([],[]).
distList([V|Vs],[D-V|Result]) :- 
	distToAgent(V,D),distList(Vs,Result).
nodesSortedByDist(Vs,Sorted) :-
	distList(Vs,DistMap),
	keysort(DistMap,SortedWithKey),
	pairs_values(SortedWithKey,Sorted).

% Choose the first valid path to the goal. 
% BFS guarantees that it is the shortest one.
nextOnPath(U) :- p([_|[U|_]]).
p(PathToGoal) :- 
	obj(agent(_),U), 
	bfs([[U]],PathToGoalReverse), 
	reverse(PathToGoalReverse,PathToGoal), 
	!.

adj((X1,Y)) :- obj(agent(_),(X,Y)), X1 is X+1.
adj((X1,Y)) :- obj(agent(_),(X,Y)), X1 is X-1.
adj((X,Y1)) :- obj(agent(_),(X,Y)), Y1 is Y+1.
adj((X,Y1)) :- obj(agent(_),(X,Y)), Y1 is Y-1.

fronting((X,Y1)) :- obj(agent(north),(X,Y)), Y1 is Y-1.
fronting((X,Y1)) :- obj(agent(south),(X,Y)), Y1 is Y+1.
fronting((X1,Y)) :- obj(agent(east),(X,Y)), X1 is X+1.
fronting((X1,Y)) :- obj(agent(west),(X,Y)), X1 is X-1.

dangerous(XY) :- obj(ball(blue), XY).
dangerous(XY) :- obj(lava, XY).

xdir(east) :- obj(agent(_),(AX,_)), nextOnPath((GX,_)), GX>AX.
xdir(west) :- obj(agent(_),(AX,_)), nextOnPath((GX,_)), AX>GX.
xdir(on_axis) :- obj(agent(_),(AX,_)), nextOnPath((GX,_)), AX=GX.
	
ydir(north) :- obj(agent(_),(_,AY)), nextOnPath((_,GY)), AY>GY.
ydir(south) :- obj(agent(_),(_,AY)), nextOnPath((_,GY)), GY>AY.
ydir(on_axis) :- obj(agent(_),(_,AY)), nextOnPath((_,GY)), AY=GY.

touching(goal) :- nextOnPath(G), adj(G), obj(goal,G).
touching(key) :- nextOnPath(G), adj(G), obj(key(_),G).
touching(door(S)) :- nextOnPath(G), adj(G), obj(door(_,S),G).
touching(gap) :- nextOnPath(G), adj(G), gap(G,_), \+ alley(G), 
	\+ touching(goal), \+ touching(door(_)), \+ touching(key).
touching(none) :- \+ touching(goal), \+ touching(door(_)), 
	\+ touching(gap), \+ touching(key).

in_gap(D) :- obj(agent(_),XY), gap(XY,D), \+ alley(XY).
in_gap(none) :- \+ in_gap(horizontal), \+ in_gap(vertical).