;;; This file contains various definitions and functions for HW2.
;;; It defines structures for lines, vertices, polygons, and scenes.
;;; A scene is a collection of polygons, a start, and a goal state.
;;;
;;; After loading this file, you should be able to print the contents of scene-3.22
;;; [1]> scene-3.22
;;;
;;; An important function is vertices-visible-from.  Pass it a vertex and the scene
;;; and it returns all vertices visible from that vertex.


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;  Defstructure Definition
;;;  Don't worry to much about these

(defmacro defstructure (type-and-args &rest slots)
  "This is just like DEFSTRUCT, except it keeps track of :include types, for
  the benefit of METHOD-FOR, and it makes printing go through PRINT-STRUCTURE."
  (if (atom type-and-args) (setf type-and-args (list type-and-args)))
  (let* ((type (first type-and-args))
	 (args (rest type-and-args))
	 (supertype (or (second (assoc ':include args)) 'structure))
	 (print-fn (if (null (assoc ':print-function args))
		       '((:print-function (lambda (x s d)
					    (declare (ignore d))
					    (print-structure x s)))))))
    `(progn (setf (get ',type ':supertype) ',supertype)
	    (defstruct (,type ,@print-fn ,@args) ,@slots))))

(defmethod print-structure ((structure t) stream)
  "Print a structure.  You can specialize this function.
  It will be called to print anything defined with DEFSTRUCTURE."
  ;(format stream "#<a ~A>" (type-of structure))
  (cond ( (equal (type-of structure) 'vertex)
  			(format stream "<~A ~A>" (type-of structure) (vertex-xy structure)))
  		( t (format stream "<~A>" (type-of structure)))
  )
)
;;; End of defstructure stuff
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;;;;;;; normal defstructs ;;;;;;;

;;;  Note that a line is a list of two numbers (12 55)
(defstruct line
  xy1 xy2)

;;; Polygon
;;;   vertices is the list of vertices
;;;   n is the number of vertices
;;;   if needed, use create-polygon to create new polygons.

(defstruct polygon
  vertices n)

;;; Vertex

(defstructure vertex
  xy           ;; the xy point for the vertex
  c-neighbor   ;; neighbour in clockwise direction
  a-neighbor   ;; neighbour in anti-clockwise direction
  visible      ;; list of vertices visible from here
  )

;;; scene

(defstruct scene
  polygons			; polygons comprising scene
  start				; vertex for start
  goal				; vertex for goal
  )

;;;;  Line util functions ;;;;;;

;;; returns the x coord.
;;; argument lxy is the xy pair (x y)
(defun xy-x (lxy)
	(first lxy))

;;; returns the y coord.
;;; argument lxy is the xy pair (x y)
(defun xy-y (lxy)
	(first (last lxy)))

(defun intersects (l1 l2)
;;; l1 is line ab; l2 is line cd
;;; assume the lines cross at alpha a + (1-alpha) b,
;;;     also known as beta c + (1-beta) d
;;; line segments intersect if 0<alpha,beta<1 unless they're parallel
  (let* ((a (line-xy1 l1))
	 (b (line-xy2 l1))
	 (c (line-xy1 l2))
	 (d (line-xy2 l2))
	 (print a)
	 (print b)
	 (print c)
	 (print d)
	 (xa (xy-x a)) (ya (xy-y a))
	 (xb (xy-x b)) (yb (xy-y b))
	 (xc (xy-x c)) (yc (xy-y c))
	 (xd (xy-x d)) (yd (xy-y d))
	 (q (- (* (- xa xb) (- yc yd))
	       (* (- ya yb) (- xc xd)))))
    (unless (= 0 q)
      (let ((alpha (/ (- (* (- xd xb) (- yc yd))
			 (* (- yd yb) (- xc xd)))
		      q))
	     (beta (/ (- (* (- xd xb) (- ya yb))
			   (* (- yd yb) (- xa xb)))
		         q)))
	(and (< 0 alpha 1) (< 0 beta 1))))))


;;;;;;;;;; Polygons ;;;;;;;;;;;;;;;

;;; Creates a polygon given a list of vertices such as
;;; '((0 0) (2 0) (2 2) (0 2))
;;; Also sets the neighbors of each vertex
;;; Neighbors are used with visibility.  The only points visible from a
;;; vertex on a convex polygon are its two neighbors.  This isn't true for concave
;;; polygons, so changes may need to be made at that point.

(defun create-polygon (points)
  ;; Assumes that points are given in anticlockwise order (or in order, anyway)
  (let* ((vertices (mapcar #'(lambda (xy) (make-vertex :xy xy)) points))
	 (poly (make-polygon :vertices vertices)))
    (setf (polygon-n poly) (length vertices))
    (dolist (v vertices)
      (let ((v2 (or (second (member v vertices)) (first vertices))))
	(setf (vertex-a-neighbor v) v2)
	(setf (vertex-c-neighbor v2) v)))
    poly))

;;; creates a scene given a start, end, and points for polygons

(defun create-scene (&key start goal polygons)
  "START and GOAL are xy points; polygons is a list of lists of vertices."
  (let ((polys (mapcar #'create-polygon
		       (list* (list start) (list goal) polygons))))
    (make-scene :start (first (polygon-vertices (first polys)))
		:goal (first (polygon-vertices (second polys)))
		:polygons polys)))

;; Find all the vertices that can be seen from this vertex.
;; Assumes Convex objects
(defun vertices-visible-from (v1 scene)
  ;; When you find them, cache them under the vertex-visible slot.
  (or (vertex-visible v1)
      (setf (vertex-visible v1) (vertices-in-view v1 scene))))

;; Find all the other vertices that can be seen from v.
(defun vertices-in-view (v scene)
  (delete v
   (let ((result nil))
    (loop for poly in (scene-polygons scene) do
	 (cond ((member-v v poly)
			(if (vertex-c-neighbor v) (push (vertex-c-neighbor v) result))
			(if (vertex-a-neighbor v) (push (vertex-a-neighbor v) result))
		   )
	       (t (loop for v2 in (polygon-vertices poly) do
		       (when (visible-p (vertex-xy v) (vertex-xy v2) scene)
			 (push v2 result))))))
    result)))

(defun visible-p (xy1 xy2 scene)
  "Predicate; return t iff xy1 is visible from xy2."
  (let ( (line (make-line :xy1 xy1 :xy2 xy2)) )
    (dolist (poly (scene-polygons scene) t)
      (if (line-intersects-poly? line poly) (return nil)))))

;;; Returns true if the xy coordinates of this vertex are
;;; equal to those of a point on this polygon.  ie the vertex
;;; v is on the polygon.

(defun member-v(v poly)
		(setf result 'nil)
		(loop for v2 in (polygon-vertices poly) do
		   (if (equal (vertex-xy v) (vertex-xy v2)) (setf result t))
		)
		result)

(defun line-intersects-poly? (line poly)
  "Predicate; return t iff line intersects poly."
  (dolist (v1 (polygon-vertices poly) nil)
    (let ((v2 (vertex-c-neighbor v1)))
      (if (intersects line
                      (make-line :xy1 (vertex-xy v1) :xy2 (vertex-xy v2)))
	  (return t)))))

;;; Definition of FIGURE 3.22 ;;;
;;; Taking the figure as being roughly 1000 by 500.
;;; The vertical axis is inverted.  (0,0) is the upper-left corner.
;;; The objects start from the rect nearest the start state and move
;;; counter-clockwise.  The points go counter clockwise usually starting
;;; from the point closest to the previous object

(setf start '(186 426))
(setf goal '(825 55))
(setf polygons  '(	((211 362) (211 490) (500 490) (500 362))
     				((545 417) (615 360) (512 223))
     				((680 362) (680 435) (724 490) (786 435) (786 362) (724 277))
     				((798 291) (811 82) (791 43) (731 68))
     				((712 50) (590 50) (590 285) (712 285))
     				((577 96) (512 32) (437 43) (437 163))
     				((405 112) (372 296) (430 296))
     				((360 124) (270 39) (182 126) (192 255) (303 285))
     			 )
)

;;; create the scene-3.22 object.

(setf scene-3.22 (create-scene :start start :goal goal :polygons polygons))