%
% There are two rooms connected by a door and two identical robots live in
% these rooms. Both robots can move freely in each room and between rooms.
% The robots can also push things around. They have little choice of items
% to push, because there only are two identical boxes and a chest. A chest
% is large and it can only be pushed by both robots. A box is small enough
% for one robot to be able to push it.

% In the initial situation, the chest and one robot are in the first room.
% The boxes and the other robot are in the second room.

% In the final situation, the boxes are in the first room and the chest is
% in the second room. The robots can be anywhere.
%

add(  at( robot(Z), P ),  goto1( P, _R, robot(Z) )  ).
add(  nextto( robot(Z), X ),  goto2( X, _R, robot(Z) )  ).
add(  nextto( box(B), Y ),
    push_box_to( box(B), Y, _R, robot(1) )  ).
add(  nextto( Y, box(B) ),
    push_box_to( box(B), Y, _R, robot(1) )  ).
add(  nextto( box(B), Y ),
    push_box_to( box(B), Y, _R, robot(2) )  ).
add(  nextto( Y, box(B) ),
    push_box_to( box(B), Y, _R, robot(2) )  ).
add(  inroom( robot(Z),  R2 ),
    gothrough1( _D, _R1, R2, robot(Z) )  ).
add(  inroom( robot(Z),  R2 ),
    push_box_through( box(_B), _D, _R1, R2, robot(Z) )  ).
add(  inroom( box(B),  R2 ),
    push_box_through( box(B), _D, _R1, R2, robot(1) )  ).
add(  inroom( box(B),  R2 ),
    push_box_through( box(B), _D, _R1, R2, robot(2) )  ).
add(  nextto( chest, Y ),  push_chest_to(chest,Y,_R)).
add(  nextto( Y, chest ),  push_chest_to(chest,Y,_R)).
add(  nextto( robot(1), chest),push_chest_to(chest,_Y,_R)).
add(  nextto( robot(2), chest),push_chest_to(chest,_Y,_R)).
add(  nextto( chest, robot(1)),push_chest_to(chest,_Y,_R)).
add(  nextto( chest, robot(2)),push_chest_to(chest,_Y,_R)).
add(inroom(robot(1),R2),push_chest_through(chest,_D,_R1,R2)).
add(inroom(robot(2),R2),push_chest_through(chest,_D,_R1,R2)).
add(inroom(chest,R2),push_chest_through(chest,_D,_R1,R2)).

del(  at( X, _Z ),  U  )    :-    moved( X, U  ).
del(  nextto( Z, robot(X) ),  U  )    :-
    !,   del(  nextto( robot(X), Z ),  U  ).
del(  nextto( robot(Z),  box(B) ),
    push_box_to( box(B), _Y, _R, robot(Z) )  )    :-    !,   fail.
del(  nextto( robot(Z),  box(B) ),
    push_box_through( box(B), _D, _R1, _R2, robot(Z) )  )   :-
	!,   fail.
del(  nextto( X, _Z ),  U  )    :-    moved(  X,  U  ).
del(  nextto( _Z, X ),  U  )    :-    moved(  X,  U  ).
del(  inroom( robot(Z), _Z ),
    gothrough1( _D, _R1, _R2, robot(Z) )  ).
del(  inroom( robot(Z), _Z ),
    push_box_through( box(_B), _D, _R1, _R2, robot(Z) )  ).
del(  inroom( box(B), _Z ),
    push_box_through( box(B), _D, _R1, _R2, robot(1) )  ).
del(  inroom( box(B), _Z ),
    push_box_through( box(B), _D, _R1, _R2, robot(2) )  ).
del(  nextto( robot(1),  chest ),
    push_chest_to( chest, _Y, _R )  )    :-    !,   fail.
del(  nextto( robot(2),  chest ),
    push_chest_to( chest, _Y, _R )  )    :-    !,   fail.
del(  nextto( robot(1),  chest ),
    push_chest_through( chest, _D, _R1, _R2 )  )   :-  !,   fail.
