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

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(clear-world-symbols)                
(clear-ops)                          

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

(def-described-symbols
  '((in        predicate 2)
    (holding   predicate 1)
    (handempty predicate 0)
    (opened   predicate 1)
    (closed   predicate 1)
    (connect predicate 2)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 2. Lisp functions used as terms. Weight and Size should be 
;;; re-defined by the user for a specific problem.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(define (weight object) 
  "Returns the weight of object"
  1)

(define (origin room)
  "returns the lower-left origin the room. This must be a list
   of x and y coordinates."
  '(0 0))

(define (height room)
  "Height of room"
  1)

(define (width room)
  "Width of room"
  1)

(define (distance room1 room2)
  "Distance between two room centers"
  (let*((origin1 (origin room1))
	(center1-x (/ (+ (first origin1) (width room1)) 2))
	(center1-y (/ (+ (second origin1) (height room1)) 2))
	(origin2 (origin room2))
	(center2-x (/ (+ (first origin2) (width room2)) 2))
	(center2-y (/ (+ (second origin2) (height room2)) 2)))
    (sqrt (+ (expt (- center1-x center2-x) 2)
	     (expt (- center1-y center2-y) 2)))))

(def-lisp-fns '(distance))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 3. Operators.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(add-strips-op
 :name '(wait)
 :priority 0
 :duration 1
 :cost 1000)

(add-strips-op 
 :name '(release ?o)
 :pre '((holding ?o))
 :del '((holding ?o))
 :add '((handempty))
 :priority 1
 :duration 1)

(add-adl-op 
 :name '(grasp ?o)
 :pre (adl-pre :var-gens '((?r1) (in robot ?r1) (?o) (in ?o ?r1))
	       :form '(and (handempty) (not (= ?o robot))))
 :add (adl-add (adl-cond :lit '(holding ?o)))
 :del (adl-del (adl-cond :lit '(handempty)))
 :priority 1
 :duration 1)

(add-strips-op 
 :name '(open ?d)
 :pre '((closed ?d) (in robot ?r1) (connect ?d ?r1 ?r2))
 :add '((opened ?d))
 :del '((closed ?d))
 :priority 1
 :duration 1)

(add-strips-op 
 :name '(close ?d)
 :pre '((in robot ?r1) (connect ?d ?r1 ?r2) (opened ?d))
 :add '((closed ?d))
 :del '((opened ?d))
 :priority 1
 :duration 1)

(add-strips-op 
 :name '(move ?r1 ?r2)
 :pre '((handempty) (in robot ?r1) (connect ?d ?r1 ?r2) (opened ?d))
 :add '((in robot ?r2))
 :del '((in robot ?r1))
 :priority 1
 :cost '(distance ?r1 ?r2)
 :duration '(distance ?r1 ?r2))

(add-strips-op 
 :name '(move ?r1 ?r2)
 :pre '((holding ?o) (in robot ?r1) (connect ?d ?r1 ?r2) (opened ?d))
 :add '((in robot ?r2) (in ?o ?r2))
 :del '((in robot ?r1) (in ?o ?r1))
 :priority 1
 :cost '(distance ?r1 ?r2)
 :duration '(distance ?r1 ?r2))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 5. Pretty print the world state. 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(set-print-world-fn 
 (lambda(world/action . args)
   (format #t "~%~S"
	   (remove-if 
	    (lambda (lit)
	      (eq? (get-operator lit) 'connect))
	    (list-world (world/action-world world/action))))))

 