;;; ********************************************************************
;;; pqueue.ss
;;; Susan Fox
;;; January 1, 2001

;;; Contains the Priority queue ADT, for keeping track of system goals
;;; in an orderly way.   Values may be inserted, and removed from
;;; any point:  there are two removal operations: dequeue and remove.
;;; dequeue removes the highest priority item, remove permits any
;;; item to be removed: it looks it up and then takes it out.  This
;;; is so goals that are achieved opportunistically or that must
;;; be discarded can be removed out of turn


;;; Requires utilities to be loaded
(if (not (top-level-bound? 'UTIL.SSLOADED))
    (load "util.ss"))

(define PQUEUE.SSLOADED #t)


;;; ********************************************************************
;;; The priority queue is a heap.  We can set a maximum number of goals,
;;; so the heap can be represented with a vector.  I haven't bothered
;;; to create a separate heap ADT, though that would be the best way.
;;;

;;; The heap is a vector, and in addition we keep track of the next
;;; empty slot in the vector (since values are added in order and
;;; then walked up or down the heap as appropriate)  Each value in
;;; the vector is a list consisting of the priority value, and the
;;; actual value associated with the priority.

;;; ------------------------------------------------------------------
;;; Heap node operations
;;;
(define heap-node (lambda (priority value) (list priority value)))
(define priority-of car)
(define value-of cadr)


;;; ------------------------------------------------------------------
;;; init-queue creates an empty queue of a default size, unless a
;;; size is specified.
;;;
(define init-queue
  (lambda size
    (if (null? size)
	(list 0 (make-vector 100 #f))
	(list 0 (make-vector size #f)))))


;;; accessors for use internally for accessing the parts of the heap
;;; representation
;;;
(define next-pos car)
(define value-vec cadr)

;;; and a mutator for the next-pos part
(define set-next-pos!
  (lambda (heap new-val)
    (set-car! heap new-val)))



;;; queue-empty? takes a queue and return #t if it is empty, and #f otherwise
;;;
(define queue-empty?
  (lambda (q)
    (zero? (next-pos q))))


;;; queue-full? takes a queue and returns #t if it is full, and #f otherwise
;;;
(define queue-full?
  (lambda (q)
    (= (next-pos q) (vector-length (value-vec q)))))


;;; first-element takes a queue and returns the first value in it
;;;
(define first-element
  (lambda (q)
    (if (queue-empty? q)
	#f
	(value-of (vector-ref (value-vec q) 0)))))


;;; ------------------------------------------------------------------
;;; Since the heap is a complete binary tree, we need to be able to negotiate
;;; the tree up and down.  The nodes in the tree are assigned starting
;;; with the root at 0, its children at 1 and 2, the children of 1 at
;;; 3 and 4, the children of 2 at 5 and 6, and so on.  That means
;;; there are simple formulae for computing (from a node at position pos)
;;; the left child:  (pos * 2) + 1
;;; the right child:  (pos * 2) + 2
;;; the parent: the quotient of (pos - 1) and 2

;;; #(a b c d e f g h ...)
;;;   0 1 2 3 4 5 6 7 ...

;;; root? takes a position and tells you if it is the root value
;;;
(define root? zero?)

;;; parent-of takes the position of a node in the tree as it is stored
;;; in the vector, and returns the position of its parent.
(define parent-of
  (lambda (index)
    (if (root? index)
	#f
	(quotient (sub1 index) 2))))


;;; left-child takes the position of a node and returns the position
;;; of its left child
;;;
(define left-child
  (lambda (index)
    (+ 1 (* 2 index))))

;;; right-child takes the position of a node and returns the position
;;; of its right child
;;;
(define right-child
  (lambda (index)
    (+ 2 (* 2 index))))


;;; ------------------------------------------------------------------

;;; a node is inserted by adding a new child at the next position
;;; and then walking the value up the tree by swapping it with any
;;; parent of lesser priority
;;;
(define insert!
  (lambda (value priority queue)
    (if (queue-full? queue)
	(error 'insert! "Can't add element to a full queue")
	(let ([new-node (heap-node priority value)])
	  (vector-set! (value-vec queue) (next-pos queue) new-node)
	  (walk-up! queue (next-pos queue) new-node)
	  (set-next-pos! queue (add1 (next-pos queue)))))))


;;; walk-up takes a queue and a starting position and moves the value
;;; at that position up to its parents position if it has a higher
;;; priority value than the parent
;;;
(define walk-up!
  (lambda (queue pos node)
    (if (not (root? pos))
	(let* ([parent-pos (parent-of pos)]
	       [parent-node (vector-ref (value-vec queue) parent-pos)])
          (if (< (priority-of parent-node) (priority-of node))
	      (begin
		(vector-set! (value-vec queue) pos parent-node)
		(vector-set! (value-vec queue) parent-pos node)
		(walk-up! queue parent-pos node)))))))
          
	  
	
;;; ------------------------------------------------------------------

;;; a node is dequeued from the front by overwriting it with the
;;; last value in the queue, then walking that value down until
;;; its priority is greater than both of its children
;;;
(define dequeue!
  (lambda (queue)
    (if (queue-empty? queue)
	(error 'dequeue! "Can't delete from an empty queue")
	(do-remove! 0 queue))))


;;; do-remove! takes a position and removes the node at that position
;;;
(define do-remove!
  (lambda (pos queue)
    (let ([moved-node (vector-ref (value-vec queue) (sub1 (next-pos queue)))])
      (vector-set! (value-vec queue) (sub1 (next-pos queue)) #f)
      (set-next-pos! queue (sub1 (next-pos queue)))
      (vector-set! (value-vec queue) pos moved-node)
      (walk-down! queue pos moved-node))))


;;; walk-down takes a queue, a position, and the node being moved,
;;; and it walks it down until it either has no children, or its priority
;;; is greater than the value of both children
;;;
(define walk-down!
  (lambda (queue pos node)
    (cond
      [(>= (left-child pos) (next-pos queue))   ;; no children of pos
       'done]
      [(= (right-child pos) (next-pos queue))   ;; left child, no right
       (let ([child (vector-ref (value-vec queue) (left-child pos))])
         (if (> (priority-of child) (priority-of node))
	     (begin
	       (vector-set! (value-vec queue) pos child)
	       (vector-set! (value-vec queue) (left-child pos) node))))]
      [else
	(let* ([which-pos
		 (if (> (priority-of
			  (vector-ref (value-vec queue) (left-child pos)))
  		       (priority-of
			 (vector-ref (value-vec queue) (right-child pos))))
		     (left-child pos)
		     (right-child pos))]
	       [which-node (vector-ref (value-vec queue) which-pos)])
	  (printf "chose node ~s~n" (value-of which-node))
          (if (> (priority-of which-node) (priority-of node))
	      (begin
		(vector-set! (value-vec queue) pos which-node)
		(vector-set! (value-vec queue) which-pos node)
		(walk-down! queue which-pos node))))])))


;;; a node is deleted by passing the queue and the node's value, then
;;; doing a simple linear search to find the node.  From the point
;;; of finding the node onward, the deletion process is the same
;;; as the dequeue process
;;;
(define delete!
  (lambda (queue node-value)
    (if (queue-empty? queue)
	(error 'delete! "Can't delete from an empty queue")
	(do-remove! (find-del-node queue node-value) queue))))


;;; find-del-node does a linear search through the queue to find
;;; the given value.  If no value matches, it pops an error
;;;
(define find-del-node
  (lambda (queue node-value)
    (let ([end-pos (next-pos queue)]
	  [vec (value-vec queue)])
      (letrec ([search
		 (lambda (pos)
		   (cond
		     [(= pos end-pos)
		      (error 'find-del-node "Value not found to delete")]
		     [(equal? node-value (value-of (vector-ref vec pos)))
		      pos]
		     [else (search (add1 pos))]))])
	(search 0)))))