;; A Star Search

;This is called the A Star Algorithm
(setq open_count 0)
(setq val 0)
(defun astar(start goal_node)
(setq goal goal_node)
(prog ()
      (setq closed nil)
      (putprop start 0 'gvalue)
      (putprop start nil 'ptr)
      (putprop start (f start) 'fvalue)

      (setq open (list start))
      (loop (cond ((null open)(return 'failure)))
          (setq n (select_best open))
          (setq open (delete n open))
          (setq closed (cons n closed))
          (if (eq n goal) (return (print(extract_path n))))
          (setq l (mapcar 'car (get n 'adj)) )
          (mapcar 'open_node (set_diff (set_diff l open) closed))
          (mapcar 'update_open  (intersect l open))
          (mapcar 'update_closed (intersect l closed))

    )
)
)


(defun select_best (lst)
   (cond ((eq (first lst) goal)(first lst))
         (T (better (first lst)(rest lst)))
   )
)

(defun better (elem lst)
   (cond ((null lst) elem)
         ((< (get elem 'fvalue)(get (first lst) 'fvalue)) elem)
         ((eq (first lst) goal)(first lst))
         (T (better elem (rest lst)))
   )
)

(defun open_node (node)

   (prog ()
     (setq open_count ( + 1 open_count))
     (putprop node (G node) 'gvalue)
     (putprop node (setq val (f node)) 'fvalue)
     (setq open (insert node open))
     (putprop node n 'ptr)
   )
)

(defun update_open (node)
   (prog ()
     (setq val (G node))
     (cond ((< val (get node 'gvalue))
                  (putprop node val 'gvalue)
                  (putprop node (F node) 'fvalue)
                  (putprop node n 'ptr)
                  (setq open (insert node (delete node open)))
     )
   )
))

(defun update_closed (node)
     (prog ()
       (setq val (G node))
       (cond ((< val (get node 'gvalue))
                        (putprop node val 'gvalue)
                        (putprop node (F node) 'fvalue)
                        (putprop node n 'ptr)
                        (setq open (insert node open))
                        (setq closed (delete node closed)))
       )
      )
)

(defun intersect (ls1 ls2)
   (cond ((null ls1) ls2)
      ((member (car ls1) ls2) (cons (car ls1) (intersect (cdr ls1) ls2)))))

(defun insert (node lst)
(cond ((null lst)(list node))
((< val (get (first lst) 'fvalue))(cons node lst))
          (T (cons (first lst)(insert node (rest lst))))
    )
)


(defun putprop (s  v p)
  (setf (get s p) v)
)

(defun set_diff (ls1 ls2)
   (cond ((null ls1) nil)
         ((member (first ls1) ls2)(set_diff (rest ls1) ls2))
         (T (cons (first ls1)(set_diff (cdr ls1) ls2)))
   )
)

;the next two functions could easily be combined but the author wanted
;to make the fvalue property self explanatory
(defun longitude_diff(n1 n2)
  (abs (- (get n1 'lg)(get n2 'lg)))   
)

(defun f(x)
(+ (get x 'gvalue) (h x))
)

(defun g(x)                               ;where n is global and is the current
 (+ (get n 'gvalue) (arc_dist n x))          ;node you are inspecting
)

(defun h(x)
   (* 10 (longitude_diff x goal))
)
  
(defun arc_dist (n n2)
   (cdr_select n2 (get n 'adj))
)

(defun cdr_select (key lst)
  (cond ((null lst) 9999)

        ((eq key (caar lst))(cdar lst))    ;(first (first lst))) (rest (first lst)))
        (T (cdr_select key (cdr lst)))
)
)


(defun extract_path (n)
  (cond ((null n) nil)
        (t (append (extract_path (get n 'ptr))
                                   (list n)))
  )
)

;lg stands for longitude.
;each city is paired with its longitude.
;notice how cleverly the mapcar effectively accomplishes 18 putprops

(mapcar #'(lambda(x) (putprop (first x)(first (rest x)) 'lg))
          '((av 48)
            (bord -6)(bre -45)
            (caen -4)(calais 18)
            (di 51)
            (gren 57)
            (lim 12)(ly 48)
            (mars 53) (mont 36)
            (nan -16)(ncy 62)(nice 73)
            (paris 23)
            (ren -17)
            (stras 77)
            (to 14))
)


;these putprops construct the graph. It is similar to Worksheet #4 except that
;the graph is much larger and the actual distances between the cities are now used

(setf (get 'av 'adj) '((gren . 227)( mars . 99)( mont . 91)( ly . 216))
      (get 'bord 'adj) '((lim . 220)(to . 253)(nan . 329))
      (get 'bre 'adj)  '((ren . 244))
      (get 'caen 'adj) '((calais . 120)( paris . 241)( ren . 176))
      (get 'calais 'adj) '((ncy . 534)( paris . 297)( caen . 120))
      (get 'di 'adj) '((stras . 335)( ly . 192)( paris . 313)( ncy . 201))
      (get 'gren 'adj) '((av  . 227)(ly . 104))
      (get 'lim 'adj)
             '((ly . 389)(to . 313)(bord . 220)(nan . 329)(paris . 396))
      (get 'ly 'adj) '((gren . 104)( av . 216)( lim . 389)( di . 192))
      (get 'mars 'adj) '((nice . 188)( av . 99))
      (get 'mont 'adj) '((av . 91)(to . 240))
      (get 'nan 'adj) '((lim . 329)(bord . 329)(ren . 107))
      (get 'ncy 'adj) '((stras . 145)( di . 201)( paris . 372)( calais . 534))
      (get 'nice 'adj) '((mars . 188))
      (get 'paris 'adj)
        '((calais . 297)(ncy . 372)(di . 313)(lim . 396)(ren . 348)(caen . 241))
      (get 'ren 'adj) '((caen . 176)( paris . 348)( bre . 244)( nan . 107))
      (get 'stras 'adj) '((di . 335)( ncy . 145))
      (get 'to 'adj) '((mont . 240)(bord . 253)(lim . 313))
)