;============================================================================================
; To find exit and optical path, robot can decide those after read all map 
by A*.
; this program only deal with finding exit.
; My algorithm is very simple.
; There is two way to find exit. (following left wall or right wall)
; I am using the way to follow left wall.
; Robot has four sensors at each direction (south east north west)
; And at each point robot will read map by sensors and record information at 
memory.
; Robot should record if the spot is the place he had gone or not before.
; And when he meets a blind lane he should go out by backtracking using 
stack(push and pop)
; but I am not useing those.
;=============================================================================================
;
; variables setting
;=========================

(setf EAST 0 WEST 1 SOUTH 2 NORTH 3) ; for robot's head

(setf N 0 E 0 W 0 S 0 )    ; direction of map

(setf L 0 R 0 F 0 B 0)     ; sensors of robot

(setf Y 0 X 0)       ; location of array

; INITIAL
;================

(setf HEAD EAST)

(setq Y 8 X 1)   ;starting point

;=====================================================================
; MAP ('*' is exit. robot dose not know the exit)
; 1 is "block" and 0 is "open"
; robot should be able to detect if here is inside of labyrinth or not
; but basically I will set the area of labyrinth.
;=====================================================================
(setf MAZE
      (make-array '(10 10) :initial-contents
                  '((1 1 1 1 1 1 1 1 1 1)
                    (1 0 0 0 0 1 0 1 0 *)
                    (1 0 1 1 1 1 0 1 0 1)
                    (1 0 0 0 0 0 0 1 0 1)
                    (1 0 0 0 0 0 0 1 0 1)
                    (1 1 1 1 1 0 0 1 0 1)
                    (1 0 0 0 1 0 0 1 0 1)
                    (1 1 1 0 1 0 0 1 0 1)
                    (1 0 0 0 0 0 0 0 0 1)
                    (1 1 1 1 1 1 1 1 1 1)) ))

(setf MEMORY (make-array '(10 10) :initial-contents
'((1 1 1 1 1 1 1 1 1 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 0 0 0 0 0 0 0 0 1)
  (1 1 1 1 1 1 1 1 1 1)) ))

(defun run(times) (dotimes (i times) (move)  ))

;=======================================================
; MOVE
; Main part of this program.
; After detecting the wall then move.
; priority is left, frond, right, and back.
;=======================================================

(defun MOVE() (detection) (M_write) (Reposition) (goal)
  (cond ( (equal L 0) (M_left) )
        ( (equal F 0) (M_foward) )
        ( (equal R 0) (M_right) )
        ( t (M_turn)      ) )        )


;==============================
; goal
; detecting exit
;==============================
(defun goal()
  (cond ( (equal L '*) (print "this is exit") )
        ( (equal F '*) (print "this is exit")  )
        ( (equal R '*) (print "this is exit")  )
        ( (equal B '*) (print "this is exit")  ) )  )


;======================================
;detection
; Reading four dirctions at a spot of robot
(defun detection()
;========================================
  (setf N (aref MAZE (1- Y) X))
  (setf W (aref MAZE Y (1- X)))
  (setf E (aref MAZE Y (1+ X)))
  (setf S (aref MAZE (1+ Y) X))   )

;=================================================
;M_write
; recording information at robot's memory
;=================================================
(defun M_write()
  (setf (aref MEMORY (1- Y) X) N)
  (setf (aref MEMORY Y (1- X)) W)
  (setf (aref MEMORY Y (1+ X)) E)
  (setf (aref MEMORY (1+ Y) X) S)  )

;==========================================================
; turn left
; defferent movement acorrding to robot's head
;=======================================================
(defun M_left()
   (cond ( (equal HEAD 0) (setq Y (1- Y)) (setf HEAD NORTH) )
         ( (equal HEAD 3) (setq X (1- X)) (setf HEAD WEST)  )
         ( (equal HEAD 1) (setq Y (1+ Y)) (setf HEAD SOUTH) )
         ( (equal HEAD 2) (setq X (1+ X)) (setf HEAD EAST) )
         )
   (print "turn left")  )

; turn rignt
;============
(defun M_right()
  (cond ((equal HEAD 0) (setq Y (1+ Y)) (setf HEAD SOUTH) )
         ((equal HEAD 3) (setq X (1+ X)) (setf HEAD EAST) )
         ((equal HEAD 1) (setq Y (1- Y)) (setf HEAD NORTH) )
         ((equal HEAD 2) (setq X (1- X)) (setf HEAD WEST) )  )
  (print "turn right")  )

; foward
;==========
(defun M_foward()
   (cond ((equal HEAD 0) (setq X (1+ X)) )
         ((equal HEAD 3) (setq Y (1- Y)) )
         ((equal HEAD 1) (setq X (1- X)) )
         ((equal HEAD 2) (setq Y (1+ Y)) )  )
  (print "foward")  )

;==================================================================
; Reposition
; mapping between directions and sensors according to robot's head
;===================================================================
(defun Reposition()
   (cond ((equal HEAD 0) (setf L N R S F E))
         ((equal HEAD 3) (setf L W R E F N))
         ((equal HEAD 1) (setf L S R N F W))
         ((equal HEAD 2) (setf L E R W F S)) ) )


; back
;========
(defun M_turn()
   (cond ((equal HEAD 0) (setq X (1- X)) (setf HEAD WEST))
         ((equal HEAD 3) (setq Y (1+ Y)) (setf HEAD SOUTH))
         ((equal HEAD 1) (setq X (1+ X)) (setf HEAD EAST))
         ((equal HEAD 2) (setq Y (1- Y)) (setf HEAD NORTH)) )
  (print "turn back")  )