(defpackage a-star
  (:use cl)
  (:export a-star test x-pos y-pos parent f-val g-val h-val))

(in-package a-star)

(defclass node ()
  ((x-pos :accessor x-pos :initarg :x-pos)
   (y-pos :accessor y-pos :initarg :y-pos)
   (parent :accessor parent :initform nil :initarg :parent)
   (f-val :accessor f-val :initarg :f-val)
   (g-val :accessor g-val :initarg :g-val)
   (h-val :accessor h-val :initarg :h-val)))

(defun make-node (parent x y g-func h-func)
  (let* ((g-val (funcall g-func parent x y))
		 (h-val (funcall h-func parent x y)))
	(make-instance 'node
				   :parent parent
				   :x-pos x
				   :y-pos y
				   :g-val g-val
				   :h-val h-val
				   :f-val (+ g-val h-val))))

(defun pos-equal (node-a node-b)
  "Returns true if node-a references the same position as node-b.  Otherwise, returns NIL."
  (and (= (x-pos node-a) (x-pos node-b))
	   (= (y-pos node-a) (y-pos node-b))))

(defun find-node-with-coords (x y node-list)
  "Returns the node in NODE-LIST with the coordinates <X, Y>.  Returns NIL if no such node was found."
  (find-if (lambda (node)
			 (and (= (x-pos node) x) (= (y-pos node) y)))
		   node-list))

(defun a-star (start-coord goal-coord valid-func g-func h-func)
  "   An implementation of the A* pathfinding algorithm by Daniel Lowe.
Given a 2 element list of START-COORD and GOAL-COORD, returns a list
of coordinates which form a path between them.  VALID-FUNC should
return t if a particular node should be considered as part of the
path, nil if not.  G-FUNC should return the distance of the given
path so far.  H-FUNC should return an estimate of the distance to
the goal, and may depend on many factors"
  (let* ((open-nodes '())
		 (closed-nodes '())
		 (start-node (make-node nil
								(first start-coord)
								(second start-coord)
								g-func
								h-func))
		 (goal-node (make-node nil
							   (first goal-coord)
							   (second goal-coord)
							   g-func
							   h-func)))
    ;; The list of open nodes starts with the beginning coordinate
	(push start-node open-nodes)
	;; Pop open nodes to check until we reach the goal coordinate
	(loop for node = (first open-nodes)
	   while (and node (not (pos-equal node goal-node))) do

	   (pop open-nodes)
	   ;; Check all the adjacent coordinates.  This method counts
	   ;; diagonals as well.  If you don't want diagonals, you can
	   ;; change it here, or you can check it with the valid-func
	   (loop for y-offset from -1 to 1
		  as y = (+ (y-pos node) y-offset) do
		  (loop for x-offset from -1 to 1
			 as x = (+ (x-pos node) x-offset)
			 ;; don't need to check the node itself
			 unless (and (zerop x-offset) (zerop y-offset))
			 ;; don't need to check already closed nodes
			 unless (find-node-with-coords x y closed-nodes)
			 ;; don't need to find already open nodes, either
			 unless (find-node-with-coords x y open-nodes)
			 ;; only check valid coordinates
			 when (funcall valid-func x y) do
			 (push (make-node node x y g-func h-func) open-nodes)))
	   ;; We exhausted the possibilities of this node, so add it
	   ;; to the closed nodes
	   (push node closed-nodes)
	   ;; If node-f increments steadily, we don't need to do
	   ;; this. However, we can't really count on it in a generic
	   ;; solution.
	   (setf open-nodes (sort open-nodes #'< :key #'f-val)))

	;; If open-nodes is non-empty, a solution was found.  We
	;; traverse them in reverse to obtain our path.
	(when open-nodes
	  (loop for trail = (first open-nodes) then (parent trail)
		 until (pos-equal trail start-node)
		 collect (list (x-pos trail) (y-pos trail)) into result
		 finally (return (cons start-coord (nreverse result)))))))