;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Some sample Planning problems for block world.
;;; And a random problem generator.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(define state0 '((clear a)   (clear b)   (clear c)
               (ontable a) (ontable b) (ontable c)
               (handempty)))

(define goal0 '((on a b) (on b c) (ontable c)))

(define state1 `((clear a) (clear c) (handempty) 
	       (on a b) (ontable b) (on c d) (ontable d)))

(define goal1 '((on b a) (on d b)))

(define state2 `((clear b) (ontable g) (handempty) 
	       (on b a) (on a c) (on c d) (on d e) (on e f) (on f g)))

(define goal2 '((on f g) (on g a) (on a b) (on b c)
              (on c d) (on d e) (ontable e)))

(define state3 `((clear b) (ontable e) (handempty) 
	       (on b a) (on a c) (on c d) (on d e)))
      
(define goal3 '((on d e) (on e a) (on a b) (on b c) (ontable c)))

;; sussman anomaly
(define state4 '((on c a) (ontable a) (ontable b) (clear c) (clear b)
                (handempty)))

(define goal4 '((on a b) (on b c)))

(define state5 '((clear c) (clear d) (ontable a) (ontable d) (on c b)
               (on b e) (on e a) (handempty)))

(define goal5 '((clear c) (clear d) (ontable e) (ontable b)
              (on c a) (on a e) (on d b) (handempty)))

(define state6 '((clear a) (ontable d) (on a b) (on b c) (on c e)
               (on e f) (on f d) (handempty)))

(define goal6 '((clear b) (ontable f) (on b d) (on d a)
              (on a c) (on c e) (on e f) (handempty)))

(define state7 '((clear g) (clear e) (clear c) (clear a) (ontable f)
               (ontable e) (ontable b) 
               (ontable a) (on g d) (on d f) (on c b) (handempty)))

(define goal7 '((clear g) (clear c) (ontable a) (ontable b)
              (on g d) (on d a) (on c e) (on e f) (on f b) (handempty)))

(define state8 '((clear h) (clear b) (clear d) (ontable e) (ontable g)
               (ontable d) (on h f) 
               (on f a) (on a c) (on c e) (on b g) (handempty)))

(define goal8 '((clear c) (clear d) (clear h) (ontable f)
              (ontable d) (ontable b) (on c g) (on g e) (on e a)
              (on a f) (on h b) (handempty)))

(define goal9 '(t-eventually (< 3 3 >) (holding d)))
(define goal10 '(and (t-always (< 0 inf+ >) (on a b))
		     (t-eventually ( < 3 3 >) (holding d))))
(define goal11  '(forall (?x) (clear ?x)
			 (t-eventually ( < 0 4 >) (ontable ?x))))
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; functions for generating a random blocks world initial state and goal
;;; state.
;;; You can execute
;;; (set-initial-world (random-world 25))
;;; to get an random initial world configuration with 25 blocks.
;;;
;;; You can also use
;;; (set-goal (random-world 25))
;;; to set up a random goal with 25 blocks.
;;;
;;; (random-goal ...) is provided as sometimes you want to use incompletely
;;; specified goals.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(define (random-world num)
  "Return a list of literals describing a complete configuration of
   a num blocks world."
  (make-literals (random-blocks num)))

(define (random-goal num)
  "Return a list of literals describing an incomplete configuration of
   a num blocks world. In particular do not include any ontable or clear
   literals."
  (make-goal-literals (random-blocks num)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(define (random-nums limit)
  "returns a list of k numbers, so that their sum  = limit"
  (do ((num (+ 1 (random limit))
            (+ 1 (if (= 0 new-limit) 0
                  (random new-limit))))
       (new-limit limit)
       (so-far 0 (+ so-far num))
       (result '()))
      
       ((= so-far limit) result)
    
       (set! new-limit (- new-limit num))
       (set! result (cons num result))))

(define (random-select-blocks num blocks)
  "randomly select num blocks from the list"
  (let ((result '()))
    (do ((index num (- index 1)))
        ((or (= 0 index) (null? blocks)))
      (set! result
            (cons (list-ref blocks (random (length blocks)))
                  result))
      (set! blocks (set-difference blocks result)))
    result))


;********* random-blocks generates a list of blocks stacks at random.

(define (random-blocks limit)
  "returns a list of sublists, each sublist is a list of blocks.
  Thus, (A B C) means on(A B) and on(B C)."

  (let* ((blocks-list '(a b c d e f g h i j k l m n o p q r s t u v
			 w x y z a1 b1 c1 d1 e1 f1 g1 h1 i1 j1 k1 l1
                       m1 n1 o1 p1 q1 r1 s1 t1 u1 v1 w1 x1 y1 z1
                       a2 b2 c2 d2 e2 f2 g2 h2 i2 j2 k2 l2
                       m2 n2 o2 p2 q2 r2 s2 t2 u2 v2 w2 x2 y2 z2
                       a3 b3 c3 d3 e3 f3 g3 h3 i3 j3 k3 l3
                       m3 n3 o3 p3 q3 r3 s3 t3 u3 v3 w3 x3 y3 z3
                       a4 b4 c4 d4 e4 f4 g4 h4 i4 j4 k4 l4
                       m4 n4 o4 p4 q4 r4 s4 t4 u4 v4 w4 x4 y4 z4))
	(sublist-quota (random-nums limit))
	(blocks '())
        (result '())
        (max (length blocks-list)))
    (if (> limit max) (set! limit max))
    (let loop ((lst blocks-list)
               (n limit))
      (cond ((<= n 0) #t)
            (else (set! blocks (cons (first lst) blocks))
                  (loop (rest lst) (- n 1)))))
    (let loop ((nums sublist-quota))
      (cond ((null? nums) #t)
            (else (set! result
                        (cons (random-select-blocks (first nums) blocks)
                              result))
                  (set! blocks (set-difference blocks (first result)))
                  (loop (rest nums)))))
    result))
    

; **************
(define (make-literals block-stacks)
  "returns a list of literals representing the state of the blocks"
  (append
   (make-clear-literals block-stacks)
   (make-ontable-literals block-stacks)
   (make-on-literals block-stacks)
   '((handempty))))

(define (make-goal-literals block-stacks)
  "returns a list of literals representing the state of the blocks"
  (append
   (make-on-literals block-stacks)
   '((handempty))))

; *************
(define (make-clear-literals block-stacks)
  "returns clear (C) literals."
  (let ((clear-blocks (map car block-stacks)))
    (map (lambda (block)
	(list 'clear block))
         clear-blocks)))

(define (make-ontable-literals block-stacks)
  "returns on (C table) literals."
  (let ((ontable-blocks
         (map (lambda (stack) (car (last stack)))
              block-stacks)))
    (map (lambda (block) (list 'ontable block))
         ontable-blocks)))

;**************
(define (make-on-literals block-stacks)
  "returns on (A C) literals."
  (apply append 
	 (map (lambda (stack) (make-on-for-one-stack stack))
              block-stacks)))

(define (make-on-for-one-stack stack)
  "returns a list of on-relations for stack"
  (cond ((null? (cdr stack)) '())
	(else (cons (list 'on (car stack) (second stack))
                    (make-on-for-one-stack (cdr stack))))))

;*****************
