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


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Define the world objects
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(setq node-proto '(
			(name .  node)
			(obj-shape . circle)
			(size . 5)
			(xpos . 0) 
			(ypos . 0)
			(color . :red)
			(lineage . nil)
			
			))

(setq square-proto '( 
			(name . obstacle )
			(obj-type . stationary)
			(obj-shape . rectangle)
			(xpos . 310) 
			(ypos . 350)
			(x2pos . 410)
			(y2pos . 410)
			(length . 20 ) 
			(width . 15)
			(size . 20)
			(color . :black)
			))



(setq goal '( 
			(name . goal )
			(obj-type . stationary)
			(obj-shape . circle)
			(xpos . 350) 
			(ypos . 350)
		   	(length . 20 ) 
			(width . 15)
			(size . 10)
			(color . :green)
			))

(setq starting-location '( 
			(name . start )
			(obj-type . stationary)
			(obj-shape . circle)
			(xpos . 10) 
			(ypos . 10)
			(size . 10)
			(lineage . nil)
		   	(color . :blue)
			))



(setq ping-proto '(
			(name .  ping-object)
			(obj-shape . circle)
		    (speed . 1)
			(xpos . 10) 
			(ypos . 10)
			(orientation . 1.2 )
		    (size . 2)
			))


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



(setq best-trip nil)
(setq shortest-distance 10000)



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



(defun run ()
  	(progn 
				
				;Initialize the world to contain the walls and the start				
			   	(setq world-objects (list walls starting-location))


				;Initialize the list of nodes to contain the start
				(setq node-list (list starting-location))

				;kick off the graphics portion of the program
				(init-graphics-pane)

				;;Wait right here until the user releases the pause button from the GUI
				(loop while (simulation-stopped) do (+ 2 2))

				;;Display all of the nodes and obstacles
				(mapcar #'show-obj world-objects)

				;;Expand the nodes using a breadth first search for the goal
				;;(setf node-list (list starting-location))
				;;(expand-node node-list)


))


(defun expand-node (nlist)
	(if nlist
		(let ((node (car nlist))
			  (children-node (get-visible-nodes (car nlist) nlist world-objects)))
			(cond
				((if (line-of-sight? node goal)
					  (found-goal  
					  	(add-node-lineage goal node))))
				((if children-node 
					  (expand-node (append children-node nlist)))))))) 


(defun get-trip-distance (list-of-nodes)
	(if (and (car list-of-nodes) (cadr list-of-nodes))
		(+ (distance-obj (car list-of-nodes) (cadr list-of-nodes))
			(get-trip-distance (car list-of-nodes)))
		0))


(defun get-visible-nodes (parent-node list-of-nodes list-of-objects)
;;node is the vertex that we want to expand
;;list-of-nodes is a list of already traversed nodes
;;list-of-objects is a list of world objects -- the recursion
;; will stop when there are no more objects in the list-of-objects
	(if list-of-objects
		(if (eq 'node (get-value 'name (car list-of-objects)))
			(let ((child-node (car list-of-objects)))
		    (if	(and (line-of-sight? child-node parent-node)
				 	 (not (duplicated? child-node list-of-nodes)))						
			   			(cons (add-node-lineage child-node parent-node) 
							(get-visible-nodes parent-node list-of-nodes 
								(cdr list-of-objects)))
						(get-visible-nodes parent-node list-of-nodes
							(cdr list-of-objects)))))))

