(defparameter *operators* '(+ - * /))

(defun random-elt (sequence)
  (let ((seq-length (length sequence)))
    (when (> seq-length 0)
      (elt sequence (random seq-length)))))

(defun random-form (operators)
  (append (list (random-elt operators))
          (loop repeat 2  ; arity
	     collect (let ((random-nr (random 100)))
		       (cond ((< random-nr 50) (random-form operators))
			     ((< random-nr 75) (random 10.0))
			     (t                '=input=))))))

(defun random-form (operators &optional (max-depth 4))
  (append (list (random-elt operators))
          (loop repeat 2		; arity
	     collect (let ((random-nr (random 100)))
		       (if (> max-depth 0)
			   (cond ((< random-nr 50)
				  (random-form operators (- max-depth 1)))
				 ((< random-nr 75) (random 10.0))
				 (t                '=input=))
			   (cond ((< random-nr 50) (random 10.0))
				 (t                '=input=)))))))

(defun run-form (form input)
  (let ((*error-output* (make-broadcast-stream)))
    (handler-case (funcall (eval `(lambda (=input=) ,form))
                           input)
      (error () nil))))

(defun create-initial-population (operators &optional (size 100))
  (loop repeat size
        collect (random-form operators)))

(defun fitness (form fitness-fn test-input)
  (loop for input in test-input
        for output = (run-form form input)
        for target = (funcall fitness-fn input)
        for difference = (when output (abs (- target output)))
        for fitness = (when output (/ 1.0 (+ 1 difference)))
        when (null output) do (return-from fitness nil)
        collect fitness into fitness-values
        finally (return (reduce #'* fitness-values))))

(defun traverse-nodes-example (form)
  (format t "0: ~S~%" form)
  (labels ((traverse-nodes (subform &optional (indent "  "))
             (loop for node in (cdr subform)
		do (format t "~D:~A ~S~%" (/ (length indent) 2) indent node)
		  (when (listp node)
		    (traverse-nodes node
				    (concatenate 'string indent "  "))))))
    (traverse-nodes form)))

(defun n-nodes (form)
  (let ((nodes 1))
    (labels ((traverse-nodes (subform)
               (loop for node in (cdr subform)
		  do (incf nodes)
		    (when (listp node)
		      (traverse-nodes node)))))
      (traverse-nodes form))
    nodes))

(defun random-node (form)
  (let* ((index 1)
         (nodes-1 (- (n-nodes form) 1))
         (random-node-index (+ (random nodes-1) 1)))
    (labels ((traverse-nodes (subform)
               (loop for node in (cdr subform)
		  do (when (= index random-node-index)
		       (return-from random-node
			 (list :index index :node node)))
		    (incf index)
		    (when (listp node)
		      (traverse-nodes node)))))
      (traverse-nodes form))))

(defun replace-node (form node-index new-node)
  ;; no remplaza los operadores; solo ramas enteras
  ;; node-index >= 1
  (let ((index 0))
    (labels ((traverse-nodes (subform)
	       (cons (car subform)
		     (loop for node in (cdr subform)
			do (incf index)
			when (= index node-index)
			collect new-node
			when (and (/= index node-index)
				  (not (listp node)))
			collect node
			when (and (/= index node-index)
				  (listp node))
			collect (traverse-nodes node)))))
      (traverse-nodes form))))

(defun cross-over (form1 form2 &key (debug nil))
  (let ((rnode1 (random-node form1))
        (rnode2 (random-node form2)))
    (when debug
      (format t "form1: ~S~%form2: ~S~%rnode1: ~S~%rnode2: ~S~%"
              form1 form2 rnode1 rnode2))
    (replace-node form1 (getf rnode1 :index) (getf rnode2 :node))))

(defun mutate (form operators &key (debug nil))
  (let ((rform (random-form operators))
        (rnode (random-node form)))
    (when debug
      (format t "form: ~S~%rform: ~S~%rnode: ~S~%" form rform rnode))
    (replace-node form (getf rnode :index) rform)))

(defun evaluate-population (population fitness-fn test-input)
  (loop for form in population
        for fitness = (fitness form fitness-fn test-input)
        when fitness collect (list :fitness fitness :form form) into result
        finally (return (sort result
                              (lambda (a b)
                                (> (getf a :fitness) (getf b :fitness)))))))

(defun head (sequence &optional (amount 1))
  (if (<= amount 0)
      nil
      (if (< (length sequence) amount)
          sequence
          (subseq sequence 0 amount))))

(defun advance-generation (population fitness-fn operators test-input
                           &optional (max-population 100))
  (let ((epop (evaluate-population population fitness-fn test-input)))
    (format t "Best fitness of current population: ~S~%"
            (getf (first epop) :fitness))
    (loop for plist in (head epop max-population)
       for i from 0
       for fitness = (getf plist :fitness)
       for form = (getf plist :form)
       collect form
       when (<= (random 1.0d0) fitness)
       collect (if (<= (random 100) 90)
		   (cross-over form (getf (random-elt epop) :form))
		   (mutate form operators))
       ;; Add a new random form to the population now and then.
       when (<= (random 100) 2) collect (random-form operators))))