(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 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)
;;; Searches for the shortest path in the vgraph.
;;; (PATH-SEARCH *vgraph-start-node* *vgraph-goal-node*) 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-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.

(define PI (atan 0 -1))
(define TWOPI (* 2 pi))
(define *DEBUG* #f)
(define *DISPLAY-SIZE* 300.)
(define *DISPLAY-OFFSET* (/ *display-size* 2.0))
(define *DISPLAY-SCALE* 5.0)
(define *DISPLAY?* #t)

(define *WINDOW* #f)
(define *WINDOW-WIDTH* 300)
(define *WINDOW-HEIGHT* 300)
(define *VGRAPH* #f)
(define *VGRAPH-START-NODE* #f)
(define *VGRAPH-GOAL-NODE* #f)

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

(define (MAKE-LINE normal offset) (list normal offset))
(define LINE-NORMAL first)
(define LINE-OFFSET second)

(define (MAKE-VGRAPH-NODE vertex polygon visible-nodes)
  (list vertex polygon visible-nodes))
(define VGRAPH-NODE-VERTEX first)
(define VGRAPH-NODE-POLYGON second)
(define VGRAPH-NODE-VISIBLE-NODES third)

(define (ADD-VISIBLE-NODE! new-vis-node node) 
  (let ((nodes (vgraph-node-visible-nodes node)))
    (cond ((null? nodes)
	   (set-car! (cddr node) (list new-vis-node)))
	  (else
	   (set-cdr! nodes (cons (car nodes) (cdr nodes)))
	   (set-car! nodes new-vis-node))))
  node)

(define X-of first)
(define Y-of second)

(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 (MIN-ANGLE v) (first (vertex-range v)))
(define (TOP-ANGLE v) (second (vertex-range v)))

;;; MAIN FUNCTIONS

(define (FIND-PATH start goal moving obstacles)
  (cond (*display?*
	 (if *window*
	     (clear-window)
	     (open-window))
	 (display-problem start goal moving obstacles)
	 (y-or-n-p "This is the input problem. Continue? ")))
  (let ((c-obstacles (map (lambda (stationary) (co moving stationary))
			  obstacles)))
    (cond (*display?*
	   (graphics-set-color *window* '(0 0 255))
	   (for-each display-polygon c-obstacles)
	   (y-or-n-p "These are the C-space obstacles. Continue? ")))
    (build-vgraph start goal c-obstacles)
    (cond (*display?*
	   (clear-window)
	   (display-problem start goal moving obstacles)
	   (graphics-set-color *window* '(255 0 0))
	   (display-vgraph)
	   (y-or-n-p "This is the Visibility Graph. Continue? ")))
    (let ((path (path-search *vgraph-start-node* *vgraph-goal-node*)))
      (cond (*display?*
	     (display-path path moving obstacles)))
      path)))

(define (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))))

(define (CO-AUX m initial-m s initial-s)
  (cond ((null? m)
	 (if (and initial-m initial-s)
	     (co-aux initial-m '() s initial-s)
	     '()))
	((null? s)
	 (if (and initial-m initial-s)
	     (co-aux m initial-m initial-s '())
	     '()))
	((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))
	(else
	 (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))
		     (else
		      (co-aux (cdr m) initial-m (cdr s) initial-s)))))))

(define (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))

(define (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))

(define (SUM-VERTS v1 v2)
  (make-trivial-vertex (map + (vertex-pos v1) (vertex-pos v2))))

(define (NEGATE-POLYGON polygon)
  (fill-in-angles
   (map (lambda (v) (make-trivial-vertex (map - (vertex-pos v))))
	polygon)))

(define (FILL-IN-ANGLES polygon)
  (let ((l (length polygon)))
    (sort (map (lambda (prev v next)
		 ;;(display (list prev v next))
		 (make-vertex (car v)
			      (list (edge-angle prev v)
				    (edge-angle v next))))

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

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

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

;;; Basic angle hacking (mod twopi) 

(define (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)))

(define (NORM-TWOPI x)
  (cond ((< x 0.0) (norm-twopi (+ x twopi)))
	((> x twopi) (norm-twopi (- x twopi)))
	(else x)))

(define (ATAN-2PI a b)
  "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))))

(define (MAKE-POLYGON verts)
  (fill-in-angles (map (lambda (v) (make-trivial-vertex v)) verts)))

(define (BUILD-VGRAPH start goal obstacles)
  (set! *vgraph* (build-vgraph-aux start goal obstacles))
  'done)

(define (BUILD-VGRAPH-AUX start goal obstacles)
  (let* ((start-vertex (make-trivial-vertex start))
	 (goal-vertex  (make-trivial-vertex goal))
	 (start-V-node (make-vgraph-node start-vertex '() '()))
	 (goal-V-node (make-vgraph-node goal-vertex '() '()))
	 (vgraph-nodes '()))
    (set! *vgraph-start-node* start-v-node)
    (set! *vgraph-goal-node* goal-v-node)
    (if (v-edge-crosses-some-polygon? start-v-node goal-v-node obstacles)
 	(begin 	 
	 ;; Create all the possible vgraph nodes 
	 (set! vgraph-nodes (cons start-v-node vgraph-nodes))
	 (set! vgraph-nodes (cons goal-v-node vgraph-nodes))
	 (for-each
	  (lambda (poly)
	    (for-each (lambda (vert)
			(set! vgraph-nodes 
			      (cons (make-vgraph-node vert poly '()) 
				    vgraph-nodes)))
		      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)
		     (cond ((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))))
			    (add-visible-node! v2 v1)
			    (add-visible-node! v1 v2))))
		 ))))
	 vgraph-nodes)
	;; Trivial case, straight line between start and goal works.
	(list (make-vgraph-node start-vertex '() (list goal-v-node))
	      (make-vgraph-node goal-vertex '() (list start-v-node))))
    ))

(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)))))
  
(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)))
    (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 point out of the polygons.
	   #f)
	  ((or (edge-inside-polygon? vertex-1 vertex-2 obst)
	       (some-edge-crosses? vertex-1 vertex-2
				   (cons (car (last obst)) obst)
				   obst))
	   (if *debug*
	       (format #t "~%  Crosses polygon ~s" obst))
	   #t)
	  (else #f))))

(define (V-EDGE-CROSSES-SOME-POLYGON? v-node-1 v-node-2 obstacles)

  (define (loop obsts)
    (cond ((null? obsts) #f)
	  ((v-edge-crosses-polygon? v-node-1 v-node-2 (car obsts)) #t)
	  (else
	   (loop (cdr obsts)))))

  (if *debug*
      (format #t "~% Testing ~s : ~s for crossings." 
	      (vertex-pos (vgraph-node-vertex v-node-1)) 
	      (vertex-pos (vgraph-node-vertex v-node-2))))

  (loop obstacles))

(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*
	(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)))

(define (ANGLE-INSIDE-RANGE angle vertex)
  (if (null? (vertex-range vertex))
      #t
      (and (>= (angle-diff angle (min-angle vertex)) -0.001)
	   (<= (angle-diff angle (top-angle vertex)) 0.001))))

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

(define (VERTEX-IN-POLYGON? vertex poly)
  (let ((x (x-of (vertex-pos vertex)))
	(y (y-of (vertex-pos vertex)))
	(inside #f))
    (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)))))
	       (set! inside (not inside))))))
    inside))

(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 ((null? pt) #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)))))

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

(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
	      '()))
	      )))

(define (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))))

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

(define (PRINT-VGRAPH)
  (for-each
   (lambda (vn)
     (format #t "~% Node at ~s:" (vertex-pos (vgraph-node-vertex vn)))
     (for-each 
      (lambda (visible)
	(format #t "~%  Can see ~s:" (vertex-pos (vgraph-node-vertex visible))))
      (vgraph-node-visible-nodes vn)))
   *vgraph*))

;;; Display 

(define (OPEN-WINDOW)
  (set! *window* (graphics-create *window-width* *window-height*))
  (graphics-set-coordinate-limits *window* 0 0 *display-size* *display-size*)
  )
    
(define (DISPLAY-POLYGON polygon)
  (define (draw-polygon window polygon)
    ;; 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 polygon)) (butlast polygon))
     polygon
     ))
  (draw-polygon *window* (map transform (map vertex-pos polygon))))

(define (DISPLAY-VGRAPH)
  (for-each 
   (lambda (vn)
     (let ((vertex (transform (vertex-pos (vgraph-node-vertex vn)))))
       (for-each
	(lambda (visible)
	  (let ((visible-vertex (transform (vertex-pos (vgraph-node-vertex visible)))))
	    (graphics-draw-line *window* 
				(x-of vertex) (y-of vertex)
				(x-of visible-vertex) (y-of visible-vertex))))
	(vgraph-node-visible-nodes vn))))
   *vgraph*))

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

(define (CLEAR-WINDOW)
  (graphics-clear *window*)
  (graphics-set-color *window* '(0 0 0)))

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

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

(define (OFFSET-POLYGON polygon pos)
  (map (lambda (v) (make-vertex (map + (vertex-pos v) pos)
				(vertex-range v)))
       polygon))

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

  (do ((p path (cdr p)))
      ((null? p))

    (graphics-set-color *window* '(0 255 255))
    (display-polygon (offset-polygon moving (first p)))

    (if (cdr p)
	(let ((p1 (transform (first p)))
	      (p2 (transform (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))
	  ))
    ))

;;; SIMPLE PATH SEARCH

(define (PATH-SEARCH start-node goal-node)
  (path-search-aux goal-node (list (list start-node)) '()))

(define (PATH-SEARCH-AUX goal open closed)
  (if (null? open)
      '()
      (let ((current (first open)))
	(if (eq? (first current) goal)
	    (map (lambda (vn) (vertex-pos (vgraph-node-vertex vn)))
		 (reverse current))
	    (path-search-aux goal 
			     (sort (append (extend-path current closed)
					   (cdr open))
				   (lambda (x y)
				     (< (path-length x) (path-length y))))
			     (cons (first current) closed))))))

(define (EXTEND-PATH current closed)
  (let ((head (first current)))
  (do ((visible (vgraph-node-visible-nodes head) (cdr visible))
       (new-nodes '()))
      ((null? visible) new-nodes)
    (if (not (member (first visible) closed))
	(set! new-nodes (cons (cons (first visible) current) new-nodes))))))
       
(define (PATH-LENGTH path)
  (do ((p path (cdr p))
       (sum 0.0))
      ((null? (cdr p)) sum)
    (set! sum
	  (+ sum
	     (vmag (vdiff (vertex-pos (vgraph-node-vertex (first p)))
			  (vertex-pos (vgraph-node-vertex (second p)))))))))

;;; A simple example.

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

(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 '((5 10) (15 10) (15 20) (5 20))
	     '((5 0) (15 0) (15 5) (5 5))
	     '((5 -15) (15 -15) (15 -5) (5 -5)))))

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

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