(progn 

(defun x-component (a)
              (CAR a))

(defun y-component (a)
              (cadr a))

(defun color-component (a)
              (cddr a))
(DEFPARAMETER *DOOR1* '(A B))
(DEFPARAMETER *DOOR2* '(A B))
(DEFPARAMETER *ROBOT1* '(A B))

(defun doors-location ()
               (setf *door1* (list  5 9))
               (setf *door2* (list  9  3)))



(defun distance (D S)
                 (setf w  (sqrt (+(* (- (x-component D ) (x-component s)) (- (x-component D ) (x-component s)))   
                            (* (- (y-component D ) (y-component s)) (- (y-component D ) (y-component s)))                  
                                ))))


(defvar *distance1* )
(defvar *distance2*)
(defvar *direction1*)
(defvar *direction2*)
(defvar *target-door*)
(defvar *y-forward*)
(defvar *x-forward*)




(setq blocks (make-hash-table :size 20))

(defun doors-location ()
               (setf *door1* (list  3  9))
               (setf *door2* (list  9  3)))

(defun blocks-initialization ()
                  (setf c 0)
                  (setf  y 3) 
                  (setf  x 0)
              
                (loop 
                    (when (= x 3) (return))
                    (setf c (incf c))
                    (setf x (incf x))
                    (setf (gethash c  blocks) (list x y)))
                      
               
               
                (loop 
                       (when (= y 6) (return))
                  (setf y (incf y))

                    (setf c (incf c))
                    (setf (gethash c  blocks) (list x y)))
                      
               
                  (loop 
                                        (when (= x 6) (return))
                    (setf c (incf c))
                    (setf x (incf x))

                    (setf (gethash c  blocks) (list x y)))
                      

               
                
                
             
               (loop 
                  
                      (when (= y 3) (return))
                    (setf c (incf c))
                 (setf y (decf y))
                    (setf (gethash c  blocks) (list x y)))
                      

               
               
                
             (loop 
                  
                      (when (= x 8) (return))
                    (setf c (incf c))
               (setf x (incf x))
                    (setf (gethash c  blocks) (list x y)))
                      

               
                       )

(defun  blocks-test ( x y c)
               (setf k 1)
               (setf w (list x y))
               (loop 
                 (setf z (gethash k blocks))
                   (when (equal w Z)  (return t))
                    (when (= k c)    (return nil))
                     (setf k (incf k))
                 ;;(print (x-component z))  (print (y-component z))
                 ))




(doors-location)
(blocks-initialization)






(defun mazeplane ()
               (setf a (x-component *robot1*))
               (setf b (y-component *robot1*))
              (setq maze (make-hash-table :size 150))
               
                 (setf x 1)
                 
                 (setf j 0) 
                      (DOTIMES
                            (x 10)
                       (setf y 1)

                          (format t "       ~&          "  )
                          (terpri)

                         (Dotimes ( y 10) 
                           
                           (setf j (incf j))
                           
                           
                        ;;(setf z (gethash j maze))
                           (cond ((AND (= x  (x-component *door1*)) (= y  (y-component *door1* ))) 
                                                  (progn (format t "  ~A    " "D") (setf (gethash j maze) (list x y 'door))))
                                  ((AND (= x  (x-component *door2*)) (= y  (y-component *door2* ))) 
                                                  (progn (format t "  ~A    " "D")  ))
                                 ((AND (= A  x) (= B  y))  (progn (format t "  ~A    " "R") (setf (gethash j maze) (list x  y 'robot) )))
                                 ((blocks-test x y 13)    (progn (format t "  ~A    " "B") (setf (gethash j maze) (list x  y 'Block))))
                                  
                                  (t (cond  ((OR (= y 0 ) (= y 9) (= x 0) (= x 9 ))
                                               (progn (format t "  ~A    " "w")  (setf (gethash j maze) (list x  y 'wall) ) ))
                                            (t (progn  (format t "  ~A    " "-")  (setf (gethash j maze) (list x  y 'pas) ))))))
                           )))


(defun  robot-moves ()
            
(search-closest-door)
  
              (LOOP 
                    
                (WHEN (= (Y-COMPONENT *TARGET-DOOR*) (Y-COMPONENT *ROBOT1*)) (RETURN))
                   (robotupdating-y-direction)
                                          
                     (progn    (print *robot1*)
                        (MAZEPLANE)
                       )
                     
                        )
              
              (LOOP 
                     (WHEN (= (X-COMPONENT *TARGET-DOOR*) (X-COMPONENT *ROBOT1*)) (RETURN))
                     (robotupdating-x-direction)
                     (progn
                        
                        (print *robot1*)
                       (MAZEPLANE)
                       )
                                        )
  )
              






(defun robotupdating-y-direction ()
               (SETF M (X-COMPONENT *ROBOT1*))
              (SETF N (Y-COMPONENT *ROBOT1*))
                 (COND ((AND (> (Y-COMPONENT *TARGET-DOOR*) (Y-COMPONENT *ROBOT1*) ) (not(blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13))) 
                              (cond (( = *y-forward* 1)  (progn (SETF N (INCF N)) (setf *x-forward* 1))) 
                                    (t (setf N (DECF N)))))
                     ((AND (> (Y-COMPONENT *TARGET-DOOR*) (Y-COMPONENT *ROBOT1*) ) (blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13))
                             (progn (setf n (decf n))  (SETF *ROBOT1* (LIST M N)) (setf *y-forward* 0) (robotupdating-x-direction)))   
                     (T(cond ((AND (< (Y-COMPONENT *TARGET-DOOR*) (Y-COMPONENT *ROBOT1*) ) (not(blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13)))
                                  (SETF N (DECF N)))
                             ((AND (< (Y-COMPONENT *TARGET-DOOR*) (Y-COMPONENT *ROBOT1*) ) (blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13))
                               (progn (setf n (incf n)) (SETF *ROBOT1* (LIST M N)) (robotupdating-x-direction)))))
                        )
                     
                     
               
               (SETF *ROBOT1* (LIST M N))

               )


(defun robotupdating-x-direction ()
               (SETF M (X-COMPONENT *ROBOT1*))
               (SETF N (Y-COMPONENT *ROBOT1*))


               (COND ((AND (> (x-COMPONENT *TARGET-DOOR*) (x-COMPONENT *ROBOT1*) ) (not(blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13)))
                              (cond ((= *x-forward* 1) (progn (SETF M (INCF M)) (setf *y-forward* 1)))
                                    (t (setf M (decf M)))))
                     ((AND (> (X-COMPONENT *TARGET-DOOR*) (X-COMPONENT *ROBOT1*) ) (blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13))
                             (progn (setf M (decf M)) (SETF *ROBOT1* (LIST M N)) (setf *x-forward* 0) (robotupdating-y-direction)))   
                     (T(cond ((AND (< (X-COMPONENT *TARGET-DOOR*) (X-COMPONENT *ROBOT1*) ) (not(blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13)))
                                  (SETF M (DECF M)))
                             ((AND (< (X-COMPONENT *TARGET-DOOR*) (X-COMPONENT *ROBOT1*) ) (blocks-test (x-COMPONENT *ROBOT1*) (Y-COMPONENT *ROBOT1*) 13))
                                  (progn (setf M (decf M)) (SETF *ROBOT1* (LIST M N)) (robotupdating-y-direction)))))
                        )
              
               (SETF *ROBOT1* (LIST M N))


               )


(defun search-closest-door ()
               
               (setf *distance1* (distance *door1* *robot1*))
                (setf *distance2* (distance *door2* *robot1*))
                (cond ((< *distance1* *distance2*) 
                         (setf *target-door* *door1*) )
                      ((> *distance1* *distance2*) 
                            (setf *target-door* *door2*))
                      
                       (t (setf *target-door* *door1*)))
                 (print  *target-door*)
                  (print   *robot1*)
               (print    *distance1*)
               (print    *distance2*)
               (print    *door1*)
                 
               )




(defun robot-navigator ()
           
              (loop 
               (search-closest-door)
               (robot-moves)
               (mazeplane)
                (when (equal  *target-door* *robot1*) (return)))
             )
              )