;;;; /info/www/ext/epa/mater/int/foo.lisp
;;;; This program file used for emerging simulation of automobile movements

;;; Features:
;;;  Turns in circular arcs.
;;;  Car runs on right-hand side of the road.
;;;  Parking
;;;
;;; Future:
;;;  Slow down realistically when turning
;;;  Multiple cars
;;;  Reversing

(setq turnradius      5)
(setq turnspeed      10) ;; the (main) speed at which a car traverses turns
(setq brake-distance 10) ;; how far from a turn a car begins to deccelerate
(setq acceleration   10)
(setq right-offset   1.5) ;; used to make cars drive on the right-hand side of the road

;;;; MAIN PROGRAM

(defun op-state (op state out out2) 
  ;; car in current state, apply op, return new state
  ;; state is (time  ray-name  distance-to-start-of-ray  velocity),
  ;; E.g. (23 L8 20 30) is a state.
  ;; op is e.g. (GTL L2 park), (GTR L1) or (GTS L5) which means turn left into a 
  ;; parking lot when at L2 (should be followed by a gts command), turn right 
  ;; when at L1 and come to a stop at L5, respectively.
  ;; 
  ;; If out is non-NIL then it is a file onto which to print the
  ;; successive positions
  
  (setq rnow  (linoray (current-ray state))) ; current ray as (angle x0 y0)
  (setq rnext (linoray (next-ray op)))       ; next event-generating ray as (angle x0 y0)
  (setq cross (crossing rnow rnext))         ; where on each ray does the other intersect? E.g. (22 34)
  (setq delta-phi (normalize-angle (- (angle rnext) (angle rnow))))
  (setq turnrad 
	(cond ((eq (plan op) 'gtl) (+ turnradius right-offset))
	      ((eq (plan op) 'gtr) (- turnradius right-offset))
	      (T 0)))
  (setq samevel
	;; T if the velocity on the next ray is to be positive compared to present velocity
	(cond ((and (eq (plan op) 'gtl) (plusp  delta-phi)) T)
	      ((and (eq (plan op) 'gtr) (minusp delta-phi)) T)
	      (T nil)))
  (setq newvel (* (velocity state) (if samevel 1 -1)))
  (setq angle-to-turn 
	(let ((delta-comp (normalize-angle (- pi delta-phi))))
	  (cond ((plusp delta-phi)
		 (cond ((eq (plan op) 'gtl) delta-phi)
		       ((eq (plan op) 'gtr) delta-comp)
		       (T 0)))
		((minusp delta-phi)
		 (cond ((eq (plan op) 'gtl) (- delta-comp))
		       ((eq (plan op) 'gtr) (- delta-phi))
		       (T 0)))
		(T (error "Error in angle-to-turn. ")))))
  (setq turnallow
	;; how far from the crossing point the turning action should start
	(cond ((eq (plan op) 'gts) 0)
	      ((or (eq (plan op) 'gtl) (eq (plan op) 'gtr)) 
	       (abs (* turnradius (tan (/ angle-to-turn 2)))))
	      (T (error "Unknown command in plan. <turnallow> "))))
  (setq crmod 
	;; crmod is a list containing the start and end points of the turn, as measured 
	;; from the beginning of each ray.   
	(list (find-turn-start (car  cross) turnallow (velocity state))
	      (find-turn-end   (cadr cross) turnallow newvel)))
  (setq durn1 
	;; how long it will take to traverse the straight part of the ray
	(abs (/  (- (car crmod) (car-pos state))   (velocity state))))
  
  (setq durn2 
	;; how long it will take to traverse the curve
	(let ((turnspeed (if (eq (flag op) 'noslow) (velocity state) turnspeed)))
	  (cond ((eq (plan op) 'gts) 0)
		((or (eq (plan op) 'gtr) (eq (plan op) 'gtl))
		 (abs (/ (* turnrad angle-to-turn) turnspeed)))
		(T 0))))
  
  (setq start-time        (roundhund (current-time state)))
  (setq straight-end-time (roundhund durn1))
  (setq curve-end-time    (roundhund durn2))
   
  (princ "State:              ") (princ state) (terpri)
  (princ "Straight-part time: ") (princ straight-end-time) (terpri) 
  (princ "curve-end-time:     ") (princ curve-end-time) (terpri)(terpri)

  (unless (null out)		


	  ;;; calculate the straight part motion
	  (cond ((plusp straight-end-time)
		 (dotimes (j straight-end-time)
			  (princ " Value - " out)
			  (prin-twodec (+ start-time j 1) out) (princ " Car[0] " out)
			  
			  ;; update position
			  ;; right-ray and right-offset are used to implement driving 
			  ;; on the right-hand side of the road.
			  (setq xy-pos-on-ray (raypoint rnow 
							(+ (car-pos state)    (* (velocity state) (+ j 1) 0.01))))
			  (setq right-ray (list (- (carangle state rnow) pi/2) 
						(car  xy-pos-on-ray) 
						(cadr xy-pos-on-ray)))
			  (setq xy-pos (raypoint right-ray right-offset)))
			  
			  ;; print new position (in cartesian coords and degrees) to file
			  (princ (car  xy-pos) out) (princ " "   out)
			  (princ (cadr xy-pos) out) (princ " 0 " out)
			  (princ (rad-to-deg (carangle state rnow)) out) (princ " 0 0 " out) (terpri out)
			  
			  (unless (null out2) 
				  (princ " Value - " out2) (prin-twodec (+ start-time j 1) out2)
				  (princ " Helicopter[0] " out2) (princ (car xy-pos) out2)
				  (princ " " out2) (princ (cadr xy-pos) out2)
				  (princ " 0 " out2) (princ (rad-to-deg (carangle state rnow)) out2)
				  (princ " 0 0 " out2) (terpri out2)))))

	  
	  ;;; calculate the curve motion
	  (cond ((plusp curve-end-time)
		 (setq omega  
		       ;; the angular velocity of the turning car
		       (cond ((plusp curve-end-time) (/ angle-to-turn curve-end-time))
			     (T 0)))
		 (setq turn-startpoint (raypoint rnow (car crmod)))
		 (setq pull-in
		       ;; how much to successively reduce the turn-ray below by to make 
		       ;; the car hit the parking ray when parking.
		       (if (eq (flag op) 'park)
			   (/ right-offset curve-end-time)
			 ;; else
			 0))
		 
		 (princ pull-in) (terpri)
		 (dotimes (i curve-end-time)
			  (princ " Value - " out) 
			  (prin-twodec (+ start-time straight-end-time i 1) out) (princ " Car[0] " out)
		   

			  ;; update position
			  (setq circle-ray (list (cond ((eq (plan op) 'gtl) (+ (carangle state rnow) pi/2))
						       ((eq (plan op) 'gtr) (- (carangle state rnow) pi/2))
						       (T (carangle state rnow)))
						 (car  (raypoint rnow (car crmod)))
						 (cadr (raypoint rnow (car crmod)))))
			  (setq turn-center (raypoint circle-ray turnradius))
			  (setq turn-ray (list (cond ((eq (plan op) 'gtl) 
						        (+ (angle circle-ray) pi (* (+ i 1) omega)))
						     ((eq (plan op) 'gtr) 
						        (- (angle circle-ray) pi (* (+ i 1) omega)))
						     (T (error "Unknown command in plan. <turn-ray> ")))
					       (car  turn-center) 
					       (cadr turn-center)))
			  (setq xy-pos (raypoint turn-ray (- turnrad (* (+ i 1) pull-in))))
			  
			  
			  ;;update angle
			  (setq new-angle
				(let ((direction (if (plusp (velocity state)) 
						     (angle rnow) 
						   ;;else
						   (normalize-angle (+ (angle rnow) pi)))))
				  (cond ((eq (plan op) 'gtr) (- direction (* (+ i 1) omega)))
					((eq (plan op) 'gtl) (+ direction (* (+ i 1) omega)))
					(T direction))))
			  

			  ;; print new position (in Cartesian coords and degrees) to file
			  (princ (car  xy-pos) out) (princ " "   out)
			  (princ (cadr xy-pos) out) (princ " 0 " out)
			  (princ  (rad-to-deg new-angle) out)
			  (princ " 0 0 " out)
			  (terpri out)
			  
			  (cond (out2 
				 (princ " Value - " out2) (prin-twodec (+ start-time straight-end-time i 1) out2)
				 (princ " Helicopter[0] " out2) (princ (car xy-pos) out2)
				 (princ " " out2) (princ (cadr xy-pos) out2)
				 (princ " 0 " out2) (princ (rad-to-deg (carangle state rnow)) out2)
				 (princ " 0 0 " out2) (terpri out2)))))))
		
	  (list (+ (current-time state) durn1 durn2)
		(next-ray op)
		(cadr crmod) ;; starting point on the new ray
		newvel))
  

;;; Some global selectors
(defun angle (ray)
  ;; returns the angle of the ray, -pi < angle < pi
  (if (and (< (car ray) pi)
	   (> (car ray) (- pi)))
      (car ray)
    ;; else
    (normalize-angle (car ray))))
(defun x0           (ray)     (cadr   ray))
(defun y0           (ray)     (caddr  ray))
(defun current-time (state)   (car    state))
(defun current-ray  (state)   (cadr   state))   ; returns the name of the ray the car is currently following. 
(defun car-pos      (state)   (caddr  state))   ; returns how far from the ray's origin the car is
(defun velocity     (state)   (cadddr state))
(defun plan         (op-list) (car    op-list)) ; returns the next command in the plan
(defun next-ray     (op-list) (cadr   op-list)) ; returns the name of the next ray that is in the plan
(defun flag         (op-list) (caddr  op-list)) ; recognized flags: noslow

(defun normalize-angle (phi)
  ;; help-function for the angle-selector. Returns the angle that 
  ;; corresponds to phi in the interval -pi to pi.
  (cond ((> phi pi)     (normalize-angle (- phi 2pi)))
	((< phi (- pi)) (normalize-angle (+ phi 2pi)))
	(T phi)))

(defun carangle (state rnow)
  ;; returns the angle of the car (in radians)
  (if (plusp (velocity state)) 
      (angle rnow)
    ;; else
    (+ (angle rnow) pi)))

(defun find-turn-start (cross-point turnallow vel) (- cross-point (* turnallow (sign vel))))
(defun find-turn-end   (cross-point turnallow vel) (+ cross-point (* turnallow (sign vel))))

(defun linoray (ray-name)
  ;; returns a list of (angle x0 y0) for a line
  (let ((s (load-adl (concatenate 'string (docdir) "!" (string ray-name) ".adl"))))
    (list (normalize-angle (getassoc s 'phi))
	  (getassoc s 'xo)
	  (getassoc s 'yo)
	  )))

(defun raypoint (ray d)
  ;; expresses a point distance d along a ray (angle x0 y0), in Cartesian coords
  (list (+ (x0 ray) (* d (cos (angle ray))))
	(+ (y0 ray) (* d (sin (angle ray))))))

(defun linpoint (name d)
  ;; expresses a point distance d along ray "name" (e.g. L20), in Cartesian coords
  (raypoint (linoray name) d))

(defun crossing (r1 r2) 
  ;; returns a list of distances e and g, characterizing the crossing 
  ;; between the rays r1 and r2. It is at distance e along r1 and at 
  ;; distance g along r2
  (solveq (transp (list (cossin (angle r1)) 
			(negvec (cossin (angle r2)))))
	  (subvec (cdr r2) (cdr r1))))

(defun keepsignp (ray1 ray2)
  ;; returns T if travel on positive direction on ray1, followed by left
  ;; turn onto ray2, results in travel in positive direction on ray2
  (if (plusp (- (angle ray2) (angle ray1)))
      T nil))


(defop 'delta
  '(princ (normalize-angle (- (angle (linoray (read))) (angle (linoray (read)))))))

(defop 'ks 
  '(block ks
    (if (keepsignp (linoray (read)) (linoray (read)))
	(princ "Keeps sign")  
      ;;else
      (princ "Reverses sign"))
    (terpri)))

;;;; MATH FUNCTIONS

(setq 2pi (* 2 pi))
(setq pi/2 (/ pi 2))

(defun rad-to-deg (phi) (* (/ phi pi) 180)) 

(defun sign (x)
  (cond ((< x 0) -1)
	((= x 0)  0)
	(T 1)))

(defun solveq (m a)
  ;; Returns the vector x that solves mx = a, where m is a 2x2 matrix and
  ;; a a 1x2 vector. The solution is obtained using Cramer's rule.
  (let ((d (determ m)))
    (if (= d 0) (error "Determinant 0"))
    (list (/ (determ (repcar  m a)) d)
	  (/ (determ (repcadr m a)) d))))

(defun determ (m) 
  ;; calculate the determinant of the 2x2 matrix m
    (- (* (caar  m) (cadadr m))
       (* (cadar m) (caadr  m))))

(defun repcar (m a) 
  ;; replace first column in m by a
  (list (cons (car a)
	      (cdar m))
         (cons (cadr a)
	       (cdadr m))))

(defun repcadr (m a) 
  ;; replace second column in m by a
  (list (list (caar m) 
	      (car a))
	(list (caadr m)
	      (cadr a))))

(defun transp (m)
;;; m is a 2x2 matrix
;;; returns the transponate of m
  (list (list (caar m)
	      (caadr m))
	(list (cadar m)
	      (cadadr m))))

(defun sincos (phi)
  (list (sin phi)
	(cos phi)))

(defun cossin (phi)
  (list (cos phi)
	(sin phi)))

(defun negvec (a)
  (list (- (car a)) 
	(- (cadr a))))

(defun subvec(a b)
  (list (- (car a)  (car b)) 
	(- (cadr a) (cadr b))))

(defun cotan (x)
  (/ 1 (tan x)))

;;; Do vectors and matrices in two dimensions
;;; A vector is a list of two numbers, v = (1 2)
;;; A 2x2 matrix is a list of vectors, m = ((1 2) (3 4))

(defun vec+vec (a b)
  (list (+ (car a)
	   (car b))
	(+ (cadr a)
	   (cadr b))))

(defun vec*vec (a b)
  (+ (* (car a)
	(car b))
     (* (cadr a)
	(cadr b))))

(defun mat*vec (m a)
  (list (vec*vec (car m) a)
	(vec*vec (cadr m) a)))

(defun roundhund (s)
  (round (* s 100)))


;;;; PRINTING

(defun prin-twodec (n out)
  (prog (s a b c)
	(cond ((minusp n) 
	       (setq s "-") 
	       (setq s (- s)))
	      (T (setq s "")))
	(setq a (truncate (/ n 100)))
	(princ a out)
	(setq b (- n (* a 100)))   ; the rest
	(cond ((= b 0) (return))
	      ((< b 10) (princ ".0" out) (princ b out) (return)))
	(princ "." out)
	(setq c (truncate (/ b 10)))
	(cond ((= b (* c 10)) 
	       (princ c out)
	       (return)))
	(princ b out)))


;;;; DEFOPS

(defop 'sts 
;;; start simulation
  '(progn
     (setq *state* (cons 0 (getpa *curag* '|document| 'STARTCAR)))
     (setq *hist* nil) 
     ))

(defop 'xss 
;;; execute simulation step
  '(prog (op)
	 (setq op (read))  
	 (setq *hist* (cons op *hist*))
	 (setq *oldstate* *state*)
	 (setq *state* (op-state op *state* nil nil))
	 ))

(defop 'xsp 
;;; execute simulation step and print
  '(prog (op out)
	 (setq out (concatenate 'string (docdir) "cars.vtr"))
	 (princ "Opened output file: ") (princ out) (terpri)
	 (setq out (open out :direction :output))
	 (setq op (read))  
	 (setq *hist* (cons op *hist*))
	 (setq *oldstate* *state*)
	 (setq *state* (op-state op *state* out nil))
	 (close out)  
	 ))

(defop 'spf 
;;; simulate plan to file
;;; getpa returns a list of the plan, e.g. ((GTL L20 noslow) (GTS L8))
  '(prog (op out1 out2)
	 (setq *state* (cons 0 (getpa *curag* '|document| 'STARTCAR)))
	 (setq *hist* nil)
	 (setq out1 (concatenate 'string (docdir) "cars.vtr"))
	 (princ out1) 
	 (terpri)
	 (setq out2 (concatenate 'string (docdir) "traject.vtr"))
	 (setq out1 (open out1 :direction :output))
	 (setq out2 (open out2 :direction :output))
	 (dolist (op (getpa *curag* '|document| 'plan))	
		 (setq *hist* (cons op *hist*))
		 (setq *oldstate* *state*)
		 (setq *state* (op-state op *state* out1 out2)))
	 (close out1)
	 (close out2) 
	 ))

(defop 'spwor   
;;; simulate plan but without rendering
  '(dolist (x '(helcam fixcam gtr0 gtr1))
	   (princ "-------------------- ")
	   (princ x)
	   (princ " --------------------")
	   (terpri)
	   (eval (get x 'exdef))
	   (terpri) 
	   (terpri)
	   ))

(defop 'spr   
;;; simulate plan with rendering
  '(dolist (x '(helcam fixcam gtr0 gtr1
		       gims0 cpc0 gmpc0 gmp0
		       gims1 cpc1 gmpc1 gmp1))
	   (princ "-------------------- ")
	   (princ x)
	   (princ " --------------------")
	   (terpri)
	   (eval (get x 'exdef))
	   (terpri) 
	   (terpri)
	   ))

(defop 'bus  
;;; backup simulation step (not for repeated use)
  '(progn
     (setq *state* *oldstate*)
     (setq *hist* (cdr *hist*))
     )) 

(defop 'dss  
;;; display simulation state
  '(prog ()
	 (princ "Time:     ") 
	 (princ (car *state*)) 
	 (terpri)
	 
	 (princ "Postion:  ") 
	 (princ (cadr *state*)) 
	 (princ " ")
	 (princ (caddr *state*)) 
	 (terpri)
	 
	 (princ "Velocity: ") 
	 (princ (cadddr *state*)) 
	 (terpri)
	 
	 (princ "Coordin:  ")
	 (princ (linpoint (cadr *state*) 
			  (cadd *state*)))
	 (terpri) 
	 (terpri)
	 ))

(defop 'dsh  
;;; display simulation history
  '(dolist (x (reverse *hist*))
	   (princ "     ") 
	   (princ x) 
	   (terpri)
	   ))

