" (c) 1990, 1991 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."

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;;  Program   : "SNLP"
;;;  Authors   : Stephen Soderland
;;;              (modified & extended by Dan Weld and Tony Barrett)
;;;  Date      : Summer/Autumn 1990
;;;
;;;  Description:
;;;  
;;;    This program is a domain independent, conjuctive, partial-order planner
;;;    which claims to be both complete and sound.  The representation of
;;;    conditions and actions is similar to STRIPS, with each step having
;;;    a list of added conditions and a list of deleted conditions.  
;;;
;;;    A plan includes a list of steps and a list of links between steps. 
;;;    Links are in the form  (id1 condition1 id2) where id1 is a step that
;;;    establishes condition1, which is in turn a precondition of step id2.
;;;    The plan also has a list of open conditions (not yet acheived) and a 
;;;    list of unsafe links (possibly clobbered by another step).  The other 
;;;    components of a plan are ordering constraints and bindings of variables.
;;;
;;;    Each iteration of the main control level creates possible next plans 
;;;    which are added to a priority queue.  New plans are created by adding
;;;    constraints to resolve unsafe links.  If there are no unsafe links,
;;;    new plans are created by adding a new step or new link to achieve
;;;    an open condition.  The plan is completed when there are no more unsafe
;;;    links or open conditions.  
;;;  
;;;    SNLPlan is adapted from a non-linear planner by David McAllester.    
;;;    As the first phase of this project, I implemented Tweak, a non-linear
;;;    planner described by David Chapman in Artificial Intelligence 1987.
;;;    Code from my Tweak program was incorporated into SNLPlan.  The data
;;;    structures for variable bindings were later revised to enhance
;;;    performance.  
;;;
;;;

