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

(setq turnrad 2)  
(setq rad-to-deg (/ 180 pi))

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

(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) (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))	
		 (terpri)
		 (princ "GLOBAL STATE:   ") (princ *state*) (terpri)
		 (if (null *state*) (return))
		 (setq *hist* (cons op *hist*))
		 (setq *oldstate* *state*)
		 (setq *state* (op-state op *state* out1 out2)))
	 (princ "Slutspår ") 
	 (fresh-line) (princ out1)
	 (close out1) (princ " stängd. ") (terpri) 
	 (fresh-line) (princ out2)
	 (close out2) (princ " stängd. ")
	 ))

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

(defun op-state (op state out out2) 
  ;; car in current state, apply op, return new state
  ;; state is (time 
  ;;           line-name 
  ;;           distance-to-start-of-line 
  ;;           velocity)
  ;; e.g. (0 L8 20 30) is a state.
  ;; op is e.g. (GTL L2)  (GTR L1)   (GTS L5)
  ;; goto-turn left when at L2, goto-turn right when at L1,  goto-stop at L5
  ;; if out is non-NIL then it is a file onto which to print
  ;; successive positions
  (prog (a
	 crmod 	  
	 cross 
	 turnpoint	 
	 durn 
	 durn1 
	 durn2 
	 newvel 
	 rnext 
	 rnow
	 samedir 	 
	 turnallow)

	(terpri)
	(princ "Spår ")
	(setq rnow  (linoray (current-ray state))) ; current ray as (angle x0 y0)
	(princ "Spår ")
	(setq rnext (linoray (next-ray op)))       ; next event-generating ray
	(princ "Spår ")	
	(setq cross (crossing rnow rnext))         ; where on each ray does the other intersect? E.g. (22 34)
	;(setq samedir (case (car op)               ; samedir == true if both rays point in the same direction?
	;		    (gtl       (below-angle-p (angle rnow) (angle rnext)))
	;		    (gtr (null (below-angle-p (angle rnow) (angle rnext))))
	;		    (gts nil)))"
	(princ "Spår ")	
	(setq samedir (let ((delta (abs (- (angle rnow) (angle rnext))))
			    (half-pi (/ pi 2)))
			(if (< delta half-pi) T 0)))
	(princ "Spår ")	
	(setq newvel (* (velocity state) (if samedir 1 -1)))  ; velocity magnitude is not changed
	
	;; An alternative to the above:
	;; (case (car op)
	;;      (gtl 1)
	;;       (gtr -1)
	;;       (gts 0))
	
	(princ "Spår ")
	(setq turnallow (cond ((eq (car op) 'gts) 0)
			      (T (abs (tan (* 0.5 (- (angle rnow) (angle rnext))))))))

					;or else (old code)
					;(t (* turnrad (abs (if samedir (tan   (* 0.5 (- (angle rnow) (angle rnext))))
					;			 ;;else
					;		 (cotan   (* 0.5 (- (angle rnow) (angle rnext))))))))))
	(princ "Spår ")
	(setq crmod 
	      ;; crmod is a list containing the start and end points of the turn 
	      (list (velsub (car  cross) turnallow (velocity state))
		    (veladd (cadr cross) turnallow newvel)))
	(princ "Spår ")
	
	RETRY
	(setq durn1  (/    (- (car crmod) (car-pos state))    (velocity state)))
	;; durn1 is how long it will take to traverse the straight part of the ray
	
	(princ "Turnpoint:      ") (princ (car crmod)) (terpri) 
	
	(cond ((< durn1 0)
	       (princ "Negative straight-part duration") (terpri) 
	       (setq state (list (car state) (cadr state) (caddr state) (- (cadddr state))))
	       (go retry)))
	   ;;	       (return)))

	(princ "Spår ")
	(setq durn2 (abs (/    (* turnrad  (- (angle rnow) (angle rnext)))    (velocity state))))
	;; durn2 is how long it will take to traverse the curve
	(princ "Spår ")
	(setq turnpoint (raypoint rnow (car crmod))) ;; the starting point of the turn in cartesian coords 
	(princ "Spår ") (terpri)
	(princ "Current ray:     ") (princ (current-ray state)) (princ ", ") (princ rnow)  (terpri)
	(princ "Next ray:        ") (princ (next-ray op))       (princ ", ") (princ rnext) (terpri)
	(princ "Start turn: x    ") (princ (car turnpoint))     (terpri)
	(princ "            y    ") (princ (cadr turnpoint))    (terpri)
	(princ "State:           ") (princ state)               (terpri)
	(princ "Spår ")
	

	(if (null out) (go end))
	(princ "Null out")

	(setq start-time (roundhund (current-time state)))
	(setq end-time   (roundhund durn1))
	(princ "Spår ")

	(dotimes (j end-time)
		 (princ " Value - " out) (prin-twodec (+ start-time j 1) out) (princ " Car[0] " out)

		 ;; update position
		 (setq a (raypoint rnow (+ (car-pos state)    (* (velocity state) (+ j 1) 0.01))))

		 ;; print new position (in cartesian coords) to file
		 (princ (car  a) out) (princ " "   out)
		 (princ (cadr a) out) (princ " 0 " out)

		 ;; print the car's angle to file
		 (princ (carangle state rnow) out)
		 (princ " 0 0 " out)
		 (terpri out)

		 (cond (out2 
			(princ " Value - " out2)
			(prin-twodec (+ start-time j 1) out2)
			(princ " Helicopter[0] " out2)
			(princ (car a) out2)
			(princ " " out2)
			(princ (cadr a) out2)
			(princ " 0 " out2)
			(princ (carangle state rnow) out2)
			(princ " 0 0 " out2)
			(terpri out2)
			)))

	
	(princ "Sista spåret ") (terpri)
	
	END 
	(return (list (+ (current-time state) durn1 durn2)
		      (next-ray op)
		      (cadr crmod) ;; starting point on the new ray
		      newvel))
	))



  