del(  nextto( robot(2),  chest ),
    push_chest_through( chest, _D, _R1, _R2 )  )   :- 	!,   fail.
del(  inroom( robot(1), _Z ),
    push_chest_through( chest, _D, _R1, _R2 )  ).
del(  inroom( robot(2), _Z ),
    push_chest_through( chest, _D, _R1, _R2 )  ).
del(  inroom( chest, _Z ),
    push_chest_through( chest, _D, _R1, _R2 )  ).

moved(  robot(Z),  goto1( _P, _R, robot(Z) )  ).
moved(  robot(Z),  goto2( _X, _R, robot(Z) )  ).
moved(  robot(Z),  push_box_to( box(_B), _Y, _R, robot(Z) )  ).
moved(  robot(Z),  gothrough1( _D, _R1, _R2, robot(Z) )  ).
moved(  robot(Z),
      pushthrough1( box(_B), _D, _R1, _R2, robot(Z) )  )  :-  !.
moved(  box(B),  push_box_to( box(B), _Y, _R, robot(1) )  ).
moved(  box(B),  push_box_to( box(B), _Y, _R, robot(2) )  ).
moved(  box(B),
      push_box_through( box(B), _D, _R1, _R2, robot(1) )  ).
moved(  box(B),
      push_box_through( box(B), _D, _R1, _R2, robot(2) )  ).
moved(robot(1),push_chest_to(chest,_Y,_R)).

moved(robot(2),push_chest_to(chest,_Y,_R)).
moved(chest,push_chest_to(chest,_Y,_R)).
moved(robot(1),push_chest_through(chest,_D,_R1,_R2)):-!.
moved(robot(2),push_chest_through(chest,_D,_R1,_R2)):-!.
moved(chest,push_chest_through(chest,_D,_R1,_R2)).

can(  goto1( P, R, robot(Z) ),
	locinroom( P, R )  &   inroom( robot(Z), R ) ).
can(  goto2( X, R, robot(Z) ),
	inroom( X, R )  &   inroom( robot(Z), R )  ).
can(  push_box_to( box(B), Y, R , robot(Z) ),
	notequal(Y, robot(Z) ) & inroom( Y, R )  &
	inroom( box(B), R )  &  nextto( robot(Z), box(B) )  ).
can(  gothrough1( D, R1, R2, robot(Z) ),
	connects( D, R1, R2 )  &  inroom( robot(Z), R1 )  &
	nextto( robot(Z), D )  ).
can(  push_box_through( box(B), D, R1, R2, robot(Z) ),
	connects( D, R1, R2 )  &
	inroom( box(B), R1 )  &  nextto( box(B), D )  &
	inroom( robot(Z), R1 )  &
	nextto( robot(Z), box(B) )  ).
can(  push_chest_to( chest, Y, R ),
    notequal( Y, robot(1) ) & notequal( Y, robot(2) )  &
    inroom( Y, R )  &  inroom( chest, R )  &
    nextto(robot(1), chest) & nextto( robot(2), chest )  ).
can(  push_chest_through( chest, D, R1, R2 ),
	connects( D, R1, R2 )  &
	inroom( chest, R1 )  & nextto( chest, D )  &
	inroom( robot(1), R1 ) & nextto( robot(1), chest) &
	inroom( robot(2), R1 )  & nextto( robot(2), chest )  ).

always(  inroom( D, R1 )  )    :-
    always(  connects( D, R1, _R2 )  ).
always(  connects( D, R1, R2 )  )    :-    connects1(  D, R1, R2  ).
always(  connects( D, R2, R1 )  )    :-    connects1(  D, R1, R2  ).
always(  locinroom(point(N),room(1))):-range(N,1,2).
always(  locinroom(point(N),room(2))):-range(N,3,5).

connects1(  door(1), room(1), room(2)  ).

range(  M, M, _  ).
range(  M, L, N  )    :-
	L  <  N,    L1  is  L + 1,    range(  M, L1, N  ).

imposs(  at( X, Y )  &  at( X, Z )  &  notequal( Y, Z )  ).

