;;; These are the functions implementing the Manhattan distance
;;; heuristic. Incorporate them in the 5-puzzle program
;;; appropriately.

(defun heuristic (s1 s2)
   (total-distance (form-table s1) (form-table s2)))

(defun form-table (state)
   (list (list (first state) (second state) (third state))
     (list (fourth state) (fifth state) (sixth state))))

(defun total-distance (t1 t2)   
   (setf distance 0)
   (setf adjustment 0)
   (setf distance (+ distance (compute-distance 
           (first (first t1)) 1 (get-row-1 (first (first t1)) t2))))
   (setf distance (+ distance (compute-distance 
           (second (first t1)) 2 (get-row-1 (second (first t1)) t2))))
   (setf distance (+ distance (compute-distance
           (third (first t1)) 3 (get-row-1 (third (first t1)) t2))))
   (setf distance (+ distance (compute-distance 
           (first (second t1)) 1 (get-row-2 (first (second t1)) t2))))
   (setf distance (+ distance (compute-distance
           (second (second t1)) 2 (get-row-2 (second (second t1)) t2))))
   (setf distance (+ distance (compute-distance
           (third (second t1)) 3 (get-row-2 (third (second t1)) t2))))
   (setf distance (+ distance adjustment)))

(defun get-row-1 (el t2)
   (if (member el (first t2)) 
       (first t2) 
       (progn (setf adjustment (+ 1 adjustment))
              (second t2))))

(defun get-row-2 (el t2)
   (if (member el (second t2)) 
       (second t2) 
       (progn (setf adjustment (+ 1 adjustment))
              (first t2))))

(defun compute-distance (el pos1 row &optional (pos2 1))
   (cond ((endp row) nil) 
         ((= el (first row)) (abs (- pos1 pos2)))
         (t (compute-distance el pos1 (rest row) (+ 1 pos2)))))