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

;;; Present features:
;;;  Turns in circular arcs.
;;;  Car runs on right-hand side of the road.
;;;  Parking
;;;  Slows down realistically when turning
;;;
;;; Future:
;;;  Multiple cars
;;;  Reversing
;;;  Changing speed
;;;  More commands

;; SIMULATION PARAMETERS. All values should be given in SI-units
(setq turnradius      4.7);; must be larger than right-offset
(setq turnspeed       2);; the speed at which a car traverses turns 
(setq acceleration    10)
(setq decceleration   -10)
(setq right-offset    1.5);; how far to the right-of-middle the cars should drive
(setq car-length      3);; how far it is between the wheel-axes of a car
(setq time-slice      0.01);; how long is a sample period

;;;; 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 noslow), (GTR L1) or (GTS L5) which means turn left 
  ;; when at L2 without slowing down for the turn, slow down to 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
  
  (if (null out) (error "No output file specified. <op-state> "))

  (setq current-time (car    state))
  (setq current-ray  (cadr   state))	; the name of the ray the car is currently following. 
  (setq car-pos      (caddr  state))	; returns how far from the ray's origin the car is
  (setq velocity     (cadddr state))
  (setq speed        (abs velocity))
  (setq direction    (sign velocity))
  
  (setq next-command     (car op))	; returns the next command in the plan
  (setq name-of-next-ray (cadr op))	; returns the name of the next ray that is in the plan
  
  (setq rnow  (linoray current-ray))	; 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 turnrad	(cond ((eq next-command 'gtl) (+ turnradius right-offset))
		      ((eq next-command '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 next-command 'gtl) (plusp  delta-phi)) T)
	      ((and (eq next-command 'gtr) (minusp delta-phi)) T)
	      (T nil)))
  (setq newvel (* velocity (if samevel 1 -1)))
  (setq angle-to-turn 
	(let* ((delta-phi  (normalize-angle (- (angle rnext) (angle rnow))))
	       (delta-comp (normalize-angle (- pi delta-phi))))
	  (cond ((plusp delta-phi)
		 (cond ((eq next-command 'gtl) delta-phi)
		       ((eq next-command 'gtr) delta-comp)
		       (T 0)))
		(T
		 (cond ((eq next-command 'gtl) (- delta-comp))
		       ((eq next-command 'gtr) (- delta-phi))
		       (T 0))))))
  (setq turnallow
	;; how far from the crossing point the turning action should start
	(cond ((eq next-command 'gts) 0)
	      ((or (eq next-command 'gtl) 
		   (eq next-command '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 the respective ray.   
	(list (find-turn-start (car  cross) turnallow velocity)
	      (find-turn-end   (cadr cross) turnallow newvel)))
  (setq carangle 
	;; the angle of the car (in radians)
	(if (plusp velocity) 
	    (angle rnow)
	  ;; else
	  (normalize-angle (+ (angle rnow) pi))))

  
  ;;; Calculate the straight-part motion
  
  (setq delta-vel       (abs (- speed turnspeed)))
  (setq delta-vel-brake (if (eq next-command 'gts) speed delta-vel))
  (setq durn1a
	;; seconds needed to accelerate from turnspeed to the state's given velocity
	(abs (/ delta-vel acceleration)))
  (setq durn1b  
	;; seconds needed to deccelerate from the velocity of the state to that of turnspeed
	(abs (/ delta-vel-brake decceleration)))
  (setq durn1c  
	;; time to traverse the constant velocity-part of the straight ray
	(let ((length-tot   (abs (- (car crmod) car-pos)))
	      (acc-length   (+ (* turnspeed durn1a) (* acceleration  durn1a durn1a 0.5)))
	      (brake-length (- (* speed     durn1b) (* decceleration durn1b durn1b 0.5))))
	  (/ (- length-tot acc-length brake-length) speed)))
  (setq durn1slow;; time to traverse the straight part of the ray at the velocity of turnspeed
	(abs (/  (- (car crmod) car-pos)  turnspeed)))

  ;; Convert all the above times to hundredparts of seconds (i.e. multiply by 100 and round off)
  (setq start-time        (roundhund current-time))
  (setq acc-time          (roundhund durn1a))
  (setq const-vel-time    (roundhund durn1c))
  (setq brake-time        (roundhund durn1b))
  (setq slow-time         (roundhund durn1slow))


  (cond ((plusp const-vel-time)

	 (setq da-acc   (/ delta-vel acc-time));; (the constant) acceleration per timestep during accel-phase
	 (setq da-brake (- (/ delta-vel brake-time)));; (the constant) acceleration per timestep during braking-phase
	 (setq position car-pos)
(princ	 (setq time-now start-time))
	 (setq carangle-deg (rad-to-deg carangle))
	 (setq offset       (if (parkp current-ray) 0 right-offset))
	 (setq current-speed (* direction turnspeed))
	 
	 (dotimes (j acc-time)
		  
		 (princ (setq time-now      (+ time-now 1)))(terpri)
		  (setq displacement  (calc-displacement current-speed acceleration)) 
		  (setq position      (+ position (* direction displacement)))
		  (setq xy-pos-on-ray (raypoint rnow position))
		  (setq xy-pos        (implement-right-hand-traffic xy-pos-on-ray carangle offset))
		  (setq current-speed (+ current-speed (/ acceleration time-slice)))
		  
		  (print-position-to-file xy-pos carangle-deg time-now out out2))


	 (dotimes (j const-vel-time)
		  
		 (princ (setq time-now      (+ time-now 1)))(terpri)
		  (setq displacement  (calc-displacement current-speed 0))
		  (setq position      (+ position (* direction displacement)))
		  (setq xy-pos-on-ray (raypoint rnow position))
		  (setq xy-pos        (implement-right-hand-traffic xy-pos-on-ray carangle offset))

		  (print-position-to-file xy-pos carangle-deg time-now out out2))


	 (dotimes (j brake-time)

		  (princ (setq time-now      (+ time-now 1)))(terpri)
		  (setq displacement  (calc-displacement current-speed da-brake)) 
		  (setq position      (+ position (* direction displacement)))
		  (setq xy-pos-on-ray (raypoint rnow position))
		  (setq xy-pos        (implement-right-hand-traffic xy-pos-on-ray carangle offset))
		  (setq current-speed (+ current-speed (/ decceleration time-slice))))

	 (print-position-to-file xy-pos carangle-deg time-now out out2))
      
	;; else
	
	(T 
	 (dotimes (j slow-time)
		  
	(princ	  (setq time-now      (+ time-now 1)))(terpri)
		  (setq displacement  (calc-displacement turnspeed 0))
		  (setq position      (+ position (* direction displacement)))
		  (setq xy-pos-on-ray (raypoint rnow position))
		  (setq xy-pos        (implement-right-hand-traffic xy-pos-on-ray carangle offset))
		  
		  (print-position-to-file xy-pos carangle-deg time-now out out2))))
  
   
  ;; End of straight-part motion. Now calculate the curve motion


  (setq durn2 
	;; time to traverse the curve
	(let ((turnspeed (cond ((or (eq (flag op) 'noslow) 
				    (< (abs angle-to-turn) 0.3))  velocity)
			       (T turnspeed))))
	  (cond ((eq next-command 'gts) 0)
		(T (abs (/ (* turnrad angle-to-turn) turnspeed))))))
  (setq curve-end-time    (roundhund durn2))
  
  
  (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
	       ;; If parking: how much to successively reduce turn-ray 
	       ;; by to make the car hit the parking ray 
	       (if (parkp current-ray) 
		   (/ right-offset curve-end-time)
		 ;; else
		 0))
	 
	 (setq circle-ray 
	       ;; A ray that goes through both the starting point of the turn 
	       ;; and the centre point of the circle that the turn follows.
	       ;; It is used to find the centre point of the circle.
	       (list (cond ((eq next-command 'gtl) (+ carangle pi/2))
			   ((eq next-command 'gtr) (- carangle pi/2))
			   (T carangle))
		     (car  turn-startpoint)
		     (cadr turn-startpoint)))
	 
	 (setq turn-center (raypoint circle-ray turnrad))
	 
	 (setq comp-angle (asin (/ car-length 2 turnrad)))
	 (princ comp-angle) (terpri)
	 
	 (dotimes (i curve-end-time)
		  (setq time-now (+ time-now 1))		  
		  (setq turn-ray
			;; ray from centre of turning circle to where the car is supposed to be
			(list (cond 
			       ((eq next-command 'gtl) (+ (angle circle-ray) pi (* (+ i 1) omega)))
			       ((eq next-command '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-on-circle (raypoint turn-ray (if (eq next-command 'gtl)
								(- turnrad (* (+ i 1) pull-in))
							      ;;else
							      (+ turnrad (* (+ i 1) pull-in)))))
		  
		  ;; below is newly added
		  
		  (setq comp-ray
			;; to make the car keep two points on the circle instead of just one
			(list (cond ((eq next-command 'gtl) (+ (angle turn-ray) pi/2 (- comp-angle)))
				    ((eq next-command 'gtr) (+ (angle turn-ray) (- pi/2) comp-angle))
				    (T (error "Unknown command in plan. <comp-ray> ")))
			      (car xy-pos-on-circle)
			      (cadr xy-pos-on-circle)))
		  
		  
		  (setq xy-pos (raypoint comp-ray (/ car-length 2)))
		  
		  ;;update angle
		  (setq new-angle
			(let ((direction (if (plusp velocity) 
					     (angle rnow) 
					   ;;else
					   (normalize-angle (+ (angle rnow) pi)))))
			  (cond ((eq next-command 'gtr) (- direction (* (+ i 1) omega)))
				((eq next-command 'gtl) (+ direction (* (+ i 1) omega)))
				(T direction))))
		  (print-position-to-file xy-pos (rad-to-deg new-angle) time-now out out2))))
  
  (terpri)
  
  (list (/ time-now 100)
	name-of-next-ray
	(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 flag         (op-list) (caddr  op-list)) ; recognized flags: noslow
(defun parkp (ray-name) (if (equal (char (string ray-name) 0) #\P) T nil))

(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 calc-displacement (vel acc)
  ;; Returns s = v*t + a*t^2/2, where t = time in seconds
  ;; Negative acc gives, quite naturally, a braking effekt.
  (+ (abs (* vel time-slice)) (* acc time-slice time-slice 0.5)))

(defun implement-right-hand-traffic (xy-pos-on-ray carangle offset)
  ;; Returns the car's position after adjusting it offset metres to the right
  ;; of the middle of the road.
  (let ((right-ray (list 
		    (- carangle pi/2)
		    (car  xy-pos-on-ray) 
		    (cadr xy-pos-on-ray))))
    (raypoint right-ray offset)))

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


;;;; PRINTING

(defun prin-twodec (n out)
  ;; prints n/100 with two decimals to file out
  (prog (s a b c)
	(if (minusp n) (error "prin-twodec-error"))
	(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)))

(defun print-position-to-file (xy-pos carangle-deg time out out2)
  ;; print new position (in cartesian coords and degrees) to file
  (princ " Value - " out) 
  (prin-twodec time out)    (princ " Car[0] " out)
  (princ (car  xy-pos) out) (princ " "   out)
  (princ (cadr xy-pos) out) (princ " 0 " out) 
  (princ carangle-deg out)  (princ " 0 0 " out) (terpri out)
  
  (unless (null out2) 
	  (princ " Value - " out2) 
	  (prin-twodec time out2)    (princ " Helicopter[0] " out2) 
	  (princ (car xy-pos) out2)  (princ " " out2) 
	  (princ (cadr xy-pos) out2) (princ " 0 " out2) 
	  (princ carangle-deg out2)  (princ " 0 0 " out2) (terpri out2)) )


;;;; 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)
  ;; returns 100*s, rounded to no decimals
  (round (* s 100)))


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












