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

;;; GENERALIZED SEARCH PROCEDURE (for 6.034 by TLP@mit.edu)

(declare (usual-integrations))

;;; One important implementation note.  The pending list is
;;; implemented in "message passing style".  That is, as a function
;;; that takes a symbol (indicating the operation) and some arguments.

;;; A global variable to keep track of the amount of work done.
(define *NUMBER-OF-SEARCH-STEPS* 0)
;;; Controls amount of printing, set to #f to limit printing
(define *VERBOSE* #t)

;;; Data structure for search-node.  Defines accessor and modifier
;;; functions: search-node-cost, set-search-node-cost!, etc.
(define-structure (SEARCH-NODE)
  cost actual estimate state predecessor id)

;;; This allows implementing most of the search strategies
;;; (depth-first, etc.) by specifying the appropriate type of pending list
;;; function. Returns either a winning search-node or #f.  This
;;; assumes a single goal state, generalization to a list is trivial.

(define (SEARCH goal			; goal state
		successors		; successor function
		pending			; pending list
		expanded		; expanded list
		visited			; visited list
		)

  (cond
   ((pending 'empty?)				; Failed, at least be cute...
    (display* "Cain't get thar from heah.")
    #f)
   (else
    (let ((current (pending 'next)))		; get (and remove) node to expand

      ;; Show some status information
      (if *verbose*			; only in verbose mode.
	  (search-node-display current "Current node: "))
      (set! *number-of-search-steps* (1+ *number-of-search-steps*))
      (cond ((= 0 (remainder *number-of-search-steps* 100))
	     (pending 'summary)))

      (cond ((and expanded
		  (expanded 'member (search-node-state current)))
	     (if *verbose* (display* "Already expanded."))
	     (search goal successors pending expanded visited))
	    ((equal? (search-node-state current) goal) ; are we there?
	     (pending 'summary)
	     (search-node-display current " Final") ; display node
	     (display* " Path = " (map state-name (search-node-path current)))
	     current)
	    (else 
	     ;; pending still has entries in it and we haven't found goal
	     ;; yet, so expand current node and merge results into pending
	     ;; and update the expanded and visited lists.
	     (search-update current (successors current) pending expanded visited)
	     (search goal successors pending expanded visited))))
    )))

;;; This is the simple version (uses more space and cannot cope with inconsistent heuristic).
(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)
			  (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))))


;;; SEARCH-NODE OPERATIONS

;; Extend a path to the neighbors of the node's state. Returns a list of
;; new search-nodes (with cost given by the path length (if a link-cost function 
;; is given) plus the heuristic function, if one is given).   
;; It does not return any node with a  state on the state list (if state-list is given).

(define (EXTEND-NODE node link-cost heuristic)
  (define (loop neighbors)
    (let* ((nbor (if (null? neighbors) #f (first neighbors)))
	   (nbor-state (neighbor-state nbor)))
      (cond ((not nbor) '())
	    ((predecessor? nbor-state node)
	     ;; skip this state - we don't want to re-visit it again.
	     (loop (rest neighbors)))
	    (else 
	     (cons
	      (let ((new-node 
		     (make-search-node 0 0 0 nbor-state node #f)))
		;; Fill in the fields of the new-node
		(set-search-node-state! new-node nbor-state)
		(set-search-node-predecessor! new-node node)
		(if link-cost
		    ;; link cost given, add to the actual path cost so far.
		    (set-search-node-actual! new-node
					     (+ (link-cost nbor)
						(search-node-actual node)))
		    ;; no link cost given, so set actual cost to 0
		    (set-search-node-actual! new-node 0))
		(if heuristic
		    ;; heuristic given, call it on the neighboring state
		    (set-search-node-estimate! new-node (heuristic nbor-state))
		    ;; no heuristic given, use 0
		    (set-search-node-estimate! new-node 0))
		;; The cost is sum of actual and estimated cost
		(set-search-node-cost! new-node (+ (search-node-actual new-node)
						   (search-node-estimate new-node)))
		new-node)
	      (loop (rest neighbors)))))))
  (loop (state-neighbors (search-node-state node))))

;;; Returns #t if state is in the predecessor chain for node.
(define (PREDECESSOR? state node)
  (cond ((equal? state (search-node-state node)) #t)
	((search-node? (search-node-predecessor node))
	 (predecessor? state (search-node-predecessor node)))
	(else #f)))

;;; Construct a path (a list of states) with the first state first
;;; from a search-node.
(define (SEARCH-NODE-PATH node)
  (define (loop sn)			; construct reversed list
    (if sn
	(cons (search-node-state sn) 
	      (loop (search-node-predecessor sn)))
	'()))
  (reverse (loop node)))

(define (SEARCH-NODE-DISPLAY node . message)
  (display* (if (null? message) 
		""
		(car message))
	    " Search node: " 
	    (state-name (search-node-state node))
	    " Cost= " 
	    (search-node-cost node)
	    " Actual Cost= " 
	    (search-node-actual node)
	    " Estimate= " 
	    (search-node-estimate node)))

(define (MAKE-START-NODE cost state)
  (make-search-node cost 0 cost state #f #f))

;;;; THE ACTUAL SEARCH METHODS

(define (DEPTH-FIRST start goal . args)
  (verify-args args)
  (let* ((start-node (make-start-node 0 start))
	 (visited (if (member 'use-visited args)
		      (make-state-list start-node)
		      #f)))

    ;; The successors function to be used in the search.
    (define (successors search-node)
      ;; We compute the path cost, but do not use that in the search.
      (extend-node search-node neighbor-cost #f))

    (set! *number-of-search-steps* 0)	; initialize count

    (search
     goal				; goal state
     successors				; successors
     (make-stack start-node) 
     #f
     visited
     )))

(define (BREADTH-FIRST start goal . args)
  (verify-args args)
  (let* ((start-node (make-start-node 0 start))
	 (visited (if (member 'use-visited args)
		      (make-state-list start-node)
		      #f)))

    ;; The successors function to be used in the search.
    (define (successors search-node)
      ;; We compute the path cost, but do not use that in the search.
      (extend-node search-node neighbor-cost #f))

    (set! *number-of-search-steps* 0)	; initialize count

    (search
     goal				; goal state
     successors				; successors
     (make-queue start-node) 
     #f
     visited
     )))

(define (BEST-FIRST start goal . args)

  (define (heuristic state) 
    ;; in general, the heuristic value will depend on the goal state
    (state-heuristic-value state goal))

  (verify-args args)
  (let* ((start-node (make-start-node (heuristic start) start))
	 (visited (if (member 'use-visited args)
		      (make-state-list start-node)
		      #f)))

    ;; The successors function to be used in the search.
    (define (successors search-node)
      ;; no link-cost is given to extend-node
      (extend-node search-node #f heuristic))

    (set! *number-of-search-steps* 0)	; initialize count

    (search
     goal				; goal state
     successors
     ;; a priority queue that will return the "best" node (by heuristic)
     ((if (member 'use-wt args) make-wt-pq make-pq) start-node) 
     #f
     visited
     )))

(define (UNIFORM-COST start goal . args)
  (verify-args args)
  (let* ((start-node (make-start-node 0 start))
	 (expanded (if (member 'use-expanded args)
		       (make-state-list)
		       #f))
	 (visited (if (member 'use-visited args)
		      (make-state-list start-node)
		      #f)))

    ;; The successors function to be used in the search.
    (define (successors search-node)
      ;; only link-cost is given to extend-node
      (extend-node search-node neighbor-cost #f))

    (set! *number-of-search-steps* 0)	; initialize count

    (search
     goal				; goal state
     successors
     ;; a priority queue that will return the "best" node
     ((if (member 'use-wt args) make-wt-pq make-pq) start-node) 
     expanded
     visited
     )))

;;; This is A* for a consistent heuristic function, which guarantees that 
;;; the first time we reach a state we have the optimal path to the state.
;;; If the heuristic is not consistent, we have to do pathmax and that requires
;;; a little more complex search implementation.  

(define (A* start goal . args)

  (define (heuristic state) 
    ;; in general, the heuristic value will depend on the goal state
    (state-heuristic-value state goal))

  (verify-args args)
  (let* ((start-node (make-start-node (heuristic start) start))
	 (expanded (if (member 'use-expanded args)
		       (make-state-list)
		       #f))
	 (visited (if (member 'use-visited args)
		      (make-state-list start-node)
		      #f)))

    ;; The successors function to be used in the search.
    (define (successors search-node)
      ;; only link-cost is given to extend-node
      (extend-node search-node neighbor-cost heuristic))

    (set! *number-of-search-steps* 0)	; initialize count

    (search
     goal				; goal state
     successors
     ;; a priority queue that will return the "best" node
     ((if (member 'use-wt args) make-wt-pq make-pq) start-node) 
     expanded
     visited
     )))

(define (verify-args args)
  (for-each (lambda (arg)
	      (or (memq arg '(use-expanded use-visited use-wt))
		  (error "Unknown argument: " arg)))
	    args))

;;; STATES

;;; This is a very primitive implementation of states, modeled on the
;;; examples in the notes.  Look at the puzzle code to see a much more
;;; realistic implementation.

;;; A little test network (the one from the on-line material).  Each
;;; sublist is (state connected-states) - this is unidirectional.
(define *GRAPH* 
  '((S ((A 2) (B 5)))
    (A ((C 2) (D 4)))
    (B ((D 1) (G 5)))
    (C ())
    (D ((C 3) (G 2)))
    (G ())))

;; Trivial heuristic values for fixed goal state G
(define *HEURISTIC-VALUES*
  '((a 2) (b 3) (c 1) (d 4) (s 10) (g 0)))

;;; Another network with a very inconsistent heuristic.  
;;; Using only extended list in A* gives the wrong answer.
(define *GRAPH* 
  '((S ((A 1) (B 2)))
    (A ((C 1)))
    (B ((C 2)))
    (C ((G 100))
    (G ()))))

(define *HEURISTIC-VALUES*
  '((a 100) (b 1) (c 90) (s 0) (g 0)))

(define (STATE-NEIGHBORS state)
  (let ((ans (assoc state *graph*)))
    (if ans
	(second ans)
	(error "state-neighbors:Unknown state" state))))

(define (NEIGHBOR-STATE n) (and n (first n)))
(define (NEIGHBOR-COST n) (and n (second n)))

;; In general, the heuristic value may depend on the goal state but,
;; here, we simply lookup heuristic value in a table specified for a
;; particular goal.
(define (STATE-HEURISTIC-VALUE state goal)
  (let ((ans (assoc state *heuristic-values*)))
    (if ans
	(second ans)
	(error "state-heuristic-value:Unknown state" state))))

(define (STATE-NAME state) state)

(define *heuristic-values*
  '((s 10) (a 5) (b 11) (c 1) (d 7) (e 6) (f 5) (h 3) (i 2) (j 1) (g 0)))

(define *graph* '((s ((a 1) (b 1) (c 10)))
		  (a ((d 1) (s 1)))
		  (b ((d 1) (s 1)))
		  (c ((h 1) (s 10)))
		  (d ((a 1) (b 1) (e 1) (f 1)))
		  (e ((d 1) (f 1)))
		  (f ((d 1) (e 1)))
		  (h ((c 1) (i 1)))
		  (i ((h 1) (j 1)))
		  (j ((i 1) (g 1)))
		  (g ((j 1)))))