(defun factorial (n)
  (do ((j n (- j 1))
       (f 1 (* j f)))
      ((= j 0) f)))

;;; The trick to computing the search space size (number of possible
;;; states) is to develop a simple way of writing a state that can be
;;; used for the combinatorics. Thus: a state is a list of cars with
;;; t-1 vertical bars that divide the cars into t tracks. For example,
;;; 6 cars on 5 tracks might look like this:
;;;                  a|bd||c*|e
;;; meaning a on track 1, bd on 2, nil on 3, c* on 4, and e on 5.
;;; The number of ways of writing this string of 10 characters is 10!.
;;; However, 10! is an overcount, because while each car is different,
;;; the vertical bars representing the tracks are identical. Since
;;; there are 4 vertical bars, we have overcounted by a factor of 4!
;;; rearrangements. Thus the total size of the search space is
;;; 10!/4! = 151,200
(defun search-space-size (cars tracks)
  (/ (factorial (+ cars (- tracks 1)))
     (factorial (- tracks 1))))

(defun choose (n r)
  (/ (factorial n)
     (* (factorial (- n r)) (factorial r))))

(defparameter *yard-1* '((1 2) (1 3) (3 5) (4 5) (2 6) (5 6)))
(defparameter *init-state-1* '((*) (e) NIL (b c a) NIL (d)))
(defparameter *goal-state-1* '((* a b c d e) NIL NIL NIL NIL NIL))

(defparameter *yard-2* '((1 2) (1 5) (2 3) (2 4)))
(defparameter *init-state-2* '((*) (d) (b) (a e) (c)))
(defparameter *goal-state-2* '((* a b c d e) NIL NIL NIL NIL))

(defparameter *yard-3* '((1 2) (1 3)))
(defparameter *init-state-3* '((*) (a) (b)))
(defparameter *goal-state-3* '((* a b) NIL NIL))

(defparameter *yard-4* '((1 2) (1 3) (1 4)))
(defparameter *init-state-4* '((*) (a) (b c) (d)))
(defparameter *goal-state-4* '((* a b c d) NIL NIL NIL))

(defparameter *yard-5* '((1 2) (1 3) (1 4)))
(defparameter *init-state-5* '((*) (a) (c b) (d)))
(defparameter *goal-state-5* '((* a b c d) NIL NIL NIL))

(defvar *expansions* 0 "global to store number of times EXPAND is called")
(defvar *visited-states* (make-hash-table :TEST #'equal)
  "Hash table storing visited states to avoid repitition")

;;;----------------------------------------------------------------
;;; Helper functions

(defun get-track (track state)
  "returns track TRACK in state STATE."
  (nth (- track 1) state))

(defun (setf get-track) (val track state)
  "This is how we tell SETF what to do when it sees 
   (setf (get-track 4 state) '(p d q))
   which would reset track 4 to the value '(p d q)."
  (setf (nth (- track 1) state) val))

(defun engine-track-p (track state)
  "returns t if the engine is on track TRACK in state STATE"
  (member '* (get-track track state)))

(defun numtracks (state)
  "Numtracks returns the number of tracks in a state"
  (length state))

(defun left-move-p (move)
  "returns t if the move is to the left"
  (eq (first move) 'LEFT))

(defun right-move-p (move)
  "returns t if the move is to the right"
  (eq (first move) 'RIGHT))

(defun left-track (move)
  "returns the number of the left track in a move.
   note that in (right 4 5) the left track end is track 5, but
   that in move (left 5 3) the left track end is still track 5."
  (if (left-move-p move)
      (second move)
    (third move)))

(defun right-track (move)
  "returns the number of the right track in a move.
   note that in (right 4 5) the right track end is track 4, but
   that in move (left 5 3) the right track end is track 3."
  (if (right-move-p move)
      (second move)
    (third move)))


;;;----------------------------------------------------------------
;;; PROBLEM 1
;;; Possible-moves returns a list of possible (left x y) and 
;;; (right x y) moves from the given state in the given yard. To do
;;; this, it generates all moves (given the engine location) and then
;;; filters out the ones that are impossible.

(defun possible-moves (yard state)
  "generate all possible legal moves from state STATE in yard YARD."
  (remove-if #'(lambda (x) (illegal-move-p x yard state))
	     (all-moves state)))

;(defun possible-moves (yard state)
;  "generate all possible legal moves from state STATE in yard YARD.
;   Sort places all LEFT moves first. This cuts DFS nodes by almost half.
;   Unfortunatly the sort  (while efficient) is called too often.
;   Much better way would be to build the generator to put Left moves first."
;  (sort (remove-if #'(lambda (x) (illegal-move-p x yard state))
;		   (all-moves state))
;	#'string<
;	:KEY #'(lambda (move) (string (first move)))))

(defun all-moves (state)
  "generate all possible moves using a double FOR loop. Don't worry about legality."
  (loop for x from 1 to (numtracks state)
	append (loop for y from 1 to (numtracks state)
		     collect (list 'RIGHT x y)
		     collect (list 'LEFT x y))))

(defun illegal-move-p (move yard state)
  "move is legal if the following three conditions are all true:
        1) the engine is on the right or left side of the move
        2) the move is across an existing switch in the yard
        3) there is at least one car on the side we are moving from "
  (not (and (or (engine-track-p (right-track move) state) 
		(engine-track-p (left-track move) state))
	    (member (list (right-track move) (left-track move)) yard 
		    :TEST #'equal)
	    (if (left-move-p move)
		(not (null (get-track (left-track move) state)))
	      (not (null (get-track (right-track move) state)))))))
		    
	   
;;;----------------------------------------------------------------  
;;;PROBLEM 2

;;; I'm tempted to do the search code in CLOS (the Common Lisp Object
;;; System) so I have nice general abstract data types and operators
;;; for things like the queue data structure, etc. but for this course
;;; that would be too complex, so I'll stick to a straightforward Lisp
;;; implementation. Also, rather than write GENERAL-SEARCH as it is in
;;; the AIMA book, I'll write the algorithm directly  rather than pass
;;; in the functions needed (just for clarity since not everyone is
;;; too familiar with Lisp and depth-first search is so obviously
;;; recursive).  

;;; DEPTH-LIMITED-SEARCH this is a user-level function that tracks the
;;; number of expanded nodes (for illustrative purposes) using the
;;; global var *expansions* (see also the function EXPAND). Otherwise
;;; it just calls depth-limited-search-internal. 
;;; SEE ALSO: ITERATIVE-DEEPENING-SEARCH below.

(defun depth-limited-search (start goal yard limit)
  (declare (special *expansions*))
  (setf *expansions* 0)
  (let ((result (depth-limited-search-internal
		   start goal yard limit)))
    (when result (format t "~&Path length is ~d." (- (length result) 1)))
    (format t "~&Expanded ~d node~P." *expansions* *expansions*)
    (values result)))

;;; DEPTH-LIMITED-SEARCH-INTERNAL is a classic recursive
;;; implementation of depth-first search. First we test if we have
;;; reached the goal (if so, return it in a list). Then we see if we
;;; have reached our depth limit (if so, return NIL). Otherwise, we
;;; expand the current node and try to reach the goal from each child,
;;; in order, decreasing the depth limit by 1. If we *CAN* reach the
;;; goal from a child-node, then cons the current state onto the
;;; good result path and return that path to our caller (this is why
;;; when we reach the goal we return a list...so that we can cons each
;;; step on our way back out of the recursion)

(defun depth-limited-search-internal (start goal yard limit)
  (when (equal start goal) (return-from depth-limited-search-internal (list goal)))
  (when (= limit 0) (return-from depth-limited-search-internal nil))
  (dolist (next-state (expand start yard))
    (let ((result (depth-limited-search-internal next-state goal yard (- limit 1))))
      (when result (return (cons start result))))))

(defun expand (state yard)
  "Expand a state in a yard. Returns a list of all states that are one
   move away from this state."
  (declare (special *expansions*))
  (incf *expansions*)
  (mapcar #'(lambda (move) (apply-move move state)) 
	  (possible-moves yard state)))

(defun apply-move (move state)
  "determine new state after (MOVE X Y) occurs. 
   We call train cars 'kars' to avoid confusion with the Lisp function
   'car'!"
  (let ((next-state (copy-list state)))
    (if (right-move-p move)
	;; then last kar of track x is moved to the front of track y
	(let ((kar (first (last (get-track (right-track move) next-state)))))
	  (setf (get-track (right-track move) next-state)
		(butlast (get-track (right-track move) next-state)))
	  (push kar (get-track (left-track move) next-state)))
      ;; else first kar of track y is placed at end of track x
      (let  ((kar (pop (get-track (left-track move) next-state))))
	(setf (get-track (right-track move) next-state)
	      (append (get-track (right-track move) next-state)
		      (list kar))))
      )
    (values next-state)))

;;; ITERATIVE-DEEPENING-SEARCH calls depth-limited-search-internal
;;; with progressively greater depths. It is guarenteed to find the
;;; optimal solution because path costs are all 1 (so the more
;;; memory-intensive breadth-first search will also be optimal)

(defun iterative-deepening-search (start goal yard)
  (declare (special *expansions*))
  (setf *expansions* 0)
  (loop for limit = 0 then (+ limit 1)
	for result = (depth-limited-search-internal
		        start goal yard limit)
	until result
	finally (when result
		      (format t "~&Path length is ~d." (- (length result) 1))) 
	        (format t "~&Expanded ~d node~P." *expansions* *expansions*)
	        (return result)
	))

;;;----------------------------------------------------------------  
;;; PROBLEM 4
;;; A* SEARCH
;;; We should be able to get a heuristic good enough so that the space doesn't
;;; blow up too much, so we can use the simpler A*. Otherwise we'd need to switch to 
;;; IDA*.

;;; HEURISTICS

;;; TELEPORTING TRAINS
;;; A good way to develop an admissable hueristic is to relax the problem.  
;;; So---imagine that train cars can teleport from track n to track m.
;;; This is gaurenteed to underestimate the true cost (is admissible).

(defun teleport-heuristic (current-state goal-state)
  "for each track, compute the number of teleportations needed as the
   difference between the current state and the goal. Cars that are
   present in current state but not in goal need to go somewhere
   else."
  (loop for current-track in current-state
	for goal-track in goal-state
	sum (length (set-difference goal-track current-track))))

;;; KARS OUT OF PLACE
;;; Yucch. Teleporting trains doesn't work very well, since it
;;; considers all states where all cars are on track 1 to be equal
;;; even if the cars are out of order! So this heuristic counts the
;;; number of cars that are pysically out of order w.r.t. to goal
;;; state. Since order-flopping takes at least one move, this is
;;; still admissible. 

(defun kars-out-of-place (current-state goal-state)
  "just add up the number out of place on each track in the yard"
  (apply #'+ (mapcar #'num-out-of-place current-state goal-state)))

(defun num-out-of-place (current-track goal-track)
  "compute the number out of place on one track in the yard"
  (loop for i from 0 to (- (length goal-track) 1)
	when (not (eq (nth i goal-track) (nth i current-track)))
	   count 1))

;;; A* search uses the AIMA book general search template.
;;; We could pass in the function as a parameter to try different
;;; heuristics if I have a chance to write a few more.

;;; Note that the book points out the difference between a state and a
;;; node in a search program. Although they were the same for the
;;; depth-limited-search version, here we need to explicitly store the
;;; path from the start to the current-state in each node (the
;;; depth-limited version used the recursion stack to store this
;;; info---another reason DFS often uses less memory). The length of a
;;; "node" minus 1 is then the current total path cost g(n) and the
;;; first elt of a "node" will be the current state (the one that the
;;; hueristic operates on). That is also why we initialize the
;;; open-list to a (list (list start-state)).

(defun A*-search (start goal yard)
  (declare (special *expansions*))
  (setf *expansions* 0)
  (let ((result (A*-search-internal start goal yard)))
    (when result (format t "~&Path length is ~d." (- (length result) 1)))
    (format t "~&Expanded ~d node~P." *expansions* *expansions*)
    (values (nreverse result))
  ))

(defun A*-search-internal (start goal yard)
  (declare (special *visited-states*))
  (clrhash *visited-states*)
  (loop for open-list = (list (list start))
	       then (queueing-fn (rest open-list) (expand state yard) node 
				 goal #'kars-out-of-place)
	for node = (first open-list)
	for state = (first node)
	do (setf (gethash state *visited-states*) t)
	;do (format t "~&    OL: ~a" open-list)
	;do (format t "~& State: ~a" state)
	when (equal state goal)
	     do (return node)
	until (null open-list)))

;;; QUEUEING FUNCTIONS
;;; Normally I'd do this in CLOS (Common Lisp Object System) with an
;;; abstract data type and methods... The premier data structure for
;;; such things is a priority-queue, ususally implemented as a heap. 

;;; Here, instead, I will use a sorted list as a queue, and Lisp's
;;; built-in MERGE sort. (Sorry, no built-in Lisp heaps :-) Thus we
;;; can use POP to remove the "best" element, but a special ENQUEUE
;;; function will place a new entry on the sorted open-list at the
;;; correct spot.

(defun enqueue (elt queue evalfn)
  "place ELT into already sorted queue QUEUE where the queue is sorted
   by the value of EVALFN (use this value as the KEY), smallest elt
   first (use #'< as the sort predicate)."
  (merge 'list (list elt) queue #'< :KEY evalfn))

(defun queueing-fn (open-list next-states node goal-state heuristic)
  "From our AIMA book. Takes the expansion of NODE and creates (length
   next-states) new nodes, and enqueues them on our queue using
   the given heuristic fn."
  (declare (special *visited-states*))
  (let ((next-nodes (mapcar #'(lambda (next) (cons next node))
			    next-states)))
    (dolist (next next-nodes)
      (unless (gethash (first next) *visited-states*)	    
	(setf open-list
	      (enqueue next open-list
		       #'(lambda (node) (+ (path-cost node)
					   (funcall heuristic (first node) goal-state)))))))
    (values open-list)))

(defun path-cost (node)
  "path cost of a node for the train problem is just the number of
   states in the node minus 1 (the number of moves made so far)."
  (- (length node) 1))