;; ==================================================================
;; Evil global variables
;; ==================================================================

(defvar *WIDTH* 5)
(defvar *HEIGHT* 5)


;; ==================================================================
;; node 
;; ------------------------------------------------------------------
;; DESCRIPTION
;; ------------------------------------------------------------------
;; PARAMETERS
;;  w :
;; ==================================================================
(defstruct node
  (coord '(-1 -1))
  (parent '(-1 -1))
  (successors '()) 
  ;; (successors (make-array '(8) :initial-element '(-1 -1)))
  ;; (num-successors 0)
  (been-seen '*)
  (f 0)
  (g 0)
  (h 0)
)

;; ==================================================================
;; init-array (w)
;; ------------------------------------------------------------------
;; DESCRIPTION
;; ------------------------------------------------------------------
;; PARAMETERS
;;  w :
;; ==================================================================
;; need to initialize each element in the array, otherwise
;; all of the elements will just point to the same location.
;; Original code looked like this: 
;;  (w (make-array '(5 5) :initial-element (make-node)) ;; world
;; The problem with that is it points to only one created instance
;; of 'node'.  This is a similar problem to Java when a new instance
;; hasn't been declared within an array.
(defun init-array(w)
  (dotimes (i (array-dimension w 0))
    (dotimes (j (array-dimension w 1))
      (setf (aref w i j) (make-node :coord (LIST i j)))
    )
  )
)


;; ==================================================================
;; a-star ()
;; ------------------------------------------------------------------
;; DESCRIPTION
;; ==================================================================
(defun a-star ()
  (let
    (
      (s '(0 0)) ;; start
      (g '(2 4)) ;; goal
      (start (make-node :been-seen 's))
      (w (make-array '(5 5))) ;; world  -- Step 1
      (OPEN '())
      (CLOSED '()) ;; Step 2
      
    )

    (init-array w) ;; initalize the world with nodes

    (setf (node-been-seen (aref w 0 0)) 's)
    (setf (node-parent (aref w 0 0)) '(0 0))
    (setf (node-been-seen (aref w 2 4)) 'g)

    (SETF OPEN (CONS (aref w 0 0) OPEN)) ;; also part of Step 1

    (print-world w)

    ;; need to start looping here
    (do
      ;; local variables to this DO loop
      (
        (ret_val 0)
        (n '()) ;; temporary node
        (i -1) ;; current row of n
        (j -1) ;; current column of n
      )
      ( (/= ret_val 0) );; loop until the return value is equal to 0
                        ;; ret_val = 1 : success!
                        ;; ret_val = -1 : error, no path found
      
        ;; Step 3
        (COND
          ( (ENDP OPEN) 
            (FORMAT T "Error: OPEN is empty.~%")
            (setf ret_val -1)
          )  
        )

        ;; Also watch if when n is being updated if the node in the world w
        ;; is also being updated, or if it needs to be updated after each loop.
        
        ;; Step 4
        (SETF n (FIRST OPEN))
        (SETF OPEN (REST OPEN))
        (SETF CLOSED (CONS n CLOSED))
        (SETF i (FIRST (node-coord n)))
        (SETF j (SECOND (node-coord n)))

        (FORMAT T "n: ~a  ~%" n)

        ;; check to see if the goal was found - Step 5
        (COND
          ( (EQUAL n NIL) (FORMAT T "N is NIL~%") )
          ( (EQUAL (node-coord n) g) 
            (FORMAT T "Found the goal.~%")
            (SETF ret_val 1)
          ) ;; if so, then trace back to mark the path
          (T
            ;; Step 6 - Generate successors
            (COND 
              ((is-successor w (- i 1) (- j 1)) 
                (add-successor n (- i 1) (- j 1)) ;; create an add-successor function
                (set-node n w (- i 1) (- j 1) g)
                (SETF OPEN (CONS (AREF w (- i 1) (- j 1)) OPEN))
                
                ;; (SETF (node-parent (AREF w (- i 1) (- j 1))) (LIST i j))
                ;; (SETF (node-been-seen (AREF w (-i 1) (- j 1))) '?)
              )
            )
            
            (COND 
              ((is-successor w (- i 1) j) 
                (add-successor n (- i 1) j) ;; create an add-successor function
                (set-node n w (- i 1) j g)
                (SETF OPEN (CONS (AREF w (- i 1) j) OPEN))
              )
            )
            
            (COND 
              ((is-successor w (- i 1) (+ j 1) )
                (add-successor n (- i 1) (+ j 1)) ;; create an add-successor function
                (set-node n w (- i 1) (+ j 1) g)
                (SETF OPEN (CONS (AREF w (- i 1) (+ j 1)) OPEN))
              )
            )
            
            (COND 
              ((is-successor w i (- j 1)) 
                (add-successor n i (- j 1)) ;; create an add-successor function
                (set-node n w i (- j 1) g)
                (SETF OPEN (CONS (AREF w i (- j 1)) OPEN))
              )
            )
            
            (COND 
              ((is-successor w i (+ j 1)) 
                (add-successor n i (+ j 1)) ;; create an add-successor function
                (set-node n w i (+ j 1) g)
                (SETF OPEN (CONS (AREF w i (+ j 1)) OPEN))
              )
            )
            
            (COND 
              ((is-successor w (+ i 1) (- j 1)) 
                (add-successor n (+ i 1) (- j 1)) ;; create an add-successor function
                (set-node n w (+ i 1) (- j 1) g)
                (SETF OPEN (CONS (AREF w (+ i 1) (- j 1)) OPEN))
              )
            )
            
            (COND 
              ((is-successor w (+ i 1) j) 
                (add-successor n (+ i 1) j) ;; create an add-successor function
                (set-node n w (+ i 1) j g)
                (SETF OPEN (CONS (AREF w (+ i 1) j) OPEN))
              )
            )
            
            (COND 
              ((is-successor w (+ i 1) (+ j 1)) 
                (add-successor n (+ i 1) (+ j 1)) ;; create an add-successor function
                (set-node n w (+ i 1) (+ j 1) g)
                (SETF OPEN (CONS (AREF w (+ i 1) (+ j 1)) OPEN))
              )
            )
            
            ;; (FORMAT T "Open-list: ~a ~%" OPEN)
            
            ;; Step 7
            ;; Step 8 - Reorder OPEN list
            (SORT OPEN #'f-compare)
            
          )
        )
        
        ;; might need to copy 'n' back into world w
        
        (print-world w)
        
        
      ) ;; end of DO loop - Step 9
      
      ;; now, trace back...
      
      ;; set the goal as G
      (SETF (node-been-seen (AREF w (FIRST g) (SECOND g))) 'g)
      ;; loop until get back to the start goal
      ;; set each new item as a T
      (DO
        ( (trail-coord g) )
        ( (EQUAL trail-coord s) )
        (SETF (node-been-seen (AREF w (FIRST trail-coord) (SECOND trail-coord))) 't)
        (SETF trail-coord (node-parent (AREF w (FIRST trail-coord) (SECOND trail-coord))))
      )
      
      (print-world w)
  )
)

;; ==================================================================
;; is-successor (w i j)
;; ------------------------------------------------------------------
;; DESCRIPTION
;; ------------------------------------------------------------------
;; PARAMETERS

;; ==================================================================
(defun is-successor(w i j)
  (COND
    ( (OR (< i 0) (>= i *WIDTH*)) NIL )
    ( (OR (< j 0) (>= j *HEIGHT*)) NIL )
    ( (EQUAL (node-been-seen (aref w i j)) 'g) T)
    ( (NOT (EQUAL (node-been-seen (aref w i j)) '* )) NIL)
    (T T)
  )
)

;; ==================================================================
;; is-successor2 (n i j)
;; ------------------------------------------------------------------
;; this is just a test case to make sure that it works
(defun is-successor2(n i j)
  (COND
    ( (OR (< i 0) (>= i *WIDTH*)) NIL )
    ( (OR (< j 0) (>= j *HEIGHT*)) NIL )
    ( (NOT (EQUAL (node-been-seen n) '* )) NIL)
    (T T)
  )
)

;; ==================================================================
;; add-successor (n i j)
;; ------------------------------------------------------------------
;; DESCRIPTION
;; ------------------------------------------------------------------
;; PARAMETERS
;;  i : successor's row coordinate
;;  j : successor's column coordinate
;; ==================================================================
(defun add-successor(n i j)
  (SETF (node-successors n) (CONS (LIST i j) (node-successors n)))
)

;; ==================================================================
;; set-node (n w i j goal)
;; ------------------------------------------------------------------
;; DESCRIPTION
;;  Set new values in the successor node, such as f, g, h values
;; ------------------------------------------------------------------
;; PARAMETERS
;;  n : current parent node
;;  w : 2-d array representing the grid space world
;;  i : successor's row coordinate
;;  j : successor's column coordinate
;;  goal : coordinates of the goal state
;; ==================================================================
(defun set-node (n w i j goal)
  (SETF (node-parent (aref w i j)) (node-coord n)) ;; set the parent
  (SETF (node-g (aref w i j)) ( + (node-g n) 1)) ;; get the real distance to node
  (SETF (node-h (aref w i j)) (+ (ABS (- i (FIRST goal)) ) (ABS (- j (SECOND goal)) ) ) )
  ;; (format t "g: ~a h: ~a ~%" (node-g (aref w i j)) (node-h (aref w i j)) )
  (SETF (node-f (aref w i j)) (+ (node-g (aref w i j)) (node-h (aref w i j)) ) )
  (SETF (node-been-seen (aref w i j)) 'x)
  ;; (SETF OPEN (CONS (aref w i j) OPEN)) ;; add node to the OPEN list
  ;; (format t " Open in set-node: ~a ~%" OPEN)
)

;; ==================================================================
;; print-world (w)
;; ------------------------------------------------------------------
;; DESCRIPTION
;; ------------------------------------------------------------------
;; PARAMETERS
;;  w : 2-d array representing the grid space world
;; ==================================================================
(defun print-world(w)

  (dotimes (i (array-dimension w 0))
    (dotimes (j (array-dimension w 1))
      (format t "~a " (node-been-seen (aref w i j)))
    )
    (format t "~%")
  )
  
)

;; ==================================================================
;; f-compare (item1 item2)
;; ------------------------------------------------------------------
;; DESCRIPTION
;; Compare the f value to see which is higher and return
;; either true or false.  This comparison is used for the built in
;; SORT function in LISP.  Want to sort the list from smallest to
;; largest.
;; ------------------------------------------------------------------
;; PARAMETERS
;;  item1 : the first item to compare
;;  item2 : the second item to compare
;; ==================================================================
(defun f-compare (item1 item2)
    (< (node-f item1) (node-f item2) )
)