(declare (usual-integrations))

;;; This looks at the problem of planning collision-free paths by using the
;;; visibility graph in configuration space.  
;;; Try typing this to get a demo: (find-path start 0 goal 180 moving obstacles)
;;; See the definitions of the the sample variables at end of file.

;;; 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 and a set of x,y bounds
;;; A vertex descriptor is (<vertex pos> <angle range>)
;;; A vertex position is (x y)
;;; An angle range is (min-angle max-angle)
;;; All polygons MUST be convex!!  None of this will work otherwise. 

;;; 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 start-angle goal goal-angle moving obstacles) 
;;; START is the (x y) start position of the moving object, 
;;; START-ANGLE is the start orientation of moving object (in degrees)
;;; GOAL is the (x y) goal position,  
;;; GOAL-ANGLE is the goal orientation of moving object (in degrees)
;;; 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 start-angle goal goal-angle c-obstacles)
;;; sets *VGRAPH* to the visibility graph also *vgraph-start-node* and 
;;; *vgraph-goal-node* to the nodes representing the start and goal states.
;;; WARNING: don't try to print *VGRAPH*, it's a circular list (prints forever)
;;; You can do (SHOW-VGRAPH) instead.  The same holds true of any vgraph-node, use
;;; (SHOW-VGRAPH-NODE node).

;;; (PATH-SEARCH start-node goal-node)
;;; Searches for the shortest path in the vgraph and sets *PATH*.
;;; (PATH-SEARCH *vgraph-start-node* *vgraph-goal-node*) does the search in the most 
;;; recently computed VGRAPH. 
;;; WARNING: don't try to print *PATH*, it's a circular list (prints forever)
;;; You can do (SHOW-PATH) instead.

;;; (OPEN-WINDOW) opens a display window.
;;; (DISPLAY-PROBLEM start start-angle goal goal-angle moving obstacles) args like FIND-PATH
;;; (DISPLAY-VGRAPH) displays most recent vgraph.
;;; (DISPLAY-POLYGON polygon) displays a polygon
;;; (DISPLAY-PATH path moving obstacles) displays a path showing the moving obstacle
;;; at all the intermediate positions along the path.

;;;---------------------------------------------------------------
;;; Make sure that these are defined (they are not standard Scheme).

(define first car)
(define second cadr)
(define third caddr)
(define (last l)
  (let ((lgth (length l))) (list-tail l (- lgth 1))))
