;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;  search.lisp
;;;  Written by:  Shaul Markovitch
;;;  Last time modified: 8/2/2000
;;;  
;;;  8/2/2000: Extract-path was enhanced to remove cycles.  While 
;;;            without using macros cycles in the solution path are
;;;            not possible, using macros can present such cycles
;;;            (since the path of the macro is expanded only at the
;;;            end).  This caused problems to the min-to-better
;;;            attention filter which could generate macros with null
;;;            end state.

;;;  
;;;  This file contains the general and specific functions that 
;;; implements search technqiues.  





;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; perform-search
;;;
;;; This is the main search function.  
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun perform-search 
  (problem domain search-strategy macro-db
	   &key (return-path t)
	   (return-data nil))
  (gc )
  (gc-off)
  (let* ((start-time (get-internal-run-time))
	 (outcome 
	  (graph-search problem domain search-strategy macro-db)))
    (setf (search-outcome-cpu-seconds outcome)
	  (compute-seconds start-time))
    (setf (search-outcome-solution-length outcome)
	  (solution-length (search-outcome-solution-path outcome)))
    (setf (search-outcome-total-cost outcome)
	  (compute-total-cost outcome))
    (when (not return-path)
	  (setf (search-outcome-solution-path
				  outcome)
				 nil))
    (when (not return-data)
	  (setf (search-outcome-data
				  outcome)
				 nil)
	  )
    (gc-on)
    (gc)
    outcome))
              



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Best-first-search
;;;
;;; This function implements the best-first technique.
;;; It gets a problem, a domain, a search-strategy and a set
;;; of macros.  It returns a search-outcome structure which contains
;;; the solution and various statistics about the search.
;;; 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defclass best-first-search
  (search)
  ((update-g :initform nil)
   (reopen-flag :initform nil)
   ))

