;;; This defines several versions of a search-queue, each of which is
;;; used to implement different types of search algorithms.
;;; The operations that a search queue must implement are:

;;; (Q 'empty?): return #t or #f
;;; (Q 'next): return a node and delete it from the search-queue
;;; (Q 'add <list of nodes>): add the nodes to the queue
;;; (Q 'count): return a count of nodes
;;; (Q 'summary): prints a summary of work done

;;; Some of the implementations support:
;;; (Q 'pending <state>): returns the node with that state if present, or #f.
;;; (Q 'expanded <state>): returns the node with that state if present, or #f.
;;; this is implemented by an additional hash table.

;;; There are several important variants on search-queues.
;;; FIFO (first in, first out) - queue (implemented as a list)
;;; LIFO (last in, first out) - stack (implemented as a list)
;;; PQ - priority queue (implemented as a list or weight-balanced tree)

;;; Any of the different kinds of search-queues can also support:
;;; VISITED LIST - keeps track of nodes that have ever been added to the
;;; search-queue (implemented as a hash table)
;;; EXPANDED LIST - keeps track of nodes that have ever been returned
;;; by a next operation on the search-queue (implemented as a hash table).


;;; LIST implementation of LIFO
(define (MAKE-STACK visited? . initial)		; LIFO
  (make-list-search-queue 'stack visited? #f initial))

;;; LIST implementation of FIFO
(define (MAKE-QUEUE visited? . initial)		; FIFO
  (make-list-search-queue 'queue visited? #f initial))

;;; LIST implementation of Priority Queue
(define (MAKE-PQ  expanded? . initial)		; PQ
  (make-list-search-queue 'pq #f expanded? initial))

(define (MAKE-LIST-SEARCH-QUEUE type visited? expanded? initial)
  (let* ((entries '())
	 (node-id 0)
	 (expansions 0)
	 (pending (make-equal-hash-table))
	 (visited (if visited? (make-equal-hash-table) #f))
	 (expanded (if expanded? (make-equal-hash-table) #f))
	 (fun
	  (lambda (op . args)
	    (let ((arg (if (null? args) #f (car args))))
	      (cond
	       ((eq? op 'empty?)
		;; Test for no entries.
		(null? entries))

	       ((eq? op 'expanded?)
		(hash-table/get expanded arg #f))

	       ((eq? op 'pending?)
		(hash-table/get pending arg #f))

	       ((eq? op 'remove-pending) ; arg is node
		(hash-table/remove! pending (search-node-state arg))
		;; take it out of the actual pending list
		(set! entries (remove! arg entries))
		)
	       
	       ((eq? op 'remove-expanded) ; arg is node
		(hash-table/remove! expanded (search-node-state arg)))

	       ((eq? op 'next)
		(if (null? entries)
		    (error "Search queue is empty."))
		;; If a priority queue, make sure best element is in front.
		(if (eq? type 'pq)
		    ;; Move the best entry (least cost) to the front.
		    (set! entries (move-best-entry-to-front entries)))
		;; remove first element and return it.
		(let ((next (first entries)))
		  (set! entries (rest entries)) ; remove first entry
		  ;; Update expanded and pending
		  (if expanded
		      (hash-table/put! expanded (search-node-state next) next))
		  (hash-table/remove! pending  (search-node-state next))
		  (set! expansions (1+ expansions)) ; keep track of amount of work
		  next))

	       ((eq? op 'add)
		;; only add nodes that have not previously been visited
		(let ((new (filter-nodes arg visited)))
		  (for-each 
		   (lambda (n)
		     (if (hash-table/get pending (search-node-state n) #f)
			 (display* "Adding duplicate state: " (search-node-state n)))
		     ;; put in the pending hash table
		     (hash-table/put! pending (search-node-state n) n)
		     (if visited
			 (hash-table/put! visited (search-node-state n) n))
		     ;; add a unique id to each node
		     (set-search-node-id! n node-id)
		     (set! node-id (1+ node-id)))
		   new)
		  ;; update the entries
		  (set! entries
			(cond
			 ((eq? type 'stack)
			  (set! entries (append new entries)))
			 ((or (eq? type 'queue) (eq? type 'pq))
			  ;; append is very inefficient since it copies the
			  ;; list of entries.  append! is done by
			  ;; side-effect on the last cons-cell with no copying.
			  (append! entries new))
			 (else
			  (error "Unknown type of search-queue:" type))))))

	       ((eq? op 'count)
		;; could keep count as we go, but we don't do this very often
		(length entries))

	       ((eq? op 'summary)
		(display* " Length of queue= " (length entries)
			  " Number of expansions = " expansions
			  " Number of nodes added = " node-id
			  )
		(display* " Entries= " entries)
		(if visited
		    (display* " Visited hash size= " (hash-table/count visited)))
		(if expanded
		    (display* " Expanded hash size= " (hash-table/count expanded)))
		(if pending
		    (display* " Pending hash size= " (hash-table/count pending))))

	       (else
		(error "Unknown operation for:" type op args))))
	    )))
    (fun 'add initial)
    fun
    ))

;;; Return the elements of nodes whose states are not in the visited hash table.

(define (FILTER-NODES nodes visited)
  (filter 
   (lambda (node)
     (if visited
	 (hash-table/get visited (search-node-state node) #f)
	 #t))
   nodes))


;;; Modify list1 by replacing the last null cdr with a pointer to list2.
(define (APPEND! list1 list2)
  (define (loop l1)
    (if (null? (cdr l1)) 
	(set-cdr! l1 list2)
	(loop (cdr l1))))
  (cond ((null? list1) list2)		; special case - null list1.
	(else (loop list1) list1)))

;;; Two different ways of moving the best element to the front.  Find
;;; and move the best element or sort the whole list.

;; Do a linear scan and move the best element to the front
(define (MOVE-BEST-ENTRY-TO-FRONT entries)
  (define (loop P best-P best-cost)
    (cond ((null? (rest P))		; no more nodes
	   (cond ((eq? (cdr best-P) entries)
		  ;; the first node is the best one, so no changes
		  entries)
		 (else
		  ;; best-P = (x best-node y ...)
		  (let ((best-node (second best-P)))
		    ;; eliminate the best-node from where it is now
		    (set-cdr! best-P (cddr best-P))
		    ;; add it back to the front of the original list
		    (cons best-node entries)))))
	  ;; the cost of the current node is better than the best so far
	  ((< (search-node-cost (second P)) best-cost)
	   (loop (rest P) P (search-node-cost (second P))))
	  ;; keep going
	  (else
	   (loop (rest P) best-P best-cost))))
  (let ((extended (cons 'anchor entries)))
    ;; add a symbol in front so we can easily remove an element from
    ;; the list, since we need to have a pointer to the cell that
    ;; points to the cell to be removed (see the set-cdr! above).
    (loop (rest extended) extended (search-node-cost (first entries))))
  )

;; Sort the paths based on the heuristic value
(define (MOVE-BEST-ENTRY-TO-FRONT nodes)
  (sort nodes (lambda (x y) (< (search-node-cost x) (search-node-cost y)))))

;;; Using MIT Scheme's Weight-Balanced Trees (WT-Tree)

;;; Comparison function for WT.  It uses a cons of the cost and the
;;; node id to construct a total order on the nodes.
(define (SEARCH-NODE-< c1 c2)
  (or (< (car c1) (car c2))
      (and (= (car c1) (car c2))
	   (< (cdr c1) (cdr c2)))))

(define (MAKE-WT-PQ expanded? initial)
  (let* ((wt (make-wt-tree (make-wt-tree-type search-node-<)))
	 (node-id 0)
	 (expansions 0)
	 (expanded (if expanded? (make-equal-hash-table) #f))
	 (presence (make-equal-hash-table))
	 (pq-fn
	  (lambda (op . args)
	    (let ((arg (if (null? args) #f (car args))))
	      (cond
	       ((eq? op 'empty?)
		;; Test for no entries.
		(wt-tree/empty? wt))

	       ((eq? op 'next)
		(if (wt-tree/empty? wt)
		    (error "Search queue is empty."))
		;; remove first element and return it.
		(let ((next (wt-tree/min-datum wt)))
		  (if expanded
		      (hash-table/put! expanded (search-node-state next) next))
		  (wt-tree/delete-min! wt) ; remove first entry
		  (set! expansions (1+ expansions)) ; keep track of amount of work
		  next))

	       ((eq? op 'add)
		;; only add nodes that have not previously been visited/expanded
		(let ((new (filter-nodes arg expanded)))
		  ;; update the wt-tree
		  (for-each (lambda (n)
			      (set-search-node-id! n node-id)
			      (wt-tree/add! wt (cons (search-node-cost n) node-id) n)
			      (hash-table/put! presence (search-node-state n) n)
			      (set! node-id (1+ node-id)))
			    new)
		  ))

	       ((eq? op 'lookup)
		(hash-table/get presence arg #f))

	       ((eq? op 'count)
		;; could keep count as we go, but we don't do this very often
		(wt-tree/size wt))

	       ((eq? op 'summary)
		(display* " Length of queue= " (wt-tree/size wt)
			  " Number of expansions= " expansions
			  " Number of nodes added= " node-id
			  )
		(if expanded
		    (display* " Expanded list size= " (hash-table/count expanded)))
		(display* " Presence hash size= " (hash-table/count presence))
		)
	       (else
		(error "Unknown operation for:" op args))))
	    )))
    (pq-fn 'add-nodes initial)
    pq-fn
    ))