" (c) 1990, 1991, 1992 Copyright (c) University of Washington
  Written by Stephen Soderland, Tony Barrett and Daniel Weld.

  All rights reserved. Use of this software is permitted for non-commercial
  research purposes, and it may be copied only for that use.  All copies must
  include this copyright message.  This software is made available AS IS, and
  neither the authors nor the University of Washington make any warranty about
  the software or its performance.

  When you first acquire this software please send mail to 
  bug-snlp@cs.washington.edu; the same address should be used for problems."

(in-package 'plan-utils)

(use-package 'variable)

(export '(make-stat stat-algo stat-date stat-prob-num stat-num-init  
	  stat-num-goal stat-plan-len stat-reached-max? stat-complete?
	  stat-time stat-visited stat-created stat-q-len stat-ave-branch
	  stat-unify-count stat-rank-unifies stat-add-bindings
	  make-plan-step plan-step-id plan-step-action plan-step-precond 
	  plan-step-add plan-step-dele 
	  *Templates* *search-limit* *trace* *nodes-visited* *plans-created* 
	  *branch* *verbose*
	  DEFSTEP RESET-DOMAIN BESTF-SEARCH CALL-IE INSTANTIATE-STEP TODAY 
	  RESET-STAT-VARS div* DISPLAY-STAT PRINT-STAT PRINT-TEMPLATES))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 1. Data Structures

(defstruct (STAT (:print-function print-stat))
  algo                                  ; tweak or strips
  date                                  ; when performed
  prob-num                              ; identifier
  num-init                              ; how many initial conditions
  num-goal
  plan-len                              ; how many steps
  reached-max?                          ; terminated because of nodes?
  complete?                             ; planner successful
  time                                  ; internal cpu time
  visited                               ; nodes-visted
  created                               ; calls to make-plan
  q-len                                 ; queue len at termination
  ave-branch                            ; average branching factor
  unify-count
  rank-unifies
  add-bindings
  )

(defstruct PLAN-STEP
  ID                      ; integer step number
  action                  ; formula such as (puton ?X1 ?Y1)
  precond                 ; list of conditions such as (clear ?X1)
  add                     ; list of conditions asserted by step
  dele                    ; list of conditions denied by step
  )

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 2. Variables

(defvar *Templates* nil)    ;; list of pairs of dummy step, bindings
(defvar *search-limit* 800) ;; max number of plans created
(defvar *trace* 0)	    ;; 5 = list Queue
                            ;; 4 = "Add Bind ..." (use function TRACE instead)
			    ;; 3 = "Unifying ..." (use function TRACE instead)
			    ;; 2 = "New Step? ..."
			    ;; 1 = "* New Step ..."
			    ;; 0 = "Plan at Current Node"
;;; Statistics related variables

(defvar *nodes-visited* 0)	   ;; Number of plans visited during the search
(defvar *plans-created* 0)	   ;; Number of plans created during the search
(defvar *branch* 0)	           ;; compute average branch factor
(defvar *verbose* nil)             ;; Print whole plan?

;;; Variables for Stuart Russell's IE search routine

(defvar *ie-limit*)
(defvar *ie-branches*)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 3. Interface functions

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  This function is used to define plan steps of a domain theory.
(defun DEFSTEP (&key action precond add dele (equals nil))
  (push (list (make-plan-step
	       :action action
	       :precond precond
	       :add add
	       :dele dele)
	      equals)
	*templates*))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Purge a previously defined domain theory.
(defun RESET-DOMAIN ()
  (setf *templates* nil))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 4. Utility functions

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  A simple best first search strategy.  Returns 3 values: the found state,
;;;  the average branching factor, and the number of generated but unexplored 
;;;  states.  The search will only generate up to LIMIT states.  
;;;  Calls search-trace for debugging purposes.
(defun BESTF-SEARCH (initial-state daughters-fn goal-p rank-fn limit)
  (let ((branches nil))                         ; compute average branch factor
    (do* ((current-entry nil (car search-queue))
          (current-state initial-state (cdr current-entry))
          (search-queue nil (cdr search-queue)))
         ((or (null current-state)
	      (funcall goal-p current-state)
              (> 0 limit))
          (values current-state
                  (if (null branches) 0
                      (div* (apply #'+ branches) (length branches)))
                  (length search-queue)))
      (incf *nodes-visited*)
      (search-trace current-state search-queue rank-fn)
      (let ((children (funcall daughters-fn current-state)))
        (setf limit (- limit (length children)))
        (setf search-queue
              (merge
                  'list search-queue
                  (sort (mapcar #'(lambda (x) (cons (funcall rank-fn x) x))
                                children)
                        #'< :key #'car)
                  #'< :key #'car))
        (push (length children) branches)))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Search trace function 
;;; Minor bug fixed 11/5/92
(defun SEARCH-TRACE (current-state search-queue &optional (rank-fn nil))
  (when (> *trace* 0.5)
    (format t "~%CURRENT PLAN (rank ~a)" (funcall rank-fn current-state)))
  (if (> *trace* 0.5)
      (format t "~%QUEUE   -  Length ~a  " (length search-queue))
      (if (> *trace* 0) (format t " * ")))
  (if (and (> *trace* 4) search-queue)
      (dolist (q-plan search-queue)
	(format t "~%Rank ~a" (funcall rank-fn (cdr q-plan)))
	(print (cdr q-plan)))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  IE search function written by Stuart Russell 
;;;  (See "Efficient Memory-Bounded Search Methods" in ECAI-92)

(defun CALL-IE (node successors goalp rank-fn limit)
  (setf *ie-limit* limit)
  (setf *ie-branches* (cons 0 0))
  (let ((solution (ie (cons (funcall rank-fn node) node)
                      goalp successors rank-fn most-positive-single-float)))
    (values solution (if (zerop (car *ie-branches*)) 0 
                         (div* (cdr *ie-branches*) (car *ie-branches*))) 0)))

(defun IE (vnode goalp successors rank-fn bound &aux children)
  (cond ((or (funcall goalp (cdr vnode)) (> 0 *ie-limit*))
         (cdr vnode))
        ((null (setf children (mapcar #'(lambda (child)
                                          (cons (funcall rank-fn child) child))
                                      (funcall successors (cdr vnode)))))
         (setf (car vnode) most-positive-single-float)
         nil)
        (t (incf *nodes-visited*)
           (decf *ie-limit* (length children))
           (incf (car *ie-branches*)) (incf (cdr *ie-branches*) 
					    (length children))
           (search-trace (cdr vnode) nil rank-fn)
           (dolist (vn children)    ;;; pathmax
             (setf (car vn) (max (car vn) (car vnode))))
           (do ()
               ((> (car vnode) bound))
             (setf children
                   (sort children #'< :key #'car))
             (let* ((best (car children))
                    (rest (cdr children))
		    ;; best sibling value
                    (new-bound (apply #'min (cons bound (mapcar #'car rest)))))
               (let ((v (ie best goalp successors rank-fn new-bound)))
                 (when v (return v)))
               (if (and rest (< (caar rest) (car best)))
                 (setf (car vnode) (caar rest))
                 (setf (car vnode) (car best))))))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;; Add new high step number to each variable id for every term in 
;;; step from template.
(defun INSTANTIATE-STEP (step num)
  (make-plan-step
   :id num
   :action (instantiate-term (plan-step-action step) num)
   :precond (instantiate-term (plan-step-precond step) num)
   :add (instantiate-term (plan-step-add step) num)
   :dele (instantiate-term (plan-step-dele step) num)))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Get today's date
(defun TODAY ()
  (let ((d (multiple-value-list (get-decoded-time))))
    (format nil "~a/~a ~a at ~a:~a:~a"
            (nth 4 d) (nth 3 d) (nth 5 d) (nth 2 d) (nth 1 d) (nth 0 d))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Reset the global variables used for collecting statistics on the
;;;  planner.
(defun RESET-STAT-VARS ()
  (setf *nodes-visited* 0)
  (setf *unify-count* 0)
  (setf *compute-rank-unifies* 0)
  (setf *add-bind-count* 0)
  (setf *branch* 0)
  (setf *plans-created* 0))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  A divide routine that does not blow up on zero.
(defun div* (x y)
  (if (= y 0) (* 99999 99999 99999) (/ x y)))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 5. Print functions

;;;;;;;;;;;;;;;;;;;;;;;;
;;; Print out statistics from single run
(defun DISPLAY-STAT (s &optional (st t) ignore)
  (declare (ignore ignore))
  (format st "~%~%~a (Init = ~2a ; Goals = ~2a) => ~a (~a steps)     CPU ~9a"
          (stat-algo s) (stat-num-init s) (stat-num-goal s)
          (if (stat-complete? s) "Win " "Lose")
          (stat-plan-len s) (stat-time s))
  (format st "~%     Nodes (V = ~4a; Q = ~4a; C = ~4a)             Branch ~10a"
          (stat-visited s) (stat-q-len s) (stat-created s)
          (stat-ave-branch s))
  (format st "~%     Working Unifies: ~25a       Bindings added: ~5a~%"
          (- (stat-unify-count s) (stat-rank-unifies s))
          (stat-add-bindings s)))

(defun PRINT-STAT (s &optional (stream t) depth)
  (declare (ignore depth))
  (if *verbose* (display-stat s stream)
    (format stream "#Stats:<cpu time = ~9a>" 
	    (stat-time s))))

;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Print function for Templates
(defun PRINT-TEMPLATES (&optional (templates *templates*))
  (format t "~&~%Templates:")
  (dolist (templ-n templates)
    (let ((action (plan-step-action (car templ-n)))
	  (pre-cond (plan-step-precond (car templ-n)))
	  (add (plan-step-add (car templ-n)))
	  (dele (plan-step-dele (car templ-n)))
	  (bind (cadr templ-n)))
      (format t "~&~a~%  Pre  : ~a~%  Add  : ~a~%  Dele : ~a~%  Bind : ~a~%"
	      action pre-cond add dele bind))))