;;;  TURTLE WORLD
;;;
;;;  Simple program to run a turtle in a 2 dimensional world while performing selected behaviors
;;;
;;;		1. go around obstacles. 
;;;		2. labyrinth (it can take a lot of time). 		 
;;;		3. follow light 
;;;		4. follow small animals (or obstacles such as balls). 
;;;		5. avoid large animals (or obstacles such as big blocks). 
;;;		6. try to be in crowd. 
;;;		7. avoid crowd. 
;;;	   	8. avoid light
;;;		9. go to goal position specified by two coordinates (exact) or one cordinate (as far as you can go to North). 
;;;		use as little energy as possible (go to any direction as far as you can from the start using your given initial total energy). 
;;;
;;;
;;;   To run this program use harlequien lispworks from http://www.xanalys.com/software_tools/products/index.html
;;;
;;;	  load and compile this file.   type (shell::run-world [option])   --- option is 1,4
;;;
;;;	  

(defpackage "SHELL" (:use "COMMON-LISP" "CAPI"))
(in-package "SHELL")


;;; --------------------
;;; constants
;;; --------------------
(setq small-size 20)
(setq large-size 50)


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Define the world objects
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(setq turtle '(
			(name .  Turtle)
			(obj-type . animal)
			(obj-shape . circle)
			(animal . turtle)
			(behavior . FindGoal )
			(movement-state . GoTowardsGoal)
		   	(speed . .25)
			(xpos . 360) 
			(ypos . 50)
			(pre-xpos . 11)
			(pre-ypos . 11)
			(orientation . 1.2 )
			(goal-xpos . 100 ) 
			(goal-ypos . 20 )
			(waypoint-xpos . 0 )
			(waypoint-ypos . 0 )
			(size . 10)
			(color . green)	
			(goal . 0)	
			(sensor-range . 100 )
			(sensor-end . 1.57)
			(sensor-start .  -1.57)
			(sensor-number . 12 )
			(sensor-resolution . 10)
			(sensor-reading . 0)
			))

(setq table '( 
			(name . Table )
			(obj-type . stationary)
			(obj-shape . rectangle)
			(xpos . 240) 
			(ypos . 100)
			(x2pos . 440)
			(y2pos . 320)
			(pre-xpos . 241)
			(pre-ypos . 200)
			(length . 20 ) 
			(width . 15)
			(size . 70)
			(color . black)
			))

(setq table2 '( 
			(name . Table )
			(obj-type . stationary)
			(obj-shape . rectangle)
			(xpos . 140) 
			(ypos . 300)
			(x2pos . 440)
			(y2pos . 320)
			(pre-xpos . 241)
			(pre-ypos . 200)
			(length . 60 ) 
			(width . 15)
			(size . 50)
			(color . black)
			))


(setq table3 '( 
			(name . Table )
			(obj-type . stationary)
			(obj-shape . rectangle)
			(xpos . 210) 
			(ypos . 390)
			(x2pos . 440)
			(y2pos . 320)
			(pre-xpos . 241)
			(pre-ypos . 200)
			(length . 20 ) 
			(width . 15)
			(size . 20)
			(color . black)
			))



(setq chair '( 
			(name . Chair )
			(obj-type . stationary)
			(obj-shape . circle)
			(xpos . 100) 
			(ypos . 100)
			(pre-xpos . 101)
			(pre-ypos . 100)
			(length . 20 ) 
			(width . 15)
			(size . 30)
			(color . black)
			))		

(setq chair2 '( 
			(name . Chair )
			(obj-type . stationary)
			(obj-shape . circle)
			(xpos . 230) 
			(ypos . 380)
			(pre-xpos . 231)
			(pre-ypos . 381)
			(length . 20 ) 
			(width . 15)
			(size . 25)
			(color . black)
			))		



(setq rabbit '( 
			(name . Rabbit )
			(obj-type . animal)
			(obj-shape . circle)
			(behavior . Random ) 
			(xpos . 200) 
			(ypos . 100)
			(pre-xpos . 201)
			(pre-ypos . 400)
			(size . 20)
			(speed . 1)
			(orientation . 1.5)
			(color . green)
			))		



(setq goal '( 
			(name . Goal )
			(obj-type . stationary)
			(obj-shape . circle)
			(xpos . 300) 
			(ypos . 400)
			(pre-xpos . 301)
			(pre-ypos . 300)
			(shape . circle ) 
			(size . 7)
			(color . red)
			))


(setq walls '(
			(name . Walls )
			(obj-type . stationary )
			(obj-shape . wall )
			(xsize . 500 )
			(ysize . 500 )
			(color . blue )))



	     

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; World definition and execution loops
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;