(defclass astar-epsilon-search
  (search)
  ((next-node-selector :initform 'select-epsilon)
   (epsilon :initform 0.1
	    :initarg :epsilon
	    :accessor search-epsilon
   )))

(defclass uniform-cost-search
  (search) nil)

(defparameter *default-weight* 0.5)
(defclass weighted-astar-search
  (search)
  ((w :initform *default-weight*
      :initarg :w
      :accessor search-w
      
  )))

(defmethod f-value ((node search-node)(strategy search))
  (+ (search-node-h node)(search-node-g node)))

(defmethod f-value ((node search-node)(strategy best-first-search))
  (search-node-h node))

(defmethod f-value ((node search-node)(strategy uniform-cost-search))
  (search-node-g node))

(defmethod f-value ((node search-node)(strategy weighted-astar-search))
  (let ((w (search-w strategy)))
    (+ (* (- 1 w)(search-node-g node))(* w (search-node-h node)))))

(defparameter *astar* (make-instance 'search))
(defparameter *uniform-cost* (make-instance 'uniform-cost-search))
(defparameter *best-first* (make-instance 'best-first-search))
(defparameter *astar-epsilon* (make-instance 'astar-epsilon-search))
(defparameter *weighted-astar* (make-instance 'weighted-astar-search))

(defun select-epsilon
  (open-list search-strategy)
  
  (loop with min-node = nil
	with min-val = most-positive-fixnum
	with lim = (* (+ 1 (search-epsilon search-strategy))
		      (search-node-f (first open-list)))
	for node in open-list
	do  
	   (when (< (search-node-h node) min-val)
		 (setf min-node node min-val (search-node-h node)))
	until (> (search-node-f node) lim)
	finally (return min-node)))


(defun graph-search
  (problem domain search-strategy
	   &optional (macro-db
		      nil))
  (let* ( (data (make-search-data))
	  (outcome (make-search-outcome :data data))
         (h-func (domain-heuristic-fn domain))
         (limit (search-resource-limit search-strategy))
         (first-node (make-search-node 
		      :state (problem-init-state problem)
		      :h (funcall h-func (problem-init-state problem)
				  (problem-goal-state problem) domain)
		      :g 0))
         )
	 
    (when (search-use-hash search-strategy)
          (initialize-search-hash data domain ))
    (setf (search-node-f first-node)
        (f-value first-node search-strategy))
    (add-to-open first-node data domain search-strategy)
    (loop until (or (null (search-data-open data))
		    (> (search-outcome-generated outcome) limit))
	     
	  ;; If the program managed to exit the loop through
	  ;; thiis point it means that it did not find a goal
	  ;; state.  Either becuase open is NIL (which means
	  ;; that there is no solution to this problem) or
	  ;; becuase the program exhausted its resource limit.
	     
	  finally (progn (setf (search-outcome-solution-found
				outcome)
			       (if (null (search-data-open data)) 'fail 'limit))
			 (return outcome))
	  for current-node =
	  (funcall
	   (search-next-node-selector search-strategy)
	   (search-data-open data) search-strategy)
	  for current-state = (search-node-state current-node)
	  do 
	  ;; When a goal state is found the program returns
	  ;; the necessary information using the search-outcome
	  ;; structure.
                
	  (when (goal-state-p current-state problem domain)
		(setf (search-outcome-solution-found outcome) t)
		(setf (search-outcome-solution-path outcome) current-node)
		(return outcome))
          (remove-from-open current-node data domain search-strategy)
          (add-to-closed current-node data domain search-strategy)
   	  (incf (search-outcome-expanded outcome) 1)
	  (loop for op in (mixup-ops (get-operators current-state problem
						    domain
						    search-strategy macro-db)
				     search-strategy)
		for succ-state = (apply-operator op current-state domain)
		when succ-state
		do 
		 (incf (search-outcome-generated outcome) 1)
		 (insert-state succ-state
			       current-node op
			       data
			       domain search-strategy problem outcome)))
	  ))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Expand-node
;;;

(defun expand-node
  (current-state problem domain search-strategy macro-db)
  (loop for op in (mixup-ops (get-operators current-state problem
							  domain
							  search-strategy macro-db)
					   search-strategy)
                      for succ-state = (apply-operator op current-state domain)
                      when succ-state
		      collect succ-state))
                    
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; insert-state
;;; 
;;; This function insert a newly generated state into open according 
;;; to its heuristic value (open is kept sorted).
;;; If the search-graph-flag field of the search strategy is t
;;; (the default), the program tests whether the new state is
;;; a member in open or closed.  Note that the comparison is doen
;;; by using the equal-state-fn.  This is a predicate for testing
;;; equality between two states and is stored in the domain structure
;;; The function should be modified soon to enable it to use the
;;; optional cache functions specified for the domain.  If such
;;; functions are supplied, the search in closed and open should 
;;; use this cache mechanisms.  For example, in the grid domain 
;;; we could keep two dimensional arrays for the open and closed
;;; list.  Then searching in this list is immediate.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun insert-state
  (state parent-node op data domain search-strategy problem outcome)
  (let* ((new-node-flag t)
	 old-node new-g)
    (when  (search-test-open search-strategy)
	   (setq old-node 
	       (find-state state
	            (search-data-open data)
	            (search-data-open-hash data)
	            domain
	            search-strategy))
	   
	  
	   (when old-node
		 (setf new-node-flag nil)
		 (when (search-update-g search-strategy)
		       (setf new-g (compute-g state parent-node op domain))
		       (when (< new-g (search-node-g
				       old-node))
			     (setf (search-node-g old-node) new-g)
			     (setf (search-node-parent old-node)
				   parent-node)
			     (setf (search-node-op old-node) op)
			     (setf (search-node-f old-node)
				   (f-value old-node search-strategy))
			     (remove-from-open old-node data domain search-strategy)
			     (add-to-open old-node data domain search-strategy)
			     ))))
    (when  (and (search-test-closed search-strategy)(null old-node))
	   (setq old-node 
	       (find-state state
	            (search-data-closed data)
	            (search-data-closed-hash data)
	            domain
	            search-strategy))
	   (when old-node
		 (setf new-node-flag nil)
		 (when (search-update-g search-strategy)
		       (setf new-g (compute-g state parent-node op domain))
		       (when (< new-g (search-node-g
				       old-node))
			     (setf (search-node-g old-node) new-g)
			     (setf (search-node-parent old-node)
				   parent-node)
			     (setf (search-node-op old-node) op)
			     (setf (search-node-f old-node)
				   (f-value old-node search-strategy))
	
			     (when (search-reopen-flag
				    search-strategy)
			     (remove-from-closed old-node data domain search-strategy)
		             (add-to-open old-node data domain search-strategy)
			     (incf (search-outcome-reopened outcome))
			     (incf (search-node-reopened old-node))

			     )))))
   
  (when new-node-flag
	  (let ((new-node (make-search-node
			       :state state
			       :g (compute-g state parent-node op domain)
			       :h (funcall (domain-heuristic-fn domain)
					    state (problem-goal-state
						   problem)
					    domain)
			       :parent parent-node
			       :op op)))
	    (setf (search-node-f new-node) (f-value new-node search-strategy))
	      (add-to-open new-node data domain search-strategy)))))


(defun find-state 
    (state data-list data-hash 
	            domain
	            search-strategy)
 (cond ((search-use-hash search-strategy)
         (funcall (domain-get-hashed-state-fn domain) state data-hash))
       (t (find state data-list :key #'search-node-state 
                       :test (domain-state-equal-fn domain)))))

(defun initialize-search-hash 
   (data domain )
   (setf (search-data-open-hash data)
      (funcall (domain-initialize-hash-fn domain) domain))
   (setf (search-data-closed-hash data)
      (funcall (domain-initialize-hash-fn domain) domain)))

(defun add-to-open
   (node data domain search-strategy)
   (setf (search-data-open data)
      (merge 'list
	 (list node)
        (search-data-open data)
	 #'<
	  :key (search-sort-accessor search-strategy)))
    (when (search-use-hash search-strategy)
       (funcall (domain-put-hashed-state-fn domain) 
       (search-node-state node) (search-data-open-hash data) node)))
       
(defun add-to-closed
   (node data domain search-strategy)
   (push node (search-data-closed data))
   (when (search-use-hash search-strategy)
       (funcall (domain-put-hashed-state-fn domain) 
       (search-node-state node) (search-data-closed-hash data) node)))
       

(defun remove-from-open
   (node data domain search-strategy)
   (setf (search-data-open data)
      (delete node (search-data-open data) :test #'eq))
   (when (search-use-hash search-strategy)
       (funcall (domain-put-hashed-state-fn domain) 
       (search-node-state node) (search-data-open-hash data) nil)))
       
(defun remove-from-closed
   (node data domain search-strategy)
   (setf (search-data-closed data)
      (delete node (search-data-closed data) :test #'eq))
   (when (search-use-hash search-strategy)
       (funcall (domain-put-hashed-state-fn domain) 
      (search-node-state node) (search-data-closed-hash data) nil)))
                                   
                                            
(defun compute-g
  (state parent-node op domain)
  (+ (search-node-g parent-node)
     (if (macro-p op)
	 (loop for o in (macro-operators op)
	       sum 
	       (funcall (domain-cost-fn domain)
			(search-node-state parent-node)
			state o))
       (funcall (domain-cost-fn domain)
			(search-node-state parent-node)
			state op))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;  goal-state-p
;;; 
;;;  A problem has two alternative ways of specifying goal state.
;;; goal state can be given explicitly in the goal-state field,
;;; or can be given as a pedicate in the goal-p filed.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun goal-state-p
  (state problem domain &aux (goal-predicate (problem-goal-p problem)))
  (if goal-predicate
      (funcall goal-predicate state domain)
       (funcall (domain-state-equal-fn domain)
                                state (problem-goal-state problem))))
                                
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; mixup-op
;;;
;;; We usually mix the operator to make the search less deterministic
;;; and enable unbiased exrimenatation.  The sarch-mixup-flag in the
;;; search strategy structure determines whether to perform the mixup
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun mixup-ops
  (ops search-strategy)
  (if (search-mixup-flag search-strategy)
      (shuffle ops)
    ops))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; apply-operator
;;;
;;; An operator can be a basic operator or a macro operator.
;;; A basic operator is applied using the apply-op-fn supplied
;;; with the domain.  Macro operators are handled by the function
;;; apply-macro-op.  
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun apply-operator
  (op state domain)
  ;; When you define a structure "macro", commonlisp automatically
  ;; creates the function macro-p that tests whether an object is a 
  ;; structure of this type.
  (cond ((macro-p op)(apply-macro-op op state domain))
	(t (funcall (domain-apply-op-fn domain) op state domain))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; apply-macro-op
;;;
;;; If the current state is the same state as the state where the
;;; macro was learned (the start-state field of the macro), the we
;;; could directly goo o the end state without bothering about
;;; the wat to get there (we only need to recover that path if the
;;; macro is part of the solution).
;;; [Note that this test is orthogonal to the utilization filter 
;;; that allows only macros of the same state.  If such a filter
;;; is active, the first condition will always succeed here.  However,
;;; if the utilization filter is more liberal, we can use this condition
;;; to save the application of the seqeunce of operators in the case
;;; where the the two states are equal.  
;;; If the states are not the same then the sequence of operators
;;; associated with the macros is applied to the current state.
;;; If one of the operator application causes an illegal state
;;; (returns NIL), then the application of the macro operator will
;;; also return NIL.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun apply-macro-op
  (macro-op state domain)
  (cond ((funcall (domain-state-equal-fn domain)
		  state (macro-start-state macro-op))
	 (macro-end-state macro-op))
	(t (loop for op in (macro-operators macro-op)
		 while state
		 do (setf state (apply-operator op state domain)))
	   state)))
	   
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; get-operators
;;;
;;; This function implements the macro usage and utlization filtering.
;;; It returned the operators that can be applied to the current
;;; state.  If macro usage is not allowed then it just returns
;;; the basic operators.  Otherwise it filter the macros and concatenates
;;; the resulted set with the basic operators.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun get-operators
  (state problem domain search-strategy macro-db)
  (let (
	(basic-operators (domain-operators domain)))
    (cond ((and (search-use-macros-flag search-strategy)
		macro-db)
	   (append basic-operators
		   (if
		     (search-use-utilization-filter-flag search-strategy)
		     (utilization-filtering
		      (search-utilization-filtering-technique
					    search-strategy)
                                            macro-db
                                            state
                                            problem
                                            domain
                                            search-strategy)
		     (macro-db-macros macro-db))))
           (t basic-operators))))
                                           
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Solution-length
;;; 
;;; A utility function that follows the parent link to measure the
;;; length of the solution path.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun solution-length
  (node)
  (when (search-node-p node)
  (loop while (search-node-parent node)
	do (setf node  (search-node-parent node))
	sum (if (macro-p (search-node-op node))
		(length (macro-operators (search-node-op node)))
	      1))))


(defun solve-problem-set
  (&optional
   (problems *default-problem-set*)
   (domain *default-domain*)
   (search-strategy *default-search-strategy*)
   (macro-db *default-macro-db*))
  
  (let*
     ((start-time (get-internal-run-time))
      (results
	(loop for problem in problems
	      do (format t ".")(force-output t)
	      collect
	      (perform-search problem
			domain search-strategy
			macro-db :return-path nil)))
    (outcome
      (make-problem-set-outcome
       :total-cpu (compute-seconds start-time)
      :n (length problems)
      :generated (collect-statistics results :key
				     #'search-outcome-generated)
      :generated-list (loop for r in results collect
			 (search-outcome-generated r))
      :expanded (collect-statistics results :key
				     #'search-outcome-expanded)
      :reopened (collect-statistics results :key
				     #'search-outcome-reopened)
      :cpu-seconds (collect-statistics results :key
				     #'search-outcome-cpu-seconds)
      :solution-cost (collect-statistics results :key
				     #'search-outcome-solution-cost)
      :solution-length (collect-statistics results :key
				     #'search-outcome-solution-length)
      :length-list (loop for r in results collect
			 (search-outcome-solution-length r))
      :total-cost (collect-statistics results :key
				     #'search-outcome-total-cost)

      )))
    
    
    (values outcome results)))

(defun total-cost (gen len ratio)
  (+ len (* ratio gen)))

(defparameter *comp/move-cost-factor* 0.01)

(defun compute-total-cost
  (outcome &key (ratio *comp/move-cost-factor*))
  (total-cost (search-outcome-generated outcome)
	      (search-outcome-solution-length outcome)
	      ratio))
	      

(defun update-total-cost (outcome ratio)
  (let ((new-outcome (copy-problem-set-outcome outcome)))
    (setf (problem-set-outcome-total-cost new-outcome)
	  (collect-statistics
	   (loop for gen in (problem-set-outcome-generated-list
			     outcome)
		 for len in (problem-set-outcome-length-list
			     outcome)
		 collect (total-cost gen len ratio))))
    new-outcome))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Extract-path
;;;
;;; One of the fields of search-outcome is the node of the goal state.
;;; This function follows the "parent" links and returns a list of
;;; pairs.  Each pair consists of a basic operator and a state 
;;; (the operator that lead to the state)
;;; The first pair is the initial state therefore its associated 
;;; operator is NIL.  The function unfolds macro operators into
;;; a sequence of basic operators and states.  This function is mainly
;;; used by the attention procedure which use the solution path for
;;; extracting macros.
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun extract-path
  (search-outcome domain &key (remove-cycles t))
  (let* ((solution-path nil)
        (current-node (search-outcome-solution-path search-outcome))
	(final-state (search-node-state current-node)))
      (loop while (search-node-parent current-node)
            for state = (search-node-state current-node)
            for op = (search-node-op current-node)
            do
            (cond ((macro-p op)
                   (setf solution-path
                     (append (loop for basic-op in (macro-operators op)
                         with next-state = (search-node-state
					    (search-node-parent current-node))
                         do (setf next-state 
                               (apply-operator basic-op next-state domain))
                         collect (list basic-op next-state
				       (funcall (domain-heuristic-fn
						 domain)
						next-state final-state )))
                         solution-path)))
                   (t (push (list op state (search-node-h current-node))
			    solution-path)))
             (setf current-node (search-node-parent current-node)))
      (push (list nil (search-node-state current-node)
		  (search-node-h current-node)) solution-path)
      (when remove-cycles
        (loop for tail on solution-path
              for cur-state = (second (first tail))
              while  (rest tail)
              do (loop for s-tail on (rest tail)
                       for next-state = (second (first s-tail))
                       when (funcall (domain-state-equal-fn domain)
                                     cur-state next-state)
                       do (setf (rest tail)(rest s-tail)))))
       solution-path))