;; Ant colony travelling salesman problem solver
;; /Mic, 2003
;; http://nangijala.no-ip.org | stabmaster_@hotmail.com
;;
;;
;; Run as a fasl if possible:
;;    (compile-file "ants.cl")
;;    (load "ants.fasl")



;; Really simple problem, consisting of 16 cities ordered in the shape of a square.
;; The number of ants used will not make any difference on this problem since it's trivial - the best possible solution is always found.
(setq 4-by-4-problem
  '(
    co-ordinates
    ((0 0)(10 0)(20 0)(30 0)
     (0 10)(10 10)(20 10)(30 10)
     (0 20)(10 20)(20 20)(30 20)
     (0 30)(10 30)(20 30)(30 30))
    ))


;; Define weights used in decision-making
(setq pheromone-factor 1.0)
(setq distance-factor 5.0)


;; Calculate the Euclidean distance between two points in the plane
;;
(defun euclidean-distance (x1 y1 x2 y2)
  (let ((dx (- x2 x1))
        (dy (- y2 y1)))
    (sqrt (+ (* dx dx) (* dy dy)))))


(defun calc-distances (coords)
  (let ((m 0) (n 0))
    (loop
      (if (= m (1- num-cities)) (return nil))
      (setf n (1+ m))
      (loop
	(if (= n num-cities) (return nil))
	(setf (aref distance m n) (euclidean-distance (car (nth m coords))
						      (cadr (nth m coords))
						      (car (nth n coords))
						      (cadr (nth n coords))))
	(setf (aref distance n m) (aref distance m n))
	(setf (aref visibility m n) (expt (/ 1 (aref distance m n)) distance-factor))
	(setf (aref visibility n m) (aref visibility m n))
	(setf n (1+ n)))
      (setf m (1+ m)))))
	 
  


;; Has ant k already visited city i on its tour ?
;;
(defun is-tabu? (k i)
  (member i (aref tour k)))


(defun prob-factor (i j)
  (* (expt (aref pheromone i j) pheromone-factor) (aref visibility i j)))


;; Calculate the probability of an ant k going to city j
;;
(defun hop-probability (k j)
  (let ((cur-city (aref ants k))
        (sum-of-factors nil))
     
    (if (is-tabu? k j) 0)
    
    (setf sum-of-factors 0)
    (dolist (tmp (aref to-visit k) nil)
      (if (not (is-tabu? k tmp))
          (setf sum-of-factors (+ sum-of-factors (prob-factor cur-city tmp)))))
    (if (= sum-of-factors 0)
        (setf sum-of-factors 1))
    
    (/ (prob-factor cur-city j) sum-of-factors)))



;; Construct tours for all ants
;;
(defun construct-tours ()
  ;; Set the tour of each ant to the empty list
  (dotimes (k num-ants nil)
    (setf (aref ants k) (random num-cities))
    (setf (aref tour k) (list (aref ants k)))
    (setf (aref tour-length k) 0)
    (dotimes (cnt num-cities nil)
      (setf (aref to-visit k) (append (aref to-visit k) (list cnt)))))
  
  ;; Repeat for all cities that remain to be visited 
  (dotimes (n (- num-cities 1) nil)
    ;; Repeat for all ants
    (dotimes (k num-ants nil)
      (setf (aref probability k) 0)
      ;; Find the best city to visit for ant k
      (dolist (tmp (aref to-visit k) nil)
	(if (not (is-tabu? k tmp))
	    (let ((p (hop-probability k tmp)))
	      (if (> p (aref probability k))
		  (progn
		    (setf (aref probability k) p)
		    (setf (aref next-city k) tmp))))))
      ;; Add the city to the tour (=tabu)
      ;(format t "Ant ~a chose to go to city ~a~%" k (aref next-city k))
      (setf (aref tour k) (append (aref tour k) (list (aref next-city k))))
      (setf (aref tour-length k) (+ (aref tour-length k) (aref distance (aref ants k) (aref next-city k))))
      (setf (aref ants k) (aref next-city k))
      ))
  
  ;; Move all ants back to their starting position
  (dotimes (k num-ants nil)
    (setf (aref tour k) (append (aref tour k) (list (car (aref tour k)))))
    (setf (aref tour-length k) (+ (aref tour-length k) (aref distance (aref ants k) (car (aref tour k)))))
    (setf (aref ants k) (car (aref tour k))))
  
  )



