;**********************************************************
;
;  Lisp functions for Class ECE 578 Intelligent Robotics
;  at PSU Portland State University
;
;  HOMEWORK Week 2
;
;  Author: Reto Toengi (RT)
;
;  Created: 13 Jan 2005
;  Modified: 20 Jan 2005
;
;**********************************************************

;----------------------------------------------------------
;
; Given is a labyrinth 0 = Wall; 1 = Corridor; (Ex) = exit
; Write a program, that the robot finds a path out
; of the labyrinth.
;
; My approach was to mark every junction with the directions
; possible or not explored. It is built as the program is a
; robot with sensors on each side, and ability to make a cell
; and read it.
;
; The advantage of my solution is: only the memory for the
; labyrinth.
; With some extension, several robots could explore the same
; labyrinth, (collision and path recognition have to be changed).
; The disadvantage of my solution is, the shortest path can
; not be found, since it is not possible to know,
; which direction has been explored or not.
;----------------------------------------------------------

 (defstruct robot mode orientation latitude longitude counter)
 (setq rob1 (make-robot :mode 'explore
                        :orientation 'N
                        :latitude 1
                        :longitude 4
                        :counter 2))

 (set 'labyrinth (make-array '(7 7)
                   :initial-contents
                 '((0 0 0 0 0 0 0)
                   (0 1 1 1 1 1 0)
                   (0 1 1 0 0 1 0)
                   (0 1 0 1 1 1 0)
                   (0 1 0 1 0 1 0)
                   ((Ex) 1 1 1 1 1 0)
                   (0 0 0 0 0 0 0))))

 (setq route ())
 (setq routetemp ())
 (setq cycles 0)
