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

(require 'sort)

(define *visited* '())
(define *expanded* '())

(define first car)
(define rest cdr)
(define second cadr)

;;; GENERALIZED SEARCH PROCEDURE

(define *left-overs* '()) ;; allows examining the Q after search ends.
(define *number-of-search-steps* 0)
(define *number-ruled-out* 0)

;;; This allows implementing most of the search strategies (depth-first, etc.) by
;;; specifying the appropriate functional args.
;;; Returns a list of winning paths (possibly null).
;;; The states on each path on Q is kept in reversed order, for ease of access to
;;; the "head" state.

(define (search goal			; goal state
		how-many-paths		; either 'all or an integer 
		;; The functions below specify the type of search
		pick-and-remove-path
		merge-paths-into-Q
		;; The successor function depends on the particular problem 
		successors
		;; The state of the search
		Q			; current Q (list of paths)
		wins			; current winning paths
		)
  (cond
   ((Q-empty? Q) 
    ;; If no paths left, return accumulated wins, if any
    (if (null? wins)
	(begin 
	  ;; Failed, at least be cute...
	  (display* "Cain't get thar from heah.")
	  wins)
	wins))
   (else
    ;; Get the next path to examine and remove it from Q (by side-effect).
    (let ((current (pick-and-remove-path Q))) ; get partial path to extend
      ;; Debugging info
      ;;(display* "Expanding a path to " (path-head-state current) 
      ;; " with value = " (path-value current))
      (set! *number-of-search-steps* (+ *number-of-search-steps* 1))
      (cond ((= 0 (remainder *number-of-search-steps* 100))
	     (display* *number-of-search-steps* 
		       ": Q=" (length Q) 
		       " C=" *number-ruled-out*
		       " value=" (path-value current))))
      (cond ((done? (path-head-state current) goal) ; are we there?
	     (goal-action current Q)	; do this upon finding a goal
	     (cond 
	      ;; see if we've found enough solutions
	      ((and (not (eq? how-many-paths 'all))
		    (<= how-many-paths (+ 1 (length wins))))
	       ;; got enuf, we're outa here
	       (set! *left-overs* Q)	; so we can examine the queue
	       ;; add current path to previous wins and return that.
	       (cons (path-reverse current) wins))
	      (else   
	       ;; found a goal state but still need to find more paths
	       (search goal 
		       how-many-paths
		       pick-and-remove-path
		       merge-paths-into-Q
		       successors
		       (merge-paths-into-Q (successors current) Q)
		       (cons (path-reverse current) wins)))))
	    (else 
	     ;; Q still has entries in it and we haven't found goal yet, so
	     ;; extend current path and merge results into path Q:
	     (search goal how-many-paths 
		     pick-and-remove-path
		     merge-paths-into-Q
		     successors
		     (merge-paths-into-Q (successors current) Q)
		     wins)))))))

;; The action which should be taken when the goal state is reached.
;; Here we simply print length of path and describe the path.

(define (goal-action current Q)
  (display* "Final path length is " (path-state-count current))
  ;; Called with reversed path, the way they are kept on Q
  (describe-path (path-reverse current)))

(define done? equal?)			; might change for some problems

(define *t:verifier* (list 'search-step-count))
(define (get-step-count) 
  (list *t:verifier* *number-of-search-steps*))

;;; PATH OPERATIONS
;;; Paths are implemented as (reversed) lists of states, prefixed
;;; by a value (e.g. from heuristic), which may be #f.

(define (path? p)
  (and (pair? p)
       (or (eq? (path-value p) #f) (number? (path-value p)))
       (list? (path-states p))))

(define (make-path value states)	; constructor
  (cons value states))

(define (path-value p)			; get the value of a path
  (first p))

(define (path-states p)			; get the states of a path
  (rest p))

(define (path-empty? p)			; is the path empty?
  (null? (path-states p)))

(define (path-head-state p)		; returns first state
  (first (path-states p)))

(define (path-reverse p)		; reverse the path
  (cons (path-value p) (reverse (path-states p))))

(define (path-state-count p)		; count the states in a path
  (length (path-states p)))

;;; Q OPERATIONS
;;; Q is implemented as a list of paths, prefixed by the symbol Q.

(define (Q? Q) 
  (and (pair? Q) (eq? (first Q) 'Q)))	; (Q paths)

(define (make-Q path)		; constructor with a single path
  (list 'Q path))

(define Q-paths rest)			; get the list of paths

(define (Q-empty? Q)			; any paths?
  (null? (rest Q)))

(define (Q-first-path Q)		; first path on Q
  (if (Q-empty? Q)
      (error "Q is empty; no first path")
      (first (Q-paths Q))))

(define (Q-rest-paths Q)		; rest of paths on Q
  (if (Q-empty? Q)
      (error "Q is empty; no rest paths")
      (rest (Q-paths Q))))

(define (Q-set-paths! Q paths)		; modify Q, set the paths
  (if (Q? Q)
      (set-cdr! Q paths)
      (error "Not a Q" Q))
  Q)

;;; Used for blind searches, just pick the first path in the Q.
;;; modifies Q to remove the first path.

(define (pick-and-remove-first-path Q)
  (if (Q-empty? Q)
      (error "Trying to get first path, but Q is empty")
      (let ((path (Q-first-path Q)))
	(Q-set-paths! Q (Q-rest-paths Q))
	path)))

;;; Used for heuristic searches, pick the path with the best heuristic value
;;; modifies Q to remove the best path.

(define (pick-and-remove-best-path Q)
  ;; This relies on knowing the implementation of Q
  (define (loop P best-P)
    (cond ((null? (rest P))		; no more paths
	   (let ((best-path (second best-P)))
	     ;; remove best path from Q
	     (set-cdr! best-P (cddr best-P))
	     ;; return it.
	     best-path))
	  ;; the value of the current path is better than the best so far
	  ((< (path-value (second P)) (path-value (second best-P)))
	   (loop (rest P) P))
	  ;; keep going
	  (else
	   (loop (rest P) best-P))))
  (if (Q-empty? Q)
      (error "Trying to get first path, but Q is empty")
      ;; Treat first path as the best and start looking for a better one.
      (loop (rest Q) Q)))

;;; Does a global sort

(define (pick-and-remove-first-path-after-sorting Q)
  (if (Q-empty? Q)
      (error "Trying to get first path, but Q is empty")
      (begin
	(Q-set-paths! 
	  (sort! (Q-paths Q)		; sort! modifies the input list
		 (lambda (p1 p2) (< (path-value p1) (path-value p2)))))
	(let ((path (Q-first-path Q)))
	    (Q-set-paths! Q (Q-rest-paths Q))
	      path))))

;;;; THE ACTUAL SEARCH METHODS

;;; DEPTH-FIRST
(define (depth-first start goal)

  ;; A Q addition function specific to depth-first search
  ;; Add the new paths to the front of the queue
  (define (merge-paths-into-Q new-paths Q)
    (Q-set-paths! Q (append new-paths (Q-paths Q)))
    )

  (define (successors path)
    (extend-path path))

  (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				; target state
   1					; only 1 path wanted
   pick-and-remove-first-path		; pick the first path from Q
   merge-paths-into-Q			; add to the front of Q
   successors				; successors of path
   ;; The initial Q, just one path = (start)
   (make-Q (make-path #f (list start))) ; initial Q
   '()					; initial wins
   ))

;;; BEST-FIRST
(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 (append new-paths (Q-paths Q)))
    )

  (define (heuristic state) 
    ;; in general, the heuristic value will depend on the goal 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-best-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
   ))

;;; Constructing successors to a path

;; Extend a path to the neighbors of the head state
;; Returns a list of extended paths (with values = #f)

(define (extend-path path)
  ;;(display* "Extending the path " (path-reverse path))
  (remove-falses
   (map
    (lambda (next-state)
      (if (member next-state (path-states path))
	  #f
	  (make-path
	   #f				; path value is not relevant here
	   (cons next-state (path-states path)))))
    (get-neighboring-states (path-head-state path)))))

#|
(define (extend-path path)
  ;;(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
	      #f			; path value is not relevant here
	      (cons next-state (path-states path))))))
    (get-neighboring-states (path-head-state path)))))
|#

;; Extend a path to the neighbors of the head state
;; Returns a list of extended paths (with values given by the heuristic function)

(define (extend-path-with-heuristic path heuristic)
  ;;(display* "Extending the path " (path-reverse path))
  (remove-falses
   (map
    (lambda (next-state)
      
      (if (member next-state (path-states path))
	  #f
	  (make-path
	   (heuristic next-state)	; path value is from calling heuristic
	   (cons next-state (path-states path)))))
    (get-neighboring-states (path-head-state path)))))

#|
(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)))))
|#

;; Sort the paths based on the heuristic value
(define (sort-paths-by-value paths)
  (sort paths (lambda (x y) (< (path-value x) (path-value y)))))

;;;; VARIOUS AUXILIARIES

(define (describe-path path)
  (display* "The path is: ")
  (for-each display* (rest path))
  'ok)

;;; This is a simple stepper to allow you to test the extend-path code
(define (step-by-step path)
  ;;; generate a simple path by taking the first successor of the first node on
  ;;; the path
  (display* "The path " path)
  (let ((extensions (extend-path path)))
    (if (null? extensions)
	(display " cannot be extended any further.\n")
	(begin
	    (display* " can be extended to get:")
	      (pretty-print extensions)
	        (display "Continue? [y or n]: ")
		  (if (eq? 'y (read))
		            (step-by-step (car extensions))
			          '())))))

;;; DATA DEPENDENT OPERATIONS

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

(define (get-neighboring-states state)
  (let ((ans (assoc state *data*)))
    (if ans
	(rest ans)
	(error "get-neighboring-states:Unknown state" state))))

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

;; in general, the heuristic value may depend on the goal state
;; but, here, we simply lookup heuristic value in a table
(define (get-heuristic-value state goal)
  (let ((ans (assoc state *heuristic-values*)))
    (if ans
	(second ans)
	(error "get-heuristic-value:Unknown state" state))))

(define *t:silent* #f)
(define (display* . l)
  ;; Print the list of arguments
  (cond (*t:silent* #f)
	(else
	 (for-each display l)
	 (newline))))

;;; Removes false entries from the list

(define (remove-falses x)
  (if (null? x)
      '()
      (if (car x)
	  (cons (car x) (remove-falses (cdr x)))
	  (remove-falses (cdr x)))))

;;; State list manipulation
;;; Note that this does not implement constant time access, e.g. with a hash list.

(define (in-state-list? state l) (member state l))

(define (add-to-state-list state l)
  (begin
    (set! *number-ruled-out* (+ 1 *number-ruled-out*))
    (cons state l)))

(define (init-state-list . start) 
  (begin
    (set! *number-ruled-out* (length start))
    (if (null? start) '() (list (car start)))))