;;
;; AI 096210 Winter 2005-6 ex2 reference solution
;;
;; (c) Alon Altman.
;;
;; License: GNU GPL
;;

;;
;; Generic DFS implementation. For partially informed, 
;; successors have to be sorted
;;
(defun DFS (start goalp successors &optional (depth -1) father)
  (if (funcall goalp start)
      (list start) ; Stop condition - found goal
    (if (= depth 0)
	() ; Stop condition - depth limit reached
      (dolist (node (funcall successors start)) ; For each successor
	(when (not (equal node father))         ; that is not the father
	  (let ((result (DFS node goalp successors (1- depth) start))) ; Call recursively
	    (when result ; If found
	      (return-from DFS (cons start result)) ; Return result
	    )))))))
;;
;; Partially informed DFS for search problem
;;
(defun sp-DFS (sp)
  (DFS (sp-start sp) (sp-goalp sp) 
       #'(lambda (n) (sort (funcall (sp-successors sp) n) 
			   #'< :key (sp-h sp))) ; Sort successors
       ))

;;
;; Iterative deepening DFS
;; Bug - will not stop if not found.
;;
(defun id-DFS (start goalp successors)
  (do* ((i 1 (1+ i))
	(result nil (DFS start goalp successors i)))
      (result result)))

;;
;; Find the minimal element of list under function fun and value.
;;
(defun minimize (list fun)
  (if (null list) ()
    (if (null (cdr list))
	(list (car list) (funcall fun (car list)))
      (let ((min (minimize (cdr list) fun))
	    (val (funcall fun (car list))))
	(if (< val (second min))
	    (list (car list) val)
	  min)
	)
      )
    )
  )

;;
;; Hill climbing with iterative deepening DFS restart
;;
(defun hill-climbing-id-DFS (start goalp successors h)
  (if (funcall goalp start) (list start) ; Stop condition - found goal
    (let* ((succ (funcall successors start)) ; Find successors
	   (min-succ (minimize succ 
			       #'(lambda (x) 
				   (if (funcall goalp x) -1 (funcall h x)))))) 
					; Find best successor by h or goalp
      (if (> (funcall h start) (second min-succ))
	  ; If found better node, continue.
	  (let ((hc (hill-climbing-id-DFS (car min-succ) goalp successors h)))
	    (if hc (cons start hc)))
	; Otherwise, run DFS
	(let* ((dfs (id-dfs start 
			    #'(lambda (x) (< (funcall h x) (funcall h start)))
			    ; Goal predicate is finding a better h value
			    successors))
	       (hc (hill-climbing-id-DFS (car (last dfs)) goalp successors h)))
	       ; Continue hill climbing from DFS result
	  (if (and dfs hc) (append dfs (cdr hc)))))))) ; Return solution path

;;
;; Hill climbing with iterative deepening DFS restart for search problem
;;
(defun sp-hill-climbing-id-DFS (sp)
  (hill-climbing-id-DFS (sp-start sp) (sp-goalp sp) (sp-successors sp) (sp-h sp)))

;;
;; A* algorithm
;;
(defun A* (start goalp successors h)
  (reverse ; Paths are stored in reverse order
   (let* ((closed (make-hash-table :test #'equal))) ; Closed is better implemented as hash table
     (do* ((open (list (list start (funcall h start) 0 (list start)))  ; Initialize open list
		 (sort open #'< :key #'second)) ; Open must be sorted every iteration
	   (node (pop open) (pop open))) ; Node is selected from open
	 ((or (null node) (funcall goalp (car node))) (fourth node)) ; End condition - no node or goal.
       (setf (gethash (car node) closed) (second node)) ; Put node in closed with f value
       (dolist (s (funcall successors (car node))) ; Check successors
	 (let ((val (+ (third node) (funcall h s) 1))) ; Compute successor's f value
	   (when (or (not (gethash s closed)) 
		     (> (gethash s closed) val)) ; Check if not already exists in closed with better value
	     (setf (gethash s closed) nil) ; Remove from closed
	     (push (list s val (1+ (third node)) (cons s (fourth node))) open) ; Put into open
	     )))))))

;;
;; A* algorithm for search problem
;;
(defun sp-A* (sp)
  (A* (sp-start sp) (sp-goalp sp) (sp-successors sp) (sp-h sp)))
  
;;
;; Test harness
;;

; Current time
(defun get-time ()
  (/ (get-internal-run-time) internal-time-units-per-second))

; Measure time difference
(defmacro measure-time (x)
 `(let ((start-point (get-time))
        (result ,x))
 (values (- (get-time) start-point) result)))

; Measure time and nodes to run search algorithm
(defun measure-time-and-nodes (search sp)
  (let* ((developed-nodes 0))
    (multiple-value-bind (time result)
	(measure-time
	 (funcall search (make-sp :start (sp-start sp)
				  :successors #'(lambda (x) (incf developed-nodes)
							(funcall (sp-successors sp) x))
				  :goalp (sp-goalp sp)
				  :h (sp-h sp)
				  :print (sp-print sp))))
      (funcall (sp-print sp) result)
      (list time developed-nodes))))