(define (butlast l)
  (cond ((null? l) l)
	((null? (cdr l)) '())
	(else (cons (car l) (butlast (cdr l))))))

;;;---------------------------------------------------------------
;;; Parameters

(define PI (atan 0 -1))			; duh
(define TWOPI (* 2 pi))			; duh duh

(define *DEBUG* #f)			; controls printing

(define *DISPLAY?* #t)			; display results?
(define *DISPLAY-SIZE* 300.)		; virtual coordinates of display
(define *DISPLAY-OFFSET* (/ *display-size* 2.0))
(define *DISPLAY-SCALE* 5.0)		; mapping from object units to display

(define *WINDOW* #f)			; display window
(define *WINDOW-WIDTH* 300)		; pixels
(define *WINDOW-HEIGHT* 300)		; pixels

(define (DEG->RAD x) (* pi (/ x 180.0))) ; Ah, radians
(define *VGRAPH-ANGLES*			; angles used for finding path
  (map deg->rad '(-90 -60 -30 0 30 60 90)))

(define *ROTATION-COST* 1.)		; radians -> length

;; These are not parameters, just globals to ease communication among different 
;; parts of the program.

(define *VGRAPH* #f)			; global vgraph
(define *VGRAPH-START-NODE* #f)		; global start node
(define *VGRAPH-GOAL-NODE* #f)		; global goal node
(define *VGRAPH-NODE-COUNTER* 0)	; global counter for node id
(define *PATH* #f)			; global path

;;;---------------------------------------------------------------
;;; Data structures

;; Lines (normal-vector and offset)
(define (MAKE-LINE normal offset) (list normal offset))
(define LINE-NORMAL first)
(define LINE-OFFSET second)

;; Vgraph nodes, they define the search graph.
(define (MAKE-VGRAPH-NODE vertex angle polygon visible-nodes)
  (set! *vgraph-node-counter* (+ 1 *vgraph-node-counter*))
  (list *vgraph-node-counter* vertex angle polygon visible-nodes))
(define VGRAPH-NODE-ID first)
(define VGRAPH-NODE-VERTEX second)
(define VGRAPH-NODE-ANGLE third)
(define (VGRAPH-NODE-POLYGON v) (list-ref v 3))
(define (VGRAPH-NODE-VISIBLE-NODES v) (list-ref v 4))
(define VGRAPH-NODE-VISIBLE-NODES-HANDLE cddddr)

;; some vgraph-node operations
;; make a copy of a vgraph-node at a different angle.
(define (DISPLACE-ANGLE node new-angle)
  (make-vgraph-node
   (vgraph-node-vertex node)
   new-angle
   (vgraph-node-polygon node)
   ;; since we modify this list, copy it...
   (append (vgraph-node-visible-nodes node) '())))

;; connect a new visible node to an existing node - modifies node.
(define (ADD-VISIBLE-NODE! new-vis-node node)
  ;;(display* "Connecting " (vgraph-node-id new-vis-node) " to " (vgraph-node-id node))
  (let ((nodes (vgraph-node-visible-nodes node)))
    (cond ((null? nodes)
	   (set-car! (vgraph-node-visible-nodes-handle node) 
		     (list new-vis-node)))
	  (else
	   (set-cdr! nodes (cons (car nodes) (cdr nodes)))
	   (set-car! nodes new-vis-node)
	   )))
  node)

;; Used for positions
(define X-of first)
(define Y-of second)

;; Vertex (position and angle range spanned by incident edges)
(define (MAKE-VERTEX pos range) (list pos range))
(define (MAKE-TRIVIAL-VERTEX posn) (make-vertex posn '()))
(define VERTEX-POS first)
(define VERTEX-RANGE second)

(define POLYGON-VERTS first)
(define POLYGON-BOUNDS second)
;; Creates a polygon (including filling the angles) from a list of vertex positions
(define (MAKE-POLYGON pos-list)
  (list (fill-in-angles (map (lambda (v) (make-trivial-vertex v)) pos-list))
	(make-polygon-bounds pos-list))	; bounds
  )

;; Used for angle ranges
(define (MIN-ANGLE v) (first (vertex-range v)))
(define (TOP-ANGLE v) (second (vertex-range v)))

;;;---------------------------------------------------------------
;;; MAIN FUNCTIONS

(define (FIND-PATH start start-angle goal goal-angle moving obstacles)
  ;; convert to radians
  (set! start-angle (deg->rad start-angle))
  (set! goal-angle  (deg->rad goal-angle))
  ;; check that the angles are valid
  (or (and (member start-angle *vgraph-angles*)
	   (member goal-angle *vgraph-angles*))
      (error "The start and end angles must be drawn from the following list:"
	     *vgraph-angles*))
  ;; initialize display with the problem display
  (cond (*display?*
	 (if *window*
	     (clear-window)
	     (open-window))
	 (display-problem start start-angle goal goal-angle moving obstacles)
	 (y-or-n-p "This is the input problem. Continue? ")))
  ;; Compute C-slices: (angle . c-obstacles) for each angle
  (let ((c-slices
	 ;; a list of slices, each is (angle . c-obsts)
	 (map (lambda (angle)
		(cons angle
		      (let ((rotated-moving (rotate-polygon moving angle)))
			(map (lambda (stationary) (co rotated-moving stationary))
			     obstacles))))
	      *vgraph-angles*)))
    ;; show the C-slices
    (cond (*display?*
	   (graphics-set-color *window* '(0 0 255))
	   (for-each (lambda (slice)
		       (for-each display-polygon (cdr slice))
		       #|(y-or-n-p "Angle=" (car slice)
				 ". These are the C-space obstacles. Continue? ")|#
		       )
		     c-slices)
	   (y-or-n-p "These are c-space obstacles. Continue? ")))
    ;; Compute the VGRAPH from the c-slices
    (build-vgraph start start-angle goal goal-angle c-slices)
    ;; Show the VGRAPH
    (cond (*display?*
	   (clear-window)
	   (display-problem start start-angle goal goal-angle moving obstacles)
	   (graphics-set-color *window* '(255 0 0))
	   (display-vgraph)
	   (y-or-n-p "This is the Visibility Graph. Continue? ")))
    ;; Search for the shortest path
    (path-search *vgraph-start-node* *vgraph-goal-node*)
    ;; Show the path, first on top of VGRAPH and then by itself.  
    (cond (*display?*
	   (display-path *path* moving obstacles)
	   (y-or-n-p "This is the final path. Continue? ")
	   (clear-window)
	   (display-path *path* moving obstacles)))
    (graphics-set-color *window* '(0 0 0))
    'done))

;;; Compute the C-space Obstacle for the moving object and one stationary obstacle.
(define (CO moving stationary)
  (let ((neg-moving (negate-polygon moving)))
    ;; Both polygons are sorted by min-angle in the angle range.
    (make-polygon
     (map vertex-pos
	  (co-aux (polygon-verts neg-moving)
		  (polygon-verts neg-moving)
		  (polygon-verts stationary)
		  (polygon-verts stationary)
		  0 0)))))

;;; Why this works is a bit subtle, look at:
;;; T. Lozano-Perez, "Spatial Planning: A Configuration Space Approach", 
;;; IEEE Transactions on Computers, February 1983.
(define (CO-AUX m initial-m s initial-s offset-m offset-s)
  (if *debug*
      (display* "m: " (if (null? m) '() (vertex-range (first m)))
		"off-m: " offset-m
		" s: " (if (null? s) '() (vertex-range (first s)))
		"off-s: " offset-s))
  (cond ((null? m)
	 (if (and initial-m initial-s)
	     (co-aux initial-m '() s initial-s twopi 0)
	     '()))
	((null? s)
	 (if (and initial-m initial-s)
	     (co-aux m initial-m initial-s '() 0 twopi)
	     '()))
	((angle-range-> (first m) offset-m (first s) offset-s)
	 (if *debug* (display* "Range m > Range s"))
	 (co-aux m initial-m (cdr s) initial-s offset-m offset-s))
	((angle-range-> (first s) offset-s (first m) offset-m)
	 (if *debug* (display* "Range s > Range m"))
	 (co-aux (cdr m) initial-m s initial-s offset-m offset-s))
	(else
	 (let ((new-vert (sum-verts (first m) (first s))))
	   (if *debug* (display* "New vert: " new-vert))
	   (cons new-vert 
		 (cond ((top-angle-> (first m) offset-m (first s) offset-s)
			(if *debug* (display* "Top m > Top s"))
			(co-aux m initial-m (cdr s) initial-s offset-m offset-s))
		       ((top-angle-> (first s) offset-s (first m) offset-m)
			(if *debug* (display* "Top s > Top m"))
			(co-aux (cdr m) initial-m  s initial-s offset-m offset-s))
		       (else
			(if *debug* (display* "Top s == Top m"))
			(co-aux (cdr m) initial-m (cdr s) initial-s offset-m offset-s)))))
	 )))

;; if an angle range looks like (2.3 1.2), it means it wrapped around 2pi.
(define (TOP-ANGLE-WRAPPED v)
  (if (< (top-angle v) (min-angle v))
      (+ (top-angle v) twopi)
      (top-angle v)))

;; checks if the angle range of vertex v1 (offset by off1) is uniformly 
;; bigger than the angle range of vertex v2 (offset by off2).
(define (ANGLE-RANGE-> v1 off1 v2 off2)
  ;; This is (>= (min-angle v1) (top-angle v2)) taken mod 2pi and
  ;; allowing for a little fudge factor in equality testing.
  (> (- (+ (min-angle v1) off1) (+ (top-angle-wrapped v2) off2)) -0.0001)
  )
;; checks if the top angle of vertex v1's range (offset by off1) is 
;; bigger than the top angle of vertex v2's range (offset by off2).
(define (TOP-ANGLE-> v1 off1 v2 off2)
  ;; This is (> (top-angle v1) (top-angle v2)) taken mod 2pi and
  ;; allowing for a little fudge factor in equality testing.
  (> (- (+ (top-angle-wrapped v1) off1) (+ (top-angle-wrapped v2) off2)) 0.0001)
  )

;; the opposite of the test above
(define (MIN-ANGLE-< v1 v2)
  (< (min-angle v1) (min-angle v2)))

;; A new vertex whose coordinate is the sum of the two input verts.
(define (SUM-VERTS v1 v2)
  (make-trivial-vertex (map + (vertex-pos v1) (vertex-pos v2))))

;; A new polygon each of whose vertex-pos is the negative of the ones in the input.
(define (NEGATE-POLYGON polygon)
  (make-polygon
   (map (lambda (v) (map - (vertex-pos v)))
	(polygon-verts polygon))))

;; Compute the angle ranges for the vertices in a polygon.
(define (FILL-IN-ANGLES verts)
  (sort (map (lambda (prev v next)
	       (make-vertex (car v)
			    (list (edge-angle prev v)
				  (edge-angle v next))))

	     (cons (car (last verts)) (butlast verts))
	     verts
	     (append (cdr verts) (list (car verts))))
	min-angle-<))

;; The angle of the line connecting two vertices (in the range 0 to 2pi)
(define (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)))))

;;; Make sure that angle is in the range [0, 2pi]
(define (NORM-TWOPI x)
  (cond ((< x 0.0) (norm-twopi (+ x twopi)))
	((> x twopi) (norm-twopi (- x twopi)))
	(else x)))

;; ATAN returns angles in the range [-pi, pi], this function returns [0, 2pi]
(define (ATAN-2PI a b)
  (let ((val (atan a b)))
    (if (>= val 0.0) val (+ twopi val))))

;; Displace (and optionally rotate) a polygon.
(define (OFFSET-POLYGON polygon pos . angle)
  (let ((verts 
	 (if (not (null? angle))
	     (rotate-pos (map vertex-pos (polygon-verts polygon)) (first angle))
	     (map 'vertex-pos (polygon-verts polygon)))))
    ;; a displacement does not change the angle range.
    (make-polygon (map (lambda (v) (map + v pos)) verts))))

;; Rotation about (0,0) - so really only intended for moving object.
(define (ROTATE-POS pos-list angle)
  (let ((c (cos angle))
	(s (sin angle)))
    (define (rot pos)
      (list (- (* c (x-of pos)) (* s (y-of pos)))
	    (+ (* s (x-of pos)) (* c (y-of pos)))))
    (map (lambda (v) (rot v)) pos-list)))

(define (ROTATE-POLYGON polygon angle)
  (make-polygon
   (rotate-pos (map vertex-pos (polygon-verts polygon)) angle)))

;;;---------------------------------------------------------------
;;; BUILD VGRAPH

(define (BUILD-VGRAPH start start-angle goal goal-angle slices)
  ;; initialize nodes for start and goal
  (set! *vgraph-start-node* 
	(make-vgraph-node (make-trivial-vertex start) start-angle '() '()))
  (set! *vgraph-goal-node*
	(make-vgraph-node (make-trivial-vertex goal) goal-angle '() '()))
  ;;; Build VGRAPH for the slices
  (set! *vgraph* 
	(add-vgraph-interconnects	; connect across slices
	 (map (lambda (slice)
		(display* "Building Vgraph for angle = " (car slice) " ...")
		(let ((initial '()))
		  ;; Add the start and goal nodes to any slice where they are free.
		  (if (= (car slice) start-angle)
		      (set! initial (cons *vgraph-start-node* initial))
		      (let ((new (displace-angle *vgraph-start-node* (car slice))))
			(if (vgraph-node-free? new (cdr slice)) ; test if it's free
			    (set! initial (cons new initial)))))
		  (if (= (car slice) goal-angle)
		      (set! initial (cons *vgraph-goal-node* initial))
		      (let ((new (displace-angle *vgraph-goal-node* (car slice))))
			(if (vgraph-node-free? new (cdr slice)) ; test if it's free
			    (set! initial (cons new initial)))))
		  ;; the VGRAPH for a slice
		  (cons (car slice) 
			(build-vgraph-aux
			 (car slice) initial (cdr slice)))))
	      slices)
	 slices))
  'done)

;; Build a VGRAPH for a single slice (angle).
(define (BUILD-VGRAPH-AUX angle vgraph-nodes obstacles)
  ;; The nodes correspond to the (free) vertices of the polygons.
  (for-each
   (lambda (poly)
     (for-each (lambda (vert)
		 (let ((node (make-vgraph-node vert angle poly '())))
		   ;; add free nodes
		   (if (vgraph-node-free? node obstacles)
		       (set! vgraph-nodes 
			     (cons node vgraph-nodes)))))
	       (polygon-verts poly)))
   obstacles)
  ;; 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)	; check for tangency to obstacles
	      (cond ((not (v-edge-crosses-some-polygon? v1 v2 obstacles))
		     ;; no collisions, so connect it.
		     (vgraph-node-connect v1 v2))))
	  ))))
  vgraph-nodes)

;; Connect the vertices in different slices.
(define (ADD-VGRAPH-INTERCONNECTS vgraph-slices slices)
  (let ((vgraph (apply append (map cdr vgraph-slices)))) 
    ;; starts with the vgraph corresponding to the union of the individual slices.
    (do ((prev-v-slices vgraph-slices (cdr prev-v-slices))
	 (prev-slices slices (cdr prev-slices))
	 (next-v-slices (cdr vgraph-slices) (cdr next-v-slices))
	 (next-slices (cdr slices) (cdr next-slices)))
	((null? next-v-slices) 
	 ;; return the full vgraph after connections
	 vgraph)
      (let ((prev-vgraph-nodes (cdr (first prev-v-slices)))
	    (next-vgraph-nodes (cdr (first next-v-slices)))
	    (prev-obstacles (cdr (first prev-slices)))
	    (next-obstacles (cdr (second prev-slices))))

	(cond (*debug*
	       (display* "Prev Slice")
	       (show-vgraph prev-vgraph-nodes)
	       (display* "Next Slice")
	       (show-vgraph next-vgraph-nodes)))

	;; Find the visible pairs
	(do ((pv-list prev-vgraph-nodes (cdr pv-list)))
	    ((null? pv-list))
	  (let ((pv (first pv-list)))
	    (do ((nv-list next-vgraph-nodes (cdr nv-list)))
		((null? nv-list))
	      (let ((nv (first nv-list)))
		(if (possibly-optimal? pv nv)
		    (cond ((not (v-edge-crosses-some-polygon? pv nv prev-obstacles))
			   (vgraph-node-connect pv nv)
			   ;; path is free in prev-slice, so introduce
			   ;; intermediate point at nv location but pv angle.
			   (let ((new (displace-angle nv (vgraph-node-angle pv))))
			     (set! vgraph (cons new vgraph))
			     (vgraph-node-connect pv new)
			     (vgraph-node-connect new nv))
			   )
			  ((not (v-edge-crosses-some-polygon? pv nv next-obstacles))
			   (vgraph-node-connect pv nv)
			   ;; path is free in next-slice, so introduce
			   ;; intermediate point at pv location but nv angle.
			   (let ((new (displace-angle pv (vgraph-node-angle nv))))
			     (set! vgraph (cons new vgraph))
			     (vgraph-node-connect pv new)
			     (vgraph-node-connect new nv))
			   )))
		))))
	))))

;; Symmetric connection (via addition to visible nodes)
(define (VGRAPH-NODE-CONNECT v1 v2)
  (if *debug*
      (display* "  " (vertex-pos (vgraph-node-vertex v1))
		"[" (vgraph-node-angle v1) "] and "
		(vertex-pos (vgraph-node-vertex v2))
		"[" (vgraph-node-angle v2) "] are mutually visible."))
  (add-visible-node! v2 v1)
  (add-visible-node! v1 v2))

;; Does some edge of polygon (repesented as parallel lists of tails and heads) 
;;; cross the edge defined by vertices v1-v2 
(define (SOME-EDGE-CROSSES? v1 v2 tails heads)
  (cond ((or (null? heads) (null? tails)) #f)
	((edges-cross? v1 v2 (first tails) (first heads)) #t)
	(else 
	 (some-edge-crosses? v1 v2 (cdr tails) (cdr heads)))))
  
;; Does the edge between two Vgraph-nodes cross an obstacle
;; Should use the bounds to avoid testing some polygon/edge combinations.
(define (V-EDGE-CROSSES-POLYGON? v-node-1 v-node-2 obst)
  (let* ((vertex-1 (vgraph-node-vertex v-node-1))
	 (vertex-2 (vgraph-node-vertex v-node-2))
	 (obst-verts (polygon-verts obst)))
    (cond ((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.
	   ;; This only works because of the "optimality" condition, which ensures 
	   ;; that the edges are tangent to the polygons.
	   #f)
	  ((not (bounds-overlap? (polygon-bounds obst)
				 (make-edge-bounds (vertex-pos vertex-1)
						   (vertex-pos vertex-2))))
	   ;; bounding boxes don't overlap, so edge can't cross obst.
	   #f)
	  ((or (edge-inside-polygon? vertex-1 vertex-2 obst)
	       (some-edge-crosses? vertex-1 vertex-2
				   (cons (car (last obst-verts)) obst-verts)
				   obst-verts))
	   (if *debug*
	       (display* "   Crosses polygon " obst))
	   #t)
	  (else #f))))

;; Does the edge between two Vgraph nodes cross ANY obstacle in obstacles?
(define (V-EDGE-CROSSES-SOME-POLYGON? v-node-1 v-node-2 obstacles)
  (if *debug*
      (display* "Testing "
	      (vertex-pos (vgraph-node-vertex v-node-1)) 
	      "[" (vgraph-node-angle v-node-1) "] and "
	      (vertex-pos (vgraph-node-vertex v-node-2))
	      "[" (vgraph-node-angle v-node-2) "] for crossings."))

  (some? (lambda (obst) (v-edge-crosses-polygon? v-node-1 v-node-2 (car obsts)))
	 obstacles))

;; Is a Vgraph node contained in any obstacle in the argument list?
(define (VGRAPH-NODE-FREE? v-node obstacles)
  (let ((v (vgraph-node-vertex v-node))
	(poly (vgraph-node-polygon v-node)))
    (not (some? (lambda (p) (and (not (eq? p poly)) (vertex-in-polygon? v p))) 
		obstacles)))) 

;;;---------------------------------------------------------------
;;; All the geometry tricks are here...

;; Checks that the edge between two Vgraph nodes is tangent to the corresponding 
;; polygons.
(define (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*
	(display* "Testing "
	      (vertex-pos vertex-1) 
	      "[" (vgraph-node-angle v-node-1) "] and "
	      (vertex-pos vertex-2)
	      "[" (vgraph-node-angle v-node-2) "] for optimality."))
    ;; First, check for the special case of the same vertex 
    ;; (which happens across angle slices).
    (or (equal? (vertex-pos vertex-1) (vertex-pos vertex-2))
	;; Check that the line between vertices is tangent to verts.
	(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*
	      (display* "  The result is " result))
	  result))))

;; Angle contained in range of vertex?
(define (ANGLE-INSIDE-RANGE angle vertex)
  (if (null? (vertex-range vertex))
      #t
      (let ((angle-twopi (norm-twopi angle)))
	(and (>= (- angle-twopi (min-angle vertex)) -0.001)
	     (<= (- angle-twopi (top-angle-wrapped vertex)) 0.001)))))

;; Is an edge (both endpoints) completely inside a polygon?
(define (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)))

;; Checks whether a vertex is in a polygon via the parity of the number of 
;; crossings of a line emanating at the vertex.  Unfortunately, this behaves a 
;; bit strangely if you are testing the vertex position is on the boundary of poly.
(define (VERTEX-IN-POLYGON? vertex poly)
  (and (pos-in-bounds? (vertex-pos vertex) (polygon-bounds poly)) ; quick
       (let ((x (x-of (vertex-pos vertex)))
	     (y (y-of (vertex-pos vertex)))
	     (poly-verts (polygon-verts poly))
	     (inside #f))
	 (do ((head-vertices poly-verts (cdr head-vertices))
	      (tail-vertices (cons (first (last poly-verts)) poly-verts)
			     (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)))))
		    (set! inside (not inside))))))
	 inside)))

;; Do two edges (not lines) cross?
(define (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))))
    (cond ((eq? pt 'parallel) #f)	; parallel, not collinear
	  ((eq? pt 'collinear)
	   (and (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))
		pt))
	  (else
	   (and (in-edge? pt e1-v1 e1-v2) 
		(in-edge? pt e2-v1 e2-v2)
		pt)))))

;; The (infinite) line supporting an edge.
(define (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)))
	 (normal (vunit (list dy (- dx)))))
    (make-line 
     normal
     (- (vdot v1 normal))	; offset
     )))

;; Intersect two (infinite) lines.  The outcomes are:
;; (x y) - the coordinates of the point of intersection
;; parallel - a symbol indicating the lines are parallel.
;; collinear - a symbol indicating they are the same line.
(define (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
	      'parallel))
	      )))

;; Assuming that pt is on the line, is it in the finite edge segment
(define (IN-EDGE? pt e-v1 e-v2)
  (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))))

(define (MAKE-POLYGON-BOUNDS pos-list)
  (let ((xmin 1e10)
	(xmax -1e10)
	(ymin 1e10)
	(ymax -1e10))
    (for-each 
     (lambda (p) 
       (let ((x (x-of p))
	     (y (y-of p)))
	 (if (< x xmin) (set! xmin x))
	 (if (< y ymin) (set! ymin y))
	 (if (> x xmax) (set! xmax x))
	 (if (> y ymax) (set! ymax y))))
     pos-list)
    (list (list xmin ymin) (list xmax ymax))))

(define (MAKE-EDGE-BOUNDS p1 p2)
  (list (list (min (x-of p1) (x-of p2)) (min (y-of p1) (y-of p2)))
	(list (max (x-of p1) (x-of p2)) (max (y-of p1) (y-of p2)))))

(define (BOUNDS-OVERLAP? bounds1 bounds2)
  (not (or (> (x-of (first bounds1)) (x-of (second bounds2)))
	   (> (y-of (first bounds1)) (y-of (second bounds2)))
	   (< (x-of (second bounds1)) (x-of (first bounds2)))
	   (< (y-of (second bounds1)) (y-of (first bounds2))))))

(define (POS-IN-BOUNDS? pos bounds)
  (and (>= (x-of pos) (x-of (first bounds)))
       (>= (y-of pos) (y-of (first bounds)))
       (<= (x-of pos) (x-of (second bounds)))
       (<= (y-of pos) (y-of (second bounds)))))

;; Basic vector operations

(define (VUNIT p) (vscale (sqrt (vdot p p)) p))

(define (VMAG p) (sqrt (vdot p p)))

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

(define (VDIFF a b) (map - a b))

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

;;;---------------------------------------------------------------
;;; Display 

(define (OPEN-WINDOW)
  (set! *window* (graphics-create *window-width* *window-height*))
  (graphics-set-coordinate-limits *window* 0 0 *display-size* *display-size*)
  )

(define (CLEAR-WINDOW)
  (graphics-clear *window*)
  (graphics-set-color *window* '(0 0 0)))
    
(define (DISPLAY-POLYGON polygon)
  (let ((poly-verts (polygon-verts polygon)))
    (define (draw-polygon window poly-verts)
      ;; Scheme does not support filled polygons under X...
      ;;(graphics-operation *window* 'fill-polygon (list->vector (apply append polygon)))
      (for-each
       (lambda (v1 v2)
	 ;;(display (list v1 v2))
	 (graphics-draw-line 
	  window (x-of v1) (y-of v1) (x-of v2) (y-of v2))
	 )
       (cons (car (last poly-verts)) (butlast poly-verts))
       poly-verts
       ))
    (draw-polygon *window* (map transform (map vertex-pos poly-verts)))))

(define (DISPLAY-VGRAPH . angles)
  (for-each 
   (lambda (vn)
     (if (or (null? angles) (member (vgraph-node-angle vn) angles))
	 (let ((vertex-pos (transform (vertex-pos (vgraph-node-vertex vn)))))
	   (for-each
	    (lambda (visible)
	      (if (or (null? angles) (member (vgraph-node-angle visible) angles))
		  (let ((visible-pos (transform (vertex-pos (vgraph-node-vertex visible)))))
		    (graphics-draw-line *window* 
					(x-of vertex-pos) (y-of vertex-pos)
					(x-of visible-pos) (y-of visible-pos)))))
	    (vgraph-node-visible-nodes vn)))))
   *vgraph*))

(define (TRANSFORM pos)
  (list (round (+ *display-offset* (* *display-scale* (x-of pos))))
	(round (+ *display-offset* (* *display-scale* (y-of pos))))))

(define (DISPLAY-PROBLEM start start-angle goal goal-angle moving obstacles)
  (graphics-set-color *window* '(0 255 255))
  (display-polygon (offset-polygon moving start start-angle))
  (display-polygon (offset-polygon moving goal goal-angle))

  (graphics-set-color *window* '(0 0 0))
  (for-each display-polygon obstacles))

(define (DISPLAY-PATH path moving obstacles)
  (graphics-set-color *window* '(0 0 0))
  (for-each display-polygon obstacles)

  (if (null? path)
      (display* "Could not find a path."))

  (do ((p path (cdr p)))
      ((null? p))
    (let ((node1 (first p)))
      (graphics-set-color *window* '(0 255 255))
      (display-polygon 
       (offset-polygon moving 
		       (vertex-pos (vgraph-node-vertex node1))
		       (vgraph-node-angle node1)))

      (if (cdr p)
	  (let ((p1 (transform (vertex-pos (vgraph-node-vertex (first p)))))
		(p2 (transform (vertex-pos (vgraph-node-vertex (second p))))))
	    (graphics-set-color *window* '(255 0 0))
	    (graphics-draw-line *window*
				(x-of p1) (y-of p1)
				(x-of p2) (y-of p2))
	    )))
    ))

;;;---------------------------------------------------------------
;;; SHOWING (printing)

(define (SHOW-PATH . p)
  (for-each display*
	    (map (lambda (x) (list (vertex-pos (vgraph-node-vertex x))
				   (vgraph-node-angle x)))
		 (if (null? p) *path* (first p)))))

(define (SHOW-VGRAPH-NODE node)
  (display* (list (vgraph-node-id node)
		  (vertex-pos (vgraph-node-vertex node))
		  (vgraph-node-angle node)
		  (map vgraph-node-id (vgraph-node-visible-nodes node)))))

(define (SHOW-VGRAPH . vg)
  (for-each show-vgraph-node (if (null? vg) *vgraph* (first vg))))

(define (SHOW-VGRAPH-CONNECTIONS . vg)
  (for-each
   (lambda (vn)
     (display* " Node at "
	     (vertex-pos (vgraph-node-vertex vn))
	     "[" (vgraph-node-angle vn) "]")
     (for-each 
      (lambda (visible)
	(display* "  Can see "
		(vertex-pos (vgraph-node-vertex visible))
		"[" (vgraph-node-angle vn) "]"))
      (vgraph-node-visible-nodes vn)))
   (if (null? vg) *vgraph* (first vg))))

;;;---------------------------------------------------------------
;;; SIMPLE PATH SEARCH - Uniform Cost

;; Search node operations (lgth . vgraph-node-list)
(define SEARCH-NODE-LENGTH car)
(define SEARCH-NODE-HEAD cadr)
(define SEARCH-NODE-PATH cdr)

(define MAKE-SEARCH-NODE cons)

(define (SHOW-SEARCH-NODE node)
  (display* "Id = " (vgraph-node-id (search-node-head node))
	    " Path length = " (search-node-length node)
	    " Pos = " (vertex-pos (vgraph-node-vertex (search-node-head node)))
	    " Angle = " (vgraph-node-angle (search-node-head node)))
  (show-path (search-node-path node))
  )

(define (NEW-SEARCH-NODE vg-node search-node)
  (make-search-node (+ (search-node-length search-node)
		       (path-length-increment (search-node-head search-node)
					      vg-node))
		    (cons vg-node (search-node-path search-node))))

;; Returns a list of vgraph-nodes (possibly null).
(define (PATH-SEARCH start-node goal-node)
  (set! *path*
	(path-search-aux goal-node (list (make-search-node 0 (list start-node))) '())))

(define (PATH-SEARCH-AUX goal open closed)
  (if *debug* (display* "Open = " (length open) " Closed = " (length closed)))
  (if (null? open)			; could not find a path
      '()
      (let ((current (first open)))	; pick first element
	(if *debug* (show-search-node current))
	(cond ((equal? (search-node-head current) goal)	; are we there?
	       ;; Yes, return the path (it is kept reversed)
	       (reverse (search-node-path current)))
	      ((member (vgraph-node-id (search-node-head current)) closed)
	       ;; Been there, done that.
	       (path-search-aux goal (cdr open) closed))
	      (else
	       (path-search-aux 
		goal 
		;; Keep the queue (open list) sorted
		(sort (append (extend-path current closed)
			      (cdr open))
		      (lambda (x y)
			(< (search-node-length x) (search-node-length y))))
		;; add expanded node to expanded (closed) list
		(cons (vgraph-node-id (search-node-head current))
		      closed)))))))

;; Generates the (unexpanded) descendants of the current node
(define (EXTEND-PATH current closed)		      
  (let ((head (search-node-head current)))
    (do ((visible (vgraph-node-visible-nodes head) (cdr visible))
	 (new-nodes '()))
	((null? visible)
	 (cond (*debug*
		(display* "New additions")
		(for-each show-search-node new-nodes)))
	 new-nodes)
      ;; if the visible nodes are not in the closed list, return them
      (if (not (member (vgraph-node-id (first visible)) closed))
	  (set! new-nodes 
		(cons (new-search-node (first visible) current)
		      new-nodes))))))
       
(define (PATH-LENGTH path)
  (do ((p path (cdr p))
       (sum 0.0))
      ((null? (cdr p)) sum)
    (set! sum
	  (+ sum (path-length-increment (first p) (second p))))))

(define (PATH-LENGTH-INCREMENT p1 p2)
  (+
   ;; displacement
   (vmag (vdiff (vertex-pos (vgraph-node-vertex p1))
		(vertex-pos (vgraph-node-vertex p2))))
   ;; rotation, note that *rotation-cost* is arbitrary (units of length/radian).
   (* *rotation-cost* 
      (abs (- (vgraph-node-angle p1) 
	      (vgraph-node-angle p2))))
   ))

;;;---------------------------------------------------------------
;;; Utilities

(define (y-or-n-p . messages)
  (for-each display messages)
  (display "(Y or N) ")
  (let ((answer (read)))
    (cond ((eq? answer 'y) #t)
	  ((eq? answer 'n) #f)
	  (else (y-or-n-p "Please enter Y or N: ")))))

(define (display* . args)
  (for-each display args)
  (newline))

(define (some? fn l)
  (cond ((null? l) #f)
	((fn (car l)) #t)
	(else (some? fn (cdr l)))))

(define (all? fn l)
  (cond ((null? l) #t)
	((fn (car l)) (all? fn (cdr l)))
	(else #f)))

;;;---------------------------------------------------------------
;;; Graphics...

(define (graphics-set-color g c)
  (let ((color (if (string-ci=? microcode-id/operating-system-name "nt")
		   c
		   (rgb->x-string c))))
    (graphics-operation g 'set-foreground-color color)))

(define (rgb->x-string c)
  (list->string (cons #\# (append (int->hex-chars (first c)) 
				  (int->hex-chars (second c))
				  (int->hex-chars (third c))))))

(define (int->hex-chars i)
  (let ((hex-list '(#\0 #\1 #\2 #\3 #\4 #\5 #\6 #\7 
                    #\8 #\9 #\A #\B #\C #\D #\E #\F)))
    (cons (list-ref hex-list (floor->exact (/ i 16)))
	  (list (list-ref hex-list (remainder i 16))))))

(define (int->list i)
  (let ((dec-list '(#\0 #\1 #\2 #\3 #\4 #\5 #\6 #\7 #\8 #\9)))
    (define (helper left)
      (if (= 0 left)
	  '()
	  (append (helper (floor->exact (/ left 10)))
		  (list (list-ref dec-list (remainder left 10))))))
    (let ((res (helper i)))
      (if (null? res)
	  '(#\0)
	  res))))

(define (graphics-create width height)
  (if (string-ci=? microcode-id/operating-system-name "nt")
      (make-graphics-device 'win32 width height 'standard)
      (if (string-ci=? microcode-id/operating-system-name "unix")
	  (make-graphics-device 
	   'x #f (list->string (append (int->list width) (list #\x) (int->list height))) #f)
	  (make-graphics-device #f))))

;;;---------------------------------------------------------------
;;; Examples.

(define MOVING-TRIANGLE
  (make-polygon '((0 0) (10 0) (5 10))))

(define MOVING 
  (make-polygon '((0 0) (10 0) (10 4) (0 4))) )

(define OBSTACLES 
  (map make-polygon
       (list '((5 10) (15 10) (15 20) (5 20))
	     '((5 -15) (15 -15) (15 -5) (5 -5)))))

(define OBSTACLES 
  (map make-polygon
       (list '((8 10) (15 10) (15 20) (8 20))
	     '((8 0) (15 0) (15 5) (8 5))
	     '((8 -15) (15 -15) (15 -5) (8 -5))
	     '((-8 -15) (-5 -15) (-5 150) (-8 150)))))

(define START '(-10 5))
(define GOAL '(20 5))