;;;
;;; Test Of Search Tools
;;; Based Roughly On The Descriptions In Russell & Norvig (1995)
;;;
;;; DAVID NOELLE  Thu Apr 18 09:44:26 1996
;;;


;;;
;;; Queue Structure
;;;

(defstruct (queue
	    (:constructor make-queue-given-list)
	    (:copier copy-queue-structure-only))
  (head nil :type list)
  (tail nil :type list))
  

;;; make-queue  --  Makes a queue with a copy of the list "elements" 
;;; as the initial contents.
(defun make-queue (&optional elements)
  (declare (type list elements))
  (let* ((lst (copy-list elements))
	 (q (make-queue-given-list :head lst :tail (last lst))))
    (declare (type list lst)
	     (type queue q))
    q))

;;; copy-queue  --  Makes a copy of the given queue, copying the
;;; conses in the list.
(defun copy-queue (q)
  (declare (type queue q))
  (let ((new-q (copy-queue-structure-only q)))
    (declare (type queue new-q))
    (setf (queue-head new-q) (copy-list (queue-head q)))
    (setf (queue-tail new-q) (last (queue-head new-q)))
    new-q))


;;;
;;; Queue Utilities
;;;

;;; empty-queue-p  --  Returns non-nil is the given queue is empty.
(defun empty-queue-p (q)
  (declare (type queue q))
  (null (queue-head q)))

;;; remove-front  --  Remove the front element from the given queue,
;;; and return that element.
(defun remove-front (q)
  (declare (type queue q))
  (let ((front (queue-head q)))
    (declare (type list front))
    (unless (null front)
      (when (eq front (queue-tail q))
	(setf (queue-tail q) nil))
      (setf (queue-head q) (rest front))
      (car front))))

;;; enqueue-at-front  --  Place a copy of "elements", in the given
;;; order, at the front of the given queue.  This is really treating
;;; the queue like a stack.
(defun enqueue-at-front (elements q)
  (declare (type list elements) (type queue q))
  (setf (queue-head q) (append elements (queue-head q)))
  (when (null (queue-tail q))
    (setf (queue-tail q) (last (queue-head q))))
  q)

;;; enqueue-at-end  --  Place a copy of "elements", in the given
;;; order, at the end of the given queue.
(defun enqueue-at-end (elements q)
  (declare (type list elements) (type queue q))
  (let ((new-tail (copy-list elements)))
    (declare (type list new-tail))
    (if (null (queue-head q))
      (setf (queue-head q) new-tail)
      (setf (cdr (queue-tail q)) new-tail))
    (unless (null new-tail)
      (setf (queue-tail q) (last new-tail))))
  q)


;;;
;;; Problem Structure
;;;

;;;
;;; States can be of any type, as long as the user is consistent.
;;;
;;; Operators are functions which take a current state as input and 
;;; return a fresh list containing child states of that current
;;; state.  The order of this returned list will determine the order
;;; in which siblings are visited.  Also, the order of operators
;;; in the "operators" field of a "problem", below, will determine
;;; the order in which operators are applied.
;;;
;;; Goal test functions take a state as input and return non-nil if
;;; the given state is a goal state.
;;;
;;; Path cost functions take a single list, representing a path, as
;;; input, and return the cost of that path.  Each element of the
;;; input list is a list containing three elements describing the
;;; application of an operation:  the parent state, the operator
;;; function, and the resulting child state.
;;;

