Pizza Delivery

; read input
(defun read-input(p)
(cond
    ((null p) nil)
    (T (cons p (read-input (read nil nil))))
)
)

(defun readinput(i)
    (cdr (read-input 1))
)

(defun heuristic (state)
  (declare (special *goal*))
  (heuristic-eval state *goal*)
)
(defun heuristic-eval (state goal)
    (+ 1 (manhattan state goal))
)

(defun manhattan (state goal)
    (+ (abs (- (nth 0 state) (nth 0 goal))) (abs (- (nth 1 state) (nth 1 goal))) )
)


(defun run-best (start goal)
(declare (special *goal*)       ;state
         (special *open*)       ;list of state-tuples
         (special *closed*)     ;list of state-tuples
)
         (setq *goal* goal)
         (setq *open* (list (build-record start nil 0
                      (heuristic start))))
         (setq *closed* nil)
         (best-first)
)




(defun build-record (state parent depth weight)
;;returns state-tuple
    (list state parent depth weight)
)
(defun insert (item sorted-list)
;;returns list of state-tuples
  (cond
     ((null sorted-list) (list item))
     ((< (get-weight item) (get-weight (car sorted-list)))
         (cons item sorted-list))
     (T (cons (car sorted-list)
              (insert item (cdr sorted-list))))
  )
)

(defun insert-by-weight (childrens sorted-list)
;;returns list of state-tuples
  (cond
     ((null childrens) sorted-list)
     (T (insert (car childrens)
        (insert-by-weight (cdr childrens) sorted-list)))
  )
)


(defun get-state (state-tuple) (nth 0 state-tuple))

(defun get-parent (state-tuple) (nth 1 state-tuple))

(defun get-depth ( state-tuple )
    (nth 2 state-tuple)
)
(defun get-weight ( state-tuple )
    (nth 3 state-tuple)
)


(defun retrieve-by-state (state list)
  (cond ((null list) nil)
        ((equal state (get-state (car list))) (car list))
        (t (retrieve-by-state state (cdr list)))))



(defun best-first ()
  (declare (special *goal*)
           (special *open*)
           (special *closed*)
           (special *moves*))
  ;(print "open =") (print *open*)
  ;(print "closed =") (print *closed*)
  (cond ((null *open*) nil)
        (t (let
           ((state (car *open*)))
           (setq *closed* (cons state *closed*))
           (cond ((equal (get-state state) *goal*)
                         (reverse (build-solution *goal*)))
                  (t (setq *open*
                          (insert-by-weight
                          (generate-descendants (get-state state)
                                 (+ 1 (get-depth state)) *moves*)
                          (cdr *open*))
                      )
                      (best-first))))))
)


(defun generate-descendants (state depth moves)
;;returns the descendants of state in a list of state-tuples
(declare (special *closed*)
         (special *open*))
(cond
   ((null moves) nil)
   (t (let ((child (funcall (car moves) state))
      (rest (generate-descendants state depth (cdr moves))))
      (cond ((null child) rest)
            ((retrieve-by-state child rest) rest)
            ((retrieve-by-state child *open*) rest)
            ((retrieve-by-state child *closed*) rest)
            (t (cons (build-record child state depth
                            (+ depth (heuristic child))
               )
                 rest
           )
))))))



(defun build-solution (state)
  (declare (special *closed*))
  (cond ((null state) nil)
        (t (cons state (build-solution
                        (get-parent
                         (retrieve-by-state state *closed*)))))))


(defvar *initial* (car (readinput nil)))
;--don't change above this line

(defun safe(state)
    (cond
        ( ( < (nth 0 state) 1) nil )
        ( ( < (nth 1 state) 1) nil )
        ( ( > (nth 0 state) 4) nil )
        ( ( > (nth 1 state) 4) nil )
        ( T state )
    )
)

(defun move1 (state)
    (safe (list (nth 0 state) (- (nth 1 state) 1)))
)

(defun move2 (state)
    (safe (list (nth 0 state) (+ (nth 1 state) 1)))
)

(defun move3 (state)
    (safe (list (- (nth 0 state) 1) (nth 1 state)))
)

(defun move4 (state)
    (safe (list (+ (nth 0 state) 1) (nth 1 state)))
)


;write your function(s) here. modify heuristic function above
(defvar *moves* '(move1 move2 move3 move4))

(defvar *solution* (list (car *initial*)))

(defun addtosolution (steps)
    (setq *solution* (append *solution* (cdr steps)))
)

(defun solve(prevstate goalstates)
    (if (not (null goalstates)) (progn (addtosolution(run-best prevstate (car goalstates))) (solve (car goalstates) (cdr goalstates)) ) )
)

(solve (car *initial*) (cadr *initial*))
(print *solution*)