;;; FILE: ga-robot.ss
;;;
;;; kyle wagner
;;; 4-16-97
;;;
;;;
;;; Implements problem-specific routines to allow the GA in ga.ss to 
;;; solve a maze problem for a simple robot.
;;;
;;; the routines are:
;;;
;;;    robot:genome->phenome
;;;    robot:eval-pop-fitness
;;;
;;;
;;; ROBOT
;;;
;;; The genome encodes for a series of moves.  The robot has no sensory
;;;  input and does not make its moves conditionally.  If a move
;;;  would bump it into a wall, then that move is ignored and the next
;;;  one is used.  
;;;
;;; Once the robot gets to the exit square of the maze, it stops, even
;;;  if its phenome had specified more moves.  It gets the highest fitness
;;;  for such an achievement.
;;;
;;; The genome is divided up into pairs of numbers, which are called codons.
;;;  Codons map to one of the four cardinal directions (N, S, E, W).
;;;  The phenome is simply a list of directions the robot is to move in.
;;;  Each direction moves it one square in the maze, unless the direction would
;;;  cause a collision with a wall.
;;;
;;;
;;; MAZE
;;;
;;; The maze has one entrance and one exit.
;;; The robot's task is to get as close to the exit as it can.  It's score
;;;  is based on how close it actually got to the maze's exit.
;;;
;;; A maze might look like this:
;;;
;;;  ######
;;;  #   x#
;;;  #  ###
;;;  #e   #
;;;  ######
;;;
;;;  where e is the entrance (where the robot starts) and x is the exit
;;;  (the goal location of the robot).
;;;
;;; Mazes are represented as matrices.  *'s represent walls, the entrance,
;;; exit and dimensions are kept as "coord" data structures.
;;;
;;; make-maze creates a maze.  init-maze makes that maze the one used by the 
;;;  robot population.
;;; 
;;; Each non-wall in the maze has a value in the matrix.  This number is
;;;  the score of the robot if it makes it here.  Thus, fitness is 
;;;  determined by the location in the maze.  These scores are determined
;;;  ahead of time.
;;;  
;;;
;;; SETTING UP A MAZE
;;;
;;; Use make-maze to set up a new maze for the robot to traverse.
;;;
;;; Use (init-maze maze2) to set *maze* to be maze2.  This can be changed to 
;;;  alter the maze used for each run of the ga.
;;;
;;; FITNESS
;;;
;;; The goodness of the robot's solution is inversely proportional to
;;;  how many moves it took to get to the solution and also is based
;;;  on the location in the maze (how close to the exit did it get?)
;;;  If the robot uses all of its moves to get to a non-exit, then 
;;;  it just gets the value of being at that location.  If it exits,
;;;  it gets the exit value *and* a point for each move remaining in its
;;;  phenome that was unused.
;;;
;;;
;;; ROUTINES FOR THE GA
;;;
;;;   robot:genome->phenome
;;;     takes the robot's genome and makes a phenome (list of directions)
;;;
;;;   robot:eval-pop-fitness
;;;     takes each robot through the maze, giving each a score which
;;;     becomes its fitness => returns the new population with fitness scores.
;;;
;;;


;;;
;;; NOTES
;;;
;;;  uses matrix.ss to do maze creation and lookup.
;;;

(load "matrix.ss")