(defstruct (problem
	    (:constructor make-problem-given-operator-list)
	    (:copier copy-problem-structure-only))
  (initial-state nil :type t)
  (operators nil :type list)
  (goal-test #'identity :type function)
  (path-cost-function #'list-length :type function))
  

;;; make-problem  --  Makes a problem, copying the given "operators" list.
(defun make-problem (&key (initial-state nil)
			  (operators (list #'identity) operators-provided-p)
			  (goal-test #'identity)
			  (path-cost-function #'list-length))
  (declare (type list operators) 
	   (type function goal-test)
	   (type function path-cost-function))
  (let ((ops
	 (if operators-provided-p
	   (copy-list operators)
	   operators)))
    (declare (type list ops))
    (make-problem-given-operator-list
     :initial-state initial-state
     :operators ops
     :goal-test goal-test
     :path-cost-function path-cost-function)))

;;; copy-problem  --  Copys the given problem, copying the conses of
;;; the operators list.
(defun copy-problem (p)
  (declare (type problem p))
  (let ((new-p (copy-problem-structure-only p)))
    (declare (type problem new-p))
    (setf (problem-operators new-p) (copy-list (problem-operators p)))
    new-p))


;;;
;;; Node Structure
;;;

(defstruct node
  (state nil :type t)
  (parent-node nil :type (or node nil))
  (operator nil :type (or function nil))
  (depth 0 :type fixnum)
  (path-cost 0 :type fixnum))
  

;;;
;;; Node Utilities
;;;

;;;
;;; These utilities count the number of calls to the "expand" function.
;;;

(defvar *expand-count* 0)

(defun clear-expand-count ()
  (setq *expand-count* 0))

(defun increment-expand-count ()
  (incf *expand-count*))

(defun current-expand-count ()
  *expand-count*)

;;; expand  --  Expand the given parent node, linking in children
;;; generated by application of the given list of operators.  Also,
;;; update the path cost to each of these new children.  Return a
;;; fresh list of the children.
(defun expand (parent-node 
	       operators 
	       &optional 
	       (path-cost-function #'list-length))
  (declare (type node parent-node) (type list operators))
  (let* ((child-depth (+ (node-depth parent-node) 1))
	 (parent-state (node-state parent-node))
	 (parent-path-cost (node-path-cost parent-node))
	 (children
	  (loop
	    for op in operators 
	    nconc 
	    (loop
	      for child-state in (funcall op parent-state)
	      collect 
	      (make-node :state child-state
			 :parent-node parent-node
			 :operator op
			 :depth child-depth
			 :path-cost
			 (+ parent-path-cost
			    (funcall 
			     path-cost-function
			     (list (list parent-state op child-state)))))))))
    (increment-expand-count)
    children))

  
;;;
;;; General Search
;;;

(defun general-search (prob 
		       qing-function 
		       &key
		       depth-limit 
		       path-cost-limit)
  (declare (type problem prob) 
	   (type function qing-function)
	   (type (or fixnum nil) depth-limit)
	   (type (or fixnum nil) path-cost-limit))
  (let ((frontier (make-queue 
		   (list 
		    (make-node :state (problem-initial-state prob)
			       :parent-node nil
			       :operator nil
			       :depth 0
			       :path-cost 0))))
	(current-node nil)
	(goal-test-function (problem-goal-test prob))
	(all-operators (problem-operators prob))
	(path-cost-function (problem-path-cost-function prob)))
    (loop
      (when (empty-queue-p frontier)
	(return nil))            ; Failure!
      (setq current-node (remove-front frontier))
      (when (funcall goal-test-function (node-state current-node))
	(return current-node))   ; Success!
      (unless (or (and depth-limit 
		       (>= (node-depth current-node) depth-limit))
		  (and path-cost-limit 
		       (>= (node-path-cost current-node) path-cost-limit)))
	;; Expand a node ...
	(setq frontier
	      (funcall qing-function
		       (expand current-node all-operators path-cost-function)
		       frontier))))))


;;;
;;; Print Path
;;;

(defun print-path (end-node 
		   &optional 
		   (stream t) 
		   (state-printing-function #'prin1))
  (declare (type node end-node))
  (let ((parent-node (node-parent-node end-node)))
    (when parent-node
      (print-path parent-node stream state-printing-function)
      (format stream " => "))
    (funcall state-printing-function (node-state end-node) stream)
    stream))

    
;;;
;;; Breadth-First Search
;;;

(defun breadth-first-search (prob &key depth-limit path-cost-limit)
  (declare (type problem prob) (type (or fixnum nil) depth-limit))
  (general-search prob #'enqueue-at-end 
		  :depth-limit depth-limit :path-cost-limit path-cost-limit))


;;;
;;; Depth-First Search
;;;

(defun depth-first-search (prob &key depth-limit path-cost-limit)
  (declare (type problem prob) (type (or fixnum nil) depth-limit))
  (general-search prob #'enqueue-at-front
		  :depth-limit depth-limit :path-cost-limit path-cost-limit))



;;;
;;; Romania Route Searching Functions
;;;

;;;
;;; State Description & Operators
;;;

(defvar *city-adjacency-list*
  '((arad . (sibiu timisoara zerind))
    (bucharest . (fagaras giurgiu pitesti urziceni))
    (craiova . (dobreta pitesti rimnicu-vilcea))
    (dobreta . (craiova mehadia))
    (eforie . (hirsova))
    (fagaras . (bucharest sibiu))
    (giurgiu . (bucharest))
    (hirsova . (eforie urziceni))
    (iasi . (neamt vaslui))
    (lugoj . (mehadia timisoara))
    (mehadia . (dobreta lugoj))
    (neamt . (iasi))
    (oradea . (sibiu zerind))
    (pitesti . (bucharest craiova rimnicu-vilcea))
    (rimnicu-vilcea . (craiova pitesti sibiu))
    (sibiu . (arad fagaras oradea rimnicu-vilcea))
    (timisoara . (arad lugoj))
    (urziceni . (bucharest hirsova vaslui))
    (vaslui . (iasi urziceni))
    (zerind . (arad oradea))))

(defun print-city (city &optional (stream t))
  (prin1 city stream))

;;; all-adjacent-cities-operator  --  Return a fresh list of all
;;; cities adjacent to the given city.      
(defun all-adjacent-cities-operator (city)
  (copy-list (cdr (assoc city *city-adjacency-list*))))


;;;
;;; Find Route Between Cities
;;;

;;; find-route  --  Use breadth-first search to find the shortest
;;; route from the "source" city to the "destination" city.  Routes
;;; are measured in number of cities visited.  Note that this
;;; function is pretty dumb.  It uses the "all-adjacent-cities-operator"
;;; and doesn't even check for cycles in the route.  Much more
;;; efficient searches would be possible if the operator used was
;;; smarter.
(defun find-route (source destination)
  (let ((problem 
	 (make-problem :initial-state source
		       :operators '(all-adjacent-cities-operator)
		       :goal-test (function
				   (lambda (city)
				     (eq city destination)))))
	(goal-node nil))
    (clear-expand-count)
    (setq goal-node (breadth-first-search 
		     problem :depth-limit (list-length *city-adjacency-list*)))
    (if goal-node
      (print-path goal-node t #'print-city)
      (format t "~&No path found between ~A and ~A!~&" source destination))
    (format t "~&Number of nodes expanded:  ~D.~&" (current-expand-count))))