;;; File: search/problems.lisp

;;; This file contains the basics for solving problems by searching.  First
;;; we define data types for problems and nodes, and for expanding nodes.
;;; Solutions are represented just by the node at the end of the path.  The
;;; function SOLUTION-ACTIONS returns a list of actions that get there.  It
;;; would be problematic to represent solutions directly by this list of
;;; actions, because then we couldn't tell a solution with no actions from a
;;; failure to find a solution.  Finally, this file shows how to hook a
;;; problem-solver up to an environment.

(defstruct (problem (:print-function print-problem))
  "Everything we need to define a problem. [p 60]"
  (initial-state (required)) ; a state in the domain
  (successor-fn (required))  ; fn: state -> alist of (action . state)s
  (goal-test (required))     ; fn: state -> boolean
  (g-cost-fn nil)            ; fn: state x state -> cost ; path-cost
  (h-cost-fn nil)            ; fn: state -> cost
  (edge-cost-fn #'one)       ; fn: state x state -> cost
  (hash-key #'identity)      ; fn: state -> list, for hashing states
  (print-fn #'princ)         ; for printing states
  (domain "Generic")         ; a string, naming the problem domain
  (display-fn #'display-expand) ; Maybe print something when a node is expanded
  (num-expanded 0)           ; Number of nodes expanded in search for solution.
  (iterative? nil)           ; Are we using an iterative algorithm?
  )

(defstruct (node (:print-function print-node))
  "Node for generic search.  A node contains a state, a domain-specific
  representation of a point in the search space.  A node also contains 
  bookkeeping information such as the cost so far (g-cost) and estimated cost 
  to go (h-cost). [p 72]"
  (state (required))        ; a state in the domain
  (parent nil)              ; the parent node of this node
  (action nil)              ; the domain action leading to state
  (successors nil)          ; list of sucessor nodes
  (unexpanded nil)          ; successors not yet examined (SMA* only)
  (depth 0)                 ; depth of node in tree (root = 0)
  (g-cost 0)                ; path cost from root to node
  (h-cost 0)                ; estimated distance from state to goal
  (f-cost 0)                ; g-cost + h-cost
  (expanded? nil)           ; any successors examined?
  (completed? nil)          ; all successors examined? (SMA* only)
  )

;;;; Functions for Expanding Nodes

(defun expand (node problem)
  "Generate a list of all the nodes that can be reached from a node."
  ;; Note the problem's successor-fn returns a list of (action . state) pairs.
  ;; This function turns each of these into a node.
  ;; If a node has already been expanded for some reason, then return no nodes,
  ;; unless we are using an iterative algorithm.
  (unless (and (node-expanded? node) (not (problem-iterative? problem)))
    (setf (node-expanded? node) t)
    (incf (problem-num-expanded problem))
    (funcall (problem-display-fn problem) node problem)
    (let ((successor-fn (problem-successor-fn problem))
	  (g-cost-fn (problem-g-cost-fn problem))
	  (h-cost-fn (problem-h-cost-fn problem))
	  (edge-cost-fn (problem-edge-cost-fn problem)))
      (with-collection ()
       (for each (action . state) in (funcall successor-fn (node-state node)) do
	    (collect
	     (let* ((g (if g-cost-fn 
			   (funcall g-cost-fn state
				    (problem-initial-state problem))
			 (+ (node-g-cost node)
			    (funcall edge-cost-fn (node-state node) state))))
		    (h (if h-cost-fn (funcall h-cost-fn state) 0)))
	       (make-node 
		:parent node :action action :state state
		:depth (1+ (node-depth node))
		:g-cost g :h-cost h
		;; use the pathmax equation for f:
		:f-cost (if h-cost-fn (max (node-f-cost node) (+ g h))
			  g)))))))))

(defun create-start-node (problem)
  "Make the starting node, corresponding to the problem's initial state."
  (let ((h (if (problem-h-cost-fn problem)
	       (funcall (problem-h-cost-fn problem)
			(problem-initial-state problem)))))
    (make-node :state (problem-initial-state problem)
	       :h-cost h :f-cost h)))

;;;; Functions for Manipulating Solutions

(defun solution-actions (node &optional (actions-so-far nil))
  "Return a list of actions that will lead to the node's state."
  (cond ((null node) actions-so-far)
	((null (node-parent node)) actions-so-far)
	(t (solution-actions (node-parent node)
			     (cons (node-action node) actions-so-far)))))

(defun solution-nodes (node &optional (nodes-so-far nil))
  "Return a list of the nodes along the path to the solution."
  (cond ((null node) nodes-so-far)
	(t (solution-nodes (node-parent node)
			   (cons node nodes-so-far)))))

(defun solve (problem &key (print nil) (algorithm (select-searcher problem)))
  "Return a list of actions that will solve the problem (if possible).
  Also return as second value the node that solves the problem, or nil."
  (setf (problem-num-expanded problem) 0)
  (let ((node (funcall algorithm problem)))
    (when print
      (print-solution problem node))
    (values (solution-actions node) node)))

(defun print-solution (problem node)
  "Print a table of the actions and states leading up to a solution."
  (if node
      (format t "~&Action ~20T State~%===== ~20T ======~%")
    (format t "~&No solution found.~&"))
  (for each n in (solution-nodes node) do
       (format t "~&~A ~20T ~A~%"
	       (or (node-action n) "") (node-state n)))
  (format t "===== ~20T ======~%Total of ~D node~:P expanded."
	  (problem-num-expanded problem))
  node)

(defun solved? (problem node)
  "Does the node's state solve the problem?
  NODE can also be a state or environment; anything we can pick a state out of."
  (let ((state (typecase node
		 (node (node-state node))
		 (environment (environment-state node))
		 (t node))))
    (if (funcall (problem-goal-test problem) state)
	node
      nil)))

(defun select-searcher (problem)
  "Select an appropriate search algorithm for this problem."
  (cond ((problem-h-cost-fn problem) #'A*-search)
	(t #'iterative-deepening-search)))

(defun node-path-length (node)
  (if (null (node-parent node)) 0 (+ 1 (node-path-length (node-parent node)))))
  
;;;; Comparing Algorithms

(defun compare-search-algorithms (problem-fn algorithms &key (n 10))
  "Run each algorithm on N problems (as generated by problem-fn)
  and compare the results for nodes expanded and for path cost."
  (let ((random-state (make-random-state t)))
    (format t "~&Solved  Cost  Length  Nodes  Algorithm")
    (format t "~&====== ====== ====== ======= =========")
    (for each algorithm in algorithms do
	 (let ((g-cost 0)
	       (num-expanded 0)
	       (num-solved 0)
	       (path-length 0)
	       (*random-state* (make-random-state random-state)))
	   (for i = 1 to n do
		(let* ((problem (funcall problem-fn))
		       (solution (funcall algorithm problem)))
		  (incf num-expanded (problem-num-expanded problem))
		  (when solution
		    (incf num-solved)
		    (incf path-length (node-path-length solution))
		    (incf g-cost (node-g-cost solution)))))
	   (let ((M (if (= num-solved 0) 1 num-solved)))
	     (format t "~&~5D  ~6,1F ~6,1F ~7,1F ~A~%"
		     num-solved (/ g-cost M) (/ path-length M)
		     (/ num-expanded N) algorithm)))))
  (values))

;;;; Printing

(defun print-problem (problem stream depth) 
  (declare (ignore depth))
  (print-unreadable-object (problem stream)
    (format stream "~A problem state:~A, expanded:~D"
            (problem-domain problem) (problem-initial-state problem)
	    (problem-num-expanded problem))))

(defun print-node (node stream depth) 
  (declare (ignore depth))
  (print-unreadable-object (node stream :type t)
    (format stream "f(~D) = g(~D) + h(~D) state:~A" (node-f-cost node)
		(node-g-cost node) (node-h-cost node) (node-state node))))

(defun display-expand (node problem)
  "A sample function to fill the problem-display-fn slot.
  This gets called whenever a node is expanded."
  (declare (ignore problem))
  (dprint 'expanding node))