(defvar V)
(defvar VV)
(defvar rows)
(defvar columns)
(defvar states)
(defvar AA)
(defvar BB)
(defvar AAprime)
(defvar BBprime)
(defvar gamma 0.9)


(defun setup ()
  (setq rows 5)
  (setq columns 5)
  (setq states 25)
  (setq AA (state-from-xy 1 0))
  (setq BB (state-from-xy 3 0))
  (setq AAprime (state-from-xy 1 4))
  (setq BBprime (state-from-xy 3 2))
  (setq V (make-array states :initial-element 0.0))
  (setq VV (make-array (list rows columns)))
)

(defun compute-V ()
  (loop for delta = (loop for x below states
                          for old-V = (aref V x)
                          do (setf (aref V x)
                                   (mean (loop for a below 4 collect
                                               (full-backup x a))))
                          sum (abs (- old-V (aref V x))))
        until (< delta 0.000001))
  (loop for state below states do 
        (multiple-value-bind (x y) (xy-from-state state)
          (setf (aref VV y x) (aref V state))))
  (sfa VV))

(defun compute-V* ()
  (loop for delta = (loop for x below states
                          for old-V = (aref V x)
                          do (setf (aref V x)
                                   (loop for a below 4 maximize
                                               (full-backup x a)))
                          sum (abs (- old-V (aref V x))))
        until (< delta 0.000001))
  (loop for state below states do 
        (multiple-value-bind (x y) (xy-from-state state)
          (setf (aref VV y x) (aref V state))))
  (sfa VV))

(defun sfa (array)
  "Show Floating-Point Array"
  (cond ((= 1 (array-rank array))
         (loop for e across array do (format t "~5,1F" e)))
        (t (loop for i below (array-dimension array 0) do
                 (format t "~%")
                 (loop for j below (array-dimension array 1) do
                       (format t "~5,1F" (aref array i j)))))))

(defun full-backup (x a)
  (let (r y)
    (cond ((= x AA)
           (setq r +10)
           (setq y AAprime))
          ((= x BB)
           (setq r +5)
           (setq y BBprime))
          ((off-grid x a)
           (setq r -1)
           (setq y x))
          (t
           (setq r 0)
           (setq y (next-state x a))))
    (+ r (* gamma (aref V y)))))

(defun off-grid (state a)
  (multiple-value-bind (x y) (xy-from-state state)
    (case a
      (0 (incf y) (>= y rows))
      (1 (incf x) (>= x columns))
      (2 (decf y) (< y 0))
      (3 (decf x) (< x 0)))))
      
(defun next-state (state a)
  (multiple-value-bind (x y) (xy-from-state state)
    (case a
      (0 (incf y))
      (1 (incf x))
      (2 (decf y))
      (3 (decf x)))
    (state-from-xy x y)))

(defun state-from-xy (x y)
      (+ y (* x columns)))

(defun xy-from-state (state)
  (truncate state columns))