(in-package 'snlp)

(use-package 'variable)
(use-package 'plan-utils)

(export '(plan display-plan))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Outline of this file
;;;
;;;  1.   Data Structures
;;;  2.   Handy interface for using the program
;;;  3.   Main control level of SNLP
;;;  4.   Ranking partial plans
;;;  5.   Handling open conditions
;;;  5.1.   Adding new steps
;;;  5.2.   Adding links to old steps
;;;  6.   Protecting causal links
;;;  6.1.   Resolving an unsafe link
;;;  6.2.   Detecting unsafety conditions
;;;  7.   handle partial orderings.
;;;  8.   Handy utility functions
;;;  9.   Print functions 

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

(defstruct (SNLP-PLAN (:constructor make-SNLP-plan*)
	    (:print-function print-plan))
  steps                   ; list of steps
  links                   ; list of (ID1 condition ID2)
  unsafe                  ; list of (link clobber-ID clobber-bind)
  open                    ; list of (condition ID)   
  ordering                ; list of (ID1 ID2)
  bindings                ; hashtable of bind 
  high-step               ; integer number of highest step in plan
  )

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 2. Handy interface for using the program

;;; This returns two values: a plan and a stat record
;;; If this is the top-level call, then both are printed.
(defun PLAN (initial
	     goals
	     &key
	     (rank-fun #'rank3)
	     (search-fun #'bestf-search))
  (multiple-value-bind (plan done? time q-len av-branch)
      (snlplan initial goals rank-fun search-fun)
    (values plan (make-stat :algo        "POCL"             
                            :date        (today)
                            :prob-num    1
                            :num-init    (length initial)       
                            :num-goal    (length goals)
                            :plan-len    (if plan (snlp-plan-high-step plan) 0)
                            :reached-max? (>= *nodes-visited* *search-limit*)
                            :complete?    done?
                            :time         time
                            :visited      *nodes-visited*     
                            :created      *plans-created*
                            :q-len        q-len
                            :ave-branch   (float av-branch)
                            :unify-count  *unify-count*
                            :rank-unifies *compute-rank-unifies*
                            :add-bindings *add-bind-count*)
            )))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 3. Main control level of SNLP

;;; Returns 5 values: final plan, t or nil denoting success, run time
;;; length of q at termination, average branching factor
(defun SNLPLAN (init goals rank-fun search-fun)
  (reset-stat-vars)			; clear globals for metering
  (let* ((init-time (get-internal-run-time))
	 (init-plan (make-SNLP-plan
		     :steps (list (make-plan-step :ID '0 :add init)
				  (make-plan-step :ID :Goal :precond goals))  
		     :open (mapcar #'(lambda (x) (cons x '(:Goal))) goals)
		     :bindings (new-bindings)
		     :high-step 0)))
    (multiple-value-bind (plan bfactor qlength)
	(funcall search-fun init-plan
		 #'plan-refinements #'plan-test rank-fun *search-limit*)
      (values plan			; the plan itself
	      (and plan
		   (null (snlp-plan-unsafe plan))
		   (null (snlp-plan-open plan))) ; plan successfull?
	      (- (get-internal-run-time) init-time) ; time
	      qlength			; final length of the queue
	      bfactor))))		; average branching factor

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Goal test function for a search.
(defun PLAN-TEST (plan)
  (not (or (SNLP-plan-unsafe plan)
	   (SNLP-plan-open plan))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Creates a list of one step refinements to the current plan.
;;;
;;;   Select one unsafe link and find all new plans that
;;; resolve it by separation, demotion, or promotion.  Separation is 
;;; adding bindings that prevent unifying of the condition with the
;;; added conditions of a possible clobbering step.  Demotion and
;;; promotion add ordering constraints so that a possibly clobbering
;;; step comes before the first step of the link or after the second
;;; step.
;;;   If there are no unsafe links, select an open condition to resolve
;;; by adding a new step or making a link to an existing step.  Adding 
;;; a new step or new link may cause a link to become unsafe (possibly 
;;; clobbered).
;;;
(defun PLAN-REFINEMENTS (plan)
  (if (SNLP-plan-unsafe plan)
      (handle-unsafe plan)
      (handle-open plan)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 4. ranking partial plans

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Perform an A* search on the number of causal links in a plan.
(defun RANK (plan)
  (let ((num-links (length (SNLP-plan-links plan)))
	(open (length (SNLP-plan-open plan))))
    (+ num-links open)))

;;;;;;;;;;;;;;;;;;;;;;;;                                                 
;;;  A slightly more agressive search, ignoring the number of steps
(defun RANK1 (plan)
  (let ((unsafe (length (SNLP-plan-unsafe plan)))
	(open (length (SNLP-plan-open plan))))
    (+ unsafe open)))

;;;;;;;;;;;;;;;;;;;;;;;;
;;; To find the plan with the fewest steps.  If a complete plan is found
;;; give it a lower rank than an incomplete plan with one fewer steps.
(defun RANK2 (plan)
  (let ((num-steps (length (SNLP-plan-steps plan)))
	(unsafe (length (SNLP-plan-unsafe plan)))
	(open (length (SNLP-plan-open plan))))
    (cond
      ((= 0 (+ unsafe open))
       (- num-steps 2))
      (t
       num-steps))))

;;;;;;;;;;;;;;;;;;;;;;;;                                                 
;;;  To find a plan with the least searching.  Ranking is based on the
;;;  number of unsafe plus the number of goals to be resolved plus the
;;;  number of steps.  Including the number of steps prevents creating
;;;  long plans that add cycles of moves getting no closer to the goal.
(defun RANK3 (plan)
  (let ((num-steps (length (SNLP-plan-steps plan)))
	(unsafe (length (SNLP-plan-unsafe plan)))
	(open (length (SNLP-plan-open plan))))
    (+ unsafe open num-steps)))

;;;;;;;;;;;;;;;;;;;;;;;;
;;; Rank function for performing a depth first search.
;;; Prefers plans with more links.
(defun DF-RANK (plan)
  (- 1000 (length (SNLP-plan-links plan))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 5. Handling open conditions

(defun HANDLE-OPEN (plan)
  (multiple-value-bind (open-cond plan2)
      (remove-open plan)
    (append (add-step open-cond plan2)
	    (new-link open-cond plan2))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Creates a new current plan with the first open condition removed, and
;;;  returns the removed open condition and the new plan.  Treats the open
;;;  goal set like a stack.
(defun REMOVE-OPEN (plan)
  (let ((open-cond (car (SNLP-plan-open plan))))
    (values open-cond
	    (tweak-plan plan
			:open (cdr (SNLP-plan-open plan))))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 5.1. Adding new steps

;;;;;;;;;;;;;;;;;;;;;;;;                                                 
;;; Returns a list of pairs of new plans and step#.  The new plan
;;; establishes the condition by adding a new step, instantiated 
;;; from a step in *templates* with post-conditions that unify with
;;; the condition. 
;;; Modified by DSW 1/91 to reduce consing
(defun ADD-STEP (open-cond plan)
  (let ((new-plans nil)
	(new-plan nil)
	(bindings nil)
	(condition (car open-cond))
	(id (cadr open-cond)))
    (dolist (templ *templates*) 
      (if (> *trace* 1) (format t "~%New step? ~a" 
				(plan-step-action (car templ))))
      (let* ((new-step-num (+ (snlp-plan-high-step plan) 1))
             (step (instantiate-step (car templ) new-step-num))
             (templ-bind (instantiate-bind (cadr templ) new-step-num)))
	(dolist (add-cond (plan-step-add step))
	  (let ((new-bind (unify condition add-cond 
				 (SNLP-plan-bindings plan))))
	    (cond
	      (new-bind
	       (setf new-bind (car new-bind))
	       (if (> *trace* 0.5) 
		   (format t "~%* New Step ~a  New bindings ~a  unify-count ~a"
			   (plan-step-action step) new-bind *unify-count*))
	       (if (> *trace* 0) (format t "S"))
	       (setf bindings 
		     (add-bind new-bind
			       (add-bind templ-bind 
					 (copy-bindings 
					  (SNLP-plan-bindings plan)))))
	       (when bindings
		 (setf new-plan
		       (tweak-plan plan
			:steps (cons step (SNLP-plan-steps plan))
			:links (cons (list (plan-step-id step) condition id)
				     (SNLP-plan-links plan))
			:ordering (if (eql id :Goal)
				      (SNLP-plan-ordering plan)
				      (cons (list (plan-step-id step) id)
					    (SNLP-plan-ordering plan)))
			:open (nconc (new-step-open step)
				     (SNLP-plan-open plan))
			:bindings bindings
			:high-step (plan-step-id step)))
		 (setf (SNLP-plan-unsafe new-plan)
		       (nconc (test-step new-plan step) 
			      (test-link new-plan
					   (car (SNLP-plan-links new-plan)))))
		 (push new-plan new-plans))
	       ))))))
    new-plans))

;;;;;;;;;;;;;;;;;;;
;;;  Adds the preconditions of the new step as open conditions of the new plan.
(defun NEW-STEP-OPEN (step)
  (let ((new-open nil))
    (dolist (precond (plan-step-precond step))
      (setf new-open (cons 
		      (list precond (plan-step-id step))
		      new-open)))
    new-open))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 5.2. Adding links to old steps

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Creates new plans to achieve an open condition.  Each possibly prior step
;;;  is tested to see if it has an add condition matching the open condition.
;;;  If so, a new plan is created with that link added.
(defun NEW-LINK (open-cond plan)
  (let ((new-plans nil)
	(new-plan nil)
	(new-link nil)
	(condition (car open-cond))
	(id (cadr open-cond))
	(prior-ids (possibly-prior (cadr open-cond) plan)))
    (if (> *trace* 2) (format t "~%Possibly prior steps ~a" prior-ids))
    (dolist (step (SNLP-plan-steps plan))
      (cond
	((member (plan-step-id step) prior-ids)
	 (if (> *trace* 1) (format t "~%New link?  step ~a"
				   (plan-step-id step)))
	 (dolist (add-cond (plan-step-add step))
	   (let ((new-bind (unify condition add-cond 
				  (SNLP-plan-bindings plan))))
	     (cond
	       (new-bind
		(setf new-bind (car new-bind))
		(setf new-link (list (plan-step-id step) condition id))
		(if (> *trace* 0.5) 
		    (format t "~% * New link ~a , bindings ~a   unify-count ~a"
			    new-link new-bind *unify-count*))
		(if (> *trace* 0) (format t "L"))
		(setf new-plan
		      (tweak-plan plan
		       :links (append (SNLP-plan-links plan) (list new-link))
		       :ordering (if (or (eql id :Goal) 
                                         (eql (plan-step-id step) '0))
				     (SNLP-plan-ordering plan)
				     (cons (list (plan-step-id step) id)
					   (SNLP-plan-ordering plan)))
		       :bindings (add-bind new-bind
					   (copy-bindings 
					    (SNLP-plan-bindings plan)))))
		(setf (SNLP-plan-unsafe new-plan) 
		      (append (test-link new-plan new-link) 
			      (SNLP-plan-unsafe new-plan)))
		(setf new-plans (cons new-plan new-plans))
		)))))))
    new-plans))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 6. Handling unsafe links

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Code used to handle a detected unsafety condition.  Returns a list
;;;  of plan refinements that resolve the threat.
(defun HANDLE-UNSAFE (plan)
  (multiple-value-bind (unsafe-link plan2)
      (remove-unsafe plan)
    (if (and (unify (third unsafe-link)          ; if threat still exists...
		    (second (car unsafe-link))
		    (SNLP-plan-bindings plan2))
	     (member (second unsafe-link)
		     (possibly-between (car (car unsafe-link))
				       (third (car unsafe-link))
				       plan2)))
	(append (separate unsafe-link plan2)
		(demote unsafe-link plan2)
		(promote unsafe-link plan2))
	(list plan2))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Creates a new current plan with the first unsafe link removed, and
;;;  returns the removed link and the new plan.
(defun REMOVE-UNSAFE (plan)
  (values (car (SNLP-plan-unsafe plan))
          (tweak-plan plan
           :unsafe (cdr (SNLP-plan-unsafe plan)))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 6.1. Resolving an unsafe link

;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;  To resolve an unsafe link, add constraints that prevent the bindings that
;;;  would clobber it.  The input is a link, the ID of the step that clobbers
;;;  it, and the bindings that clobber it.  The output is a list of new plans.
(defun SEPARATE (unsafe-link plan)
  (let ((ord (append (when (< 0 (caar unsafe-link))
		       `((,(caar unsafe-link) ,(second unsafe-link))))
		     (when (numberp (third (car unsafe-link)))
		       `((,(second unsafe-link) ,(third (car unsafe-link)))))
		     (SNLP-plan-ordering plan))))
    (mapcar #'(lambda (x)
		(tweak-plan plan :ordering ord :bindings x))
	    (not-unify (third unsafe-link) 
		       (second (car unsafe-link))
		       (SNLP-plan-bindings plan)))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  To resolve an unsafe link, add an ordering constraint so that the clobber
;;;  step comes before the establishing step.  The output is a list of new
;;;  plans.
(defun DEMOTE (unsafe-link plan)
  (let* ((clobber-id (cadr unsafe-link))
	 (id (caar unsafe-link))
	 (demotable (member clobber-id (possibly-prior id plan))))
    (if (> *trace* 1) (format t "~%   Demote? step ~a" clobber-id))
    (if (> *trace* 0.5) 
	(if demotable (format t "~%   * Demoting step ~a < ~a" clobber-id id)))
    (if demotable
	(list (tweak-plan plan
	       :ordering (cons (list clobber-id id)
			       (SNLP-plan-ordering plan)))))))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  To resolve an unsafe link, add an ordering constraint so that the clobber
;;;  step comes after the second step of the link.  The output is a list of new
;;;  plans.
(defun PROMOTE (unsafe-link plan)
  (let* ((clobber-id (cadr unsafe-link))
	 (link (car unsafe-link))
	 (id (third link))
	 (promotable (member id (possibly-prior clobber-id plan))))
    (if (> *trace* 1) (format t "~%   Promote? step ~a" clobber-id))
    (if (> *trace* 0.5) 
	(if promotable (format t "~%   * Promoting step ~a" clobber-id)))
    (if promotable
	(list (tweak-plan plan
               :ordering (cons (list id clobber-id)
			       (SNLP-plan-ordering plan)))))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 6.2. Detecting unsafety conditions

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Tests whether a link is possibly clobbered by the delete conditions of any
;;;  steps possibly between the ID1 and ID2 of the link.  Returns nil if the 
;;;  link is safe, otherwise returns a list of (link clobber-id clobber-bind)
(defun TEST-LINK (plan link)
  (let ((new-unsafe nil)
	(bind2 (SNLP-plan-bindings plan))
	(between-ids (intersection (possibly-prior (caddr link) plan)
				   (possibly-after (car link) plan))))
    (if (> *trace* 1) 
        (format t "~%Test link ~a with steps ~a" link between-ids))
    (dolist (step (SNLP-plan-steps plan))
      (cond
	((member (plan-step-id step) between-ids)
	 (if (> *trace* 2) 
             (format t "~% Test step ~a" (plan-step-id step)))
	 (dolist (dele-cond (plan-step-dele step) new-unsafe)
	   (when (unify dele-cond (second link) bind2)
	     (push (list link (plan-step-id step) dele-cond)
		   new-unsafe)

	     (if (> *trace* 0.5) 
		 (format t "~%   * New unsafe ~a   unify-count ~a"
			 (list link (plan-step-id step) dele-cond)
			 *unify-count*))
	     ))
	 (dolist (add-cond (plan-step-add step) new-unsafe)
	   (when (unify add-cond (second link) bind2)
	     (push (list link (plan-step-id step) add-cond) new-unsafe)
	     (if (> *trace* 0.5) 
		 (format t "~%   * New unsafe ~a   unify-count ~a"
			 (list link (plan-step-id step) add-cond)
			 *unify-count*))
	     )))))
    new-unsafe))

;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Tests whether a step possibly clobberes any link.  Returns nil if all
;;;  links are safe, otherwise returns a list of (link clobber-id clobber-bind)
;;;  Assumes that step is only constrained wrt start and a single other point.
;;;  This lets us only have to check if a link's source is prior to the step.
(defun TEST-STEP (plan step &aux (ret nil))
  (let ((prior (possibly-prior (plan-step-id step) plan)))
    (dolist (l (snlp-plan-links plan))
      (when (member (car l) prior)
	(dolist (c (plan-step-add step))
	  (when (unify c (cadr l) (snlp-plan-bindings plan))
	    (push (list l (plan-step-id step) c) ret)
	    (if (> *trace* 0.5) 
		 (format t "~%   * New unsafe ~a   unify-count ~a"
			 (list l (plan-step-id step) c)
			 *unify-count*))
	    ))
	(dolist (c (plan-step-dele step))
	  (when (unify c (cadr l) (snlp-plan-bindings plan))
	    (push (list l (plan-step-id step) c) ret)
	    (if (> *trace* 0.5) 
		 (format t "~%   * New unsafe ~a   unify-count ~a"
			 (list l (plan-step-id step) c)
			 *unify-count*))
	    ))))
    ret))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 7. handle partial orderings.

(defun POSSIBLY-BETWEEN (s1 s2 plan)
  (intersection (possibly-after s1 plan) (possibly-prior s2 plan)))

;;;;;;;;;;;;;;;;;;;;;;;;
;;; Returns list of step-id's of steps possibly prior to a given
;;; step.  Possibly prior always includes the initial conditions.
;;; First build a list of steps constrained to be not prior by
;;; the ordering constraints.  Then add to possibly prior all
;;; steps that aren't in the not-prior list.
(defun POSSIBLY-PRIOR (step-id plan)
  (if (not (eql step-id '0))
      (let ((not-prior (list step-ID :Goal))
	    (poss-prior (list '0)))
	(do* ((queue (list step-id) (cdr queue))
	      (np-step step-id (car queue)))
	     ((null queue) not-prior)
	  (setf not-prior
		(dolist (order (SNLP-plan-ordering plan) not-prior)
		  (cond ((eql np-step (car order))
			 (unless (member (cadr order) not-prior)
                           (setf queue (nconc queue 
                                              (list (cadr order))))
                           (setf not-prior (nconc not-prior 
                                                  (list (cadr order))))))))))
	(dotimes (n (SNLP-plan-high-step plan))
	  (if (not (member (+ n 1) not-prior))
	      (setf poss-prior (cons (+ n 1) poss-prior))))
	poss-prior)))

;;;;;;;;;;;;;;;;;;;;;;;;
;;; Returns list of step-id's of steps possibly after a given
;;; step.  Possibly after always includes the goal conditions.
;;; First build a list of steps constrained to be not prior by
;;; the ordering constraints.  Then add to possibly after all
;;; steps that aren't in the not-after list.
(defun POSSIBLY-AFTER (step-id plan 
		       &aux 
		       (not-after (list step-ID '0))
		       (poss-after (list :Goal)) )
  (when (not (eql step-id :Goal))
    (do* ((queue (list step-id) (cdr queue))
	  (np-step step-id (car queue)))
	 ((null queue) nil)
      (dolist (order (SNLP-plan-ordering plan))
	(when (and (eql np-step (cadr order))
		   (not (member (car order) not-after)))
	  (setf queue (nconc queue (list (car order))))
	  (setf not-after (nconc not-after
				 (list  (car order))))))))
  (dotimes (n (SNLP-plan-high-step plan))
    (if (not (member (+ n 1) not-after))
	(setf poss-after (cons (+ n 1) poss-after))))
  poss-after)

;;; Topological Sort   
;;; Returns correct order: first step at head
;;; Input: max is an integer
;;;    Ordering is a list of pairs (f l) where step number f must be before l
;;;    f, l <= max
;;; See Aho, Hopcoft, Ullman p70 for faster way
(defun TOP-SORT (ordering max)
  (let ((a (top-sort1 (copy-tree ordering) max))
	(b nil))
    (dotimes (i max (nconc a b))
      (when (not (member (1+ i) a :test #'eql))
	(push (1+ i) b)))))

;;; Topological Sort util  -   This code is DESTRUCTIVE!  Pass it a copy!
(defun TOP-SORT1 (ordering max)
  (when ordering
    (let ((as (mapcar #'cadr ordering)))
      (do ((p ordering (cdr p)))
	  ((not (member (caar p) as))
	   (cons (caar p)
		 (top-sort1 (delete-if #'(lambda (x) 
					   (eql (car x) (caar p))) ordering)
			    (- max 1))))))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 8. Handy utility functions

(defun tweak-plan (plan &key           ; initially written by jsp
                        (steps :same)
                        (links :same)
                        (unsafe :same)
                        (open :same)
                        (ordering :same)
                        (bindings :same)
                        (high-step :same))
  "Return a new plan that is a copy of PLAN with the supplied
   instance variable tweaked."
  (flet ((tweak-it (keyword plan-accessor)
           (if (eq keyword :same)
               (funcall plan-accessor plan)
               keyword)))
    (make-snlp-plan
     :steps (tweak-it steps #'snlp-plan-steps)
     :links (tweak-it links #'snlp-plan-links)
     :unsafe (tweak-it unsafe #'snlp-plan-unsafe)
     :open (tweak-it open #'snlp-plan-open)
     :ordering (tweak-it ordering  #'snlp-plan-ordering)
     :bindings (tweak-it bindings #'snlp-plan-bindings)
     :high-step (tweak-it high-step #'snlp-plan-high-step))))


(defun MAKE-SNLP-PLAN (&rest args)
  (setq *plans-created* (+ 1 *plans-created*))
  (apply #'make-SNLP-plan* args))

(defun prop-output (prop plan)
  (cons (car prop)
	(mapcar #'(lambda (x) 
		    (bind-variable x (snlp-plan-bindings plan)))
		(cdr prop))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;  9. Print functions 

;;;;;;;;;;;;;;;;
;;; This version does a toplogical sort and then prints out the steps
;;; in the order in which they should be executed
(defun DISPLAY-PLAN (plan &optional (stream t) ignore)
  (declare (ignore ignore))
  (format stream "~%")
  (let ((steps (make-array (+ 1 (snlp-plan-high-step plan))))
        (order (top-sort (snlp-plan-ordering plan) 
			 (snlp-plan-high-step plan)))
        (goal nil))
    (dolist (step-n (SNLP-plan-steps plan))
	    (cond 
	     ((eql (plan-step-id step-n) '0)
	      (format stream "~%Initial  : ~a~%" (plan-step-add step-n)))
	     ((eql (plan-step-id step-n) :Goal)
	      (setf goal (plan-step-precond step-n)))
	     (t
	      (setf (aref steps  (plan-step-id step-n)) step-n))))
    (dotimes (i (snlp-plan-high-step plan))
	     (let* ((sn (nth i order))
		    (step (aref steps sn)))
	       (format stream "~%Step ~3a : ~15a   Created ~2a" 
		       (+ 1 i)
		       (when step
		         (prop-output (plan-step-action step) plan))
		       sn)))
    (format stream "~%~%Goal     : ~a" goal)
    (if (or (SNLP-plan-unsafe plan)
            (SNLP-plan-open plan))
        (format stream "~%Unsafe   : ~a ~%Open     : ~a"
                (mapcar #'(lambda (x) 
			    `((,(caar x) 
			       ,(prop-output (cadar x) plan) ,(caddar x))
			      ,(cadr x)))
			(SNLP-plan-unsafe plan))
                (mapcar #'(lambda (x) 
			    `(,(prop-output (car x) plan) ,(cadr x)))
			(SNLP-plan-open plan)))
      (format stream "~%Complete!"))))

(defun PRINT-PLAN (plan &optional (stream t) depth)
  (declare (ignore depth))
  (if *verbose* (display-plan plan stream)
    (format stream "#SNLP-Plan<S=~a; O=~a; U=~a>" 
	    (- (length (snlp-plan-steps plan)) 2)
	    (length (snlp-plan-open plan))
	    (length (snlp-plan-unsafe plan)))))