(defun run-world (mode)
	(cond 
		((= mode 1)			;;; avoid obstacles
			(progn 
				(setq world-objects (list turtle table chair walls goal rabbit
					 table2 chair2 (copy-alist rabbit)))

				(set-value 'goal turtle goal)
				(simulate world-objects)))
		((= mode 4)
			(progn 
				(setq world-objects (list turtle  walls goal rabbit
					 (copy-alist rabbit)
					 (copy-alist rabbit)
					 (copy-alist rabbit)
					 (copy-alist rabbit)
					 (copy-alist rabbit)
					 ))
				(set-value 'speed turtle 1)
				(set-value 'speed rabbit .85)
				(set-value 'goal turtle rabbit)
				(set-value 'size rabbit 12)
				(simulate world-objects)))

				
	))

(defun simulate (list_of_objects)
	(setq new_list_of_objects (one_step_of_world list_of_objects))
    (simulate new_list_of_objects))

(defun one_step_of_world (list_of_objects)
	(mapcar #'show-obj list_of_objects)
	(mapcar #'move-obj list_of_objects))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Collision Detection Utilities
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun detect-collision (object list_of_objects)
    (cond 
	((eq list_of_objects NIL) NIL)
	((eq (get-value 'name (car list_of_objects)) (get-value 'name object)) (detect-collision object (cdr list_of_objects)))  ;;; don't detect yourself
  	((eq (get-value 'obj-shape (car list_of_objects)) 'wall)
		(if (collided-walls? object (car list_of_objects))
			(cons (car list_of_objects) (detect-collision object (cdr list_of_objects))) 	;;; collisiion
			(detect-collision object (cdr list_of_objects))))	;;; no collision this time			
  	((and (eq (get-value 'obj-shape (car list_of_objects)) 'circle) 
		  (eq (get-value 'obj-shape object) 'circle)) 
		  (if  (collided-circles? object (car list_of_objects)) 
				(cons (car list_of_objects) (detect-collision object (cdr list_of_objects))) 	;;; collisiion
				(detect-collision object (cdr list_of_objects))))	;;; no collision this time
	((and (eq (get-value 'obj-shape (car list_of_objects)) 'rectangle) 
		  (eq (get-value 'obj-shape object) 'circle)  ) 
		  (if  (collided-rectangle? object (car list_of_objects)) 
				(cons (car list_of_objects) (detect-collision object (cdr list_of_objects))) 	;;; collisiion
				(detect-collision object (cdr list_of_objects))))	;;; no collision this time
  	(t (detect-collision object (cdr list_of_objects)))))

(defun collided-circles? (obj1 obj2)
	(< (distance-obj obj1 obj2) 
				(+ (get-value 'size obj1) (get-value 'size obj2)))) 
				
(defun collided-walls? (object walls)
	(or 
		(< (get-value 'xpos object) 0) 
		(< (get-value 'ypos object) 0)
		(> (get-value 'xpos object) (get-value 'xsize walls))
		(> (get-value 'ypos object) (get-value 'ysize walls))))
		 
(defun collided-rectangle? (object rect)
	 (and 
		(> (+ (get-value 'xpos object) (get-value 'size object)) (get-value 'xpos rect))
		(> (+ (get-value 'ypos object) (get-value 'size object)) (get-value 'ypos rect))
		(< (- (get-value 'xpos object) (get-value 'size object)) (get-value 'x2pos rect))
		(< (- (get-value 'ypos object) (get-value 'size object)) (get-value 'y2pos rect))))
	  
;;; square root of the squares
(defun distance-obj (obj1 obj2)
   (sqrt (+ 
   (*	(- (cdr (assoc 'xpos obj1)) (cdr (assoc 'xpos obj2))) 
		(- (cdr (assoc 'xpos obj1)) (cdr (assoc 'xpos obj2))))
   (*	(- (cdr (assoc 'ypos obj1)) (cdr (assoc 'ypos obj2))) 
		(- (cdr (assoc 'ypos obj1)) (cdr (assoc 'ypos obj2)))))))
	


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Movement utilities
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun next-move (object)
	(set-value 'pre-xpos object (get-value 'xpos object))
	(set-value 'pre-ypos object (get-value 'ypos object))
	(set-value 'xpos object (+ (get-value 'xpos object) (* (cos (get-value 'orientation object)) (get-value 'speed object))))
	(set-value 'ypos object (+ (get-value 'ypos object) (* (sin (get-value 'orientation object)) (get-value 'speed object))))

	(if t object object))

(defun display-object (object)
	(print (cdr (assoc 'name object)))
	(print (cdr (assoc 'xpos object))))


(defun move-obj (object)
	(if  (eq (get-value 'obj-type object) 'animal) 
		(cond 
			((eq (get-value 'behavior object) 'Random) (move-obj-random object))
			((eq (get-value 'behavior object) 'FindGoal) (move-obj-toward-goal object))
			))
		(if t object object))	 

(defun move-obj-random (object)
   	(if (detect-collision (next-move (copy-alist object)) world-objects )
		(set-random-orientation object)
		(next-move object)))

(defun move-obj-toward-goal (object)
	(if 
		(exist-path-toward-goal? object) 
			(progn
				(set-value 'movement-state object 'GoTowardsGoal)
				(set-value 'orientation object (get-orientation-towards-goal object))
				(next-move object))
		
			(progn
				(set-value 'orientation object (get-best-orientation-towards-goal object))
				(set-value 'movement-state object 'AvoidObstacle)
				(next-move object)))
	 )	

	
(defun step-obj (object count)
	(if (> count 0)
		(progn
			(next-move object)
			(show-obj object)
			(step-obj object (- count 1)))))
		
					
		
(defun get-best-orientation-towards-goal (object)
	(let
		((sweep (get-sensor-sweep object))
		(angular-step (/ (- (get-value 'sensor-end object) (get-value 'sensor-start object))
							(- (get-value 'sensor-number object) 1))) 
		(start-angle (get-value 'sensor-start object)))
		(progn
			(setq theta (- (get-orientation-towards-goal object) (+ (get-value 'orientation object) (get-value 'sensor-start object)))) 
			
		   
			;;right turn
			(setq sensor (max 0 (round (/ theta angular-step)))) 
		  ;;;	(print sensor)

			(setq right-side-of-sweep (subseq sweep sensor (- (length sweep) 1) ))
		   ;;;	(print right-side-of-sweep)
			(+ (* angular-step (+ sensor (search (list (vmax right-side-of-sweep))  right-side-of-sweep) ) )
			   (+ (get-value 'orientation object) (get-value 'sensor-start object)))		
	
		)
	)
)

(defun vmax (sweep)
	(if (eq (cdr sweep) nil) (car sweep)
		(max (car sweep) (vmax (cdr sweep)))))


(defun dialate-object (object value)
	(set-value 'size object (* value (get-value 'size object))))

(defun rotate-object (object orientation)
	(set-value 'orientation object orientation))

(defun get-orientation-towards-goal (object)
	(atan   (- (get-value 'ypos (get-value 'goal object)) (get-value 'ypos object))
			(- (get-value 'xpos (get-value 'goal object)) (get-value 'xpos object) )))

(defun set-random-orientation (object)
		(set-value 'orientation object (* 2 (* pi (/ (random 100) 100))))
		(if t object object))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Sensor utilities
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


(defun exist-path-toward-goal? (object)
	 ;;; search the sensor readings and determine if the direction is clear
	 ;;; assumes for now that the goal is always within the angular capture range
	 ;;; of the sensors.  This needs to be fixed for more complicated cases.
	 ;;;
	 ;;; FIX ME FOR A REAL ROBOT -- I just read a sensor which does not exsist
	  (progn
			(setq ping-object (copy-alist object))
			(set-value 'size ping-object  (get-value 'sensor-resolution object))
			(set-value 'speed ping-object (get-value 'sensor-resolution object))
			(set-value 'orientation ping-object (get-orientation-towards-goal object)) 
			(= (get-radial-sensor-reading object ping-object) (get-value 'sensor-range object))
			  	
	 ))			
	
	
(defun get-sensor-sweep (object)
	   (let (
	   	  (end-angle (get-value 'sensor-end object))
		  (start-angle (get-value 'sensor-start object))
		  (angular-step (/ (- (get-value 'sensor-end object) (get-value 'sensor-start object))
							(- (get-value 'sensor-number object) 1)))) 
		 (loop for angle from start-angle by angular-step to end-angle collect
		  	(progn
				(setq ping-object (copy-alist object))
				(set-value 'size ping-object  (get-value 'sensor-resolution object))
				(set-value 'speed ping-object (get-value 'sensor-resolution object))
				(set-value 'orientation ping-object 
					(+ (get-value 'orientation object) angle)) 
				(get-radial-sensor-reading object ping-object)
		  	)			
		 )))

 

(defun get-radial-sensor-reading (object ping-object)
	(cond
		;;case where we have collided with something in the world
		((detect-collision (next-move ping-object) world-objects)
		  (min (get-value 'sensor-range object) (distance-obj ping-object object)))

		;;case where we have extended beyond sensor range  -- return range
		((> (distance-obj ping-object object) (get-value 'sensor-range object ))
			(get-value 'sensor-range object))

		;;case where we continue along the radial line
		(t (get-radial-sensor-reading object (next-move ping-object)))
		
		))



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Graphic Utilities
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;



(defun show-obj (object)
	(if
   	   (eq (get-value 'obj-shape object) 'circle)
				(progn
				(gp:draw-circle output-pane 
					(get-value 'pre-xpos object)
					(get-value 'pre-ypos object)
							(get-value 'size object)
					:filled t
					:foreground :white )
				(gp:draw-circle output-pane 
					(get-value 'xpos object)
					(get-value 'ypos object)
					(get-value 'size object)
						:filled t
						:foreground :red)   )
	 (if
	 	(eq (get-value 'obj-shape object) 'rectangle)
	 		(progn
	 		(gp:draw-rectangle output-pane 
	 			(get-value 'xpos object)
	 			(get-value 'ypos object)
	 			(- (get-value 'x2pos object) (get-value 'xpos object))
	 			(- (get-value 'y2pos object) (get-value 'ypos object))
	 			:filled t
	 			:foreground :blue ) ) ) 
				
						
						  )  						
)
				   
(setq output-pane
	(contain
		(make-instance 'output-pane
				:best-width 300
				:best-height 300)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Object Manipulations Utility Functions
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun get-value (key object)
	(cdr (assoc key object)))

(defun set-value (key object value)
	(progn
		(setf (cdr (assoc key object)) value)
		(if t object)))