% This is a prolog implementation of the Blocks World Domain
% Shapes: cubes, pyramids, triangles, boxes, and rectangles
% Colors: red, green, blue
% Position: is determined by block middle point (centre) in 3D space (X,Y,Z).
% all block sizes must be even!
% All blocks rest either on the table or on other blocks.
% Boxes are always placed with the empty side facing upwards
%----------------------------------------------------------------------------------
% SHAPE, COLOR, POSITION, TOP, BOTTOM, HEIGHT

% shape(+Identifier, ?Shape)
%shape(b0, rectangle).
shape(b1, cube).
shape(b2, cube).
shape(b3, rectangle).
shape(table, cube).

% color(+Identifier, ?Color)
%color(b0, blue).
color(b1, red).
color(b2, blue).
color(b3, green).

% bottom(+B,W,L) is true if bottom rectangle of block B has width W and length L
%bottom(b0,2,2).
bottom(b1,2,2).
bottom(b2,2,2).
bottom(b3,2,2).
bottom(table,8,8).

% top(+B,W,L) is true if top rectangle of block B has width W and length L
top(B, W, W):-bottom(B,W,W), shape(B, cube).
top(B, W, L):-bottom(B,W,L), shape(B, rectangle).
top(B, 0, L):-bottom(B,_,L), shape(B, triangle).
top(B, 0, 0):-shape(B, pyramid).

% height(+B, ?H)
%height(b0,2).
height(b1,2).
height(b2,2).
height(b3,2).
height(table,2).

% position(+B,+PX,+PY,+PZ) is true if middle point of block B
% note: x and y are plane coordinates on the table (0 - 30), z is the height (0-200)
:- dynamic position/4.

position(b1,1,1,1).
position(b2,1,1,3).
position(b3,1,1,5).
position(table,3,3,-1).
%----------------------------------------------------------------------------------

% on(+B,?B1) true if block B is supported (directly or indirectly) by B1

on(B1,B2):-	shape(B1,_),
		shape(B2,_),
		B1\==B2,
%		writeln([B1,B2]), 
		height(B1,H1),
		height(B2,H2),
		position(B1,X1,Y1,Z1), % middle point of B1
		position(B2,X2,Y2,Z2), % middle point of B2
		Z3 is Z1-H1/2,	% bottom of B1
		Z4 is Z2+H2/2,	% top of B2
		Z5 is Z2-H2/2,	% bottom of B2
		rests_on(B1,B2,Z3,Z4,Z5,X1,Y1,X2,Y2).

% rests_on(+B) is a helper for on/2 returning true if block's B1 bottom Z coordinate is equal to block B2 top OR bottom Z coordinate, 
% AND the bottom rectangle of B1 intersects B2's top rectangle

rests_on(B1,B2,BottomB1,TopB2,_,X1,Y1,X2,Y2):- 	BottomB1==TopB2,
						bottom(B1,W1,L1), % B1's bottom rectangle 
						top(B2, W2,L2),   % B2's top rectangle
						BLX1 is X1 - W1/2, BLY1 is Y1 - L1/2,
						BLX2 is X2 - W2/2, BLY2 is Y2 - L2/2,
						TRX1 is X1 + W1/2, TRY1 is Y1 + L1/2,
						TRX2 is X2 + W2/2, TRY2 is Y2 + L2/2,
						intersect(BLX1,BLY1,TRX1,TRY1,BLX2,BLY2,TRX2,TRY2).

rests_on(B1,B2,BottomB1,_,BotB2,X1,Y1,X2,Y2):- 	BottomB1==BotB2,
						bottom(B1,W1,L1), % B1's bottom rectangle
						bottom(B2,W2,L2),   % B2's bottom rectangle
						BLX1 is X1 - W1/2, BLY1 is Y1 - L1/2,
						BLX2 is X2 - W2/2, BLY2 is Y2 - L2/2,
						TRX1 is X1 + W1/2, TRY1 is Y1 + L1/2,
						TRX2 is X2 + W2/2, TRY2 is Y2 + L2/2,
						intersect(BLX1,BLY1,TRX1,TRY1,BLX2,BLY2,TRX2,TRY2).
				

% intersect (BLX1, BLY1, TRX1, TRY1, BLX2, BLY2, TRX2, TRY2) is true if the rectangles specified by the given corner points intersect
intersect(X1,Y1,X2,Y2,X3,Y3,_,_):- pInRect(X3,Y3, X1,Y1, X2, Y2),!.
intersect(X1,Y1,X2,Y2,_,_,X4,Y4):- pInRect(X4,Y4, X1,Y1, X2, Y2),!.
intersect(X1,Y1,_,_,X3,Y3,X4,Y4):- pInRect(X1,Y1, X3,Y3, X4, Y4),!.
intersect(_,_,X2,Y2,X3,Y3,X4,Y4):- pInRect(X2,Y2, X3,Y3, X4, Y4).


% pInRect(X0,Y0,X1,Y1,X2,Y2) is a helper predicate used by intersect to determine if  a point (X0, Y0) 
% is in rectangle specified by bottom left corner (X1, Y1) and top right corner (X2, Y2)
pInRect(X0,Y0,X1,Y1,X2,Y2):- X0>=X1, X0=<X2, Y0>=Y1, Y0=<Y2.


