;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 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))))
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 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)
      (clear       predicate 1)
      (handempty   predicate 0)
      (table-space predicate 1)
      (holding     predicate 1)
      (ontable     predicate 1)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 2. The defined predicates.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 1. Table-Has-Space: True iff the table has space.
;;;

(def-defined-predicate table-has-space ()
  (exists (?n) (table-space ?n) (> ?n 0)))

;;; 2. Goodtowers: ?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. Furthermore, it must either be gounded in a goal
;;; ontable condition, or all goal ontable conditions must be satisfied.
;;;
;;; 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.

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

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

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 3. Computed functions/Predicates.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(define blocks-count 0)

(def-computed-predicate incf-blocks-count (world)
  (incf blocks-count)
  #t)

(def-defined-predicate blocks-count-to-top (?x)
  (or (clear ?x)
      (exists (?y) (on ?y ?x)
	      (and (incf-blocks-count) (blocks-count-to-top ?y)))))

(def-computed-fn depth (block world)
  (set! blocks-count 0)
  (eval-formula `(blocks-count-to-top ,block) world '())
  blocks-count)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 4. The lisp predicates and functions.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(def-lisp-fns '(+ -))

(def-lisp-predicates '(< > <= >=))

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

;;(set-tl-control
;; '(always (TRUE)))
;;
;;(set-tl-control
;; '(always
;;   (if-then-else
;;    (exists (?x) (holding ?x))
;;    (exists (?x)
;;     (holding ?x)
;;     (and
;;      (implies (and (goal (ontable ?x)) (table-has-space))
;;               (next (ontable ?x)))
;;      (implies (exists (?y) (goal (on ?x ?y)) (goodtower ?y))
;;                (next (goodtower ?x)))))
;;    (if-then-else
;;     (exists (?x) (clear ?x)
;;      (and (goal (ontable ?x)) (table-has-space) (not (ontable ?x))))
;;     (exists (?x) (clear ?x)
;;      (and (goal (ontable ?x)) (not (ontable ?x))
;;           (next (holding ?x))))
;;     (if-then-else
;;      (exists (?x) (clear ?x)
;;              (exists (?y) (goal (on ?x ?y))
;;                      (and (not (on ?x ?y)) (goodtower ?y))))
;;      (exists (?x) (clear ?x)
;;              (exists (?y) (goal (on ?x ?y))
;;                      (and (not (on ?x ?y)) (goodtower ?y)
;;                           (next (holding ?x)))))
;;      (TRUE))))))

(set-tl-control
 '(always
   (if-then-else
    (exists (?x) (holding ?x))
    (exists (?x)
     (holding ?x)
      (and
       (implies (and (goal (ontable ?x)) (table-has-space))
                (next (ontable ?x)))
       (implies (exists (?y) (goal (on ?x ?y)) (goodtower ?y))
                (next (goodtower ?x)))))
    (if-then-else
     (exists (?x) (clear ?x)
      (and (goal (ontable ?x)) (table-has-space) (not (ontable ?x))))
     (exists (?x) (clear ?x)
      (and (goal (ontable ?x)) (not (ontable ?x))
           (next (holding ?x))))
     (if-then-else
      (exists (?x) (clear ?x)
              (exists (?y) (goal (on ?x ?y))
                      (and (not (on ?x ?y)) (goodtower ?y))))
      (exists (?x) (clear ?x)
              (exists (?y) (goal (on ?x ?y))
                      (and (not (on ?x ?y)) (goodtower ?y)
                           (next (holding ?x)))))
      (if-then-else
       (exists (?x) (clear ?x) (and (goal (ontable ?x)) (not (ontable ?x))))
       (and
        (exists (?x) (clear ?x)
                (and (goal (ontable ?x)) (not (ontable ?x))
                     (next (next (clear ?x)))))
        (exists (?y) (ontable ?y)
                (and (not (goal (ontable ?y)))
                     (forall (?z) (ontable ?z)
                             (implies (not (goal (ontable ?z)))
                                      (<= (depth ?y) (depth ?z))))
                     (exists (?d) (= (depth ?y))
                             (next (or (holding ?y) (< (depth ?y) ?d)))))))
       (if-then-else
        (exists
         (?x ?y) (goal (on ?x ?y)) 
         (and (not (on ?x ?y)) (clear ?x) (goodtowerbelow ?y)))
        (exists
         (?x ?y) (goal (on ?x ?y))
         (and (not (on ?x ?y)) (clear ?x) (goodtowerbelow ?y)
              (forall (?z1 ?z2) (goal (on ?z1 ?z2))
                      (implies 
                       (and
                        (not (on ?z1 ?z2)) (clear ?z1) (goodtowerbelow ?z2))
                       (<= (depth ?y) (depth ?z2))))
              (next
               (next
                (or (clear ?x)
                    (exists (?z1 ?z2)
                            (goal (on ?z1 ?z2))
                            (and (not (on ?z1 ?z2))
                                 (clear ?z1)
                                 (goodtowerbelow ?z2)
                                 (< (depth ?z2) (depth ?y)))))))
              (exists (?d) (= (depth ?y))
                      (next (< (depth ?y) ?d)))))
        (if-then-else
         (exists (?x) (goal (ontable ?x))
                 (and (not (ontable ?x)) (not (clear ?x))))
         (exists
          (?x) (goal (ontable ?x))
          (and (not (ontable ?x)) (not (clear ?x))
               (forall
                (?y) (goal (ontable ?y))
                (implies
                 (and (not (ontable ?y)) (not (clear ?y)))
                 (<= (depth ?x) (depth ?y))))
               (exists (?d) (= (depth ?x))
                       (next (< (depth ?x) ?d)))
               (exists (?y) (ontable ?y)
                       (and (not (goal (ontable ?y)))
                            (forall (?z) (ontable ?z)
                                    (implies (not (goal (ontable ?z)))
                                             (<= (depth ?y) (depth ?z))))
                            (exists
                             (?d) (= (depth ?y))
                             (next (next
                                    (or (<= (depth ?y) ?d)
                                        (exists
                                         (?z) (clear ?z)
                                         (and
                                          (not (= ?z ?x))
                                          (or
                                           (and (goal (ontable ?z)) (not (ontable ?z)))
                                           (exists
                                            (?z1) (goal (on ?z ?z1))
                                            (and (not (on ?z ?z1))
                                                 (goodtowerbelow ?z1))))))))))))))
                                                 
         (if-then-else
          (exists
           (?x ?y) (goal (on ?x ?y))
           (and (not (on ?x ?y)) (not (clear ?x)) (goodtowerbelow ?y)))
          (exists
           (?x ?y) (goal (on ?x ?y))
           (and (not (on ?x ?y)) (not (clear ?x)) (goodtowerbelow ?y)
                (forall
                 (?z1 ?z2) (goal (on ?z1 ?z2))
                 (implies 
                  (and (not (on ?z1 ?z2)) (not (clear ?z1)) (goodtowerbelow ?z2))
                  (<= (depth ?x) (depth ?z1))))
                (exists (?d) (= (depth ?x))
                        (next (< (depth ?x) ?d)))
                (exists (?d) (= (depth ?y))
                        (next (next
                               (or (<= (depth ?y) ?d)
                                   (exists
                                    (?z) (clear ?z)
                                    (and (not (= ?z ?x))
                                         (or
                                          (and (goal (ontable ?z)) (not (ontable ?z)))
                                          (exists
                                           (?z1) (goal (on ?z ?z1))
                                           (and (not (on ?z ?z1))
                                                (goodtowerbelow ?z1))))))))))))
          ;;Final case achieve and preserve all clear conditions.
          (and
           (forall (?x) (goal (clear ?x))
                   (implies (clear ?x)
                            (and (next (clear ?x))
                                 (next (next (clear ?x))))))
           (implies
            (exists (?x) (goal (clear ?x)) (not (clear ?x)))
            (exists (?x) (goal (clear ?x))
                    (and
                     (not (clear ?x))
                     (forall (?y) (goal (clear ?y))
                             (implies (not (clear ?y))
                                      (<= (depth ?x) (depth ?y))))
                     (exists (?d) (= (depth ?x))
                             (next (< (depth ?x) ?d))))))))))))))))
      
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 4. Operators
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
     
(add-strips-op 
 :name '(pickup ?x)
 :pre  '((handempty) (clear ?x) (ontable ?x) (table-space ?n))
 :add  '((holding ?x) (table-space (+ 1 ?n)))
 :del  '((handempty) (clear ?x) (ontable ?x) (table-space ?n)))

(add-strips-op
 :name '(putdown ?x) 
 :pre  '((holding ?x) (table-space ?n) (> ?n 0))
 :add  '((ontable ?x) (clear ?x) (handempty) (table-space (- ?n 1))) 
 :del  '((holding ?x) (table-space ?n)))

(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)
