(set-goal-type 'extended)

;;;
;;; ---------------------------------------------|
;;; |r1        |r2        |r7        |r8         |
;;; |          -          -          -           |
;;; |                                            | 
;;; |          -          -          -           |
;;; |          |          |          |           |
;;; |---|  |---|----|  |--|----| |---|----|  |----
;;; |r3        |r4        |r9        |r10        |
;;; |          |          |          |           |
;;; |          |          |          |           | 
;;; |          |          |          |           |
;;; |          |          |          |           |
;;; |---|  |---|----|  |--|----| |---|-----------|
;;; |r5        |r6        |r11       |r12        |
;;; |          -          |          -           |
;;; |                     |                      | 
;;; |          |          |          |           |
;;; |          |          |          |           |
;;; ---------------------------------------------|
;;; 
;;; state0 is the left half of the structure.
;;; state1 is the whole thing. You must add objects as
;;; given in the initial states. Room sizes are as
;;; specified below. The origin (0 0) is the lower left
;;; corner.
;;; 


(define (origin room)
  "Origin of room"
  (case room
    (r1 '(0 14)) (r2 '(4 14)) (r3 '(0 6)) (r4 '(4 0)) (r5 '(0 0))
    (r6 '(4 0)) (r7 '(12 14)) (r8 '(20 14)) (r9 '(12 6)) (r10 '(20 6))
    (r11 '(12 0)) (r12 '(20 0))))

(define (height room)
  "Height of room in some unit (tileworld cell)"
  (case room
    ((r1 r2 r7 r8 r3 r4 r9 r10) 8)
    (else 6)))

(define (width room)
  "Width of room in some unit (tileworld cell)"
  (case room
    ((r1 r3 r5 r8 r10 r12) 4)
    (else 8)))

(define state0 '((connect d11 r1 r2) (connect d11 r2 r1) 
	       (connect d13 r1 r3) (connect d13 r3 r1) 
	       (connect d24 r2 r4) (connect d24 r4 r2) 
	       (connect d35 r3 r5) (connect d35 r5 r3) 
	       (connect d46 r4 r6) (connect d46 r6 r4) 
	       (connect d56 r5 r6) (connect d56 r6 r5) 
	     
	       (opened d11) (opened d13) (opened d24)
	       (opened d35) (opened d46) (opened d56) 
	       
	       (in robot r2) (in o1 r1) (in o2 r2) (in o3 r3) 
	       (in o4 r4) (in o5 r5) (in o6 r1) (in o7 r3) (in o8 r2) 
	       (handempty)))

(define state1 '((connect d11 r1 r2) (connect d11 r2 r1) 
	       (connect d13 r1 r3) (connect d13 r3 r1) 
	       (connect d24 r2 r4) (connect d24 r4 r2) 
	       (connect d35 r3 r5) (connect d35 r5 r3) 
	       (connect d46 r4 r6) (connect d46 r6 r4) 
	       (connect d56 r5 r6) (connect d56 r6 r5) 
	       (connect d27 r2 r7) (connect d27 r7 r2) 
	       (connect d78 r7 r8) (connect d78 r8 r7) 
	       (connect d79 r7 r9) (connect d79 r9 r7) 
	       (connect d810 r8 r10) (connect d810 r10 r8) 
	       (connect d911 r9 r11) (connect d911 r11 r9) 
	       (connect d1112 r11 r12) (connect d1112 r11 r12) 
	       
	       (opened d11) (opened d13) (opened d24) (opened d35) 
	       (opened d46) (opened d56) (opened d27) (opened d78)
	       (opened d79) (opened d810) (opened d911) (opened d1112) 
	       
	       (in robot r2) (in o1 r1) (in o2 r2) (in o3 r3) 
	       (in o4 r4) (in o5 r5) (in o6 r1) (in o7 r3) (in o8 r2)
               (handempty)))


;;; goals for state0
(define goal0 '(t-eventually (< 0 20 >) (in robot r5)))
(define goal1 '(t-eventually (< 0 20 >) (in robot r3)))

;;; goals for state1
(define goal2 '(t-eventually (< 0 20 >) (in robot r8)))
(define goal3 '(t-eventually (< 0 90 <) 
			   (and (in robot r2) (in o1 r2) (in o3 r5))))
(define goal4 '(t-eventually (< 0 90 <) 
			   (and (in robot r12) (in o1 r2) (in o3 r5))))

;;; safety constraints for goals 2 to 4.
;;; a) when in room 1, never stay there more than 2 time units and 
;;; don't come back there before 20 time units have elapsed.
(define safety-room
      '(always (implies (in robot r1)
			(t-always ( < 2 10 < ) (not (in robot r1))))))

