;This file contains functions to implement a general search algorithm.
;  The top-level function is called SEARCH and takes 4 arguments
;     1.  A description of the start state
;     2.  A predicate recognizing goal states
;     3.  A function creating a list of successors of a given state
;     4.  A function estimating the value of a given state
;           (low values correspond to good states)
;  In the case of a minimization problem, if the estimator is always optimistic
;     (i.e., if it always underestimates the value of the best solution
;     attainable from a given state), then the algorithm will return the
;     correct minimal solution
;  In SEARCH, states created but not expanded are stored with their estimated
;     values in a priority queue.  As successors are created, they are paired
;     with their values and inserted into the queue.  The algorithm is based
;     on that of Charniak & McDermott, p.269.




;;;;;;;;;;;;;;;;;;;;;;;;;; THE PRIORITY QUEUE ADT ;;;;;;;;;;;;;;;;;;;;;

; CREATE-QUEUE Creates and returns an empty priority queue accepting messages
;   INSERT, DELETE, and EMPTY?
; The priority queue is implemented as a heap that's implemented as a binary
;   tree.  Note that tree nodes structure may be left in the heap after 
;   deletions.  Thus the heap operations must refer to the tree size, 
;   rather than check for empty nodes, to determine whether the bottom
;   of the heap has been reached.
; The order of insertion for a heap                     1
;   with 15 nodes is given at right:              2           3
;                                             4      6     5     7
;                                            8 12  10 14  9 13 11 15