;;; Some global selectors
(defun angle        (ray)     (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 next-ray     (op-list) (cadr   op-list)) ; returns the name of the next ray that is in the plan




(defun carangle (state rnow)
;;; returns the angle of the car, in degrees (not radians)
  (prog (v)	
	(setq v (* (angle rnow) rad-to-deg))
	;; will go backwards if negative velocity - should be turned around
	;; but following line has effect of mirroring it in x axis, wrong!
	;;    (cond ((< (cadddr state) 0)(setq v (- v))))
	(return v)))



(defun velsub (x d v)
  (- x (* d (sign v))))


(defun veladd (x d vel)
; returns 
  (+ x (* d (sign vel))))
	

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


(defun raypoint (ray d)
; expresses a point distance d along a ray, in terms of the 
; global Cartesian plane
  (list (+ (x0 ray)  (* d (cos (angle ray))))
	(+ (y0 ray) (* d (sin (angle ray))))))


(defun linpoint (name d)
  (raypoint (linoray name) d))


(defun crossing (r1 r2) 
; returns a list of 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 below-angle-p (phi psi)
; takes two angles and returns true if the difference between them
; is less than -pi or between 0 and pi. Also prints the results to 
; the screen.
;
; Graphic truth-function:
;                  ^
;                  |
; ---------i       |-------i         #t
;          |       |       |
;          |       |       |
; ---------i-------|-------i-------  #f
;                  |
;         -pi      0       pi        difference psi-phi
;

;  (let* ((delta (- psi phi)) 
;	 (is-below (if (< delta 0) (< delta (- pi))
;		     ;;else
;		     (< delta pi)))) 
;    
    ;; end of assign, begin function body
;    (princ "Below-angle:    ") (princ (list phi psi is-below)) (terpri)
;    is-below))



(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
  (below-angle-p (angle (linoray ray1)) (angle (linoray ray2))))
				    

(defop 'ks                          
  '(cond ((keepsignp (read) (read))  
	  (princ "Keeps sign")	    
	  (terpri))
	 
	 (T  (princ "Reverses sign")
	     (terpri)
	     )))



;;;; MATH FUNCTIONS

(defun solveq (m a)
  ;; returns the vector x that solves (= (mat*vec m x) a)
  ;; the solution is obtained from Cramer's rule
  (let ((d (determ m)))
    (if (= d 0) 
	((princ "Determinant 0") 
	 (terpri))
      (list (/ (determ (repcar  m a)) d)
	    (/ (determ (repcadr m a)) d)
	    ))))

(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 determ (m) 
  ;; calculate the determinant
    (- (* (caar m)
	  (cadadr m))
       (* (cadar m)
	  (caadr m))))

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


;;;; PRINTING

(defun prin-twodec (n out)
  (prog (s a b c)
	(cond ((< n 0) 
	       (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)))

