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

;;; Extensions of SEARCH.SCM to handle optimal searches.

;; Extend a path to the neighbors of the head state
;; Returns a list of extended paths (with values given by the sum of path length)
;; This is used in Uniform Cost Search

(define (extend-path-with-path-length path)
  ;;(display* "Extending a path to " (path-head-state path) " with value = " (path-value path))
  (remove-falses
   (map
    (lambda (next)			; (state length)
      (let ((next-state (first next))
	    (next-link-length (second next)))
	(if (member next-state (path-states path))
	    #f
	    (make-path
	     (+ (path-value path)
		next-link-length	; update path length to next-state
		)
	     (cons next-state (path-states path))))))
    ;; Returns a list of lists (state length)
    (get-neighboring-states-and-cost (path-head-state path)))))

;;; This is used in A*, note the handling of the heuristic!

(define (extend-path-with-path-length-plus-heuristic path heuristic)
  ;;(display* "Extending a path to " (path-head-state path) " with value = " (path-value path))
  (remove-falses
   (map
    (lambda (next)			; (state length)
      (let ((next-state (first next))
	    (next-link-length (second next)))
	(if (member next-state (path-states path))
	    #f
	    (make-path
	     (+ (- (path-value path)	; this value includes previous heuristic
		   (heuristic (path-head-state path)) ; so, remove it.
		   )
		next-link-length	; update path length to next-state
		(heuristic next-state)	; add in the new heuristic estimate
		)
	     (cons next-state (path-states path))))))
    ;; Returns a list of lists (state length)
    (get-neighboring-states-and-cost (path-head-state path)))))


;;; DATA DEPENDENT OPERATIONS

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


(define *expanded* #f)

(define (extend-path-with-path-length path)
  (cond ((in-state-list? (path-head-state path) *expanded*)
	 ;; don't extend from expanded states, so empty list of extensions
	 '())
	(else
	 ;; UPDATE EXPANDED	 
	 (set! *expanded* (add-to-state-list (path-head-state path) *expanded*))
	 (remove-falses
	  (map
	   (lambda (next)		; (state length)
	     (let ((next-state (first next))
		   (next-link-length (second next)))
	       (if (member next-state (path-states path))
		   #f
		   (make-path
		    ;; update path length to next-state
		    (+ (path-value path) next-link-length)
		    (cons next-state (path-states path))))))
	   ;; Returns a list of lists (state length)
	   (get-neighboring-states-and-cost (path-head-state path)))))))

(define (extend-path-with-path-length-plus-heuristic path heuristic)
  (cond ((in-state-list? (path-head-state path) *expanded*)
	 ;; don't extend from expanded states, so empty list of extensions
	 '())
	(else
	 ;; UPDATE EXPANDED	 
	 (set! *expanded* (add-to-state-list (path-head-state path) *expanded*))
	 (remove-falses
	  (map
	   (lambda (next)		; (state length)
	     (let ((next-state (first next))
		   (next-link-length (second next)))
	       (if (member next-state (path-states path))
		   #f
		   (make-path
		    (+ (- (path-value path) ; this value includes previous heuristic
			  (heuristic (path-head-state path)) ; so, remove it.
			  )
		       next-link-length	; update path length to next-state
		       (heuristic next-state) ; add in the new heuristic estimate
		       )
		    (cons next-state (path-states path))))))
	   ;; Returns a list of lists (state length)
	   (get-neighboring-states-and-cost (path-head-state path)))))))

(define (uniform-cost start goal)

  ;; A Q addition function specific to uniform-cost search
  ;; Add the new paths to the front of the queue, could go anywhere
  (define (merge-paths-into-Q new-paths Q)
    (Q-set-paths! Q (append new-paths (Q-paths Q)))
    )

  ;; We don't need the heuristic function, so we got rid of it
  ;; then we called the correct extend-path... function
  (define (successors path)		; NOTE CHANGE
    (extend-path-with-path-length path))

  (set! *number-of-search-steps* 0)
  (set! *expanded* (init-state-list))
  ;; Fire up generalized search using Q constructor defined above:
  (search
   ;; Just start with a partial path including only start state.
   goal				; goal state
   1					; only 1 path wanted
   ;; USE THE FUNCTION TO UPDATE EXPANDED
   pick-and-remove-best-path		; pick the best 
   merge-paths-into-Q			; add to the front of Q
   successors				; successors
   ;; The initial Q, just one path = (start), PATH-LENGTH = 0
   (make-Q (make-path 0 (list start)))	; initial Q
   '()					; initial wins
   ))

(define (path-< p1 p2) (< (path-value p1) (path-value p2)))

(define (A* start goal)

  ;; A Q addition function specific to uniform-cost search
  ;; Add the new paths to the front of the queue, could go anywhere
  (define (merge-paths-into-Q new-paths Q)
    (Q-set-paths! 
     Q (merge! (sort! new-paths path-<)
	       (Q-paths Q) 
	       path-<)))
  
  (define (heuristic state) 
    ;; in general, the heuristic value may depend on the goal state
    (get-heuristic-value state goal))

  ;; We don't need the heuristic function, so we got rid of it
  ;; then we called the correct extend-path... function
  (define (successors path)
    (extend-path-with-path-length-plus-heuristic path heuristic))

  (set! *number-of-search-steps* 0)
  (set! *expanded* (init-state-list))
  ;; Fire up generalized search using Q constructor defined above:
  (search
   ;; Just start with a partial path including only start state.
   goal				; goal state
   1					; only 1 path wanted
   pick-and-remove-first-path		; pick the best (which is first)
   merge-paths-into-Q			; add to the front of Q
   successors				; successors
   ;; The initial Q, just one path = (start), PATH-LENGTH = 0
   (make-Q (make-path (heuristic start) (list start)))	; initial Q
   '()					; initial wins
   ))

(define (best-first start goal)

  ;; A Q addition function specific to best-first search
  ;; Add the new paths to the front of the queue, could go anywhere
  (define (merge-paths-into-Q new-paths Q)
    (Q-set-paths! 
     Q (merge! (sort! new-paths path-<)
	       (Q-paths Q) 
	       path-<)))

  (define (heuristic state) 
    ;; in general, the heuristic value may depend on the goaln state
    (get-heuristic-value state goal))

  (define (successors path)
    (extend-path-with-heuristic path heuristic))

  (set! *number-of-search-steps* 0)
  (set! *visited* (init-state-list start))

  ;; Fire up generalized search using Q constructor defined above:
  (search
   ;; Just start with a partial path including only start state.
   goal				; goal state
   1					; only 1 path wanted
   pick-and-remove-first-path		; pick the best
   merge-paths-into-Q			; add to the front of Q
   successors				; successors, using heuristic
   ;; The initial Q, just one path = (start)
   (make-Q (make-path (heuristic start) (list start))) ; initial Q
   '()					; initial wins
   ))

(define (extend-path-with-heuristic path heuristic)
  ;;(display* "Extending the path " (path-reverse path))
  (remove-falses
   (map
    (lambda (next-state)
      (cond ((in-state-list? next-state *visited*)
	     #f)
	    (else
	     (set! *visited* (add-to-state-list next-state *visited*))
	     (make-path
	      (heuristic next-state)	; path value is from calling heuristic
	      (cons next-state (path-states path))))))
    (get-neighboring-states (path-head-state path)))))