;;;
;;; GLOBAL VARS
;;;
(define *maze* 'junk)
(define *verbose* #f)  ;; If this is #t, robot:eval-pop-fitness will call
                       ;;  maze-print a lot to show every move of every robot




;;;
;;; COORDS
;;;
;;;   (row . col)
;;;
(define make-coord
  (lambda (r c)
    (cons r c)))
(define coord->row car)
(define coord->col cdr)



;;;
;;; genome->codon
;;;
;;; INPUT
;;;   genome : a list of bits
;;; OUTPUT
;;;   a "codon", which is just a fixed number of bits (a list of the bits)
;;; OPERATION
;;;   take the first 2 bits => that's the codon
;;;
(define genome->codon
  (lambda (genome)
    (list (car genome) (cadr genome))))



;;;
;;; remove-codon
;;;
;;; INPUT
;;;   genome
;;; OUTPUT
;;;   what's left of genome after 1st codon has been removed
;;;
(define remove-codon cddr)




;;;
;;; codon->dir
;;;
;;; INPUT
;;;   codon : list of 2 bits
;;; OUTPUT
;;;   a symbol: a cardinal direction (NSE or W)
;;;
(define codon->dir
  (lambda (codon)
    (cond
     [(equal? codon '(0 0)) 'n]
     [(equal? codon '(0 1)) 'e]
     [(equal? codon '(1 0)) 's]
     [(equal? codon '(1 1)) 'w]
     [else (error 'codon->dir "bad codon")])))



;;;
;;; make-maze
;;;
;;; INPUT
;;;   num-rows, num-cols : rows and cols in maze
;;;   enter-coords, exit-coords : coords of entrance and exit
;;;   maze : a list (row-major format) of items in maze
;;; OUTPUT
;;;   a maze, which is a list containing (dimensions maze-matrix)
;;;
(define make-maze
  (lambda (num-rows
	   num-cols
	   enter-coords
	   exit-coords
	   maze)
    (let* ([maze-matrix (make-matrix num-rows num-cols 'x)]
	   [maze-dims (make-coord num-rows num-cols)])

      ;; Fill up maze-matrix with values
      (let loop ([row 0] [col 0] [maze-left maze])
	(cond

	 ;; Finished with maze-matrix
	 [(>= row num-rows) 'done]

	 ;; Next row - finished with current one
	 [(>= col num-cols) (loop (add1 row) 0 maze-left)]
	 
	 ;; Take item from maze-left and put into maze-matrix
	 [else
	  (begin
	    (matrix-set! maze-matrix row col (car maze-left))
	    (loop row (add1 col) (cdr maze-left)))]))

      ;; Create a new maze with maze-matrix and other info
      (list maze-dims enter-coords exit-coords maze-matrix))))




;;;
;;; access routines for mazes:
;;;
;;;   maze->num-rows, maze->num-cols, maze->matrix
;;;
(define maze->num-rows 
  (lambda (maze)
    (coord->row (car maze))))
(define maze->num-cols
  (lambda (maze)
    (coord->col (car maze))))
(define maze->enter-coord cadr)
(define maze->exit-coord caddr)
(define maze->matrix cadddr)



;;;
;;; maze-find
;;;
;;; INPUT 
;;;   maze : a maze structure
;;;   coord : a coordinate structure
;;; OUTPUT
;;;   the item at coord in maze
;;;
(define maze-find
  (lambda (maze coord)
    (matrix-ref (maze->matrix maze) (coord->row coord) (coord->col coord))))



;;;
;;; wall?
;;;
(define wall?
  (lambda (maze-item)
    (eq? maze-item '*)))


;;;
;;; exit?
;;;
;;; #t if coord is the exit coord, #f otherwise
;;;
(define exit?
  (lambda (maze coord)
    (let ([exit (maze->exit-coord maze)])
      (equal? exit coord))))



;;;
;;; maze-print
;;;
;;; INPUT
;;;   maze
;;;   robot-coord : loc of robot
;;; OPERATION
;;;   show the maze in nice fashion.  Show entrance(e), exit(x), robot(R).
;;;
(define maze-print
  (lambda (maze robot-coord)
    (let* ([enter-coord (maze->enter-coord maze)]
	   [exit-coord (maze->exit-coord maze)]
	   [num-rows (maze->num-rows maze)]
	   [num-cols (maze->num-cols maze)]
	   
	   ;; Print the upper and lower boundaries of maze
	   [print-wall-row
	    (lambda ()
	      (let loop ([col 0])
		(if (<= col num-cols)
		    (begin
		      (printf "#")
		      (loop (add1 col)))
		    (printf "#~%"))))])

      ;; Upper maze boundary
      (print-wall-row)

      (let loop ([row 0]
		 [col 0])
	;;
	;; Print vertical boundaries of maze
	;;
	(if (and (zero? col) (< row num-rows))
	    (printf "#"))
	 
	;;
	;; Print contents of maze
	;;
	(cond
	 
	 ;; New line: print RHS vertical boundary
	 [(>= col num-cols)
	  (begin
	    (printf "#~%")
	    (loop (add1 row) 0))]

	 ;; Done
	 [(>= row num-rows) (print-wall-row) (newline)]

	 ;; Get maze item.  If wall, print it, if not, print space.
	 ;; If either robot, exit or enter coords, print symbol for that.
	 [else
	  (let ([cur-coord (make-coord row col)])
	    ;; Print the symbol
	    (cond
	     [(equal? cur-coord robot-coord)
	      (printf "R")]
	     [(equal? cur-coord enter-coord)
	      (printf "e")]
	     [(equal? cur-coord exit-coord)
	      (printf "x")]
	     [(wall? (maze-find maze cur-coord))
	      (printf "#")]
	     [else
	      (printf " ")])
	    
	    ;; Next column
	    (loop row (add1 col)))])))))
	    
	  




;;;
;;; new-coord
;;;
;;; INPUT
;;;   coord : current loc
;;;   dir : a direction
;;; OUTPUT
;;;   new coord - new location
;;;
(define new-coord
  (lambda (coord dir)
    (let ([row (coord->row coord)]
	  [col (coord->col coord)])
      (case dir
	[n (make-coord (sub1 row) col)]
	[s (make-coord (add1 row) col)]
	[e (make-coord row (add1 col))]
	[w (make-coord row (sub1 col))]))))




;;;
;;; legal-move?
;;;
;;; INPUT
;;;    maze : a maze structure
;;;    coord : coord structure
;;; OUTPUT
;;;    #t if coord is a legal spot in maze (neiter out of bounds, nor a wall)
;;;    #f otherwise
;;;
(define legal-move?
  (lambda (maze coord)
    (let ([row (coord->row coord)]
	  [col (coord->col coord)]
	  [num-rows (maze->num-rows maze)]
	  [num-cols (maze->num-cols maze)])
      (and

       ;; Check to see if the coord is in bounds
       (>= row 0)
       (>= col 0)
       (< row num-rows)
       (< col num-cols)

       ;; Can't be a wall
       (not (wall? (maze-find maze coord)))))))



;;;
;;; new-legal-coord
;;;
;;; INPUT
;;;   maze : maze structure
;;;   coord : current loc
;;;   dir : direction to move
;;; OUTPUT
;;;   new coord, but it's guaranteed legal
;;; OPERATION
;;;   use new-coord to get new coordinates, then legal-move? to check
;;;   if it's an ok move.  If so, return new coord, otherwise return
;;;   the old coord.
;;;
(define new-legal-coord
  (lambda (maze coord dir)
    (let ([new-coord (new-coord coord dir)])
      (if (legal-move? maze new-coord)
	  new-coord
	  coord))))




;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; The problem-specific GA routines
;;;



;;;
;;; init-maze
;;;
(define init-maze
  (lambda (maze)
    (set! *maze* maze)))




;;;
;;;   robot:genome->phenome
;;;
;;; INPUT
;;;   genome : a robot's genome
;;; OUTPUT
;;;   the robot's phenome
;;; OPERATION
;;;   Extract each codon from the genome, one at a time.  Decode the codons
;;;   with codon->dir.  These are placed in a growing list of the phenome.
;;;
(define robot:genome->phenome
  (lambda (genome)
    (cond

     ;; Done
     [(null? genome) '()]

     ;; Remove a codon, decode it, cons it onto the recursive call to
     ;; convert the rest of the genome
     [else
      (let* ([codon (genome->codon genome)]
	     [dir (codon->dir codon)]
	     [genome-left (remove-codon genome)])
	(cons dir (robot:genome->phenome genome-left)))])))
	



;;;
;;;   robot:eval-pop-fitness
;;;
;;; INPUT
;;;   pop : a population of robot indivs
;;; OUTPUT
;;;   pop with fitness scores added to each indiv
;;; OPERATION
;;;   Evaluates each robot with robot:eval-indiv-fitness.  Adds this fitness
;;;    to indiv's score and remakes that indiv with fitness inside.  Does this
;;;    for all robots in the pop.
;;;
;;;   Accesses a global variable, *maze*, which is the maze to be used for
;;;    all members of this population.
;;;
(define robot:eval-pop-fitness
  (lambda (pop)
    (let ([new-robot (lambda (robot)
		       (let* ([genome (indiv->genome robot)]
			      [phenome (indiv->phenome robot)]
			      [fitness (robot:eval-indiv-fitness *maze* 
								 phenome)])
			 (make-indiv fitness genome phenome)))])
      (map new-robot pop))))
    


;;;
;;; robot:eval-indiv-fitness
;;;
;;; INPUT
;;;   maze : the maze for this robot
;;;   robot : a robot phenome (just a list of cardinal dir's: moves)
;;; OUTPUT
;;;   value of final loc in maze -> the robot's fitness
;;;
(define robot:eval-indiv-fitness
  (lambda (maze robot)
    (let* ([start-coord (maze->enter-coord maze)]
	   [exit-coord (maze->exit-coord maze)]
	   [exit? (lambda (coord) (equal? coord exit-coord))])

      (if *verbose*
	  (printf (format "~%Robot: ~S~%" robot)))

      (letrec ([robot-mover
		(lambda (robot-moves-left robot-coord)
		  
		  ;; Show current maze and robot's loc
		  (if *verbose*
		      (maze-print maze robot-coord))

		  (cond
		   
		   ;; Done if either no moves left or robot is at exit
		   [(or (exit? robot-coord)
			(null? robot-moves-left))

		    ;; Score is loc on board + number of moves leftover
		    ;;  (this is a simple measure of efficiency - 
		    ;;   the fewer moves to get to exit, the better
		    ;;   the score, and the more #moves left).
		    (let* ([num-moves-left (length robot-moves-left)])

		      (if *verbose*
			  (begin
			    (printf (format "Done.  Score=~S~%" 
					    (maze-find maze robot-coord)))
			    (maze-print maze robot-coord)))

		      (+ num-moves-left
			 (maze-find maze robot-coord)))]
		   
		   ;; Get new robot move
		   [else
		    (let* ([robot-dir (car robot-moves-left)]
			   [new-coord (new-legal-coord maze
						       robot-coord
						       robot-dir)])
		      ;; Show robot's movement direction
		      (if *verbose*
			  (printf (format "   Now Moving:   ~S~%" robot-dir)))


		      (robot-mover (cdr robot-moves-left) new-coord))]))])
	(robot-mover robot start-coord)))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; TESTING
;;;


;;;
;;; maze1 A simple maze
;;;
(define maze1-contents
  '(1 1 * 6
    1 1 4 5
    * * 3 *
    0 1 2 1))
(define rows1 4)
(define cols1 4)
(define enter1 '(3 . 0))
(define exit1 '(0 . 3))

(define maze1 (make-maze rows1 cols1 enter1 exit1 maze1-contents))


;;;
;;; maze2:  A larger maze
;;;
(define maze2-contents
  '(3 4 5 * 9
    2 * 6 7 8
    1 * 4 * *
    0 1 3 * 0
    * * 0 * 0
    0 0 0 0 0))
(define rows2 6)
(define cols2 5)
(define enter2 '(3 . 0))
(define exit2 '(0 . 4))

(define maze2 (make-maze rows2 cols2 enter2 exit2 maze2-contents))


;;;
;;; Automatically start with maze1
;;;
(init-maze maze1)



;;
;; Lazy stuff
;;
(define r:g->p robot:genome->phenome)
(define r:e-indiv robot:eval-indiv-fitness)
(define r:e-pop robot:eval-pop-fitness)

;testing only
;(define *pop* (make-pop-phenomes r:g->p (random-pop 4 8)))


;; From ga.ss - necessary for one routine
;;;
;;; INDIVIDUALS
;;;
(define make-indiv 
  (lambda (fitness genome phenome)
    (list fitness genome phenome)))
(define indiv->fitness car)
(define indiv->genome cadr)
(define indiv->phenome caddr)