(defun found-goal (node)
	(let 
	((distance (get-trip-distance (get-value 'lineage node))))
	(if (< distance shortest-distance)
		(progn
			(setq shortest-distance distance)
			(describe distance)
			(setq best-trip (get-value 'lineage node))))))


(defun duplicated? (node list-of-nodes)
	(if list-of-nodes 
		(if (and 
				(= (get-value 'xpos node) (get-value 'xpos (car list-of-nodes)))
				(= (get-value 'ypos node) (get-value 'ypos (car list-of-nodes))))
			t (duplicated? node (car list-of-nodes))) 
		nil  ))



(defun add-node-lineage (child-node parent-node)
	(progn
		(gp:draw-rectangle display-pane
			(get-value 'xpos child-node)
			(get-value 'ypos child-node)
			(get-value 'xpos parent-node)
			(get-value 'ypos parent-node))
	        (set-value 
	   			'lineage 
	   			child-node 
	    		(append parent-node (get-value 'lineage parent-node)))
			
			))


(defun gxv (object)
	(get-value 'xpos object))
(defun gyv (object)
	(get-value 'ypos object))
(defun sxv (object value)
	(set-value 'xpos object value))
(defun syv (object value)
	(set-value 'xpos object value))





;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Collision Detection Utilities
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun detect-collision (object list_of_objects)
    (cond 
	((eq list_of_objects NIL) NIL)
    ((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 (= (get-value 'xpos object) (get-value 'xpos (car list_of_objects)))
		  (= (get-value 'ypos object) (get-value 'ypos (car list_of_objects))))
		  (detect-collision object (cdr list_of_objects))) ;;; don't detect yourself

  	((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) 0) (get-value 'xpos rect))
		(> (+ (get-value 'ypos object) 0) (get-value 'ypos rect))
		(< (- (get-value 'xpos object) 0) (get-value 'x2pos rect))
		(< (- (get-value 'ypos object) 0) (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)))))))
	




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


(defun line-of-sight? (objectA objectZ)
	  ;;Determine if the two objects can see each other
	  ;;if a ping-object moves from objectA to objectZ without
	  ;;hitting anything else then the two objects can see each other
	  (progn
			(setq ping-object (copy-alist ping-proto))
			(set-value 'xpos ping-object (get-value 'xpos objectA))
			(set-value 'ypos ping-object (get-value 'ypos objectA))
			(set-value 'orientation ping-object 
				(get-orientation-towards-object objectA objectZ)) 

			;;This "if" statement is a very big kludge.  The get-first-object-in-direction
			;;function will not detect objects which are on the same x or y
			;;values.  There is a hard check placed here to capture these
			(if 
				(or (= (get-value 'xpos objectZ) (get-value 'xpos objectA))
					(= (get-value 'ypos objectZ) (get-value 'ypos objectA)))
				(if t t)

			(let ((collided-object (get-first-object-in-direction ping-object)))
				(progn
					(describe collided-object)
				    (if (and 
						(= (get-value 'xpos collided-object) 
							(get-value 'xpos objectZ))
						(= (get-value 'ypos collided-object) 
							(get-value 'ypos objectZ))) t Nil))))))
	 

(defun get-first-object-in-direction (ping-object)
	(if	(detect-collision (next-move ping-object) world-objects)
		(car (detect-collision (next-move ping-object) world-objects))
	  	(progn 
			(show-obj ping-object)
	  		(get-first-object-in-direction (next-move ping-object)))))
	  

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

	

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Movement utilities
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun next-move (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))




;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; 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)))

(defun add-object-to-world (name x y x2 y2)
	(cond ((eq name 'obstacle)
		(progn
			(setq temp-square (copy-alist square-proto))
			(set-value  'xpos 	temp-square	  (min x x2))
			(set-value  'ypos 	temp-square	  (min y y2))
			(set-value  'x2pos	temp-square	  (max x x2))
			(set-value  'y2pos	temp-square	  (max y y2))
			(set-value 	'name   temp-square	  name)
			(setq world-objects (append world-objects (list temp-square))) 

			;Add the nodes
			(setq temp-node (copy-alist node-proto))
			(set-value 'xpos temp-node x)
			(set-value 'ypos temp-node y)
			(set-value 'name temp-square 'test-node)
			(setq world-objects (append world-objects (list temp-node))) 

			(setq temp-node (copy-alist node-proto))
			(set-value 'xpos temp-node x2)
			(set-value 'ypos temp-node y)
			(set-value 'name temp-square 'test-node)
			(setq world-objects (append world-objects (list temp-node))) 

			(setq temp-node (copy-alist node-proto))
			(set-value 'xpos temp-node x)
			(set-value 'ypos temp-node y2)
			(set-value 'name temp-square 'test-node)
			(setq world-objects (append world-objects (list temp-node))) 

			(setq temp-node (copy-alist node-proto))
			(set-value 'xpos temp-node x2)
			(set-value 'ypos temp-node y2)
			(set-value 'name temp-square 'test-node)
			(setq world-objects (append world-objects (list temp-node)))))
		((eq name 'goal)
			(set-value  'xpos 	goal  (/ (+ x x2) 2))
			(set-value  'ypos 	goal  (/ (+ y y2) 2))
			(set-value 	'name   goal  name)
			(setq world-objects (append world-objects (list goal))))
		((eq name 'start)
			(set-value  'xpos 	starting-location  (/ (+ x x2) 2))
			(set-value  'ypos 	starting-location  (/ (+ y y2) 2))
			(set-value 	'name   starting-location  name)
			(setq world-objects (append world-objects (list starting-location)))))) 

			 
  

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


(defun show-obj (object)
  	(cond 
  	((eq (get-value 'obj-shape object) 'rectangle)
		(progn
		(gp:draw-rectangle display-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 (get-value 'color object))))
	((eq (get-value 'obj-shape object) 'circle)
		(progn
			(gp:draw-circle display-pane 
			(get-value 'xpos object)
			(get-value 'ypos object)
			(get-value 'size object)
				:filled t
				:foreground (get-value 'color object))))))




;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; UI Functions
;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun simulation-not-stopped ()
	(or 
		(eq 'go (pinboard-test-mode global-pinboard-test))
		(eq 'pause (pinboard-test-mode global-pinboard-test)))) 
 

(defun simulation-stopped ()
	(eq 'stop (pinboard-test-mode global-pinboard-test)))

;;----------------------------------------------------------------------------
;;
;; examples/capi/graphics/pinboard-test.lisp
;;
;; This example demonstrates the uses of pinboard-objects and
;; pinboard-layouts in the CAPI.
;;
;; To try it, compile and load this file and then execute:
;;
;;      (CL-USER::TEST-PINBOARD)
;;
;;----------------------------------------------------------------------------
;; Copyright (c) 1987--98 Harlequin Group plc. All rights reserved.
;;----------------------------------------------------------------------------

;;----------------------------------------------------------------------------
;; Define an interface
;;----------------------------------------------------------------------------

(capi:define-interface pinboard-test ()
  ((new-class :accessor new-class :initform nil)
   (start-x :accessor start-x)
   (start-y :accessor start-y)
   (last-x :accessor last-x)
   (last-y :accessor last-y))
  (:panes
   (mode-buttons
    capi:radio-button-panel
    :accessor mode-buttons
    :title "Action:"
    :title-position :top
    :layout-class 'capi:column-layout
    :items '(Stop Go)
    :print-function 'string-capitalize)
   (object-buttons
    capi:radio-button-panel
    :accessor object-buttons
    :title "Object:"
    :title-position :top
    :layout-class 'capi:column-layout
    :items '(goal obstacle start)
    :print-function 'string-capitalize)
   (style-buttons
    capi:radio-button-panel
    :accessor style-buttons
    :title "Style:"
    :title-position :top
    :layout-class 'capi:column-layout
    :items '(:solid :outline)
    :print-function 'string-capitalize))
  (:layouts
   (pinboard
    capi:pinboard-layout
    '()
    :background :white
    :input-model '(((:button-1 :press)   press-button-1)
                   ((:motion :button-1)  drag-button-1)
                   ((:button-1 :release) release-button-1)
                   ((:button-2 :press)   press-button-2))
    :drawing-style (:initarg :display-type)
    :vertical-scroll t
    :horizontal-scroll t
    :min-width 500
    :min-height 500)
   (button-layout
    capi:column-layout
    '(mode-buttons object-buttons style-buttons)
    :y-gap 10)
   (sub-layout
    capi:column-layout
    '(pinboard)
    :title "Drag button 1 to add an object and button 2 to delete one")
   (main-layout
    capi:row-layout
    '(button-layout sub-layout)
    ))
  (:default-initargs
   :layout 'main-layout
   :title "Pinboard Test"
   :best-height 500
   :best-width 500))

(defun pinboard-test-object (pinboard-test)
  (capi:choice-selected-item (object-buttons pinboard-test)))

(defun pinboard-test-filled (pinboard-test)
  (eq (capi:choice-selected-item (style-buttons pinboard-test)) :solid))

(defun pinboard-test-mode (pinboard-test)
  (capi:choice-selected-item (mode-buttons pinboard-test)))


;;----------------------------------------------------------------------------
;; A few useful utilities
;;----------------------------------------------------------------------------

;; x-y-width-and-height is a simple function which given two coordinates
;; returns the top-left x and y, and the width and height
(defun x-y-width-and-height (x1 y1 x2 y2)
  (values (min x1 x2)
          (min y1 y2)
          (abs (- x1 x2))
          (abs (- y1 y2))))

;; WITH-XOR makes all graphics-port operations done within its body be drawn
;; using exclusive-or.
(defmacro with-xor ((port) &body body)
  `(gp:with-graphics-state (,port
                            :foreground (gp:compute-xor-pixel ,port)
			    :operation boole-xor)
    ,@body))


;;----------------------------------------------------------------------------
;; Define a square pinboard-object
;;----------------------------------------------------------------------------

(defclass square (capi:pinboard-object)
  ((foreground :accessor foreground :initform nil :initarg :foreground)
   (filled :accessor filled :initform nil :initarg :filled))
  (:default-initargs
   :min-width 30
   :min-height 30))

(defmethod capi:draw-pinboard-object (pinboard (square square) &key)
  (capi:with-geometry square
    (let ((filled? (filled square)))
      (gp:draw-rectangle pinboard
                         capi:%x% capi:%y%
                         (if filled? capi:%width% (1- capi:%width%))
                         (if filled? capi:%height% (1- capi:%height%))
                         :foreground (or (foreground square)
				         (capi:simple-pane-foreground pinboard))
                         :filled filled?))))

(defmethod draw-object-outline (pinboard (square square) x1 y1 x2 y2)
  (multiple-value-bind
      (x y width height)
      (x-y-width-and-height x1 y1 x2 y2)
    (with-xor (pinboard)
      (gp:draw-rectangle pinboard x y width height))))


;;----------------------------------------------------------------------------
;; Define an ellipse pinboard object
;;----------------------------------------------------------------------------

(defclass ellipse (capi:pinboard-object)
  ((foreground :accessor foreground :initform nil :initarg :foreground)
   (filled :accessor filled :initform nil :initarg :filled))
  (:default-initargs
   :min-width 30
   :min-height 30))

(defmethod capi:draw-pinboard-object (pinboard (ellipse ellipse) &key)
  (capi:with-geometry ellipse
    (let ((x-radius (floor (1- capi:%width%) 2))
          (y-radius (floor (1- capi:%height%) 2)))
      (gp:draw-ellipse pinboard
		       (+ capi:%x% x-radius)
                       (+ capi:%y% y-radius)
                       x-radius
                       y-radius
		       :foreground (or (foreground ellipse)
                                       (capi:simple-pane-foreground pinboard))
                       :filled (filled ellipse)))))

(defmethod draw-object-outline (pinboard (ellipse ellipse) x1 y1 x2 y2)
  (multiple-value-bind
      (x y width height)
      (x-y-width-and-height x1 y1 x2 y2)
    (let ((x-radius (floor width 2))
          (y-radius (floor height 2)))
      (with-xor (pinboard)
        (gp:draw-ellipse pinboard 
			 (+ x x-radius)
			 (+ y y-radius)
			 x-radius
			 y-radius)))))


;;----------------------------------------------------------------------------
;; Define a text pinboard object
;;----------------------------------------------------------------------------

(defclass text (capi:pinboard-object)
  ((foreground :accessor foreground :initform nil :initarg :foreground)
   (filled :accessor filled :initform nil :initarg :filled))
  (:default-initargs
   :min-width 30
   :min-height 30))

(defmethod capi:draw-pinboard-object (pinboard (text text) &key)
  (capi:with-geometry text
    (let ((foreground (or (foreground text) (capi:simple-pane-foreground pinboard)))
          (background (capi:simple-pane-background pinboard))
          (filled (filled text)))
      (gp:draw-x-y-adjusted-string pinboard
                                   "This is a test."
		                   capi:%x%
		                   capi:%y%
                                   :y-adjust :top
                                   :foreground (if filled background foreground)
                                   :background (if filled foreground background)
                                   :block (filled text)))))

(defmethod draw-object-outline (pinboard (text text) x1 y1 x2 y2)
  (multiple-value-bind
      (x y width height)
      (x-y-width-and-height x1 y1 x2 y2)
    (with-xor (pinboard)
      (gp:draw-rectangle pinboard x y width height))))


;;----------------------------------------------------------------------------
;; The callbacks
;;----------------------------------------------------------------------------

(defun press-button-1 (pinboard x y)
  (let* ((interface (capi:element-interface pinboard))
         (shape 'square))
    (setf (new-class interface) (clos:class-prototype (find-class shape)))
    (setf (start-x interface) x)
    (setf (start-y interface) y)
    (setf (last-x interface) nil)))

(defun drag-button-1 (pinboard x y)
  (let* ((interface (capi:element-interface pinboard))
         (object (new-class interface)))
    (when (last-x interface)
      (draw-object-outline pinboard object
                           (start-x interface) (start-y interface)
                           (last-x interface) (last-y interface)))
    (setf (last-x interface) x
          (last-y interface) y)
    (draw-object-outline pinboard object
                         (start-x interface) (start-y interface)
                         (last-x interface) (last-y interface))))

(defun release-button-1 (pinboard x y)
  (let* ((interface (capi:element-interface pinboard))
         (object (new-class interface))
         (old-x (start-x interface))
         (old-y (start-y interface))
         (color (if (eq (pinboard-test-object interface) 'obstacle) :black :green))
         (filled (pinboard-test-filled interface))
         (width (abs (- old-x x)))
         (height (abs (- old-y y))))

	;; log the position of the object to the bug routine
	(add-object-to-world (pinboard-test-object interface) old-x old-y x y )
	(setf display-pane pinboard)



    ;; Remove the outline
    (when (last-x interface)
      (draw-object-outline pinboard object
                           (start-x interface) (start-y interface)
                           (last-x interface) (last-y interface)))

    ;; Create the appropriate pinboard-object and place it into
    ;; the pinboard-layout. We append it to the end of the list so
    ;; that it appears on top.
    (when (and (> width 1)
               (> height 1))
      (setf (capi:layout-description pinboard)
            (append (capi:layout-description pinboard)
                    (list
                     (make-instance (class-of object)
                                    :foreground color
                                    :filled filled
                                    :x (min old-x x)
                                    :y (min old-y y)
                                    :min-width  width
                                    :min-height height)))))))

(defun press-button-2 (pinboard x y)
  (let ((object (capi:pinboard-object-at-position pinboard x y)))
    (when object
      (setf (capi:layout-description pinboard)
            (remove object (capi:layout-description pinboard))))))


;;----------------------------------------------------------------------------
;; The test function
;;----------------------------------------------------------------------------

(defun init-graphics-pane ()
 (progn
  (setq global-pinboard-test (contain (make-instance 'pinboard-test :display-type :local-pixmap)))
  (capi:display global-pinboard-test)))