(define (create-queue)

  ; two state variables
  (define size 0)
  (define q '())

  ; tree selectors & constructors
  (define (data x) (car x))
  (define (left x) (cadr x))
  (define (right x) (caddr x))
  (define (build a b c) (list a b c))

  ; selectors & constructors allowing bundling of estimators with states
  (define (state x) (car x))
  (define (estimate x) (cdr x))
  (define (build-pair s e) (cons s e))
  
  ; does the root of SUBTREE have worse priority than the number EST?
  (define (prior? est subtree)
    (cond ((null? subtree) #t)
          (else (< est (estimate (data subtree))))))
  
  ; INSERT calls a recursive helper to do the work 
  (define (insert st est)
    (define (tree-insert st est size q)
      (cond ((= size 1) (build (build-pair st est) '() '()))
            ((even? size) 
             (if (prior? est q)
                 (build (build-pair st est) 
                        (tree-insert (state (data q)) (estimate (data q))
                                     (/ size 2) (left q))
                        (right q))
                 (build (data q)
                        (tree-insert st est (/ size 2) (left q))
                        (right q))))
            (else
              (if (prior? est q)
                  (build (build-pair st est)
                         (left q)
                         (tree-insert (state (data q)) (estimate (data q))
                                      (/ (-1+ size) 2) (right q)))
                  (build (data q) 
                         (left q)
                         (tree-insert st est (/ (-1+ size) 2) (right q)))))))
    (set! size (+ size 1))
    (set! q (tree-insert st est size q)))

  ; DELETE assumes the queue isn't empty!
  (define (delete)
    ; returns data value from bottom of heap
    (define (swap size q)
      (cond ((= size 1) (data q))
            ((even? size) (swap (/ size 2) (left q)))
            (else (swap (/ (-1+ size) 2) (right q)))))
    ; note size BEFORE deletion is used
    (define (move-down dat q posn level)
      ; check whether there's a left subtree
      (if (< (+ posn level) size)
          ; and also a right subtree
          (if (< (+ posn level level) size)
              ; if so, check whether the left child has priority
              (if (prior? (estimate (data (left q))) (right q))
                  (if (prior? (estimate dat) (left q))
                      (build dat 
                             (left q)
                             (right q))
                      (build (data (left q))
                             (move-down dat (left q) 
                                        (+ posn level) (* 2 level))
                             (right q)))
                  ;else the right child has priority
                  (if (prior? (estimate dat) (right q))
                      (build dat 
                             (left q)
                             (right q))
                      (build (data (right q))
                             (left q)
                             (move-down dat (right q) 
                                        (+ posn level level) (* 2 level)))))
              ; just LST exists
              (if (prior? (estimate dat) (left q))
                  (build dat 
                         (left q)
                         (right q))
                  (build (data (left q))
                         (move-down dat (left q) (+ posn level) (* 2 level))
                         (right q))))          
          ; no subtree exists
          (build dat '() '())))
    ; the main body of DELETE
    (let ((root (data q))
          (new-root (swap size q)))         
      (set! q (move-down new-root (build new-root (left q) (right q)) 1 1))
      (set! size (-1+ size))
      (state root)))

  ; EMPTY? is defined in terms of SIZE, since the tree may have extra nodes
  (define (empty?)
    (zero? size))

  ; decode messages
  (define (dispatch message)
    (cond ((eq? message 'insert) insert)
          ((eq? message 'delete) delete)
          ((eq? message 'empty?) empty?)
          (else (error 'illegal-message))))
  dispatch)


;;;;;;;;;; Syntactic sugar for priority queue operations ;;;;;;;;;;;;;

(define (insert queue state estimate) ((queue 'insert) state estimate))
(define (delete queue)                ((queue 'delete)))
(define (empty? queue)                ((queue 'empty?)))


;;;;;;;;;;;;;;;;;;;;;;;;;; END OF PRIORITY QUEUE ;;;;;;;;;;;;;;;;;;;;;;;;;;;




;Inserts each state in the list given by the first argument into the priority
;  queue given by the third argument.  The second argument is an estimator
;  used to compute the priority of each state.

(define (insert-all statelist estimator queue)
  (cond ((null? statelist) queue)
        ( else (insert queue (car statelist) (estimator (car statelist)))
               (insert-all (cdr statelist) estimator queue))))


;This is the main function.  It initializes the queue and calls a recursive
;  search function

(define (search start goal? successors estimator)
  (let ((q (create-queue)))
    (insert q start (estimator start))
    (rec-search q goal? successors estimator)))


;This is the recursive version of the search function which does all the work.
;  As long as the queue is not empty, this function will fetch and expand the
;  first state in the queue, and merge the successors into the queue.
;  If the state fetched from the queue is ever a goal state, this goal
;  state will be returned.  Otherwise, the empty list is returned.

(define (rec-search queue goal? successors estimator)
  (cond ((empty? queue) '( ))
        (else (let ((current (delete queue)))
                (cond ((goal? current) current)
                      (else (rec-search 
                              (insert-all 
                                (successors current) 
                                estimator
                                queue)
                              goal?
                              successors
                              estimator)))))))


;this function just makes execution a little easier

(define (go) (search start goal? successors estimator))



;;;;;;;;;;;;;;;;;;;;;;;; THE SUM OF SUBSETS PROBLEM ;;;;;;;;;;;;;;;;;;;;;

;These are the four arguments for a sample instance of the sum of subsets
;  problem.  A state for this problem consists of
;     in position 3, the elements not yet considered
;     in position 2, the elements included so far
;     in position 1, the sum of the elements included so far
;  Here the target sum is 100, and the legal elements are
;     1, 2, 4, 8, 16, 32, 64, and 128.


;  Selectors & constructors

(define (sum-so-far s) (car s))
(define (included-so-far s) (cadr s))
(define (unconsidered s) (caddr s))
(define (build-state s i u) (list s i u))

(define target 100)
(define start (build-state 0 '() '(128 64 32 16 8 4 2 1)))
(define (goal? state) (= (sum-so-far state) target))

      ;This function builds a list of the states obtained by first including
      ;  and then excluding the current element

(define (successors state)
  (cond ((null? (unconsidered state)) '())
        (else (list 
                (build-state
                  (+ (sum-so-far state) (car (unconsidered state)))
                  (cons (car (unconsidered state)) (included-so-far state))
                  (cdr (unconsidered state)))
                (build-state (sum-so-far state) 
                             (included-so-far state) 
                             (cdr (unconsidered state)))))))

      ;States which have already exceeded the target sum are given very
      ;  large estimates.  Otherwise the estimate is just the distance
      ;  from the target
(define (estimator state)
  (cond ((> (sum-so-far state) target) 99999)
        (else (- target (sum-so-far state)))))