(in-package 'user)

;;; This is LAB 5 for 6.034
;;; This Lab looks at the problem of planning collision-free paths by using the
;;; visibility graph in configuration space.

;;; ASSIGNMENT:
;;; Can you make everything work for non-convex obstacles? 
;;; Write PATH-SEARCH as an A* search of the visibility graph.

;;; To get the displays, you need to run a different version of KCL than normal.
;;; Attach KCL as usual: attach -n kcl
;;; If you normally just run kcl, then run xkcl instead.
;;; Otherwise, do
;;; emacs -l /mit/6.034/bin/.emacs-xkcl
;;; and do Meta-X clisp-startup
;;; When lisp has started, do:
;;; (load "/mit/6.034/mtv")
;;; (load "/mit/6.034/lab5")
;;; Then, try typing this to LISP to get a demo.
;;; (find-path start goal moving obstacles)

;;; How to make other polygons:
;;;  (make-polygon '((0 0) (10 0) (5 10)))
;;; The input list are the (x y) coordinates of the polygon vertices as you go around
;;; the polygon in a COUNTERCLOCKWISE direction.

;;; POLYGONS are a list of vertex descriptors
;;; A vertex descriptor is (<vertex pos> <angle range>)
;;; A vertex position is (x y)
;;; An angle range is (min-angle max-angle)

;;; It is essential that the moving object be defined with (0 0) corresponsing to its
;;; reference point.

;;; The top-level function is:
;;; (FIND-PATH start goal moving obstacles) 
;;; START is the (x y) start position of the moving object, 
;;; GOAL is the (x y) goal position,  
;;; MOVING is a polygon describing the moving object, and 
;;; OBSTACLES is a list of polygons describing the stationary obstacles.

;;; The following functions are important:
;;; (CO moving stationary) returns a polygon that describes the configuration space
;;; obstacle for the moving object given the stationary obstacle.

;;; (BUILD-VGRAPH start goal c-obstacles)
;;; sets *VGRAPH* to the visibility graph.

;;; (PATH-SEARCH start-node goal-node vgraph)
;;; Searches for the shortest path in the vgraph.
;;; (PATH-SEARCH) does the search in the most recently computed VGRAPH.

;;; (OPEN-WINDOW) opens a display window.
;;; (DISPLAY-PROBLEM start goal moving obstacles) same args as FIND-PATH
;;; (DISPLAY-VGRAPH) displays most recent vgraph.
;;; (DISPLAY-POLYGONS polygons) displays a list of polygons
;;; (DISPLAY-PATH path moving obstacles) displays a path showing the moving obstacle
;;; at all the intermediate positions along the path.

(load "/mit/6.034/mtv")

(defparameter TWOPI (* 2 pi))
(defparameter *DEBUG* nil)
(defparameter *DISPLAY-SIZE* 300.)
(defparameter *DISPLAY-OFFSET* (/ *display-size* 2.0))
(defparameter *DISPLAY-SCALE* 5.0)
(defparameter *DISPLAY?* t)

(defvar *X-WINDOW*)
(defvar *VGRAPH*)
(defvar *VGRAPH-START-NODE*)
(defvar *VGRAPH-GOAL-NODE*)

(defstruct LINE
  normal
  offset)

(defstruct VGRAPH-NODE
  vertex
  polygon
  visible-nodes)

(defun X-of (v) (first v))

(defun Y-of (v) (second v))

(defun MIN-ANGLE (v) (first (second v)))

(defun TOP-ANGLE (v) (second (second v)))

(defun VERTEX-POS (v) (first v))

;;; MAIN FUNCTIONS

(defun FIND-PATH (start goal moving obstacles)
  (when *display?*
    (unless (boundp '*x-window*)
      (open-window))
    (display-problem start goal moving obstacles)
    (y-or-n-p "This is the input problem. Continue? "))
  (let ((c-obstacles (mapcar #'(lambda (stationary) (co moving stationary))
			     obstacles)))
    (when *display?*
      (clear-window)
      (display-polygons c-obstacles)
      (y-or-n-p "These are the C-space obstacles. Continue? "))
    (build-vgraph start goal c-obstacles)
    (when *display?*
      (clear-window)
      (display-vgraph)
      (y-or-n-p "This is the Visibility Graph. Continue? "))
    (let ((path (path-search)))
      (when *display?*
	(clear-window)
	(display-path path moving obstacles))
      path)))

(defun CO (moving stationary)
  (let ((neg-moving (negate-polygon moving)))
    ;; Both polygons are sorted by min-angle
    (fill-in-angles (co-aux neg-moving neg-moving stationary stationary))))

(defun CO-AUX (m initial-m s initial-s)
  (cond ((null m)
	 (if (and initial-m initial-s)
	     (co-aux initial-m nil s initial-s)))
	((null s)
	 (if (and initial-m initial-s)
	     (co-aux m initial-m initial-s nil)))
	((angle-range-> (first m) (first s))
	 (co-aux m initial-m (cdr s) initial-s))
	((angle-range-> (first s) (first m))
	 (co-aux (cdr m) initial-m s initial-s))
	(t
	 (cons (sum-verts (first m) (first s)) 
	       (cond ((top-angle-> (first m) (first s))
		      (co-aux m initial-m (cdr s) initial-s))
		     ((top-angle-> (first s) (first m))
		      (co-aux (cdr m) initial-m  s initial-s))
		     (t
		      (co-aux (cdr m) initial-m (cdr s) initial-s)))))))

(defun ANGLE-RANGE-> (v1 v2)
  ;; This is (>= (min-angle v1) (top-angle v2)) taken mod 2pi and
  ;; allowing for a little fudge factor in equality testing.
  (> (angle-diff (min-angle v1) (top-angle v2)) -0.0001))

(defun TOP-ANGLE-> (v1 v2)
  ;; This is (> (top-angle v1) (top-angle v2)) taken mod 2pi and
  ;; allowing for a little fudge factor in equality testing.
  (> (angle-diff (top-angle v1) (top-angle v2)) 0.0001))

(defun SUM-VERTS (v1 v2)
  (list (mapcar #'+ (vertex-pos v1) (vertex-pos v2))
	nil))

(defun NEGATE-POLYGON (polygon)
  (fill-in-angles
   (mapcar #'(lambda (v) (list (mapcar #'- (vertex-pos v)) nil))
	   polygon)))

(defun FILL-IN-ANGLES (polygon)
  (sort (mapcar #'(lambda (prev v next)
		    (list (car v)
			  (list (edge-angle prev v)
				(edge-angle v next))))

		(cons (car (last polygon)) polygon)
		polygon
		(append (cdr polygon) (list (car polygon))))
	#'min-angle-<))

(defun MIN-ANGLE-< (v1 v2)
  (< (min-angle v1) (min-angle v2)))

(defun EDGE-ANGLE (v1 v2)
  (atan-2pi (- (y-of (vertex-pos v2)) (y-of (vertex-pos v1)))
	    (- (x-of (vertex-pos v2)) (x-of (vertex-pos v1)))))

;;; Basic angle hacking (mod twopi) 

(defun ANGLE-DIFF (x y)
  (let ((z (norm-twopi (- x y))))
    ;; This is a kludgy attempt to handle the singularity ...
    (if (> z (- pi 0.001)) (- z twopi) z)))

(defun NORM-TWOPI (x)
  (cond ((< x 0.0) (norm-twopi (+ x twopi)))
	((> x twopi) (norm-twopi (- x twopi)))
	(t x)))

(defun ATAN-2PI (a b)
  "LISP:ATAN returns angles in the range 0 -> pi and 0 -> -pi,
   this function returns 0 -> 2pi"
  (let ((val (atan a b)))
    (if (>= val 0.0) val (+ twopi val))))

(defun MAKE-POLYGON (verts)
  (fill-in-angles (mapcar #'(lambda (v) (list v nil)) verts)))

(defun BUILD-VGRAPH (start goal obstacles)
  (setq *vgraph* (build-vgraph-aux start goal obstacles))
  'done)

(defun BUILD-VGRAPH-AUX (start goal obstacles &aux vgraph-nodes)
  (let* ((start-vertex (make-trivial-vertex start))
	 (goal-vertex  (make-trivial-vertex goal))
	 (start-V-node (make-vgraph-node :vertex start-vertex))
	 (goal-V-node (make-vgraph-node :vertex goal-vertex)))
    (setq *vgraph-start-node* start-v-node)
    (setq *vgraph-goal-node* goal-v-node)
    (if (v-edge-crosses-some-polygon? start-v-node goal-v-node obstacles)
 	(progn 	 
	  ;; Create all the possible vgraph nodes 
	  (push start-v-node vgraph-nodes)
	  (push goal-v-node vgraph-nodes)
	  (dolist (poly obstacles)
	    (dolist (vert poly)
	      (push (make-vgraph-node :vertex vert :polygon poly) 
		    vgraph-nodes)))
	  ;; Find the visible vertex pairs
	  (do ((v1-list vgraph-nodes (cdr v1-list)))
	      ((null (cdr v1-list)))
	    (let ((v1 (first v1-list)))
	      (do ((v2-list (cdr v1-list) (cdr v2-list)))
		  ((null v2-list))
		(let ((v2 (first v2-list)))
		  (if (possibly-optimal? v1 v2)
		      (when (not (v-edge-crosses-some-polygon? v1 v2 obstacles))
			(if *debug*
			    (format t "~% ~s and ~s are mutually visible."
				    (vertex-pos (vgraph-node-vertex v1))
				    (vertex-pos (vgraph-node-vertex v2))))
			(push v2 (vgraph-node-visible-nodes v1))
			(push v1 (vgraph-node-visible-nodes v2))))))))
	  vgraph-nodes)
	;; Trivial case, straight line between start and goal works.
	(list (make-vgraph-node :vertex start-vertex
				:visible-nodes (list goal-v-node))
	      (make-vgraph-node :vertex goal-vertex
				:visible-nodes (list start-v-node))))
    ))

(defun MAKE-TRIVIAL-VERTEX (posn)
  (list posn nil))

(defun V-EDGE-CROSSES-SOME-POLYGON? (v-node-1 v-node-2 obstacles)
  (let ((vertex-1 (vgraph-node-vertex v-node-1))
	(vertex-2 (vgraph-node-vertex v-node-2)))
    (if *debug*
	(format t "~% Testing ~s : ~s for crossings." 
		(vertex-pos vertex-1) (vertex-pos vertex-2)))
    (if (and (vgraph-node-polygon v-node-1)
	     (eq (vgraph-node-polygon v-node-1) (vgraph-node-polygon v-node-2)))
	;; Same polygon.  This assumes polygons are convex and that we know
	;; that the line from v1 to v2 is possibly optimal.
	(progn
	  (if *debug*
	      (format t "~%  Vertices of same polygon, so no."))
	  nil)
	(dolist (obst obstacles)
	  (if (not (or (eq (vgraph-node-polygon v-node-1) obst)
		       (eq (vgraph-node-polygon v-node-2) obst)))
	      ;; Don't test edge against their own polygons.
	      (when (or (edge-inside-polygon? vertex-1 vertex-2 obst)
			(do ((heads obst (cdr heads))
			     (tails (cons (car (last obst)) obst) (cdr tails)))
			    ((null heads))
			  (if (edges-cross? vertex-1 vertex-2 
					    (first tails) (first heads))
			      (return t))))
		(if *debug*
		    (format t "~%  Crosses polygon ~s" obst))
		(return t))))
	)))

(defun POSSIBLY-OPTIMAL? (v-node-1 v-node-2)
  (let ((vertex-1 (vgraph-node-vertex v-node-1))
	(vertex-2 (vgraph-node-vertex v-node-2)))
    (if *debug*
	(format t "~% Testing ~s : ~s for optimality." vertex-1 vertex-2))
    (let* ((angle1 (edge-angle vertex-1 vertex-2))
	   (angle2 (norm-twopi (+ angle1 pi)))
	   (result (and (or (angle-inside-range angle1 vertex-1)
			    (angle-inside-range angle2 vertex-1))
			(or (angle-inside-range angle1 vertex-2)
			    (angle-inside-range angle2 vertex-2)))))
      (if *debug*
	  (format t "~%  The results is ~s" result))
      result)))

(defun ANGLE-INSIDE-RANGE (angle vertex)
  (if (null (second vertex))
      t
      (and (>= (angle-diff angle (min-angle vertex)) -0.001)
	   (<= (angle-diff angle (top-angle vertex)) 0.001))))

(defun EDGE-INSIDE-POLYGON? (v1 v2 polygon)
  ;; Assumes convexity and that neither point is on the boundary of the polygon.
  (and (vertex-in-polygon? v1 polygon)
       (vertex-in-polygon? v2 polygon)))

(defun VERTEX-IN-POLYGON? (vertex poly &aux inside)
  (let ((x (x-of (vertex-pos vertex)))
	(y (y-of (vertex-pos vertex))))
    (do ((head-vertices poly (cdr head-vertices))
	 (tail-vertices (cons (first (last poly)) poly)
			(cdr tail-vertices)))
	((or (null head-vertices) (null tail-vertices)))
      (let ((head (vertex-pos (first head-vertices)))
	    (tail (vertex-pos (first tail-vertices))))
	(cond ((and (or (and (>= (y-of tail) y) (< (y-of head) y))
			(and (< (y-of tail) y) (>= (y-of head) y)))
		    (let ((dy (- (y-of head) (y-of tail)))
			  (dx (- (x-of head) (x-of tail))))
		      (if (>= dy 0)
			  (< (* (- x (x-of tail)) dy)
			     (* (- y (y-of tail)) dx))
			  (> (* (- x (x-of tail)) dy)
			     (* (- y (y-of tail)) dx)))))
	       (setq inside (not inside))))))
    inside))

(defun EDGES-CROSS? (e1-v1 e1-v2 e2-v1 e2-v2)
  (let ((pt (line-line-intersection (line-from-edge e1-v1 e1-v2)
				    (line-from-edge e2-v1 e2-v2))))
    (when (cond ((null pt) nil)		; parallel, not collinear
		((eq pt 'collinear)
		 (or (in-edge? (vertex-pos e1-v1) e2-v1 e2-v2)
		     (in-edge? (vertex-pos e1-v2) e2-v1 e2-v2)
		     (in-edge? (vertex-pos e2-v1) e1-v1 e1-v2)
		     (in-edge? (vertex-pos e2-v2) e1-v1 e1-v2)))
		(t
		 (and (in-edge? pt e1-v1 e1-v2) 
		      (in-edge? pt e2-v1 e2-v2))))
      pt)))

(defun LINE-FROM-EDGE (e-v1 e-v2)
  (let* ((v1 (vertex-pos e-v1))
	 (v2 (vertex-pos e-v2))
	 (dx (- (x-of v2) (x-of v1)))
	 (dy (- (y-of v2) (y-of v1)))
	 (line (make-line)))
    (setf (line-normal line) (vunit (list dy (- dx))))
    (setf (line-offset line) (- (vdot v1 (line-normal line))))
    line))

(defun LINE-LINE-INTERSECTION (l1 l2)
  (let ((det (- (* (x-of (line-normal l1)) (y-of (line-normal l2)))
		(* (y-of (line-normal l1)) (x-of (line-normal l2))))))
    (if (> (abs det) 0.0001)
	(list (/ (- (* (y-of (line-normal l1)) (line-offset l2))
		    (* (y-of (line-normal l2)) (line-offset l1)))
		 det)
	      (/ (- (* (x-of (line-normal l2)) (line-offset l1))	
		    (* (x-of (line-normal l1)) (line-offset l2)))
		 det))
	(let ((sign (if (> (vdot (line-normal l1) (line-normal l2)) 0.0)
			1.0
			-1.0)))
	  (if (< (abs (- (* sign (line-offset l1))
			 (line-offset l2)))
		 0.01)
	      'collinear))
	      )))

(defun IN-EDGE? (pt e-v1 e-v2)
  ;; Assuming that it is on the line
  (let ((tail (vertex-pos e-v1))
	(head (vertex-pos e-v2)))
    (and (<= (- (x-of pt) (max (x-of head) (x-of tail))) 0.01)
	 (>= (- (x-of pt) (min (x-of head) (x-of tail))) -0.01)
	 (<= (- (y-of pt) (max (y-of head) (y-of tail))) 0.01)
	 (>= (- (y-of pt) (min (y-of head) (y-of tail))) -0.01))))

(defun VUNIT (p) (vscale (sqrt (vdot p p)) p))

(defun VMAG (p) (sqrt (vdot p p)))

(defun VDOT (v1 v2) (+ (* (x-of v1) (x-of v2)) (* (y-of v1) (y-of v2))))

(defun VDIFF (a b) (mapcar #'- a b))

(defun VSCALE (c v) (list (* c (x-of v)) (* c (y-of v))))

(defun PRINT-VGRAPH (&optional (vgraph *vgraph*))
  (dolist (vn vgraph)
    (format t "~% Node at ~s:" (vertex-pos (vgraph-node-vertex vn)))
    (dolist (visible (vgraph-node-visible-nodes vn))
      (format t "~%  Can see ~s:" (vertex-pos (vgraph-node-vertex visible))))))

;;; Display 

(defun OPEN-WINDOW ()
  (let ((host 
	 (progn
	   (format t "~% Please enter the name of your workstation as
a string (e.g. \"E1-100-7\"): ")
	   (read))))
    (mtv:initialize-mtv host)
    (setq *x-window* (mtv:open-new-window *display-size* *display-size*))
    (mtv:expose-graphics-display *x-window*)
    *x-window*))
    
(defun DISPLAY-POLYGONS (polygons &optional (window *x-window*))
  (dolist (poly polygons)
    (mtv:draw-polygon window 
		      (mapcar #'transform (mapcar #'vertex-pos poly)) t)))

(defun DISPLAY-VGRAPH (&optional (vgraph-nodes *vgraph*) (window *x-window*))
  (dolist (vn vgraph-nodes)
    (let ((vertex (transform (vertex-pos (vgraph-node-vertex vn)))))
      (dolist (visible (vgraph-node-visible-nodes vn))
	(let ((visible-vertex (transform (vertex-pos (vgraph-node-vertex visible)))))
	  (mtv:draw-line window 
			 (x-of vertex) (y-of vertex)
			 (x-of visible-vertex) (y-of visible-vertex)))))))

(defun TRANSFORM (vert)
  (list (round (+ *display-offset* (* *display-scale* (x-of vert))))
	(- *display-size* 
	   (round (+ *display-offset* (* *display-scale* (y-of vert)))))))

(defun CLEAR-WINDOW (&optional (window *x-window*))
  (mtv:clear-graphics-display window))

(defun DISPLAY-PROBLEM (start goal moving obstacles)
  (clear-window)
  (display-polygons (list (offset-polygon moving start)))
  (display-polygons (list (offset-polygon moving goal)))
  (display-polygons obstacles))

(defun OFFSET-POLYGON (polygon pos)
  (mapcar #'(lambda (v) (list (mapcar #'+ (vertex-pos v) pos)
			      (second v)))
	  polygon))

(defun DISPLAY-PATH (path moving obstacles)
  (clear-window)
  (display-polygons obstacles)
  (do ((p path (cdr p)))
      ((null p))
    (if (cdr p)
	(let ((p1 (transform (first p)))
	      (p2 (transform (second p))))
	  (mtv:draw-line *x-window*
			 (x-of p1) (y-of p1)
			 (x-of p2) (y-of p2))))
    (display-polygons (list (offset-polygon moving (first p))))
    ))

;;; SIMPLE PATH SEARCH

(defun PATH-SEARCH (&optional (start-node *vgraph-start-node*)
			      (goal-node *vgraph-goal-node*))
  (path-search-aux goal-node (list (list start-node)) nil))

(defun PATH-SEARCH-AUX (goal open closed)
  (let ((current (first open)))
    (if (eq (first current) goal)
	(mapcar #'(lambda (vn) (vertex-pos (vgraph-node-vertex vn)))
		(reverse current))
	(path-search-aux goal 
			 (sort (append (extend-path current closed)
				       (cdr open))
			       #'<
			       :key #'path-length)
			 (cons (first current) closed)))))

(defun EXTEND-PATH (current closed &aux (head (first current)))
  (do ((visible (vgraph-node-visible-nodes head) (cdr visible))
       (new-nodes nil))
      ((null visible) new-nodes)
    (if (not (member (first visible) closed :test #'eq))
	(push (cons (first visible) current) new-nodes))))
       
(defun PATH-LENGTH (path)
  (do ((p path (cdr p))
       (sum 0.0))
      ((null (cdr p)) sum)
    (incf sum
	  (vmag (vdiff (vertex-pos (vgraph-node-vertex (first p)))
		       (vertex-pos (vgraph-node-vertex (second p))))))))

;;; A simple example.

(defparameter MOVING 
  (make-polygon '((0 0) (10 0) (5 10))))

(defparameter OBSTACLES 
  (mapcar #'make-polygon
	  (list '((5 10) (15 10) (15 20) (5 20))
		'((5 -15) (15 -15) (15 -5) (5 -5)))))

(defparameter START '(-10 5))

(defparameter GOAL '(20 5))