(SETQ LOCATION 'i6)
(SETQ GOAL 'd2)
(SETQ PLAN '(h9 i5 h7 i4 h6 i2 h3 i3 h4 d2))

(SETQ FLOOR-MAP '(
(i1 h1 h2 h12)
(i2 h1 h3 h6)
(i3 h3 h4 h5)
(i4 h6 h7 h13)
(i5 h7 h8 h9)
(i6 h9 h10)
(i7 h10 h11 h13 d5)
(i8 h11 h12)
(d1 h2)
(d2 h4)
(d3 h5)
(d4 h8)
(d5 i7)
(h1 i1 i2)
(h2 i1 d1)
(h3 i2 i3)
(h4 i3 d2)
(h5 i3 d3)
(h6 i2 i4)
(h7 i4 i5)
(h8 i5 d4)
(h9 i5 i6)
(h10 i6 i7)
(h11 i7 i8)
(h12 i1 i8)
) )

(DEFUN RUN ()
  ; User Interface.
  (EXECUTE-PLAN)
)

(DEFUN EXECUTE-PLAN ()
  ; High-Level command function.
  (DO
    (
      ; Remove the first item from the plan,
      (PLAN PLAN (CDR PLAN))
      ; and use it as the next location
      (NEXT-LOCATION (CAR PLAN) (CADR PLAN))
    )
    ; If we have arrived at our destination, run out of plan,
    ; or an error occurs, exit with the result.
    ( (SETQ RESULT (CHECK-POSITION PLAN LOCATION NEXT-LOCATION)) RESULT)
    ; otherwise, move to the next location
    (MOVE NEXT-LOCATION)
  )
)

(DEFUN CHECK-STEP (LOCATION NEXT-LOCATION)
  ; High-Level logic function.
  ; return next-location in assoc(location, floor-map);
  (member NEXT-LOCATION (ASSOC LOCATION FLOOR-MAP))
)

(DEFUN MOVE (NEW-LOC)
  ; Mid-Level action function.
  ; Would call low-level movement routines.

  ; First, report what we're doing...
  (WRITE-STRING (STRING LOCATION))
  (WRITE-STRING " -> ")
  (WRITE-STRING (STRING NEW-LOC))
  (FRESH-LINE)
  ; Then actually do it.
  (SETQ LOCATION NEW-LOC)
)

(DEFUN CHECK-POSITION (PLAN LOCATION NEXT-LOCATION)
  ; High-Level logic function.
  ; If there is a reason to stop execution of the plan, return said reason:
  ;  LOCATION if we are there or have no plan left
  ;  A STRING error if there is a problen with the plan
  ; else, return NIL
  (COND
    (
      (EQUAL LOCATION GOAL)
      ; We have arrived at our goal.
      LOCATION
    )
    (
      (NULL PLAN)
      ; No more plan.
      LOCATION
    )
    (
      (NULL (CHECK-STEP LOCATION NEXT-LOCATION))
      ; Plan contains an error.
      "Invalid step in plan!"
    )
    (
      T
      ; No problems, continue execution.
      NIL
    )
  )
)