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

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


;;;; HELICOPTER SIMULATION PARAMETERS
(setq *heli-state* '(0 200 700 60 0 0 0))	; (time x y z angle p r)
(setq *point-list* '())			; A list of the crossing points and times when cars are at them
(setq *close-val*  20)			; How close is close enough?
(setq *F*  10)				; Rotor lift
(setq omega 10)
(setq dw   (* omega *sample-time*))
(setq dp   0.01)
(setq dr   dp)
(setq maxp 0.3)
(setq maxr (/ maxp 2))

(defun h-angle () (normalize (car (cddddr *heli-state*))))
(defun x       () (cadr  *heli-state*))
(defun y       () (caddr *heli-state*))
(defun p       () (cadr  (cddddr *heli-state*)))
(defun r       () (caddr (cddddr *heli-state*)))

(defun angle-to-point (point) (let ((x (car point)) (y (cadr point))) (atan (/ y x))))

(defun distance (p1 p2) 
  (let* ((x1 (car  p1)) (x2 (car  p2))  
	 (y1 (cadr p1)) (y2 (cadr p2)) 
	 (x  (- x2 x1)) (y  (- y2 y1)))
    (sqrt (+ (* x x) (* y y)))))

(defun close-enough-p (p1 p2) (if (< (distance p1 p2) 20) T nil))

(defun calc-heli-move (x y z angle p r)
  ;; Returns a list of (x y z angle p r) after moving one sample period.
  (let ((v (* *F* (cos p)))		; forward velocity
	(s (* *F* (cos r))))		; sideways velocity, skid
    (list (+ x (+ (* v (cos angle) *sample-time*) (* s (sin angle) *sample-time*)))
	  (+ y (+ (* v (sin angle) *sample-time*) (* s (cos angle) *sample-time*)))
	  z angle p r)))

(defun move-heli (poslist out)
  (princ "Spår ") (terpri)
  (let ((endtime  (caar poslist))
	(target   (cdar poslist))
	(position (list 1 2)))
    (princ "Spår 1") (terpri)
    (dotimes (j endtime)
	     (setq time (+ 1 j time))
	     (if (> (angle-to-point target) (h-angle)) (setf ((h-angle)) (+ (h-angle) dw)) 
	       (setf ((h-angle)) (- (h-angle) dw)))
	     (cond ((not (close-enough-p (position target))) (if (< (p) maxp) (setf ((p)) (+ (p) dp))))
		   (T ((if (> (p) 0) (setf ((p)) (- (p) (* 3 dp)))))))
	     (setq *heli-state* (calc-heli-move (x) (y) (z) (h-angle) (p) (r)))
	     (print-helipos-to-file time (x) (y) (z) (rad-to-deg (h-angle)) (p) (r) out)
	     (princ *heli-state*) (terpri)))
  (princ "Spår ") (terpri))

;;;; 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  direction),
  ;; E.g. (23 'L8 20 8 1) is a state (23 seconds, following ray L8, presently
  ;; 20 metres from it's origin, running at 8 m/s, with the car pointing in 
  ;; the ray's positive direction). Negative velocity means that the car is 
  ;; reversing.
  ;;
  ;; 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. (SPD 30) means change
  ;; max speed to 30.
  ;; If out is non-NIL then it is a file onto which to print the
  ;; successive positions
  (terpri)

  (setq 
   current-time (car     state)
   current-ray  (cadr    state)		; the name of the ray the car is currently following.
   car-pos      (caddr   state)		; returns how far from the ray's origin the car is
   velocity     (cadddr  state)
   carangle     (car (cddddr state))
   command      (car op))

  (princ "  Command: ") (princ command) (terpri)  
  (princ state) (terpri)

  (setq new-state
	(cond ((eq command 'gtl)  (move-and-turn-left  op state out out2))
	      ((eq command 'gtr)  (move-and-turn-right op state out out2))
	      ((eq command 'gts)  (move-and-stop       op state out out2))
	      ((eq command 'spd)  (setq *speed-limit*  (cadr op)) state)
	      ((eq command 'gbl)  (back-left           op state out out2))
	      ((eq command 'gbr)  (back-right          op state out out2))
	      ((eq command 'gbs)  (back-to-stop        op state out out2))
	      ((eq command 'wait) (wait                (cadr op) state out out2))
	      (T (error "Unknown command in plan. <op-state> "))))
  
  ;; (princ new-state) (terpri)
  
  (setq *point-list* (cons (car new-state) (linpoint (cadr new-state) (caddr new-state)))) (terpri)
  (setq heli-out (open "/home/emtek/media/witas-videos/009/02/heli.vtr" :direction :output))
  (princ "Spår ") (terpri)
  (move-heli *point-list* heli-out)
  (terpri)
  new-state)



;;;; CORE FUNCTIONS

(defun move-and-turn-left (op state out out2)
  (setq next-ray      (cadr     op))
  (setq current-time  (car      state))
  (setq current-ray   (cadr     state))	
  (setq car-pos       (caddr    state))         ; how far from the ray's origin the car is
  (setq velocity      (cadddr   state))
  (setq direction     (car (cddddr  state)))
  (setq rnow          (linoray  current-ray))   ; current ray as (angle x0 y0)
  (setq rnext         (linoray  next-ray))      ; 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 carangle      (if (plusp direction) (angle rnow) (normalize (+ pi (angle rnow)))))
  (setq delta-phi     (normalize (- (angle rnext) (angle rnow))))
  (setq newdir        (if (plusp direction) (if (< delta-phi 0) -1 1) (if (< delta-phi 0) 1 -1)))
  (setq angle-to-turn (if (plusp delta-phi) delta-phi (- (normalize (- pi delta-phi)))))
  (setq turnspeed     (if (< (abs angle-to-turn) 0.3) *speed-limit* *turnspeed*))
  (setq dec           (if (< (abs angle-to-turn) 0.3) 0 *decceleration*))
  (setq turn-start    (find-turn-start (car  cross) angle-to-turn direction))
  (setq turn-end      (find-turn-end   (cadr cross) angle-to-turn newdir))
  (setq current-time  
	(move-straight current-time current-ray direction car-pos turn-start 
		       velocity *speed-limit* *turnspeed* *acceleration* dec out out2))
  (setq current-time
	(turn-left current-time current-ray next-ray carangle turn-start turnspeed angle-to-turn out out2))
  (list (/ current-time 100) next-ray turn-end turnspeed newdir))


(defun move-and-turn-right (op state out out2)
  (setq next-ray      (cadr     op))
  (setq current-time  (car      state))
  (setq current-ray   (cadr     state))	
  (setq car-pos       (caddr    state))	        ; how far from the ray's origin the car is
  (setq velocity      (cadddr   state))  
  (setq direction     (car (cddddr  state)))
  (setq rnow          (linoray  current-ray))	; current ray as (angle x0 y0)
  (setq rnext         (linoray  next-ray))	; 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 rnext) (angle rnow))))
  (setq newdir        (if (plusp direction) (if (< delta-phi 0) 1 -1) (if (< delta-phi 0) -1 1)))
  (setq angle-to-turn (if (plusp delta-phi) (normalize (- pi delta-phi)) (- delta-phi)))
  (setq turnspeed     (if (< (abs angle-to-turn) 0.3) *speed-limit* *turnspeed*))
  (setq dec           (if (< (abs angle-to-turn) 0.3) 0 *decceleration*))
  (setq turn-start    (find-turn-start (car  cross) angle-to-turn direction))   
  (setq turn-end      (find-turn-end   (cadr cross) angle-to-turn newdir)) 
  (setq current-time  
	(move-straight current-time current-ray direction car-pos turn-start 
		       velocity *speed-limit* *turnspeed* *acceleration* dec out out2))
  (setq current-time
	(turn-right current-time current-ray next-ray carangle turn-start turnspeed angle-to-turn out out2))
  (list (/ current-time 100) next-ray turn-end turnspeed newdir))


(defun move-and-stop (op state out out2)
  (setq next-ray      (cadr     op))		; returns the name of the next ray that is in the plan
  (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))	        ; how far from the ray's origin the car is
  (setq velocity      (cadddr   state))  
  (setq direction     (car (cddddr  state)))
  (setq rnow          (linoray  current-ray))	; current ray as (angle x0 y0)
  (setq rnext         (linoray  next-ray))	; 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 move-end      (car cross))
  (setq current-time  
	(move-straight current-time current-ray direction car-pos move-end 
		       velocity *speed-limit* 0 *acceleration* *decceleration* out out2))
  (list (/ current-time 100) current-ray move-end 0 direction))


(defun back-left (op state out out2)
  (setq next-ray      (cadr     op))
  (setq current-time  (car      state))
  (setq current-ray   (cadr     state))	
  (setq car-pos       (caddr    state))         ; how far from the ray's origin the car is
  (setq velocity      (cadddr   state))
  (setq direction     (car (cddddr  state)))
  (setq rnow          (linoray  current-ray))   ; current ray as (angle x0 y0)
  (setq rnext         (linoray  next-ray))      ; 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 carangle      (if (plusp direction) (angle rnow) (normalize (+ pi (angle rnow)))))
  (setq delta-phi     (normalize (- (angle rnext) (angle rnow))))
  (setq newdir        (if (plusp direction) (if (< delta-phi 0) -1 1) (if (< delta-phi 0) -1 1)))
  (setq angle-to-turn (if (plusp delta-phi) delta-phi (- (normalize (- pi delta-phi)))))
  (setq turn-start    (find-turn-start (car  cross) angle-to-turn (- direction)))
  (setq turn-end      (find-turn-end   (cadr cross) angle-to-turn (- newdir)))
  (setq current-time  
	(move-straight current-time current-ray direction car-pos turn-start 
		       velocity (- *turnspeed*) (- *turnspeed*) (- *acceleration*) (- *decceleration*) out out2))
  (setq current-time
	(turn-left current-time current-ray next-ray carangle turn-start (- *turnspeed*) angle-to-turn out out2))
  (list (/ current-time 100) next-ray turn-end (- *turnspeed*) newdir))


(defun back-right (op state out out2)
  (setq next-ray      (cadr     op))
  (setq current-time  (car      state))
  (setq current-ray   (cadr     state))	
  (setq car-pos       (caddr    state))         ; how far from the ray's origin the car is
  (setq velocity      (cadddr   state))
  (setq direction     (car (cddddr  state)))
  (setq rnow          (linoray  current-ray))   ; current ray as (angle x0 y0)
  (setq rnext         (linoray  next-ray))      ; 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 carangle      (if (plusp direction) (angle rnow) (normalize (+ pi (angle rnow)))))
  (setq delta-phi     (normalize (- (angle rnext) (angle rnow))))
  (setq newdir        (if (plusp direction) (if (< delta-phi 0) 1 -1) (if (< delta-phi 0) 1 -1))) 
  (setq angle-to-turn (if (plusp delta-phi) delta-phi (- (normalize (- pi delta-phi)))))
  (setq turn-start    (find-turn-start (car  cross) angle-to-turn (- direction)))
  (setq turn-end      (find-turn-end   (cadr cross) angle-to-turn (- newdir)))
  (setq current-time  
	(move-straight current-time current-ray direction car-pos turn-start 
		       velocity (- *turnspeed*) (- *turnspeed*) (- *acceleration*) (- *decceleration*) out out2))
  (setq current-time
	(turn-right current-time current-ray next-ray carangle turn-start (- *turnspeed*) angle-to-turn out out2))
  (list (/ current-time 100) next-ray turn-end (- *turnspeed*) newdir))


(defun back-to-stop (op state out out2)
  (setq next-ray      (cadr     op))		; returns the name of the next ray that is in the plan
  (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))	        ; how far from the ray's origin the car is
  (setq velocity      (cadddr   state))  
  (setq direction     (car (cddddr  state)))
  (setq rnow          (linoray  current-ray))	; current ray as (angle x0 y0)
  (setq rnext         (linoray  next-ray))	; 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 move-end      (car cross))
  (setq current-time  
	(move-straight current-time current-ray direction car-pos move-end 
		       velocity (- *turnspeed*) 0 (- *acceleration*) (- *decceleration*) out out2))
  (list (/ current-time 100) current-ray move-end 0 direction))


(defun wait (time state out out2)
  (setq current-time (car      state))
  (setq current-ray  (cadr     state))	
  (setq car-pos      (caddr    state))        
  (setq velocity     (cadddr   state))
  (setq direction    (car (cddddr  state)))
  (setq rnow         (linoray  current-ray))  
  (setq carangle     (if (plusp direction) (angle rnow) (normalize (+ pi (angle rnow)))))
  (setq offset       (if (parkp current-ray) 0 *right-offset*))
  (setq time-now     (adjust-to-sample-time current-time))
  (setq end-time     (adjust-to-sample-time time))
  (calc-move-to-file time-now end-time car-pos carangle offset 0 0 direction out out2)
  (list (+ current-time time) current-ray car-pos 0 direction))


;;;; MOVEMENT FUNCTIONS

(defun move-straight (current-time current-ray direction start-pos end-pos vel1 vel2 vel3 acc dec out out2)

  ;; Calculates and prints to files out and out2 the successive positions of a car.
  ;;
  ;; The linear movement is either divided into three phases; accelerating to the 
  ;; speed-limit, running at constant speed and then deccelerating to the turnspeed 
  ;; again, or if there is not time for that: runs with constant (possibly zero) 
  ;; acceleration the entire distance. 
  
  (setq rnow            (linoray current-ray))
  (setq carangle        (if (plusp direction) (angle rnow) (normalize (+ pi (angle rnow)))))
  (setq delta-vel-acc   (- vel2 vel1))
  (setq delta-vel-brake (- vel3 vel2))
  (setq delta-vel-slow  (- vel3 vel1))
  ;; Times needed for acceleration-, constant-velocity- and braking-phase of the motion.
  (setq acc-seconds     (if (zerop acc) 0 (abs (/ delta-vel-acc   acc))))
  (setq brake-seconds   (if (zerop dec) 0 (abs (/ delta-vel-brake dec))))
  (setq length-tot      (abs (- end-pos start-pos)))
  (setq const-seconds	(let ((acc-length   (+ (* (abs vel1) acc-seconds)
					       (* *acceleration*  acc-seconds acc-seconds 0.5)))
			      (brake-length (+ (* (abs vel2)     brake-seconds) 
					       (* *decceleration* brake-seconds brake-seconds 0.5))))
			  (/ (- length-tot acc-length brake-length)  (abs vel2))))
  ;; Time needed for slow traversal of the ray
  (setq slow-seconds   (abs (/ length-tot (+ vel1 (/ delta-vel-slow 2)))))
  (setq slow-acc       (/ delta-vel-slow slow-seconds))

  (setq time-now       (adjust-to-sample-time current-time))
  (setq acc-time       (adjust-to-sample-time acc-seconds))
  (setq const-vel-time (adjust-to-sample-time const-seconds))
  (setq brake-time     (adjust-to-sample-time brake-seconds))
  (setq slow-time      (adjust-to-sample-time slow-seconds))

  (setq position       start-pos)
  (setq offset         (if (parkp current-ray) 0 *right-offset*))
  (setq current-vel    vel1)
  (setq pos-list       (list time-now position current-vel))
  
  (cond ((plusp const-vel-time)
	 (setq pos-list    
	       (calc-move-to-file time-now acc-time position carangle offset 
				  current-vel acc direction out out2))
	 (setq time-now (car pos-list)) (setq position (cadr pos-list)) (setq current-vel (caddr pos-list))
	 (setq pos-list 
	       (calc-move-to-file time-now const-vel-time position carangle offset 
				  current-vel 0 direction out out2))
	 (setq time-now (car pos-list)) (setq position (cadr pos-list)) (setq current-vel (caddr pos-list))
	 (setq pos-list
	       (calc-move-to-file time-now brake-time position carangle offset 
				  current-vel dec direction out out2))
	 (setq time-now (car pos-list)))
	
	;;else
	(T (setq pos-list
		 (calc-move-to-file time-now slow-time position carangle offset 
				    current-vel slow-acc direction out out2))
	   (setq time-now (car pos-list))))
  time-now)


(defun turn-left (current-time current-ray next-ray carangle turn-start turnspeed angle-to-turn out out2)
  (setq turnrad         (if (not (parkp current-ray)) (+ *turnradius* *right-offset*) *turnradius*))
  (setq curve-end-time  (adjust-to-sample-time (abs (/ (* turnrad angle-to-turn) turnspeed))))
  (setq omega           (* (/ angle-to-turn curve-end-time) (sign turnspeed))) ; the angular velocity 
  (setq turn-startpoint (raypoint rnow turn-start))
  (setq pull-in
	;; If parking: how much to successively reduce turn-ray 
	;; by to make the car hit the parking ray 
	(cond ((and (parkp next-ray) (not (parkp current-ray)))
	       (princ "Parking! " ) (terpri)
	       (- (/ *right-offset* curve-end-time)))
	      ((and (parkp current-ray) (not (parkp next-ray)))
	       (princ "Unparking! " ) (terpri)
	       (- (- (/ *right-offset* curve-end-time))))
	      (T 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 (+ carangle (/ pi 2))
	      (car  turn-startpoint)
	      (cadr turn-startpoint)))
  (setq turn-center (raypoint circle-ray *turnradius*))
  (dotimes (i curve-end-time)
	   (setq time-now (+ time-now 1))		  
	   (setq new-angle (+ carangle (* (+ i 1) omega)))
	   (setq turn-ray
		 ;; ray from centre of turning circle to where the car is supposed to be
		 (list (+ (angle circle-ray) pi (* (+ i 1) omega))
		       (car  turn-center) 
		       (cadr turn-center)))
	   (setq xy-pos-rht (raypoint turn-ray (+ turnrad (* (+ i 1) pull-in))))
	   (setq xy-pos (calc-middle-point xy-pos-rht new-angle))
	   (print-position-to-file xy-pos (rad-to-deg new-angle) time-now out out2))
  time-now)


(defun turn-right (current-time current-ray next-ray carangle turn-start turnspeed angle-to-turn out out2)
  (setq turnrad         (if (not (parkp current-ray)) (- *turnradius* *right-offset*) *turnradius*))
  (setq curve-end-time  (adjust-to-sample-time (abs (/ (* turnrad angle-to-turn) turnspeed))))
  (setq omega           (* (/ angle-to-turn curve-end-time) (sign turnspeed))) ; the angular velocity
  (setq turn-startpoint (raypoint rnow turn-start))
  (setq pull-in         (cond ((and (parkp next-ray) (not (parkp current-ray)))
			       (- (/ *right-offset* curve-end-time)))
			      ((and (parkp current-ray) (not (parkp next-ray)))
			       (- (- (/ *right-offset* curve-end-time))))
			      (T 0)))
  (setq circle-ray      (list (- carangle (/ pi 2)) (car  turn-startpoint) (cadr turn-startpoint)))
  (setq turn-center     (raypoint circle-ray *turnradius*))
  (dotimes (i curve-end-time)
	   (setq time-now (+ time-now 1))		  
	   (setq new-angle (- carangle (* (+ i 1) omega)))
	   (setq turn-ray		; ray from centre of turning circle to where the car is supposed to be
		 (list (- (angle circle-ray) pi (* (+ i 1) omega))
		       (car  turn-center) 
		       (cadr turn-center)))
	   (setq xy-pos-rht (raypoint turn-ray (+ turnrad (* (+ i 1) pull-in))))
	   (setq xy-pos (calc-middle-point xy-pos-rht new-angle))
	   (print-position-to-file xy-pos (rad-to-deg new-angle) time-now out out2))
  time-now)


;;;; AUXILLIARY FUNCTIONS

(defun angle (ray)			; returns the angle of the ray, -pi < angle < pi
  (if (and (< (car ray) pi) (> (car ray) (- pi))) (car ray)
    (normalize (car ray))))
(defun x0           (ray)      (cadr   ray))
(defun y0           (ray)      (caddr  ray))
(defun parkp        (ray-name) (if (equal (char (string ray-name) 0) #\P) T nil))

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

(defun calc-middle-point (xy-pos carangle)
  ;; Translates point xy-pos (which is in the middle of the rear axis)
  ;; to the car's centre point.
  (list (+ (car  xy-pos) (* *axis-distance* (cos carangle)))
	(+ (cadr xy-pos) (* *axis-distance* (sin carangle)))))

(defun find-turn-start (cross-point angle-to-turn dir) 
  (let ((turnallow (abs (* *turnradius* (tan (/ angle-to-turn 2)))))) ; how far from the crossing point turn starts
    (- cross-point (* turnallow dir))))

(defun find-turn-end (cross-point angle-to-turn dir) 
  (let ((turnallow (abs (* *turnradius* (tan (/ angle-to-turn 2))))))
    (+ cross-point (* turnallow dir))))

(defun calc-move-to-file (time-now end-time position carangle offset current-vel acc dir out out2)
  ;; Returns time and position at the end of the move and prints the time and position at
  ;; every timestep to file.

    (dotimes (j end-time)  
	     (setq time-now      (+ time-now 1))
	     (setq displacement  (calc-displacement (* current-vel dir) (* acc dir) *sample-time*))
	     (setq position      (+ position displacement))
	     (setq current-vel   (+ current-vel (* acc *sample-time*)))
	     (setq xy-pos-on-ray (raypoint rnow position))
	     (setq xy-pos-rht    (implement-right-hand-traffic xy-pos-on-ray carangle offset))
	     (setq xy-pos        (calc-middle-point xy-pos-rht carangle))
	     (print-position-to-file xy-pos (rad-to-deg carangle) time-now out out2))
    (list time-now position current-vel))

(defun calc-displacement (spd acc time) (+ (* spd time) (* acc time time 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 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 (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 (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
  (unless (null out)
    (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)
    (print-fixcam-pos-to-file time 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)))


(defun print-helipos-to-file (time x y z angle p r out)
  (unless (null out)
    (princ " Value - " out) 
    (prin-twodec time out) (princ " Helicopter[0] " out)
    (princ x out) (princ " " out)
    (princ y out) (princ " " out)
    (princ z out) (princ " " out) 
    (princ angle out) (princ " " out)
    (princ p out) (princ " " out)
    (princ r out) (princ " " out)))
  

(defun print-fixcam-pos-to-file (time out)
  (let ((x            180) 
	(y            725) 
	(z            180)
	(heading      180) 
	(pitch        100)
	(roll         100)
	(viewdistance 100) 
	(viewradius    50) 
	(viewangle     50)
	(Xangle         0) 
	(Yangle         0))
    (princ " Value - " out) 
    (prin-twodec time out)    (princ " Camera[0] " out)
    (princ x out) (princ " " out)
    (princ y out) (princ " " out) 
    (princ z out) (princ " " out)
    (princ heading out)      (princ " " out) 
    (princ pitch out)        (princ " " out) 
    (princ roll out)         (princ " " out) 
    (princ viewdistance out) (princ " " out) 
    (princ viewradius out)   (princ " " out) 
    (princ viewangle out)    (princ " " out) 
    (princ Xangle out)       (princ " " out) 
    (princ Yangle out)       (terpri out)))


(defun print-car-vector-to-file (xy-pos carangle-deg time out out2)
  ;; print new position (as x y z fdirx fdiry fdirz upx upy upz) to file
  (let ((x     (car xy-pos))
	(y     (cadr xy-pos))
	(z     0)
	(fdirx (cos (deg-to-rad carangle-deg)))
	(fdiry (sin (deg-to-rad carangle-deg)))
	(fdirz 0)
	(upx   0)
	(upy   0)
	(upz   1))
    (print-vector-to-file time x y z fdirx fdiry fdirz upx upy upz out out2)))

(defun print-vector-to-file (time x y z fdirx fdiry fdirz upx upy upz out out2)    
    (unless (null out)
      (princ " Value - " out) 
      (prin-twodec time out)    (princ " Car[0] " out)
      (princ x out)     (princ " " out)
      (princ y out)     (princ " " out)
      (princ z out)     (princ " " out)
      (princ fdirx out) (princ " " out)
      (princ fdiry out) (princ " " out)
      (princ fdirz out) (princ " " out)
      (princ upx out)   (princ " " out)
      (princ upy out)   (princ " " out)
      (princ upz out)   (terpri out))
    (unless (null out2) 
      (princ " Value - " out2) 
      (prin-twodec time out2)    (princ " Helicopter[0] " out2) 
      (princ x out)     (princ " " out)
      (princ y out)     (princ " " out)
      (princ z out)     (princ " " out)
      (princ fdirx out) (princ " " out)
      (princ fdiry out) (princ " " out)
      (princ fdirz out) (princ " " out)
      (princ upx out)   (princ " " out)
      (princ upy out)   (princ " " out)
      (princ upz out)   (terpri out)))
  

;;; MATH FUNCTIONS

(defun rad-to-deg (phi) (* (/ phi pi) 180)) 
(defun deg-to-rad (phi) (* (/ phi 180) pi))
(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. <solveq> "))
    (list (/ (determ (repcar  m a)) d)
	  (/ (determ (repcadr m a)) d))))

(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 determ  (m)   (- (* (caar  m)  (cadadr m))  (* (cadar m) (caadr  m)))) ; Calculate the determinant of m
(defun repcar  (m a) (list (cons (car a)  (cdar m))  (cons (cadr a)  (cdadr m)))) ; Replace first column in m by a
(defun repcadr (m a) (list (list (caar m) (car a))   (list (caadr m) (cadr a)))) ; Replace second column in m by a
(defun transp  (m)   (list (list (caar m) (caadr m)) (list (cadar m) (cadadr m)))) ; Returns the transponate of m.
(defun vec+vec (a b) (list (+ (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 vec*vec (a b) (+ (* (car a)  (car b))  (* (cadr a)  (cadr b))))
(defun adjust-to-sample-time (s) (round (/ s *sample-time*)))


;;;; 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"))
	 (setq out2 (concatenate 'string (docdir) "traject.vtr"))
	 (princ out1) (terpri)
	 (princ out2) (terpri)
	 (setq out1 (open out1 :direction :output))
	 (setq out2 (open out2 :direction :output))

					;	 (with-open-file (out1 :direction :output)
					;	 (with-open-file (out1 :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)
	   ))