given(  strips1,  at( box(1), point(4) )  ).
given(  strips1,  at( box(2), point(5) )  ).
given(  strips1,  inroom( box(1), room(2) )  ).
given(  strips1,  inroom( box(2), room(2) )  ).
given(  strips1,  at( robot(1), point(1) )  ).
given(  strips1,  inroom(robot(1), room(1) )  ).
given(  strips1,  inroom(robot(2), room(2) )  ).
given(  strips1,  at( robot(2), point(3) )  ).
given(  strips1,  at( chest, point(2) )  ).
given(  strips1,  inroom(chest, room(1) )  ).

% A few tests.
p( X )  :-  plans( X, strips1 ).

wait :-  see( user ),  nl,  display( 'Press Enter to continue' ),
         rdch( _ ),    seen.


:-nl,write('***** TESTS *****'),nl,nl.
:-nl,write('Testing robot 1 movement'),nl.
:-p(at(robot(1),point(1))).
:-p(at(robot(1),point(2))).
:-p(at(robot(1),point(3))).
:-p(at(robot(1),point(4))).
:-p(at(robot(1),point(5))).
:- wait.
:-p(nextto(robot(1),box(1))).
:-p(nextto(robot(1),box(2))).
:-p(nextto(robot(1),chest)).
:-p(nextto(robot(1),door(1))).
:- wait.
:-nl,write('Testing robot 2 movement'),nl.
:-p(at(robot(2),point(1))).
:-p(at(robot(2),point(2))).
:-p(at(robot(2),point(3))).
:-p(at(robot(2),point(4))).
:-p(at(robot(2),point(5))).
:- wait.
:-p(nextto(robot(2),box(1))).
:-p(nextto(robot(2),box(2))).
:-p(nextto(robot(2),chest)).
:-p(nextto(robot(2),door(1))).
:-p(nextto(robot(1),robot(2))).
:-p(nextto(robot(2),robot(1))).
:- wait.
:-nl,write('Testing box movement'),nl.
:-p(nextto(box(1),box(2))).
:-p(nextto(box(2),box(1))).
:-p(nextto(chest,box(1))).
:- wait.
:-p(nextto(box(1),chest)).
:-p(nextto(box(2),chest)).
:-p(nextto(chest,box(2))).
:- wait.
:-p(nextto(box(1),door(1))).
:-p(nextto(door(1),box(1))).
:-p(nextto(box(2),door(1))).
:- wait.
:-p(nextto(door(1),box(2))).
:-p(inroom(box(1),room(1))).
:-p(inroom(box(2),room(1))).
:- wait.
:-nl,write('Testing chest movement'),nl.
:-p(nextto(chest,door(1))).
:-p(nextto(door(1),chest)).
:-p(inroom(chest,room(2))).
:- wait.
:-nl,write('Putting both boxes in room 1'),nl.
:-p(inroom(box(1),room(1)) & inroom(box(2),room(1))).
:- wait.
:-nl,write('Putting box 1 in room 1 and the chest in room 2'),nl.
:-p(inroom(box(1),room(1)) & inroom(chest,room(2))).
:- wait.
:-nl,write('---- Assignment Problem ----'),nl.
:-write('Swapping rooms of the boxes and the chest'),nl.
:-p(inroom(box(1),room(1)) & inroom(box(2),room(1)) &
    inroom(chest,room(2))).
:- wait.
:-nl,write('Swapping rooms of the boxes, the chest and the robots'),nl.
:-p(inroom(chest,room(2)) & inroom(box(1),room(1)) &
    inroom(box(2),room(1)) & inroom(robot(1),room(2)) &
    inroom(robot(2),room(1))).  %% LOOPS
:- wait.
:-nl,
  write('Putting the boxes and the chest in room 1 and the robots in room 2'),
  nl.
:-p(inroom(chest,room(1)) & inroom(box(1),room(1)) &
    inroom(box(2),room(1)) & inroom(robot(1),room(2)) &
    inroom(robot(2),room(2))).
:-nl,write('End testing'),nl.
end.
