;; A genetic algorithm engine.
;; Copyright 2006 Patrick May (pjm@spe.com)

(in-package :common-lisp-user)

(defpackage :com.spe.ga
  (:use :common-lisp
        :sb-thread
        :sb-unix
        :sb-ext))

(in-package :com.spe.ga)

;; Utility functions.
(defmacro while (test &body body)
  "A little syntactic sugar around DO."
  `(do () ((not ,test)) ,@body))

(defun bit-vector->integer (bits)
  "Create a positive integer from a bit vector."
  (reduce (lambda (first-bit second-bit) (+ (* first-bit 2) second-bit)) bits))

(defun integer->bit-vector (integer)
  "Create a bit-vector from a positive integer."
  (labels ((integer->bit-list (integer &optional (accumulator nil))
             (cond ((> integer 0) (integer->bit-list (truncate integer 2)
                                                     (push (mod integer 2)
                                                           accumulator)))
                   ((null accumulator) (push 0 accumulator))
                   (t accumulator))))
    (coerce (integer->bit-list integer) 'bit-vector)))

(defun bit-vector->gray-code (bits)
  "Convert from standard binary to binary reflected Gray code."
  (let ((gray-bits (make-array (length bits)
                               :element-type 'bit
                               :initial-element 0)))
    (dotimes (i (length bits) gray-bits)
      (if (zerop i)
          (setf (bit gray-bits i) (bit bits i)) ; big-endian
          (setf (bit gray-bits i) (logxor (bit bits (1- i)) (bit bits i)))))))

(defun gray-code->bit-vector (gray-bits)
  "Convert from binary reflected Gray code to standard binary."
  (let ((bits (make-array (length gray-bits)
                          :element-type 'bit
                          :initial-element 0)))
    (dotimes (i (length gray-bits) bits)
      (if (zerop i)
          (setf (bit bits i) (bit gray-bits i)) ; big-endian
          (setf (bit bits i) (logxor (bit bits (1- i)) (bit gray-bits i)))))))

;; The genetic algorithm functions.

(defun make-genome (length &optional (distribution 0.5))
  "Create a randomized bit array of the specified LENGTH.  A value
  between 0 and 1 can be specified for DISTRIBUTION to modify the mean
  relative number of 1 bits."
  (let ((genome (make-array length :element-type 'bit :initial-element 0)))
    (map-into genome (lambda () (if (> distribution (random 1.0)) 1 0)))))

(defun mutate-genome (genome rate)
  "Flip bits in the GENOME bit-vector with a percentage chance equal to
  the specified RATE (ranging from 0 to 1)."
  (bit-xor genome (make-genome (length genome) rate)))

(defun single-crossover (parent-one parent-two)
  "Create two new genomes by crossing over PARENT-ONE and PARENT-TWO at
  a single, randomly selected point."
  (let ((crossover-point (random (length parent-one))))
    (list (concatenate 'bit-vector
                       (subseq parent-one 0 crossover-point)
                       (subseq parent-two crossover-point))
          (concatenate 'bit-vector
                       (subseq parent-two 0 crossover-point)
                       (subseq parent-one crossover-point)))))

(defun segment-crossover (parent-one parent-two &optional (segments 1))
  "Create two new genomes by crossing over SEGMENTS segments of
  PARENT-ONE and PARENT-TWO."
  (let ((child-one (copy-seq parent-one))
        (child-two (copy-seq parent-two)))
    (dotimes (i segments (list child-one child-two))
      (let* ((start (random (length child-one)))
             (end (+ start (random (- (length child-one) start))))
             (segment-one (copy-seq (subseq child-one start end)))
             (segment-two (copy-seq (subseq child-two start end))))
        (setf (subseq child-one start end) segment-two)
        (setf (subseq child-two start end) segment-one)))))

(defun make-gene-pool (size genome-length)
  "Create a list of SIZE genomes, each of length GENOME-LENGTH."
  (let ((pool nil))
    (dotimes (count size pool)
      (push (make-genome genome-length) pool))))

(defun most-fit-genome (gene-pool fitness-comparator)
  "Return the most fit genome in GENE-POOL based on FITNESS-COMPARATOR.
  FITNESS-COMPARATOR must take two genomes as arguments and return T if
  the first is the the most fit of the two."
  (let ((most-fit nil))
    (dotimes (i (length gene-pool) most-fit)
      (let ((genome (elt gene-pool i)))
        (when (or (null most-fit) (funcall fitness-comparator genome most-fit))
          (setf most-fit genome))))))

(defun tournament-select (gene-pool fitness-comparator
                          &optional (tournament-size
                                     (max 2 (ceiling (/ (length gene-pool)
                                                        200)))))
  "Randomly select TOURNAMENT-SIZE genomes from the GENE-POOL and apply
  FITNESS-COMPARATOR to return the best one.  FITNESS-COMPARATOR must
  take two genomes as arguments and return T if the first is the the
  most fit of the two."
  (let* ((pool-size (length gene-pool))
         (tournament nil))
    (while (< (length tournament) tournament-size)
      (pushnew (elt gene-pool (random pool-size)) tournament))
    (most-fit-genome tournament fitness-comparator)))

(defun evolve-gene-pool (gene-pool fitness-comparator mutation-rate)
  "Create a new gene pool of the same size as GENE-POOL by replacing
  half the population with mutated offspring of tournament selection
  winners selected by FITNESS-COMPARATOR.  The other half of the
  population consists of the parent genomes.  MUTATION-RATE must be
  between 0 and 1."
  (let ((size (length gene-pool))
        (new-pool nil))
    (dotimes (i (/ size 4) new-pool)
      (let* ((parent-one (tournament-select gene-pool fitness-comparator))
             (parent-two (tournament-select gene-pool fitness-comparator))
             (children (mapcar (lambda (genome)
                                 (mutate-genome genome mutation-rate))
                               (single-crossover parent-one parent-two))))
        (push (copy-seq parent-one) new-pool)
        (push (copy-seq parent-two) new-pool)
        (push (car children) new-pool)
        (push (cadr children) new-pool)))))


;; Steiner problem genome:
;; bits                             | description
;; 0-xbits                          | x coordinate of variable node 0
;; xbits-ybits                      | y coordinate of variable node 0
;; . . .
;; (* (+ xbits ybits) n)            | start of x coordinate of variable node n
;; (+ (* (+ xbits ybits) n) xbits)  | start of y coordinate of variable node n
;; . . .
;; remainder                        | connection mask

(defclass steiner-problem ()
  ((fixed-nodes :initarg :fixed-nodes
                :reader fixed-nodes
                :initform (make-array 0 :element-type 'list :fill-pointer t))
   (variable-node-count :initarg :variable-node-count
                        :accessor variable-node-count
                        :initform 0)
   (x-dimension :initarg :x-dimension :reader x-dimension :initform 1024)
   (y-dimension :initarg :y-dimension :reader y-dimension :initform 1024)
   (x-coord-bits :accessor x-coord-bits :initform 10)
   (y-coord-bits :accessor y-coord-bits :initform 10))
  (:documentation "The configuration for a Steiner network problem."))

(defmethod initialize-instance :after ((problem steiner-problem) &rest rest)
  "Compute all derived values."
  (declare (ignore rest))
  (setf (x-coord-bits problem) (ceiling (log (x-dimension problem) 2)))
  (setf (y-coord-bits problem) (ceiling (log (y-dimension problem) 2))))

;; todo: eliminate need for x/y-coords to be 2^n - 1.
;;       consider implementing a constructor
;;       compute x-dimension and y-dimension from fixed nodes

(defmethod total-node-count ((problem steiner-problem))
  "Return the maximum number of nodes possible in the specified PROBLEM."
  (+ (length (fixed-nodes problem)) (variable-node-count problem)))

(defmethod genome-length ((problem steiner-problem))
  "Determine the number of bits required for a genome to solve the
  specified Steiner network."
  (let ((total-nodes (total-node-count problem)))
    (+ (* (variable-node-count problem)
          (+ (x-coord-bits problem) (y-coord-bits problem)))
       (* total-nodes (/ (1- total-nodes) 2)))))

(defmethod variable-node-coords ((problem steiner-problem) genome index)
  "Return a list containing the x and y coordinates contained in the
  GENOME of the variable node identified by INDEX."
  (let* ((x-bits (x-coord-bits problem))
         (y-bits (y-coord-bits problem))
         (x-start (* index (+ x-bits y-bits)))
         (y-start (+ x-start x-bits)))
    (list (bit-vector->integer
           (gray-code->bit-vector (subseq genome x-start y-start)))
          (bit-vector->integer
           (gray-code->bit-vector
            (subseq genome y-start (+ y-start y-bits)))))))
;;     (list (bit-vector->integer (subseq genome x-start y-start))
;;           (bit-vector->integer (subseq genome y-start (+ y-start y-bits))))))

(defmethod variable-nodes ((problem steiner-problem) genome)
  "Return a vector of the locations of the variable nodes in the
  specified GENOME based on the characteristics of the particular
  PROBLEM."
  (let ((nodes (make-array 0 :element-type 'list :fill-pointer t)))
    (dotimes (i (variable-node-count problem) nodes)
      (vector-push-extend (variable-node-coords problem genome i) nodes))))

(defmethod nodes ((problem steiner-problem) genome)
  "Return a vector of the locations of the fixed and variable nodes in
  the specified GENOME based on the characteristics of the particular
  PROBLEM."
  (concatenate 'vector (fixed-nodes problem) (variable-nodes problem genome)))

(defmethod connection-mask ((problem steiner-problem) genome index)
  "Return the bit-vector containing the connections associated with the
  node identified by INDEX in the specified GENOME."
  (labels ((summation (limit)
             (* (/ limit 2) (1+ limit))))
    (let ((start (+ (* (variable-node-count problem)
                       (+ (x-coord-bits problem) (y-coord-bits problem)))
                    (- (* index (total-node-count problem))
                       (summation index))))
          (mask-length (1- (- (total-node-count problem) index))))
      (subseq genome start (+ start mask-length)))))
                              
(defmethod node-connections ((problem steiner-problem) genome index)
  "Return a list containing the other nodes to which the node identified
  by INDEX is connected in the specified GENOME based on the
  characteristics of the particular PROBLEM."
  (let ((connection-mask (connection-mask problem genome index))
        (connections nil))
    (dotimes (i (length connection-mask) connections)
      (unless (zerop (bit connection-mask i))
        (push (+ index i 1) connections)))))

(defmethod connections ((problem steiner-problem) genome)
  "Return a list of all unique pairs of connected nodes in the specified
  GENOME."
  (let ((connections nil))
    (dotimes (i (total-node-count problem) connections)
      (dolist (node (node-connections problem genome i))
        (push (list i node) connections)))))

(defmethod distance ((problem steiner-problem) genome node-one node-two)
  "Compute the distance between NODE-ONE and NODE-TWO within the
  specified GENOME constrained by the PROBLEM."
  (let* ((nodes (nodes problem genome))
         (coord-one (aref nodes node-one))
         (coord-two (aref nodes node-two))
         (x-diff (abs (- (car coord-one) (car coord-two))))
         (y-diff (abs (- (cadr coord-one) (cadr coord-two)))))
    (sqrt (+ (* x-diff x-diff) (* y-diff y-diff)))))

(defun connected-nodes (node-list connections)
  "Identify all nodes reachable from the nodes in NODE-LIST based on the
  specified CONNECTIONS."
  (let ((associated-connections
         (remove-if (lambda (connection)
                      (null (intersection connection node-list)))
                    connections)))
    (if (null associated-connections)
        (values node-list connections)
        (connected-nodes (remove-duplicates
                          (union node-list
                                 (reduce #'append associated-connections)))
                         (set-difference connections
                                         associated-connections
                                         :test 'equal)))))

;; todo: consider applying a penalty based on number of disconnected subgraphs

(defmethod connected ((problem steiner-problem) genome)
  "Determine whether or not the GENOME specifies a fully connected graph
  given the constraints of the PROBLEM."
  (labels ((connected-nodes (node-list connections)
             (let ((associated-connections
                    (remove-if (lambda (connection)
                                 (null (intersection connection node-list)))
                               connections)))
               (if (null associated-connections)
                   (values node-list connections)
                   (connected-nodes (remove-duplicates
                                     (union node-list
                                            (reduce #'append
                                                    associated-connections)))
                                    (set-difference connections
                                                    associated-connections
                                                    :test 'equal)))))
           (fixed-node-ids (problem)
             (let ((ids nil))
               (dotimes (i (length (fixed-nodes problem)) ids)
                 (push i ids)))))
    (let ((connections (connections problem genome)))
      (multiple-value-bind (connected-nodes other-connections)
          (connected-nodes (car connections) (cdr connections))
        (declare (ignore other-connections))
        (null (set-difference (fixed-node-ids problem) connected-nodes))))))

(defconstant +unconnected-penalty+ 2000)

(defmethod fitness ((problem steiner-problem) genome)
  "The fitness function for a Steiner problem is the total length of all
  the paths between connected points, with a large penalty for solutions
  that do not connect all the nodes."
  (let ((total-length
         (reduce #'+ (mapcar (lambda (connection)
                               (distance problem genome
                                         (car connection) (cadr connection)))
                             (connections problem genome)))))
    (if (connected problem genome)
        total-length
        (+ total-length +unconnected-penalty+))))

(defmethod fitness-comparator ((problem steiner-problem))
  "Return a fitness comparator function that takes two genomes and
  returns T if the first is more fit according to the characteristics of
  the PROBLEM."
(lambda (genome-one genome-two)
    (< (fitness problem genome-one) (fitness problem genome-two))))

(defmethod best-fitness ((problem steiner-problem) gene-pool)
  "Return the fitness value of the most fit genome in the GENE-POOL."
  (car (sort (mapcar (lambda (genome)
                       (fitness problem genome))
                     gene-pool)
             #'<)))

(defmethod average-fitness ((problem steiner-problem) gene-pool)
  "Return the average fitness of the GENE-POOL."
  (/ (reduce #'+ (mapcar (lambda (genome) (fitness problem genome)) gene-pool))
     (length gene-pool)))

(defmethod most-fit ((problem steiner-problem) gene-pool)
  "Return the most fit genome in the GENE-POOL."
  (car (sort (copy-list gene-pool) (fitness-comparator problem))))

(defmethod solve ((problem steiner-problem)
                  pool-size
                  generations
                  mutation-rate)
  "Evolve a solution to PROBLEM using a gene pool of POOL-SIZE over
  GENERATIONS generations."
  (let ((gene-pool (make-gene-pool pool-size (genome-length problem)))
        (comparator (fitness-comparator problem)))
    (dotimes (i generations)
      (setf gene-pool (evolve-gene-pool gene-pool comparator mutation-rate))
      (format t "~&Generation:  ~D, best fitness = ~F~%"
              i (best-fitness problem gene-pool)))
    (let ((best-genome (most-fit problem gene-pool)))
      (format t "~%Best = ~F~%Average = ~F~%Nodes = ~S~%Connections = ~S~%"
              (fitness problem best-genome)
              (average-fitness problem gene-pool)
              (nodes problem best-genome)
              (connections problem best-genome)))))

;; Simple problem from Dave Thomas at The Panda's Thumb:
;; Four fixed points at (0 0), (400 0), (0 300), and (400 300).
;; Steiner solution is a "bow tie."
(defparameter *bow-tie-problem*
  (make-instance 'steiner-problem
                 :fixed-nodes (make-array 4
                                          :element-type 'list
                                          :fill-pointer t
                                          :initial-contents '((0 0)
                                                              (400 0)
                                                              (0 300)
                                                              (400 300)))
                 :variable-node-count 5
                 :x-dimension 400
                 :y-dimension 300))

(solve *bow-tie-problem* 1000 250 0.02)
; Best = 919.6167
; Average = 1129.6854
; Nodes = #((0 0) (400 0) (0 300) (400 300) (501 387) (313 150) (87 150)
;           (363 211) (155 11))
; Connections = ((5 6) (3 5) (2 6) (1 5) (0 6))

;; Example problem from Dave Thomas at The Panda's Thumb:
;; Five fixed points at (150 0), (450 0), (0 260), (600 260), and
;; (300 433).  The Steiner solution has a total segment length of 1212
;; with three variable nodes.

(defparameter *five-point-problem*
  (make-instance 'steiner-problem
                 :fixed-nodes (make-array 5
                                          :element-type 'list
                                          :fill-pointer t
                                          :initial-contents '((150 0)
                                                              (450 0)
                                                              (0 260)
                                                              (600 260)
                                                              (300 433)))
                 :variable-node-count 7
                 :x-dimension 600
                 :y-dimension 433))

(solve *five-point-problem* 1000 250 0.02)
; Best = 1246.6409
; Average = 1754.9652
; Nodes = #((150 0) (450 0) (0 260) (600 260) (300 433) (441 236) (335 84)
;           (140 62) (463 339) (421 464) (594 137) (268 499))
; Connections = ((4 8) (3 8) (1 3) (0 1) (0 2))

;; Challenge problem from Dave Thomas at The Panda's Thumb:
;; Six fixed points at (0,0), (400,0), (800,0), (0,300), (400,300), and
;; (800,300).  The Steiner solution has a total segment length of
;; 1586.53 with four variable nodes.
(defparameter *panda-problem*
  (make-instance 'steiner-problem
                 :fixed-nodes (make-array 6
                                          :element-type 'list
                                          :fill-pointer t
                                          :initial-contents '((0 0)
                                                              (400 0)
                                                              (800 0)
                                                              (0 300)
                                                              (400 300)
                                                              (800 300)))
                 :variable-node-count 9
                 :x-dimension 800
                 :y-dimension 300))

(solve *panda-problem* 1000 250 0.02)
; Best = 1678.3081
; Average = 2238.4536
; Nodes = #((0 0) (400 0) (800 0) (0 300) (400 300) (800 300) (33 385)
;           (457 222) (397 295) (863 497) (458 221) (409 29) (752 360)
;           (419 267) (585 226))
; Connections = ((10 11) (8 13) (7 10) (7 13) (5 10) (4 8) (3 8) (2 5)
;                (1 11) (0 3))