;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; grid2d.lisp
;; a simple 2d grid world, for demonstrating ideas in Nils Nilsson's 
;; Artificial Intelligence, A new Synthesis (1999, Morrgan Kaufmann)
;; c) 2000, Lee Spector, lspector@hampshire.edu
#|

The graphic output routines assume that you are using Macintosh Common
Lisp (available from http://www.digitool.com) and that you have loaded
mcl-color-draw-3.lisp (available from 
http://hampshire.edu/lspector/courses/mcl-color-draw-3.lisp)

Disclaimer: This is a quick hack for classroom demonstration. Neither
efficiency nor style are up to very high standards.

TO RUN THIS AS-IS:

1. load mcl-color-draw-3.lisp
2. load this file
3. evaluate (follow-boundaries)

|#

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; world data structure and general utilities


;; The grid world is represented as a square array, with elements being
;; NIL (for free/empty), WALL, or ROBOT.

(defparameter *grid-size* 20
  "The number of grid squares along each edge of the world.")

(defparameter *grid*
  (make-array (list *grid-size* *grid-size*)
              :initial-element nil)
  "The grid world, stored as a 2d array.")

(defun make-wall (x-low x-high y-low y-high)
  "A utility for making lots of wall squares in a line or rectangle."
  (loop for x upfrom x-low upto x-high do
        (loop for y upfrom y-low upto y-high do
              (setf (aref *grid* x y) 'wall))))

(defun robot-xy ()
  "Returns a list of the x and y coordinates of the robot's location."
  (let (robot-x robot-y)
    (dotimes (x *grid-size*)
      (dotimes (y *grid-size*)
        (when (eq (aref *grid* x y) 'robot)
          (setq robot-x x
                robot-y y))))
    (list robot-x robot-y)))

(defun wrap (n)
  "Returns n 'wrapped' so that it is always a valid index for *grid*."
  (mod n *grid-size*))

(defun pause ()
  (sleep 0.2))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; action functions

(defun north ()
  "Moves the robot north."
  (let* ((xy (robot-xy))
         (robot-x (first xy))
         (robot-y (second xy))
         (destination-x robot-x)
         (destination-y (wrap (- robot-y 1))))
    (unless (aref *grid* destination-x destination-y)
      (setf (aref *grid* destination-x destination-y) 'robot)
      (setf (aref *grid* robot-x robot-y) nil))))

(defun east ()
  "Moves the robot east."
  (let* ((xy (robot-xy))
         (robot-x (first xy))
         (robot-y (second xy))
         (destination-x (wrap (+ robot-x 1)))
         (destination-y robot-y))
    (unless (aref *grid* destination-x destination-y)
      (setf (aref *grid* destination-x destination-y) 'robot)
      (setf (aref *grid* robot-x robot-y) nil))))

(defun south ()
  "Moves the robot south."
  (let* ((xy (robot-xy))
         (robot-x (first xy))
         (robot-y (second xy))
         (destination-x robot-x)
         (destination-y (wrap (+ robot-y 1))))
    (unless (aref *grid* destination-x destination-y)
      (setf (aref *grid* destination-x destination-y) 'robot)
      (setf (aref *grid* robot-x robot-y) nil))))

(defun west ()
  "Moves the robot west."
  (let* ((xy (robot-xy))
         (robot-x (first xy))
         (robot-y (second xy))
         (destination-x (wrap (- robot-x 1)))
         (destination-y robot-y))
    (unless (aref *grid* destination-x destination-y)
      (setf (aref *grid* destination-x destination-y) 'robot)
      (setf (aref *grid* robot-x robot-y) nil))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; perception (see Nilsson's text to make sense of this)
    
(defun sense ()
  "Returns a list of Nilsson's x1, x2, x3, and x4 features."
  (let* ((xy (robot-xy))
         (x (first xy))
         (y (second xy))
         (s1 (aref *grid* (wrap (- x 1)) (wrap (- y 1))))
         (s2 (aref *grid* x (wrap (- y 1))))
         (s3 (aref *grid* (wrap (+ x 1)) (wrap (- y 1))))
         (s4 (aref *grid* (wrap (+ x 1)) y))
         (s5 (aref *grid* (wrap (+ x 1)) (wrap (+ y 1))))
         (s6 (aref *grid* x (wrap (+ y 1))))
         (s7 (aref *grid* (wrap (- x 1)) (wrap (+ y 1))))
         (s8 (aref *grid* (wrap (- x 1)) y)))
    (list (or s2 s3) ;; x1
          (or s4 s5) ;; x2
          (or s6 s7) ;; x3
          (or s8 s1) ;; x4
          )))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; initialization

(defun init-grid-world ()
  "Creates the grid world and places the robot in it."
  ;; empty the world
  (make-array (list *grid-size* *grid-size*)
              :initial-element nil)
  ;; make the walls
  ;; top
  (make-wall 1 6 1 1)
  (make-wall 6 6 2 4)
  (make-wall 6 13 4 4)
  (make-wall 13 13 1 4)
  (make-wall 13 18 1 1)
  ;; left
  (make-wall 1 1 1 18)
  ;; bottom
  (make-wall 1 8 18 18)
  (make-wall 8 8 11 17)
  (make-wall 9 11 11 11)
  (make-wall 11 11 12 18)
  (make-wall 12 16 18 18)
  (make-wall 16 16 14 18)
  (make-wall 16 18 14 14)
  ;; right
  (make-wall 18 18 1 14)
  ;; place the robot
  (setf (aref *grid* 5 10) 'robot))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; graphics (requires mcl-color-draw-3.lisp)

(defparameter *pixels-per-grid-square* 20
  "The number of pixels per edge of a grid square.")

(defun grid-drawing-exists ()
  "Returns non-nil if there's already a window for drawing the grid."
  (and *draw-window* (wptr *draw-window*)))

(defun draw-free (x-left y-top)
  "Draws a free square with the specified upper left corner."
  (draw-color-rectangle x-left 
                        y-top 
                        *pixels-per-grid-square*
                        *pixels-per-grid-square*
                        65535
                        65535
                        65535))

(defun draw-wall (x-left y-top)
  "Draws a wall square with the specified upper left corner."
  (draw-color-rectangle x-left 
                        y-top 
                        *pixels-per-grid-square*
                        *pixels-per-grid-square*
                        0
                        0
                        0))

(defun draw-robot (x-left y-top)
  "Draws the robot with the specified upper left corner."
  (draw-color-oval x-left 
                   y-top 
                   *pixels-per-grid-square*
                   *pixels-per-grid-square*
                   0
                   0
                   65535))

(defun draw-grid-world ()
  "Draws the whole grid world, creating the window if necessary."
  ;; create the window if necessary
  (unless (grid-drawing-exists)
    (init-draw-window "Grid 2D"
                      (* *grid-size* *pixels-per-grid-square*)
                      (* *grid-size* *pixels-per-grid-square*)))
  (show-drawing) ;; bring it to the front if necessary
  ;; draw each square as appropriate
  (dotimes (x *grid-size*)
    (dotimes (y *grid-size*)
      (let ((grid-contents (or (aref *grid* x y) 'free))
            (x-left (* x *pixels-per-grid-square*))
            (y-top (* y *pixels-per-grid-square*)))
        (case grid-contents
          (free (draw-free x-left y-top))
          (wall (draw-wall x-left y-top))
          (robot (draw-free x-left y-top) ;; first clear the space
                 (draw-robot x-left y-top)))))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; top-level boundary-following procedure

(defun follow-boundaries ()
  "Initializes the grid world and enters an infinite loop of
boundary-following behavior (drawing the world and pausing at
each step)."
  (init-grid-world)
  (draw-grid-world)
  (loop (let* ((percept (sense))
               (x1 (first percept))
               (x2 (second percept))
               (x3 (third percept))
               (x4 (fourth percept)))
          (cond ((and x1 (not x2))
                 (east))
                ((and x2 (not x3))
                 (south))
                ((and x3 (not x4))
                 (west))
                (t (north))))
        (pause)
        (draw-grid-world)))

;; end