(defclass World ()((barriers :accessor World-barriers :initform (list) :initarg :barriers)
 (bounds                                                                        :accessor
   World-bounds                                                                :initform
     (cons 5 5)                                                                :initarg
      :bounds)                                                                 (agents
       :accessor                                                         World-agents
        :initform                                                             (list) 
         :initarg                                                        :agents)))
          (defclass                                                        Agent()
           ((movesuccess                                                :accessor
            Agent-movesuccess                                          :initform
             t                                                         :initarg
              :movesuccess)))                                        (defclass
               Agentreflex                                            (Agent)
                ((movesuccess                                      :accessor
                 Agent-movesuccess                                :initform
                  t                                               :initarg
                   :movesuccess)                                   (leftp
                    :accessor                                Agent-leftp 
                     :initform                                        t
                      :initarg                               :leftp)))
                       (defclass                         Agenthistory
                        (Agent)                        ((movesuccess
                         :accessor                Agent-movesuccess 
                          :initform t                     :initarg
                           :movesuccess)                   (leftp
                            :accessor                Agent-leftp 
                             :initform                        t
                              :initarg                 :leftp)
                               (moves               :accessor
                                Agent-moves        :initform
                                 (list)            :initarg
                                  :moves)           (state
                                   :accessor  Agent-state
                                  :initform          'lll1
                                 :initarg         :state)))
                                (defclass          Simulator
                               ()((activeagentslist :accessor
                              Simulator-activeagentslist;;;;;;
                             :initform                   (list)
                            :initarg          :activeagentslist)
                          (theworld                     :accessor
                         Simulator-theworld              :initform
                        (make-instance                      'World)
                       :initarg                         :theworld)))
                      (defmethod                              makedir
                     ((w                                        World)
                    (a                                           Agent)
                   reldir)                                         (cond
                  ((equal                                          reldir
                 'l1l1l)                                           'l1l1l)
                ((equal                                              reldir
               'forward)                                             (facing
              w                                                           a))
             ((equal                                                    reldir
            'right)                                                        (mod
           (+                                                            (facing
          w                                                                    a)
         1)                                                                    4))
        ((equal                                                              reldir
       'back)                                                                   (mod
      (+                                                                      (facing
     w                                                                              a)
    2)                                                                              4))
   ((equal                                                                        reldir
  'left)                                                                             (mod 
 (+                                                                                (facing
w a) 3) 4))((print "Invalid reldir in makedir" ) ) ) ) (defmethod turn ((w World) (a Agent)
 direction)(cond ((equal direction 'turnright)(setf (cddr (assoc a (World-agents w)))(mod(
  + 1 (cddr (assoc a (World-agents w)))) 4)))    ((equal direction 'turnleft) (setf (cddr
   (assoc a (World-agents w)))(mod (+ 3 (cddr (assoc a (World-agents w)))) 4)) )( (print
    "Turn function caused a problem!"))))(defmethod updateloc ((w World) (a Agent) loc)
     (setf (cadr (assoc a (World-agents w))) loc))(defmethod output :after ((w World)(
      a Agent) (direction INTEGER))(printworld w))(defmethod move ((w World) (a Agent
       ) (direction INTEGER))(setf newloc (incloc (location w a) direction))(cond( (
        not (blocked w newloc))(updateloc w a newloc))(nil ) ))(defun  incloc  (loc 
         direct)(cond((= direct 0)(cons (car loc) (- (cdr loc) 1)))  ((= direct 1)
          (cons (+ 1 (car loc)) (cdr loc)))((= direct 2)(cons (car loc) (+ 1 (cdr
           loc))))((= direct 3)(cons (- ( car loc ) 1 ) ( cdr loc ) ) ) ( (print
            "Invalid direction"))))(defmethod searchloc ((w World)loc)(loop for
             loc2 in (World-barriers w)do(if (and (= (car loc) (car loc2)) (=(
              cdr loc) (cdr loc2))) (return-from searchloc t)))  (return-from
               searchloc nil))(defmethod blocked ((w World) loc) (or (> (car
                loc) (car (World-bounds w)))(> (cdr loc) (cdr (World-bounds
                 w)))(< (car loc) 1)(< (cdr loc) 1) (searchloc  w loc ) ))
                  (defmethod facing ((w World) (a Agent))(cddr (assoc a (
                   World-agents w))))(defmethod location ((w World )  (a
                    Agent))(cadr (assoc a (World-agents w))))(defmethod
                     printworld((w World))(setq i 0)(loop for piece in
                      ( World-barriers  w ) do ( incf  i ) ( format t
                       "~@(~:R~) barrier is at (~d, ~d).~%"  i  (car
                        piece)(cdr piece)))(setq i 0)(loop for agnt
                         in (World-agents w) do(incf i)  (format t
        "~@(~:R~) agent(~A) is at (~d, ~d) facing [~d] [0 1 2 3 maps to N E S W].~%" 
         i(car agnt)(caadr agnt)(cdadr agnt)(cddr agnt)))(format t "~%"))(defmethod
          sense ((a Agent) (w World))(blocked w(incloc(location w a)(facing w a)))
           )(defmethod sense2 ((a Agent) (w World) q)(cond((equal q 'left)(turn w
            a 'turnleft)(setq tmp (sense a w))(turn w a 'turnright)tmp)((equal q
             'right ) ( turn  w  a  'turnright) (setq tmp (sense a w))(turn w a
              'turnleft)tmp)((equal q 'forward)(sense a w)) ( (equal q 'back)(
               turn w a 'turnright)(turn w a 'turnright)(setq tmp (sense a w)
                ) ( turn w a 'turnleft ) ( turn w a 'turnleft) tmp ) ((print
                 "invalid direction in sense"))))(defmethod mv ((a Agent)(w
                  World)(dir SYMBOL))( cond ( ( equal  dir  'left ) ( setf
                   (Agent-leftp a) t)dir)((not (equal dir 'left ) ) (setf
                    (Agent-leftp a ) nil )  dir ) ) ) (defmethod lll1 ((
                     a Agenthistory) (w World))(cond((and (not (sense a
                      w))(Agent-leftp a))(mv a w 'forward ) ) ( (not (
                       sense2 a w 'left))(mv a w 'left))((not(sense a
                        w))(mv a w 'forward))((not(sense2 a w 'right)
                         )(setf(Agent-state a)'ll1l)(mv a w 'right))
                          ( ( not (  sense2  a  w  'back ) ) ( setf
                           (Agent-state a) 'll11) (mv a w 'back))(
                            'l1l1l)));;;;;;;;;;;;;;;;;;;;;;;;;;;;
                             ;LOL So the real question is.......
                              ;which piece of code is harder to
                               ;read.  This ascii art section,
                                ;or the next unaltered   LISP
                                 ;;;;;;;;;;section!;;;;;;;;;
                                  ;;;;;;;;;;;;;;;;;;;;;;;;;
                                   ;;;;;;;;;;;;;;;;;;;;;;;
                                    ;;;;;;;;;;;;;;;;;;;;;
                                     ;;;;;;;;;;;;;;;;;;;
                                      ;;;;;;;;;;;;;;;;;
                                       ;;;;;;;;;;;;;;;
                                        ;;;;;;;;;;;;;
                                         ;;;;;;;;;;;
                                          ;;;;;;;;;
                                           ;;;;;;;
                                            ;;;;;
                                             ;;;
                                              ;
   
                                                                 
; Yeah, this is definately less readable than the first section!                                                                  
(defmethod ll1l ((a Agent) (w World))
  (cond
    ((sense a w)
     (setf (Agent-state a) 'l1l1)
     (mv a w 'left))
    ((setf (Agent-state a) 'l1ll)
     (mv a w 'forward))
  )
)

(defmethod l111 ((a Agent) (w World))
  (setf (Agent-state a) 'l1l1)
  (mv a w 'forward)
)

(defmethod l1l1 ((a Agent) (w World))
  (setf (Agent-state a) 'll11)
  (mv a w 'back)
)

(defmethod l1ll ((a Agent) (w World))
  (cond
    ((sense a w)
     (setf (Agent-state a) 'l111)
     (mv a w 'back))
    ((setf (Agent-state a) 'lll1)
     (mv a w 'forward))
  )
)

(defmethod ll11 ((a Agent) (w World))
  (cond
    ((sense2 a w 'left)
     (setf (Agent-state a) 'l1l1l)
     (mv a w 'forward))
    ((setf (Agent-state a) 'l11l)
     (mv a w 'left))
  )
)

(defmethod llll ((a Agent) (w World))
  (setf (Agent-state a) 'l1l1l)
  (mv a w 'forward)
)

(defmethod l11l ((a Agent) (w World))
  (cond
    ((sense2 a w 'left)
     (setf (Agent-state a) 'llll)
     (mv a w 'right))
    ((setf (Agent-state a) 'lll1)
     (mv a w 'left))
  )
)

(defmethod l1l1l ((a Agent) (w World))
  'l1l1l
)

(defmethod poll ((a Agenthistory) (w World))
  (funcall (Agent-state a) a w)
)

(defmethod poll ((a Agentreflex) (w World))
  (cond
    ((Agent-leftp a)
     (setf (Agent-leftp a) nil) 
     'forward)
    ((setf (Agent-leftp a) 'left))
    )
)



(defmethod simulate ((s Simulator) numfails)
  (loop while (Simulator-activeagentslist s) do
       (loop for agpair in (Simulator-activeagentslist s)
   do
     (setq thepoll (makedir (Simulator-theworld s)
       (car agpair)
       (poll (car agpair) (Simulator-theworld s))))  
     (cond
       ((or (equal thepoll 'l1l1l) (>= (cdr agpair) numfails))
        (setf (Simulator-activeagentslist s)
       (remove (assoc (car agpair) (Simulator-activeagentslist s)) 
        (Simulator-activeagentslist s))))                  
       ((not (equal thepoll 'l1l1l))                                      
        (setq mvbool (move (Simulator-theworld s) (car agpair) thepoll)) 
        (if mvbool (printworld (Simulator-theworld s)))
        (if (not mvbool) (incf (cdr agpair)) (setf (cdr agpair) 0))  
        (setf (Agent-movesuccess (car agpair)) mvbool))                
       ((print "how did you get here?!?!"))
       )
     )
       )
  )

(defun run-simulator ()
  (setq a1 (make-instance 'Agenthistory))
  (setq a2 (make-instance 'Agentreflex))
  (setq a3 (make-instance 'Agenthistory))
  
  (setq aglst (list (cons a1 (cons (cons 3 3) 1)) 
      (cons a2 (cons (cons 3 3) 1))))

  (setq aglst2 (list (cons a3 (cons (cons 3 3) 1))))

  (setq barrierlst (list (cons 4 2)))
  (setq barrierlst2 (list (cons 3 2) (cons 4 2) (cons 4 3)))
  
  (format t "~%World w1:~%~%")
  (setq w1 (make-instance 'World :barriers barrierlst :agents aglst))
  (printworld w1)
  (format t "~%World w2:~%~%")
  (setq w2 (make-instance 'World :barriers barrierlst2 :agents aglst2))
  (printworld w2)

  (setq aglstfors1 (list (cons a1 0) (cons a2 0)))
  (setq aglstfors2 (list (cons a3 0)))
  (setq s1 (make-instance 'Simulator :activeagentslist aglstfors1 :theworld w1))
  (setq s2 (make-instance 'Simulator :activeagentslist aglstfors2 :theworld w2))
  
  (format t "~%Simulating s1 (no consecutive barriers):~%~%")
  
  (simulate s1 5)

  (format t "~%Simulating s2 (three consecutive barriers):~%~%")
  
  (simulate s2 5)
)

(run-simulator)