;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Due to a problem I have with macros in MIT Scheme, I have to
;;; put them temporally in every domain definition file

(define-macro (def-defined-predicate name parameters formula)
  `(insert-symbol-info
    ',name 'PREDICATE ,(length parameters) 'DEFINED
    (list ',parameters ',formula)))

(define-macro (def-computed-fn name parameters . body)
  `(begin
     (define (,name ,@parameters) ,@body)
     (define-computed-function ',name ,name ,(length parameters))))

(define-macro (def-computed-predicate name parameters . body)
  `(begin
     (define (,name ,@parameters) ,@body)
     (define-computed-predicate ',name ,name ,(length parameters))))

(define-macro (def-generator name parameters . body)
  `(begin
     (define (,name ,@parameters) ,@body)
     (define-computed-generator ',name ,name ,(length parameters))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;                                                                       ;;;
;;; blocksworld.scm
;;; New code to set up the blocks world---we don't need irrelevant towers!!
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Setup Symbols.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(clear-world-symbols)                   ;Remove old domain symbols
(clear-ops)                             ;Remove old operators

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 1. Define the described symbols.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(def-described-symbols
    '((on        predicate 2)
      (holding   predicate 1)
      (handempty predicate 0)
      (clear     predicate 1)
      (ontable   predicate 1)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 2. The defined predicates.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Goodtower
;;; ?x is a goodtower if it is on top of a tower and none of the
;;; on-relationships in the tower violate a goal condition and it is not an
;;; irrlevant tower.
;;; To test the on-relationships we need to test both what should be below ?x
;;; and what should be above the block it is below.
;;; Tue Mar 21 10:08:42 1995: Fix pointed out by David Etherington.
;;; If you want to be holding a non-clear block in a tower the tower is bad.
;;;

(def-defined-predicate goodtower (?x)
  (and (clear ?x)
       (goodtowerbelow ?x)))

(def-defined-predicate goodtowerbelow (?x)
  (or (and (ontable ?x) (not (goal (exists (?y) (on ?x  ?y)))))
      (exists (?y) (on ?x ?y)
              (and
               (not (goal (ontable ?x)))
               (not (goal (clear ?y)))
               (not (goal (holding ?y)))
               (goal (forall (?z) (on ?x ?z) (= ?z ?y)))
               (goal (forall (?z) (on ?z ?y) (= ?z ?x)))
               (goodtowerbelow ?y)))))

;;; ?x has a goodtowerabove if the on-relationships above it do not
;;; violate any goal conditions. In this case ?x does not have to be
;;; on top of the tower. We cannot be holding it.
;;;
;;; Tue Mar 21 10:13:06 1995: Same Fix pointed out by DE.

(def-defined-predicate goodtowerabove (?x)
  (or (clear ?x)
      (exists (?y) (on ?y ?x)
              (and
              (not (goal (clear ?x)))
              (not (goal (holding ?x)))
              (not (goal (ontable ?y)))
              (goal (forall (?z) (on ?z ?x) (= ?z ?y)))
              (goal (forall (?z) (on ?y ?z) (= ?z ?x)))
              (goodtowerabove ?y)))))

;;; ?x is a badtower if it is not good.

(def-defined-predicate badtower (?x)
  (and (clear ?x)
       (not (goodtower ?x))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 3. The temporal control formula.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;; The three types of control described in the paper 

(define no-control '(always (true)))

(define bw-control1
  '(always
    (forall (?x) (clear ?x)
     (implies (goodtower ?x) (next (goodtowerabove ?x))))))

(define bw-control2
  '(always
    (forall (?x) (clear ?x)
     (and
      (implies (goodtower ?x) (next (goodtowerabove ?x)))
      (implies (badtower ?x) (next (not (exists (?y) (on ?y ?x)))))))))

(define bw-control3
  '(always
    (forall (?x) (clear ?x)
     (and
      (implies (goodtower ?x) (next (goodtowerabove ?x)))
      (implies (badtower ?x) (next (not (exists (?y) (on ?y ?x)))))
      (implies (and (ontable ?x)
                    (exists (?y) (goal (on ?x ?y)) (not (goodtower ?y))))
       (next (not (holding ?x))))))))

(define bw-control4
  '(always
    (forall (?x) (clear ?x)
     (and
      (implies (goodtower ?x) (next (goodtowerabove ?x)))
      (implies (badtower ?x) (next (not (exists (?y) (on ?y ?x)))))
      (implies (and (ontable ?x)
                    (exists (?y) (goal (on ?x ?y)) (not (goodtower ?y))))
	       (next (not (holding ?x))))
      (implies (goodtower ?x)
	       (forall (?y) (holding ?y)
		       (implies (goal (on ?y ?x))
				(next (on ?y ?x)))))))))

;;; Use control3 by default

(set-tl-control bw-control3)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 4. Operators
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
     
(add-strips-op 
 :name '(pickup ?x)
 :pre  '((handempty) (clear ?x) (ontable ?x))
 :add  '((holding ?x))                     
 :del  '((handempty) (clear ?x) (ontable ?x)))

(add-strips-op
 :name '(putdown ?x) 
 :pre  '((holding ?x)) 
 :add  '((ontable ?x) (clear ?x) (handempty)) 
 :del  '((holding ?x)))

(add-strips-op
 :name '(stack ?x ?y)
 :pre  '((holding ?x) (clear ?y))
 :add  '((on ?x ?y) (clear ?x) (handempty))
 :del  '((holding ?x) (clear ?y)))

(add-strips-op 
 :name '(unstack ?x ?y)
 :pre  '((clear ?x) (on ?x ?y) (handempty))
 :add  '((holding ?x) (clear ?y))
 :del  '((on ?x ?y) (clear ?x) (handempty)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 5. Pretty print the world state. This also illustrates the
;;; usefulness of the evaluator.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;

(define *stream* #t)
(define *tower* '())
(define *towers* '())
(define *holding* '())

(def-computed-predicate print-holding-block (world)
  (if *holding*
      (format *stream* "~% HOLDING ~S~%" *holding*))
  #t)

(def-computed-predicate print-towers (world)
  ;;(format *stream* "~%~S" *towers*)
  ;; All this fancy stuff to print towers from the normal
  ;; (horizontal) perspective.
  ;;
  (let ((max-height 0)
	(block '()))
    (set! max-height (- (reduce max 0 (map length *towers*)) 1))
    (do ((limit (+ max-height 1))
	 (i 0 (+ 1 i)))
	((= i limit))
      (format *stream* "~% ")
      (for-each (lambda (tower)
		  (set! block (nth (- max-height i) tower))
		  (if block
		      (format *stream* " ~S" block)
		      (format *stream* "  ")))
		*towers*)))
  #t)
    
(def-computed-predicate print-block (block world)
  (format *stream* "~S " block)
  #t)

(def-computed-predicate print-new-line (world)
  (format *stream* "~%")
  #t)

(def-computed-predicate print-space (world)
  (format *stream* "   ")
  #t)

(def-computed-predicate push-block (block world)
  (set! *tower* (cons block *tower*))
  #t)

(def-computed-predicate push-space (n world)
  (do ((i 0 (+ 1 i)))
      ((>= i n))
    (set! *tower* (cons '? *tower*)))
  #t)

(def-computed-predicate end-tower (world)
  (set! *towers* (cons (reverse *tower*) *towers*))
  (set! *tower* '())
  #t)

(def-computed-predicate clear-accumulators (world)
  (set! *tower* '())
  (set! *towers* '())
  (set! *holding* '())
  #t)

(def-defined-predicate accumulate-tower (?x)
  ;; The final (end-tower) is so that goal towers that are not terminated
  ;; by clear blocks can be properly accumulated.
  (and (push-block ?x)
       (or (and (clear ?x) (end-tower))
	   (exists (?y) (on ?y ?x) (accumulate-tower ?y))
	   (and (push-space 1) (end-tower)))))

(def-computed-predicate accumulate-holding (block world)
  (set! *holding* block)
  #t)
           
(define (print-blocks-world world  . optional-args)
  (let ((stream (if (null? optional-args) 
		    *trace-stream*
		    (car optional-args))))
    (set! *stream* stream)
    (eval-formula '(and
		    (clear-accumulators)
		    (forall (?x) (ontable ?x) (accumulate-tower ?x))
		    (forall (?x) (holding ?x) (accumulate-holding ?x))
		    (print-holding-block)
		    (print-towers))
		  world '())))

(set-print-world-fn print-blocks-world)
