;;; General data structures
(defstruct node
  state
  pred
  op
)

(defstruct problem
  initial-state
  operators
  goal-test
)

;;; 3-puzzle problem

(defun left (node)
  (setf loc (space-loc (node-state node)))
  (if (member loc '(1 3))
      (make-node :state (swap loc (- loc 1) (node-state node))))
  )
(defun right (node)
  (setf loc (space-loc (node-state node)))
  (if (member loc '(0 2))
      (make-node :state (swap loc (+ loc 1) (node-state node))))
)
(defun down (node)
  (setf loc (space-loc (node-state node)))
  (if (member loc '(0 1))
      (make-node :state (swap loc (+ loc 2) (node-state node))))
)
(defun up (node)
  (setf loc (space-loc (node-state node)))
  (if (member loc '(2 3))
      (make-node :state (swap loc (- loc 2) (node-state node))))
)

(defun 3goal (state)
  "Returns T if the state is the goal state"
  (equal state '(1 2 3 -))
)

(defvar 3puzz)
(setf 3puzz
      (make-problem :initial-state '(1 2 - 3)
		    :operators (list #'left #'right #'down #'up)
		    :goal-test #'3goal))

(defun test3-dfs ()
  (general-search 3puzz #'bfs-q))

(defun test3-bfs ()
  (general-search 3puzz #'dfs-q))

;; problem describes the start state, operators, goal test, and operator costs
;; queueing-function is a comparator function that ranks two states
;; general-search returns either a goal node or failure

(defun general-search (problem q-fn &aux nodes (visited nil))
  (setf nodes (list (make-node (problem-initial-state problem))))
  (while t 
    do (progn
	 (if (null nodes) (return-from 'general-search "failure"))
	 (setf node (pop nodes))
	 ;; Don't test or expand this node if already visited.
	 ;; (This assumes that we don't care about finding the shortest
	 ;; path length!!)
	 (when (not (member (node-state node) visited :test #'equal))
	   (format t "Testing ~s (~s nodes visited, ~s nodes open)~%"
		   (node-state node) (length visited) (length nodes))
	   (push (copy-tree (node-state node)) visited)
	   (if (funcall (goal-test problem) (node-state node))
	       (return-from 'general-search node))
	   (setf nodes
		 (funcall q-fn nodes 
			  (expand node (problem-operators problem))))))))


(defun expand (node operators)
  (mapcan #'(lambda (op) (funcall op node))
	  operators)
  )

;;; QUEUING FUNCTIONS

(defun bfs-q (nodes new-nodes)
  (append nodes new-nodes))

(defun dfs-q (nodes new-nodes)
  (append new-nodes nodes))

(defun space-loc (state)
  (loop for i from 0 to (- (length state) 1)
    do (if (eq (nth i state) '-) (return-from space-loc i)))
  (error "can't find space in ~s!!!" state))

(defun swap (i j state &aux temp)
  (setf temp (nth i state))
  (setf (nth i state) (nth j state))
  (setf (nth j state) temp))