;;;; /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
;;;  Realistic turning (instead of tanklike rotation around it's centrepoint)
;;;  Changing speed
;;;
;;; Future:
;;;  Multiple cars
;;;  Reversing
;;;  More commands

(setq *breakflag* t)

;; The car is represented as a point and an angle. The point is located 
;; in the middle between the rear wheels. The angle is between the fixed 
;; x-axis and the car's forward direction.

;; SIMULATION PARAMETERS. All values should be given in SI-units
(setq *turnradius*      4.5);; must be larger than *right-offset*
(setq *turnspeed*       5);; the speed at which a car traverses turns 
(setq *acceleration*    5)
(setq *decceleration*   -5)
(setq *right-offset*    1.5);; how far to the right-of-middle the cars should drive
(setq *axis-distance*   1);; how far it is between the wheel-axes of a car
(setq *sample-time*      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 park) 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. (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
  
  (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 command      (car op))		
  (princ "  Command: ") (princ command) (terpri)  

  (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 'gbl) (back-left           op state out out2))
	((eq command 'spd) (list current-time current-ray car-pos (if (plusp velocity) (cadr op) (- (cadr op)))))
	(T (error "Unknown command in plan. <op-state> "))))


;;;; CORE FUNCTIONS

(defun move-and-turn-left (op state out out2)
  (setq next-ray      (cadr op))		; 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 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 (- (angle rnext) (angle rnow))))
  (setq newvel        (if (plusp  delta-phi) velocity (- velocity))) 	; Velocity on the new ray.
  (setq carangle      (if (plusp velocity) (angle rnow) (normalize-angle (+ (angle rnow) pi))))
  (setq angle-to-turn (let ((delta-comp (normalize-angle (- pi delta-phi))))
			(if (plusp delta-phi) delta-phi (- delta-comp))))
  (setq turn-start    (find-turn-start (car  cross) 'gtl angle-to-turn velocity))
  (setq turn-end      (find-turn-end   (cadr cross) 'gtl angle-to-turn newvel))
  (setq turnrad       (+ *turnradius* *right-offset*))
  (setq carangle      (if (plusp velocity) (angle rnow) (normalize-angle (+ (angle rnow) pi))))
  (setq current-time  (move-straight current-time current-ray 'gtl carangle car-pos turn-start velocity out out2))
  (setq current-time  (turn current-time current-ray 'gtl turn-start velocity angle-to-turn out out2))
  (list (/ current-time 100) next-ray turn-end newvel))


(defun move-and-turn-right (op state out out2)
  (setq next-ray      (cadr op))		; 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))	        ; returns how far from the ray's origin the car is
  (setq velocity      (cadddr 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 (- (angle rnext) (angle rnow))))
  (setq newvel        (if (minusp delta-phi) velocity (- velocity)))
  (setq carangle      (if (plusp velocity) (angle rnow) (normalize-angle (+ (angle rnow) pi))))
  (setq angle-to-turn (let ((delta-comp (normalize-angle (- pi delta-phi))))
			(if (plusp delta-phi) delta-comp (- delta-phi))))
  (setq turn-start    (find-turn-start (car  cross) 'gtr angle-to-turn velocity))
  (setq turn-end      (find-turn-end   (cadr cross) 'gtr angle-to-turn newvel))
  (setq turnrad       (- *turnradius* *right-offset*))
  (setq current-time  (move-straight current-time current-ray 'gtr carangle car-pos turn-start velocity out out2))
  (setq current-time  (turn current-time current-ray 'gtr turn-start velocity angle-to-turn out out2))  
  (list (/ current-time 100) next-ray turn-end newvel))


(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))	        ; returns how far from the ray's origin the car is
  (setq velocity      (cadddr 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 velocity) (angle rnow) (normalize-angle (+ (angle rnow) pi))))
  (setq move-end      (car cross))
  (setq current-time  (move-straight current-time current-ray 'gts carangle car-pos move-end velocity out out2))
  (list (/ current-time 100) current-ray move-end 0))


(defun back-left (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))	        ; returns how far from the ray's origin the car is
  (setq velocity      (cadddr state))
  (princ current-ray) (terpri)
  (princ next-ray) (terpri)
  (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 (- (angle rnext) (angle rnow))))
  (setq newvel        (if (plusp  delta-phi) velocity (- velocity))) 	; Velocity on the new ray.
  (setq carangle      (if (plusp velocity) (normalize-angle (+ (angle rnow) pi)) (angle rnow)))
  (setq angle-to-turn (let ((delta-comp (normalize-angle (- pi delta-phi))))
			(if (plusp delta-phi) delta-phi (- delta-comp))))
  (setq turn-start    (find-turn-start (car  cross) 'gtl angle-to-turn velocity))
  (setq turn-end      (find-turn-end   (cadr cross) 'gtl angle-to-turn newvel))
  (setq turnrad       (+ *turnradius* *right-offset*))
  (setq carangle      (if (plusp velocity) (angle rnow) (normalize-angle (+ (angle rnow) pi))))
  (setq current-time  (move-straight current-time current-ray 'gtl carangle car-pos turn-start velocity out out2))
  (setq current-time  (turn current-time current-ray 'gtl turn-start velocity angle-to-turn out out2))
  (list (/ current-time 100) next-ray turn-end newvel))






(defun move-straight (current-time current-ray command carangle start-pos end-pos velocity out out2)

  ;; Calculates and prints to files out and out2 the successive positions of a car
  ;; that runs in a straight line and either slows down to turn at the end (which is then
  ;; handled by the turn-function) or slows down to a stop. Which is decided on basis 
  ;; of the "command" parameter. If it is 'stop then the car slows to stop, else just to 
  ;; turn (which is the global variable *turnspeed*).
  ;;
  ;; The linear movement is either divided into three phases, acc, const and dec, or, 
  ;; if the distance is too short for accelerating to the speed-limit and then 
  ;; deccelerating to the turnspeed again, runs with constant (slow) speed the entire 
  ;; distance.
  
  (setq rnow            (linoray current-ray))
  (setq direction       (sign velocity))
  (setq speed           (abs velocity))
  (setq delta-vel-acc   (- velocity (* direction *turnspeed*)))
  (setq delta-vel-brake (- (if (or (eq command 'stop) (parkp next-ray))  velocity  delta-vel-acc)))
  ;; Times needed for acceleration-, constant-velocity- and braking-phase of the motion.
  (setq acc-seconds     (abs (/ delta-vel-acc *acceleration*)))
  (setq brake-seconds   (abs (/ delta-vel-brake *decceleration*)))
  (setq const-seconds	(let ((length-tot   (abs (- end-pos start-pos)))
			      (acc-length   (+ (* *turnspeed* acc-seconds) 
					       (* *acceleration*  acc-seconds acc-seconds 0.5)))
			      (brake-length (+ (* speed     brake-seconds) 
					       (* *decceleration* brake-seconds brake-seconds 0.5))))
			  (/ (- length-tot acc-length brake-length)  speed)))
  ;; Time needed for slow traversal of the ray
  (setq slow-seconds (abs (/  (- end-pos start-pos)  *turnspeed*)))
  
  (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  (* direction *turnspeed*))
  (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 *acceleration* 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 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 *decceleration* out out2))
	 (setq time-now (car pos-list))
	 (setq position (cadr pos-list))
	 (setq current-vel (caddr pos-list)))
	 
	 ;;else
	(T (setq pos-list
		 (calc-move-to-file time-now slow-time    position carangle offset current-vel 0 out out2))
	   (setq time-now (car pos-list))
	   (setq position (cadr pos-list))))
  time-now)


  


(defun turn-left (current-time current-ray command carangle turn-start velocity angle-to-turn out out2)
  (setq curve-end-time 
	(adjust-to-sample-time 
	 (let ((turnspeed (if (< (abs (normalize-angle angle-to-turn)) 0.3)  (abs velocity) *turnspeed*)))
	   (abs (/ (* turnrad angle-to-turn) turnspeed)))))
  (cond ((plusp curve-end-time)
	 (setq omega                       ; the angular velocity 
	       (cond ((plusp curve-end-time) (/ angle-to-turn curve-end-time))
		     (T 0)))	 
	 (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 
	       (if (parkp current-ray) 
		   (if (eq command 'gtl)
		       (- (/ *right-offset* curve-end-time))
		     ;;else
		     (/ *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 command 'gtl) (+ carangle (/ pi 2)))
			   ((eq command 'gtr) (- carangle (/ pi 2)))
			   (T carangle))
		     (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
			(let ((direction (if (plusp velocity) (angle rnow) 
					   ;;else
					   (normalize-angle (+ (angle rnow) pi)))))
			  (cond ((eq command 'gtr) (- direction (* (+ i 1) omega)))
				((eq command 'gtl) (+ direction (* (+ i 1) omega)))
				(T direction))))
		  (setq turn-ray
			;; ray from centre of turning circle to where the car is supposed to be
			(list (cond ((eq command 'gtl) (+ (angle circle-ray) pi (* (+ i 1) omega)))
				    ((eq 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-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)))
	(T (error "Turn error. Negative time. <turn-left> ")))
  time-now)
 



(defun turn-right (current-time current-ray command carangle turn-start velocity angle-to-turn out out2)
  (setq curve-end-time 
	(adjust-to-sample-time
	 (let ((turnspeed (if (< (abs (normalize-angle angle-to-turn)) 0.3)  (abs velocity) *turnspeed*)))
	   (abs (/ (* turnrad angle-to-turn) turnspeed)))))
	
  (cond ((plusp curve-end-time)
	 (setq omega (/ angle-to-turn curve-end-time)) 	 ; the angular velocity
	 (setq turn-startpoint (raypoint rnow turn-start))
	 (setq pull-in (if (parkp current-ray) (/ *right-offset* curve-end-time) 0))
	 ;; If parking: how much to successively reduce turn-ray by to make the car hit the parking ray 
	 (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)


   

;;;; AUXILLIARY FUNCTIONS

(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 (* 2 pi))))
	((< phi (- pi)) (normalize-angle (+ 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 command angle-to-turn vel) 
  (let ((turnallow			; how far from the crossing point the turning action should start
	 (cond ((eq command 'gts) 0)
	       ((or (eq command 'gtl) (eq command 'gtr)) 
		(abs (* *turnradius* (tan (/ angle-to-turn 2)))))
	       (T (error "Unknown command in plan. <turnallow> ")))))
    (- cross-point (* turnallow (sign vel)))))

(defun find-turn-end (cross-point command angle-to-turn vel) 
  (let ((turnallow			; how far from the crossing point the turning action should start
	 (cond ((eq command 'gts) 0)
	       ((or (eq command 'gtl) (eq command 'gtr)) 
		(abs (* *turnradius* (tan (/ angle-to-turn 2)))))
	       (T (error "Unknown command in plan. <turnallow, ~a> " command)))))
    (+ cross-point (* turnallow (sign vel)))))

(defun calc-move-to-file (time-now end-time position carangle offset current-vel acc out out2)
  ;; Returns time and position at the end of the move and prints the time and position at
  ;; every timestep to file as a side-effect.
  (let ((dacc       (* acc *sample-time*))
	(direction  (sign current-vel)))
    
    (dotimes (j end-time)  
	     (setq time-now      (+ time-now 1))
	     (setq displacement  (calc-displacement (abs current-vel) acc *sample-time*))
	     (setq position      (+ position (* direction displacement)))
	     (setq current-vel   (+ current-vel (* direction dacc)))
	     (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-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
  (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))
  
  (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

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