#+SBCL (DECLAIM (SB-EXT:MUFFLE-CONDITIONS CL:STYLE-WARNING))

(defconstant fail nil)

(defun tree-search (states goal-p successors combiner)
	   (blab :search "~&;; Search: ~a" states)
	   (cond ((null states) fail)
		 ((funcall goal-p (first states)) (first states))
		 (t (tree-search
			  (funcall combiner
					   (funcall successors (first states))
					   (rest states))
			  goal-p successors combiner))))

(defun depth-first-search (start goal-p successors)
	   (tree-search (list start)  ; initially, our states are just "start"
					goal-p        ; a function to recognize "success"
					successors    ; a generator of states next to current
					#'append      ; dfs= explore new before old
					))

(defun binary-tree (x) (print x) (print (list (* 2 x) (+ 1 (* 2 x)))))

(defun is (value) #'(lambda (x) (eql x value)))

(defun blab (what str data)
	   (declare (ignore what))
	   (format t str data))

(defun demo-dfs ()
	   (depth-first-search 1 (is 12) #'binary-tree))

(defun prepend (x y) "Prepend y to start of x" (append y x))

(defun breadth-first-search (start goal-p successors)
	   "Search old states first until goal is reached."
	   (tree-search (list start) goal-p successors #'prepend))

(defun demo-bfs ()
	   (breadth-first-search 1 (is 12) #'binary-tree))

(defun finite-binary-tree (n)
	   "Return a successor function that generates a binary tree
	   with n nodes."
	   #'(lambda (x)
				 (remove-if #'(lambda (child) 
									  (> (log child 2)  ; depth of search
										 n))
							(binary-tree x))))

(defun dfid-search (start goal-p successors max &optional (now 1))
	   (unless (> now max)
			   (blab  :dfid "~&;; Extending leash to : ~a" now)
			   (or  (depth-first-search start 
										goal-p 
										(funcall successors now))
					(dfid-search start goal-p successors max (1+ now)))))

(defun demo-dfid (&optional (max 5))
	   (dfid-search 1 (is 12) #'finite-binary-tree max))

(defun diff (num) 
	   "Return the function that finds the difference from num."
	   #'(lambda (x) (abs (- x num))))

(defun sorter (cost-fn)
	   "Return a combiner function that sorts according to cost-fn."
	   #'(lambda (new old)
				 (sort (append new old) #'< :key cost-fn)))

(defun best-first-search (start goal-p successors cost-fn)
	   "Search lowest cost states first until goal is reached."
	   (tree-search (list start) goal-p successors (sorter cost-fn)))

(defun demo-bestfs1 (&optional (goal 12))
	   (best-first-search 1 (is goal) #'binary-tree (diff goal)))

(defun price-is-right (price)
	   "Return a function that measures the difference from price,
	   but gives a big penalty for going over price."
	   #'(lambda (x) (if (> x price) 
					   most-positive-fixnum
					   (- price x))))

(defun demo-bestfs2 (&optional (goal 12))
	   (best-first-search 1 (is goal) #'binary-tree (price-is-right goal)))

(defun beam-search (start goal-p successors cost-fn beam-width)
	   "Search highest scoring states first until goal is reached,
	   but never consider more than beam-width states at a time."
	   (tree-search (list start) goal-p successors 
					#'(lambda (old new)
							  (let ((sorted (funcall (sorter cost-fn) old new)))
								(if (> beam-width (length sorted))
								  sorted
								  (subseq sorted 0 beam-width))))))

(defstruct (city (:type list)) name long lat)

(defparameter *cities*
			  '((Atlanta      84.23 33.45) (Los-Angeles   118.15 34.03)
										   (Boston       71.05 42.21) (Memphis        90.03 35.09)  
										   (Chicago      87.37 41.50) (New-York       73.58 40.47) 
										   (Denver      105.00 39.45) (Oklahoma-City  97.28 35.26)
										   (Eugene      123.05 44.03) (Pittsburgh     79.57 40.27) 
										   (Flagstaff   111.41 35.13) (Quebec         71.11 46.49)
										   (Grand-Jct   108.37 39.05) (Reno          119.49 39.30)
										   (Houston     105.00 34.00) (San-Francisco 122.26 37.47)
										   (Indianapolis 86.10 39.46) (Tampa          82.27 27.57)
										   (Jacksonville 81.40 30.22) (Victoria      123.21 48.25)
										   (Kansas-City  94.35 39.06) (Wilmington     77.57 34.14)))

(defun neighbors (city)
	   "Find all cities within 1000 kilometers."
	   (remove-if-not #'(lambda (c)
								(and (not (eq c city))
									 (< (air-distance c city) 1000.0)))
					  *cities*))

(defun city (name) 
	   "Find the city with this name."
	   (assoc name *cities*))

(defun trip (start dest)
	   "Search for a way from the start to dest."
	   (beam-search start (is dest) #'neighbors
					#'(lambda (c) (air-distance c dest))
					1))

(defconstant earth-diameter 12765.0
			 "Diameter of planet earth in kilometers.")

(defun air-distance (city1 city2)
	   "The great circle distance between two cities."
	   (let ((d (distance (xyz-coords city1) (xyz-coords city2))))
		 ;; d is the straight-line chord between the two cities,
		 ;; The length of the subtending arc is given by:
		 (* earth-diameter (asin (/ d 2)))))

(defun xyz-coords (city)
	   "Returns the x,y,z coordinates of a point on a sphere.
	   The center is (0 0 0) and the north pole is (0 0 1)."
	   (let ((psi (deg->radians (city-lat city)))
			 (phi (deg->radians (city-long city))))
		 (list (* (cos psi) (cos phi))
			   (* (cos psi) (sin phi))
			   (sin psi))))

(defun distance (point1 point2)
	   "The Euclidean distance between two points.
	   The points are coordinates in n-dimensional space."
	   (sqrt (reduce #'+ (mapcar #'(lambda (a b) (expt (- a b) 2))
								 point1 point2))))

(defun deg->radians (deg)
	   "Convert degrees and minutes to radians."
	   (* (+ (truncate deg) (* (rem deg 1) 100/60)) pi 1/180))

(defun demo-trip ()
	   (format t "one way...~%")
	   (trip (city 'san-francisco) (city 'boston))
	   (format t "~%~%and the other way...~%")
	   (trip (city 'boston) (city 'san-francisco) ))