;;; b) Doorway d27 is only for access from room 2.
;;; Doorway d78 is only for access from room 7.
(define safety-door 
      '(always 
	(and (implies (in robot r5) (next (not (in robot r3))))
	     (implies (in robot r7) (next (not (in robot r2))))
	     (implies (in robot r8) (next (not (in robot r7)))))))

;;; control rule for just moving the robot from one room
;;; to another with the arm empty. JUST an illustration.
;;; It does not cover all the possible prunings.
(define search-control0 
      '(always (and (forall (?d ?r1 ?r2) (connect ?d ?r1 ?r2)
		      (implies (opened ?d) (next (opened ?d))))
		    ;; never grasp irrelevant objects
		    (forall (?r) (in robot ?r) 
		      (forall (?o) (in ?o ?r)
			(next (not (holding ?o))))))))

;;; Control rule for goals 3 and 4.
;;; JUST for illustration. It does not cover all the possible prunings.
;;; Also, see the comments below.

(define search-control1 
      '(always (and (forall (?d ?r1 ?r2) (connect ?d ?r1 ?r2)
		      (implies (opened ?d) (next (opened ?d))))
		    ;; maintain goal object until reaching its room
		    (implies (holding o1) (until (holding o1) (in o1 r2)))
		    (implies (holding o3)(until (holding o3) (in o3 r5)))
		    ;; release goal object in its room, and
		    ;; henceforth never grasp it again
		    (implies (and (holding o1) (in o1 r2))
			     (next (and (handempty) 
					(always (not (holding o1))))))
		    (implies (and (holding o3) (in o3 r5))
			     (next (and (handempty) 
					(always (not (holding o3))))))
		    ;; never grasp irrelevant objects
		    (forall (?r) (in robot ?r) 
		      (forall (?o) (in ?o ?r)
			(implies (and (not (= ?o o1))	
				      (not (= ?o o3)))
				 (next (not (holding ?o)))))))))

;;Heuristic function for best-first strategy
(define (h-reach-room-fn room)
  (lambda(plan)
    (let*((world (plan-world plan))
	  (facts (predicate-instances 'in world))
	  (current-room (third (find 'robot facts :key second))))
      (distance current-room room))))

(define (predicate-instances pred-name world)
  "Returns all the literals that are instances of predicate pred-name 
   in world."
  (let*((i (get-symbol-index pred-name)))
    (if i (arg-tree-to-list pred-name  (get-symbol-args world i)))))

;;; 
;;; Note: the above search control formulas and heuristic functions
;;; could be made more general (problem-independent, but still domain
;;; dependent) if we had the equivalent of "goal-world" for 
;;; temporally-extended goals.
;;; 
;;; I made tests with the following states and goal combinations:
;;; 
;;; 1. State0, goal0, goal1 and goal2 are merely for tests.
;;;   On the one hand state0 involves few rooms and the goals 
;;;   are just to move the robot from one room to another.
;;;   I wanted to stress the desirability of being able to exploit
;;;   room distance as heuristic as euclidean-distance is exploited
;;;   in motion planning with A* search.
;;; 
;;;   To have the same experiments as mine, first 
;;;      (set-initial-world state0).
;;;   Then choose among(set-goal goal0) or (set-goal goal1).
;;; 
;;;   I experimented the following search strategies:
;;;       -- depth-first-priority
;;;       -- best-first with default cost-fn
;;;       -- best-first with cost-fn set to 
;;;          (a* (h-reach-room-fn room)), with room the goal room.
;;;         
;;;         For example, for goal0, you will have:
;;;          (a* (h-reach-room-fn 'r5))
;;;         because you want the robot to reach room 4.
;;; 
;;;       In general, best-first with this last cost fn performs better
;;;       and produces optimal plans. Depth-first is not so bad because
;;;       the state space is quite small.
;;; 
;;;    In addition, you can conjoin goal0 or goal1 with 
;;;    the control formula search-control0 as follows:
;;;       (set-goal `(and ,goal0 ,search-control0)) or
;;;       (set-goal `(and ,goal1 ,search-control0)) 
;;; 
;;;    Whatever search-strategy you are using this is faster.
;;; 
;;; 2) State1 illustrates the complexity of adding more rooms and 
;;;    objects.
;;; 
;;;    For state1, you can try the same goals and heuristic as
;;;    before. But, the goals goal3 and goal4 are more interesting.
;;; 
;;;    Each goal can be conjoined with safety-door.
;;;    Furthermore, you can add search-control1 to illustrate the
;;;    power of prunning.
;;; 
;;;    For goal3 and goal4, the cost-fn is the default one.
;;; 