;----------------------------------------------------------
; help functions
;----------------------------------------------------------
(defun del-nil (ls)
 (cond ((member (car ls) dir-north) (cons (car ls) (del-nil(cdr ls))))
       ((numberp (car ls)) (cons (car ls) (del-nil(cdr ls))))
       ((and (listp ls) (null ls)) '())
       (T (del-nil (cdr ls)))))

(defun del-direction (ls direction)
 (cond ((and (not (equal (car ls) direction)) 
             (member (car ls) dir-north))
         (cons (car ls) (del-direction (cdr ls) direction)))
       ((numberp (car ls)) (cons (car ls) (del-direction (cdr ls) direction)))
       ((and (listp ls) (null ls)) '())
       (T (del-direction (cdr ls) direction))))

;----------------------------------------------------------
; Constants
;----------------------------------------------------------

(setq priority '(left front right back))
(setq priority2 '(right front left back))
(setq dir-north '(N E S W))
(setq dir-east '(E S W N))
(setq dir-south '(S W N E))
(setq dir-west '(W N E S))

;----------------------------------------------------------
; data access functions
;----------------------------------------------------------

;---Robot--------------------------------------------------

(defun mode (rob)
 (robot-mode rob))

(defun orientation (rob)
 (robot-orientation rob))

(defun latitude (rob)
 (robot-latitude rob))

(defun longitude (rob)
 (robot-longitude rob))

(defun counter (rob)
 (robot-counter rob))

(defun set-mode (rob m)
 (setf (robot-mode rob) m))

(defun set-orientation (rob orient)
 (setf (robot-orientation rob) orient))

(defun set-latitude (rob pos)
 (setf (robot-latitude rob) pos))

(defun set-longitude (rob pos)
 (setf (robot-longitude rob) pos))

(defun set-counter (rob nr)
 (setf (robot-counter rob) nr))

(defun robot-data (rob)
 (list (mode rob)
       (orientation rob)
       (latitude rob)
       (longitude rob)
       (counter rob)))

;---Labyrinth---------------------------------------------

(defun get-cell (lat long)
 (aref labyrinth lat long))

(defun set-cell (lat long val)
 (setf (aref labyrinth lat long) val))

(defun check-dir (rob direction)
 (cond ((or (listp (world rob direction)) (> (world rob direction) 0)) direction)))

(defun read-dir (rob directions)
 (cond ((null directions) ())
       (T (del-nil (cons (check-dir rob (car directions)) (read-dir rob (cdr directions)))))))

(defun read-cell (rob)
 (cond ((listp (world rob 'actual)) (world rob 'actual))
       ((and (check-dir rob (front rob))
             (not (or (check-dir rob (left rob))
                      (check-dir rob (right rob)))))
        (world rob 'actual))
       (T (cons (world rob 'actual) (read-dir rob dir-north)))))

;----------------------------------------------------------
; robot direction conversion functions
;----------------------------------------------------------

(defun left (rob)
 (car (last (direction (orientation rob)))))

(defun right (rob)
 (cadr (direction (orientation rob))))

(defun front (rob)
 (car (direction (orientation rob))))

(defun back (rob)
 (caddr (direction (orientation rob))))

(defun direction (direction)
 (cond ((equal direction 'N) dir-north)
       ((equal direction 'E) dir-east)
       ((equal direction 'S) dir-south)
       ((equal direction 'W) dir-west)))

(defun next-dir (rob order)
 (cond ((null order) nil)
       ((member (cond ((equal (car order) 'left) (left rob))
                      ((equal (car order) 'right) (right rob))
                      ((equal (car order) 'front) (front rob))
                      ((equal (car order) 'back) (back rob))) (read-cell rob))
                  (cond ((equal (car order) 'left) (left rob))
                        ((equal (car order) 'right) (right rob))
                        ((equal (car order) 'front) (front rob))
                        ((equal (car order) 'back) (back rob))))
       (T (next-dir rob (cdr order)))))

(defun world (rob direction)
 (cond ((equal direction 'actual) (get-cell (latitude rob) (longitude rob)))
       ((equal direction 'N) (get-cell (- (latitude rob) 1) (longitude rob)))
       ((equal direction 'E) (get-cell (latitude rob) (+ (longitude rob) 1)))
       ((equal direction 'S) (get-cell (+ (latitude rob) 1) (longitude rob)))
       ((equal direction 'W) (get-cell (latitude rob) (- (longitude rob) 1)))
       (T (and (print 'direction_does_not_exist) nil))))

;----------------------------------------------------------
; robot action functions
;----------------------------------------------------------

(defun orient (rob)
 (set-orientation rob (next-dir rob priority)))

(defun orient2 (rob)
 (set-orientation rob (next-dir rob priority2)))

(defun overwrite-rob-counter (rob)
 (set-counter rob (cond ((numberp (read-cell rob)) (read-cell rob))
                        ((listp (read-cell rob)) (car (read-cell rob)) ) )))

(defun move-on (rob)
 (progn
  (cond ((equal (mode rob) 'explore) (set-counter rob (+ (counter rob) 1)))
        ((equal (mode rob) 'renumber-backtrack) (set-counter rob (+ (counter rob) 1)))
        ((equal (mode rob) 'renumber-explored) (set-counter rob (+ (counter rob) 1)))
        (T (set-counter rob (- (counter rob) 1))))
  (cond ((equal (orientation rob) 'N) (set-latitude rob (- (latitude rob) 1)))
        ((equal (orientation rob) 'S) (set-latitude rob (+ (latitude rob) 1)))
        ((equal (orientation rob) 'E) (set-longitude rob (+ (longitude rob) 1)))
        ((equal (orientation rob) 'W) (set-longitude rob (- (longitude rob) 1)))
        (T print 'direction not possible))))

(defun mark-cell (rob mark)
 (set-cell (latitude rob) (longitude rob) mark))

(defun mark-deadend (rob direction)
 (cond ((numberp (read-cell rob))
         (mark-cell rob (cons (read-cell rob) (del-direction (read-dir rob dir-north) direction))))
       (T (mark-cell rob (del-direction (read-cell rob) direction)))))

(defun overwrite-cell-number (rob)
 (cond ((numberp (read-cell rob)) (mark-cell rob (robot-counter rob)))
       (T (mark-cell rob (cons (robot-counter rob) (cdr (read-cell rob)))))))

(defun remove-dir (rob direction)
 (cond ((numberp (read-cell rob)) (print 'nothing_to_remove))
       (T (mark-cell rob (del-direction (read-cell rob) direction)))))

;----------------------------------------------------------
; labyrinth check functions
;----------------------------------------------------------

(defun no-way-out (rob)
 (cond ((numberp (read-cell rob)) nil)
       (T (null (cdr (read-cell rob))))))

(defun way-out (rob)
 (cond ((numberp (read-cell rob)) nil)
       ((listp (read-cell rob)) nil)
       (T (equal (read-cell rob) 'Ex))))

(defun deadend (rob)
 (cond ((numberp (read-cell rob)) nil)
       (T (null (cddr (read-cell rob))))))

(defun corner (rob)
 (cond ((numberp (read-cell rob)) nil)
       (T (and (listp (cdr (read-cell rob))) (or (left rob) (right rob))))))

(defun multiplejunction (rob)
 (cond ((numberp (read-cell rob)) nil)
       (T (not (null (cdr (cddr (read-cell rob))))))))

(defun high_number (rob)
 (< (counter rob) (cond ((numberp (read-cell rob)) (read-cell rob))
                        (T (car (read-cell rob))))))

(defun low_number (rob)
 (> (counter rob) (cond ((numberp (read-cell rob))
                          (cond ((> (read-cell rob) 1) (read-cell rob))
                                (T (+ (robot-counter rob) 1))))
                        (T (cond ((> (car (read-cell rob)) 1) (car (read-cell rob)))
                                 (T (+ (robot-counter rob) 1)) )))))
(defun start (rob)
 (cond ((numberp (read-cell rob)) (equal (read-cell rob) 2))
       (T (equal (car (read-cell rob)) 2))))

(defun final (rob)
 (cond ((numberp (read-cell rob)) nil)
       (T (equal (car (read-cell rob)) 'Ex))))

;----------------------------------------------------------
; robot mode check functions
;----------------------------------------------------------
(defun check-rob-mode (rob)
 (cond ((equal (mode rob) 'explore) (check-explore rob))
       ((equal (mode rob) 'backtrack) (check-backtrack rob))  
       ((equal (mode rob) 'renumber-backtrack) (check-renumber-backtrack rob))  
       ((equal (mode rob) 'renumber-explored) (check-renumber-explored rob))
       ((equal (mode rob) 'way-in) (check-way-in rob))
       ((equal (mode rob) 'stop) ())
       (T (progn (print 'unknown-state) (set-mode rob 'stop)))))

;---Explore------------------------------------------------

(defun check-explore (rob)
 (cond ((final rob) (explore_final rob))
       ((no-way-out rob) (set-mode rob 'stop))
       ((way-out rob) (set-mode rob 'stop))
       ((deadend rob) (explore_backtrack rob))
       ((start rob) (explore_renumber-backtrack rob))
       ((multiplejunction rob) (cond ((low_number rob) (explore_renumber-backtrack rob))
                                     ((high_number rob) (explore_renumber-explored rob))
                                     (T (explore rob))))
       (T (explore rob))))

(defun explore (rob)
 (cond ((corner rob) (progn (overwrite-cell-number rob) (orient rob)))
       (T (overwrite-cell-number rob))))

(defun explore_backtrack (rob)
 (progn (overwrite-cell-number rob) (orient rob) (set-mode rob 'backtrack)))

(defun explore_renumber-backtrack (rob)
 (progn (overwrite-rob-counter rob) (set-orientation rob (back rob)) (set-mode rob 'renumber-backtrack)))

(defun explore_renumber-explored (rob)
 (progn (overwrite-cell-number rob) (orient rob) (set-mode rob 'renumber-explored)))

(defun explore_final (rob)
 (progn (print 'exit_found) (set-orientation rob (back rob)) (set-mode rob 'way-in)))

;---Backtrack----------------------------------------------

(defun check-backtrack (rob)
 (cond ((multiplejunction rob) (progn (set-mode rob 'stop) (backtrack_explore rob)))
       ((start rob) (backtrack_explore rob))
       (T (backtrack rob))))

(defun backtrack (rob)
 (cond ((corner rob) (orient rob))))

(defun backtrack_explore (rob)
 (progn (remove-dir rob (back rob)) (overwrite-rob-counter rob)
      (orient rob) (set-mode rob 'explore)))

;---Renumber-Backtrack-------------------------------------

(defun check-renumber-backtrack (rob)
 (cond ((low_number rob) (renumber-backtrack_backtrack rob))
       ((multiplejunction rob) (renumber-backtrack_explore rob))
       (T (renumber-backtrack rob))))

(defun renumber-backtrack (rob)
 (progn (overwrite-cell-number rob) (cond ((corner rob) (orient rob)))))

(defun renumber-backtrack_explore (rob)
 (progn (overwrite-cell-number rob)
      (orient rob) (set-mode rob 'explore)))

(defun renumber-backtrack_backtrack (rob)
 (progn (cond ((or (multiplejunction rob) (corner rob)) (orient rob)))
        (mark-deadend rob (front rob)) (set-mode rob 'backtrack) ))

;---Renumber-Explored--------------------------------------

(defun check-renumber-explored (rob)
 (cond ((final rob) (renumber-explored_final rob))
       ((low_number rob) (renumber-explored_backtrack rob))
       ((multiplejunction rob) (renumber-explored_explore rob))
       (T (renumber-explored rob))))

(defun renumber-explored (rob)
 (progn (overwrite-cell-number rob) (cond ((corner rob) (orient rob)))))

(defun renumber-explored_explore (rob)
 (progn (overwrite-cell-number rob)
      (orient rob) (set-mode rob 'explore)))

(defun renumber-explored_backtrack (rob)
 (progn (mark-deadend rob (back rob))
        (set-orientation rob (back rob))
        (set-mode rob 'backtrack)))

(defun renumber-explored_final (rob)
 (progn (print 'exit_found) (set-orientation rob (back rob)) (set-mode rob 'stop)))

;---way-in-------------------------------------------------

(defun check-way-in (rob)
 (cond ((start rob) (way-in_explore rob))
       (T (way-in rob))))

(defun way-in (rob)
 (progn (setq routetemp (cons (list (longitude rob) (latitude rob)) routetemp))
     (cond ((or (multiplejunction rob) (corner rob)) (orient2 rob)))))

(defun way-in_explore (rob)
 (progn
      (print 'last_path)
      (print routetemp)
      (print labyrinth)
      (cond ((null route) (progn (setq route routetemp)
                                 (set-mode rob 'explore)))
            ((not (equal (length routetemp) (length route))) (setq route routetemp) (set-mode rob 'explore))
            (T (progn (print 'shortest_path)
                      (print route)
                      (set-mode rob 'stop))))
      (setq routetemp ())
      (orient rob)
      (overwrite-cell-number rob)))

;----------------------------------------------------------
; robot main program
;----------------------------------------------------------
(defun main (rob)
 (progn
  (check-rob-mode rob)
  (print (robot-data rob))
  (setq cycles (+ cycles 1))
  (cond ((> cycles 120) (set-mode rob 'stop)))
  (cond ((not (equal (robot-mode rob) 'stop)) (move-on rob)))
  (cond ((not (equal (robot-mode rob) 'stop)) (main rob)))
 ))