>;;;; -*- mode:Scheme -*- ;;;;

;;; SEARCH-UPDATE (for 6.034 by TLP@mit.edu)
;;; The complete version of search-update, handles inconsistent 
;;; heuristics and detects duplicates added to pending.

(declare (usual-integrations))

;;; This is the more complete version (note the call to keep-best)
(define (SEARCH-UPDATE current neighbors pending expanded visited)
  ;; We've expanded current, so add it to expanded list.
  (if expanded (expanded 'add (list current)))
  (cond ((or expanded visited)
	 (let ((kept-nodes
		(filter (lambda (node)
			  ;; Update the estimate using pathmax (if necessary)
			  (pathmax-update-estimate current node)
			  ;; Check if this state has been seen before
			   (if (and expanded visited)
			      ;; General case - look at actual costs
			      (keep-best-nodes node pending expanded visited)
			      ;; The simple case - just exclude members of either list
			      (not ((or visited expanded) 'member (search-node-state node)))))
			neighbors)))
	   ;; Add the new nodes to the pending (and visited) list
	   (pending 'add kept-nodes)
	   (if visited (visited 'add kept-nodes))))
	(else
	 ;; just add to pending.
	 (pending 'add neighbors))))

;; If the heuristic is inconsistent, then it is possible to find a better 
;; path to an already expanded node.  In that case, we want to remove the old node
;; from the expanded list and keep it for insertion back in pending.
;; If the node is not in expanded but is in the visited list, that means that it is
;; on pending and we want to remove it from there, and keep the new node for insertion in pending.
;; This last part is just to keep the size of the search-queue small.

;; A similar situation obtains when we use PATHMAX, the estimate now depends on the
;; path, so we need to see if we get to a node with a larger (more accurate) estimate.

;; In general, we keep shorter paths and larger estimates.

(define (KEEP-BEST-NODES node pending expanded visited)

  (define (forget-expanded node)
    (expanded 'remove node)
    (visited 'remove node))

  (define (forget-visited node)
    (pending 'remove node)
    (visited 'remove node))

  (define (copy-path from-node to-node)
    (set-search-node-actual! to-node (search-node-actual from-node))
    (set-search-node-predecessor! to-node (search-node-predecessor from-node)))
    
  
  (let ((enode (expanded 'member (search-node-state node)))
	(vnode (visited 'member (search-node-state node)))
	(state (search-node-state node)))
    (cond
     (enode
      (display "E")
      ;; Node is in expanded list
      (cond ((< (search-node-actual node) (search-node-actual enode))
	     (if #t ;;*verbose* 
		 (display* "Found a better path to expanded state: "
			   (state-name state) " "
			   (search-node-actual enode) "->"
			   (search-node-actual node)))
	     ;; found a beter path, forget the old one
	     (forget-expanded enode)
	     ;; add the new node (better path) to pending (and visited)
	     #t)
	    ((> (search-node-estimate node) (search-node-estimate enode))
	     (if #t ;; *verbose* 
		 (display* "Found a better estimate for expanded state: " 
			   (state-name state)  " "
			   (search-node-estimate enode) "->"
			   (search-node-estimate node)))
	     ;; found a beter estimate, forget the old node but copy path
	     (forget-expanded enode)
	     (copy-path enode node)
	     ;; add the new node (old path, new estimate) to pending (and visited)
	     #t)
	    (else 
	     ;; old path is equally good or better, skip this node
	     #f)))
     (vnode
      (display "V")
      ;; Node in visited list but not expanded (so it is in pending).
      (cond ((< (search-node-actual node) (search-node-actual vnode))
	     (if #t ;; *verbose* 
		 (display* "Found a better path to visited state: " 
			   (state-name state) " "
			   (search-node-actual vnode) "->"
			   (search-node-actual node)))
	     ;; found a beter path, forget the old one
	     (forget-visited vnode)
	     ;; add the new node (better path) to pending (and visited)
	     #t)
	    ((> (search-node-estimate node) (search-node-estimate vnode))
	     (if #t ;; *verbose* 
		 (display* "Found a better estimate for visited state: " 
			   (state-name state) " "
			   (search-node-estimate vnode) "->"
			   (search-node-estimate node)))
	     ;; found a beter estimate, forget the old node but copy path
	     (forget-visited vnode)
	     (copy-path vnode node)
	     ;; add the new node (old path, new estimate) to pending (and visited)
	     #t)
	    (else 
	     ;; old path is equally good or better, skip this node
	     #f)))
     (else
      ;; node is not present in either expanded or visited, so keep it.
      #t)
     )))

;;; Pathmax: updates the estimate of the node to be consistent
(define (PATHMAX-UPDATE-ESTIMATE current node)
  (let ((min-estimate
	 (- (search-node-estimate current) ; prev estimate
	    ;; link cost = (- node-actual cur-actual)
	    (- (search-node-actual node) 
	       (search-node-actual current)))))
    ;; if it violates consistency condition, update the estimate
    (cond ((< (search-node-estimate node) min-estimate)
	   (display* "Pathmax: " 
		     (state-name (search-node-state node)) " "
		     (search-node-estimate node)  "->"
		     min-estimate)
	   (set-search-node-estimate! node min-estimate)
	   (set-search-node-cost! node
				  (+ (search-node-actual node) min-estimate))))))