;;;-*- Mode: Lisp; Package: TSP -*-
(defpackage TSP)
(in-package TSP)

#|
After you load this file, in order
To use the code in this package, you must first be in
the tsp package, to do this, type:

(in-package :tsp)

Then call the tsp function below with *distances* as an argument.
|#

(defun tsp (distances)
  (setf *distance-array* (make-array (list (length distances) (length (car distances)))
                                     :initial-contents distances))
  (cl-user::graphsearch (make-state :city 0 :visited '(0) :cost 0)
                        #'expand #'goalp #'lessp #'result))

(defvar *distances*
  '((0 15 20 25 30)
    (20 0 20 10 5)
    (20 14 0 13 12)
    (30 23 11 0 10)
    (30 10 10 10 0)))

(defvar *distance-array*)

(defvar *cities* '(0 1 2 3 4))

(defstruct (state) parent city visited cost)

(defun expand (state lessp-fn)
  (format t "~a: ~{~a ~}~%" (state-cost state) (state-visited state))
  (sort (loop with visited = (state-visited state)
              for city in (set-difference *cities* visited)
              collect (make-state :parent state 
                                  :city city 
                                  :visited (cons city visited)
                                  :cost (+ (aref *distance-array* (state-city state) city)
                                           (state-cost state))))
        lessp-fn))
  
(defun goalp (state)
  (= (length (state-visited state)) (length *cities*)))

(defun lessp (state1 state2)
  (< (state-cost state1) (state-cost state2)))

(defun result (state &optional cities dis)
  (cond ((state-parent state)
         (result (state-parent state) (cons (state-city state) cities) (or dis (state-cost state))))
        ((format t "~%~%Shortest Path:~{~a ~}~%Distance: ~a~%" (cons (state-city state) cities) dis))))