% on_list(+B, ?L) is true if B is supported by each block in list L. Note, on_list(B,[]) is true if B rests on the table
on_list(B,L):-shape(B,_), findall(B2,on(B,B2),L).


% volume (+B,?V) returns volume for block B, calculated based on height, and attributes by a unique for each shape
volume(Block,Volume):-	height(Block, Height), 
			bottom(Block, Width, Length),
			Volume is Height*Width*Length.

% pickupBlock(+B) is true if block identified by B is the topmost block in a stack
% if B is not part of any list of supporting blocks
% this predicate is needed by the STRIPS pickup. It is true only if a certain block can be picked up:
% if there is nothing on it and there is nothing ALREADY picked up by the  hand
pickupBlock(B):- shape(B,_),\+ on(_,B), \+ position(_,_,_,1000).

% dropBlock(+B,+Z) is true if block B can be dropped, and Z is the new position (vertical).
dropBlock(B,Z):-	shape_list(L), 
			dropBlockHelper(L,B,0,Z).


dropBlockHelper([],B, CurrentMax,CurrentMax).

% B is the picked up block.. B1 is the one in the list..
dropBlockHelper([B1|Bs],B2,CurrentMax,Z):- B1==B2,!,writeln([0,B1]),dropBlockHelper(Bs,B2,CurrentMax,Z).
dropBlockHelper([B1|Bs],B2,CurrentMax,Z):- 	
						height(B2,H2),
						height(B1,H1),
						position(B1,X1,Y1,Z1), % middle point of B1
						position(B2,X2,Y2,Z2), % middle point of B2
						Z4 is Z1+H1/2,	% top of B1
						Z5 is Z1-H1/2,	% bottom of B1

						ZTemp is Z1+H1/2+H2/2,   % possible Z position to drop to.						
						writeln([5,ZTemp]),
						Z3 is ZTemp-H2/2,   	% bottom rectangle Z position of hanging block

						(
						  ( rests_on(B2,B1,Z3,Z4,Z5,X2,Y2,X1,Y1),!,
		 				  ZMax is max(ZTemp,CurrentMax),
						  writeln([1,B1,ZMax]),
						  dropBlockHelper(Bs,B2,ZMax,Z));

						  writeln([2,B1,CurrentMax]),
						  dropBlockHelper(Bs,B2,CurrentMax,Z)).

% shape_list(L) is true if L is the list of all blocks 
shape_list(L):-findall(X,shape(X,_),L).

% num_gen(Max,X) generates numbers from 0 to Max (helper for ValidPos)
num_gen(M,M).
num_gen(M,X) :- M1 is M - 1, M1 >= 0, num_gen(M1,X).

% validPos(X,Y) is true if X,Y represent coordinates that fall inside the table coordinates
validPos(X,Y):-bottom(table,MX,MY),num_gen(MX,X), num_gen(MY,Y).

%------------------------------------------------------------------------------------
% BIG, SMALL, MEDIUM
%------------------------------------------------------------------------------------
% size(+B,?S) is true if B has S size (big small or medium)
% size(B,big) is true if block's volume is bigger than 100 (for instance cube: 5*5*5);
% size(B,medium) is true if block's volume is between than 20 and 100 (example: box [3x3x3])
% size(B,small) is true if block's volume is smaller than 20 (example: rectangle (2x2x2))

size(B,small):-volume(B,V), V<20.
size(B,medium):-volume(B,V), V>=20, V=<100.
size(B,big):-volume(B,V), V>100.
%-----------------------------------------------------------------------------------

% arch(L) is true if L is a list containing some blocks that create an arch
arch(L):- 	member(T,L), member(S1,L), member(S2,L), 
		T\==S1, T\==S2, S2\==S1, 
		on(T,S2), on(T,S1),dist(S1,S2).

% dist(B1,B2) is true if B1 and B2 do not touch each other sides
dist(B1,B2):-  position(B1,X1,Y2,_), position(B2,X2,Y2,_),
		bottom(B1,W1,L1), 				% B1's bottom rectangle
		bottom(B2,W2,L2),   				% B2's bottom rectangle
		BLX1 is X1 - W1/2, BLY1 is Y1 - L1/2,
		BLX2 is X2 - W2/2, BLY2 is Y2 - L2/2,
		TRX1 is X1 + W1/2, TRY1 is Y1 + L1/2,
		TRX2 is X2 + W2/2, TRY2 is Y2 + L2/2,
		\+ intersect(BLX1,BLY1,TRX1,TRY1,BLX2,BLY2,TRX2,TRY2).


%--------------------------------------------------------------------------------------

% STRIPS

strips(pickup(B), 
	[pickupBlock(B)],
	[position(B,X,Y,_)],
	[position(B,X,Y,1000)]).
strips(move(B,X,Y), 
	[shape(B,_),position(B,_,_,1000),validPos(X,Y)],
	[position(B,_,_,1000)],
	[position(B,X,Y,1000)]).
strips(drop(B,Z), 
	[shape(B,_),position(B,X,Y,1000),dropBlock(B,Z)],
	[position(B,_,_,1000)],
	[position(B,X,Y,Z)]).