(defun uses-edge? (k i j)
  (let ((m 0) (citym nil) (cityn nil) (res nil))
    (loop
      (if (= m (1- num-cities)) (return nil))
      (setf citym (nth m (aref tour k)))
      (setf cityn (nth (1+ m) (aref tour k)))
      (if (or (and (= i citym) (= j cityn))
	      (and (= j citym) (= i cityn)))
	  (progn
	    (setf res t)
	    (return nil)))
      (setf m (1+ m)))
    res))

(defun pheromone-delta (i j)
  (let ((delta 0))
    (dotimes (k num-ants nil)
      (if (uses-edge? k i j)
	  (setf delta (+ delta (/ 100 (aref tour-length k))))))
    delta)
  )


(defun update-trails ()
  (let ((j 0))
    (dotimes (i (1- num-cities) nil)
      (setf j (1+ i))
      (loop
	(if (= j num-cities) (return nil))
	(setf (aref pheromone i j) (+ (* (aref pheromone i j) evaporation-rate) (pheromone-delta i j)))
	(setf (aref pheromone j i) (aref pheromone i j))
	(setf j (1+ j))))))
  

(defun load-problem (problem)
  (cond
   ((eq (car problem) 'co-ordinates)
    (setq num-cities (length (cadr problem)))
    (setq num-ants num-cities)
    (setq ants (make-array num-ants))
    (setq tour (make-array num-ants))
    (setq tour-length (make-array num-ants))
    (setq to-visit (make-array num-ants))
    (setq probability (make-array num-ants))
    (setq next-city (make-array num-ants))
    (setq pheromone (make-array (list num-cities num-cities) :initial-element 0.00001))
    (setf evaporation-rate 0.5)
    (setq best-tour nil)
    (setq best-tour-length 100000000000000)
    (setq distance (make-array (list (length (cadr problem)) (length (cadr problem)))))
    (setq visibility (make-array (list (length (cadr problem)) (length (cadr problem)))))
    (calc-distances (cadr problem)))
   (t
    (format t "Unrecognized format.~%"))))


(defun as-tsp ()
  (tagbody get-num-ants
    (format t "~%How many ants should be used (1-100)? ")
    (setf num-ants (read))
      (if (not (integerp num-ants))
	  (progn
	    (format t "That's not an integer. Try again.~%")   
	    (go get-num-ants))
	(if (or (< num-ants 1) (> num-ants 100))
	    (progn
	      (format t "Value out of range: ~D. Try again.~%" num-ants)
	      (go get-num-ants)))))
  (format t "Loading problem..")
  (load-problem 4-by-4-problem)
  (format t "~%Working..")
  (dotimes (foo 3 nil)
    (construct-tours)
    (select-best-tour)
    (update-trails))
  (format t "~%Shortest tour: ~D~%" best-tour-length))



(defun select-best-tour ()
  (let ((best (my-min tour-length)))
    (dotimes (k num-ants nil)
      (if (< (car best) best-tour-length)
	  (progn
	    (setf best-tour-length (car best))
	    (setf best-tour (aref tour (cadr best))))))))

	    
(defun my-min (array)
  (let ((min-value (aref array 0))
	(pos 0))
    (dotimes (k (1- (array-total-size array)) nil)
      (if (< (aref array (1+ k)) min-value)
	  (progn
	    (setf min-value (aref array (1+ k)))
	    (setf pos (1+ k)))))
    (list min-value pos)))


(format t "------------------------------------~%Ant colony TSP solver~%/Mic, 2003~%~%Run (as-tsp) to start the simulation~%------------------------------------~%")