(defun a* (problem)
    (setf initialNode (make-node :state (problem-initial-state problem) :g 0 :h (funcall (problem-fn-h problem) (problem-initial-state problem)) :f (funcall (problem-fn-h problem) (problem-initial-state problem))))
    (setf standbyNodeList nil)
    (setf standbyPositionList nil)
    (push initialNode standbyNodeList)
    (push (state-pos (node-state initialNode)) standbyPositionList)
    (setf visitedPositions nil)
    (setf currentNode nil)
    (loop
        (if (null standbyNodeList)
            (return-from a* nil)
        )
        (setf standbyNodeList (sort standbyNodeList #'fNodeCompare))
        (setf currentNode (first standbyNodeList))

        (if (funcall (problem-fn-isGoal problem) (node-state currentNode))
                (return-from a* (objectivePath currentNode))
        )
        (push (state-pos (node-state currentNode)) visitedPositions)
        (setf nextStatesList (funcall (problem-fn-nextStates problem) (node-state currentNode)))
        (loop for x in nextStatesList do
            (setf childNode (make-node :parent currentNode :state x :g (+ (node-g currentNode) (state-cost x)) :h (funcall (problem-fn-h problem) x)))
            (setf (node-f childNode) (+ (node-g childNode) (node-h childNode)))
            (if (not (isMemberList (state-pos (node-state childNode)) visitedPositions))
                (if (not (isMemberList (state-pos (node-state childNode)) standbyPositionList))
                    (progn
                        (push childNode standbyNodeList)
                        (push (state-pos (node-state childNode)) standbyPositionList)
                    )
                    (if (isMemberList (state-pos (node-state childNode)) standbyPositionList)
                        (if (> (node-f (nth (isMemberList (state-pos (node-state childNode)) standbyPositionList) standbyNodeList)) (node-f childNode))
                            (progn
                                (setf standbyNodeList (remove-nth (isMemberList (state-pos (node-state childNode)) standbyPositionList) standbyNodeList))
                                (setf standbyPositionList (remove-nth (isMemberList (state-pos (node-state childNode)) standbyPositionList) standbyPositionList))
                                (push childNode standbyNodeList)
                                (push (state-pos (node-state childNode)) standbyPositionList)
                            )
                        )
                    )
                )
            )
        )
    )
)