;;; /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 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
  '(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"))
	 ;(princ "Manintrace 1")
	 (setq out1 (open out1 :direction :output))
	 ;(princ "Maintrace 2")
	 (setq out2 (open out2 :direction :output))
	 ;(princ "Maintrace 3")
	 (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*) 
			  (caddr *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 dd durn durn1 durn2 newvel rnext rnow samedir turnallow)
	(princ "Trace -n")
	(terpri)

	(setq rnow  (linoray (current-ray state))) ; current ray as (angle x0 y0)
	(princ "First trace")
	(princ (current-ray state)) (terpri)
	(princ (next-ray op)) (terpri)
	(setq rnext (linoray (next-ray op)))       ; next event-generating ray
  
	(princ "Trace -2")
	(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)))	
	(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 "Trace -1")
	(setq turnallow 
	      ;; how high up on the other ray does the turn last?
	      ;;          \|
	      ;;           X 
	      ;;           |\          
	      ;;           | \                            turnallow
	      ;;           |  \            tan(0.5*phi) = ---------  =>  turnallow = turnrad * tan(0.5*phi)
	      ;;           |   \                           turnrad
	      ;;           |    \      
	      ;;  phi*0.5 -|     \ 
	      ;;          {|\..   \
	      ;; turnallow{|   \.. \ phi               
	      ;; _________{|______\(\_______ray 1___ 
	      ;;           | turnrad \ 
	      ;;           |          \
	      ;;           |           \
	      ;;           |          ray 2
	      ;;           |             \

	      (cond ((eq (car op) 'gts) 0)
		    (t (* turnrad (abs (if samedir 
					   (tan   (* 0.5 (- (angle rnow) (angle rnext))))
					   ;;else
					   (cotan (* 0.5 (- (angle rnow) (angle rnext))))))))))
	(princ "Trace 0")
	(setq crmod 
	      ;; crmod (crossing, modified) is a list consisting of the start and 
	      ;; end points of the turn arc
	      ;;
	      ;;    |v-- start
	      ;;    |\
	      ;;    | \_
	      ;;    |   \__  i---- end
	      ;;    |      \ v
	      ;;----|-------=------------>-----------
	      ;;

	      (list (velsub (car  cross) turnallow (velocity state))
		    (veladd (cadr cross) turnallow newvel)))
	
	(princ "Trace 1")
	(setq durn1  (/    (- (car crmod) (position state))    (velocity state)))

	(if (< durn1 0)
	    ((princ "Negative straight-part duration")
	     (terpri) 
	     (return)))
	(princ "Trace 2")
	(setq durn2 (abs (/    (* turnrad    (- (car rnow) (car rnext))    )    (cadddr state))))

	(setq dd (raypoint rnow (car crmod)))


	(princ "Same direction: ") (princ samedir)       (terpri)
	(princ "Direction:      ") (princ (angle rnow))  (terpri)
	(princ "Next direction: ") (princ (angle rnext)) (terpri)       
	(princ "Turnallow:      ") (princ turnallow)     (terpri)	
	(princ "Time parts:     ") (princ durn1)         (princ " ") (princ durn2)     (terpri)	
	(princ "Start turn:     ") (princ (car dd))      (princ " ") (princ (cadr dd)) (terpri)

	(if (null out) (go end))

	(setq ti  (roundhund (car state)))
	(setq til (roundhund durn1))

	(dotimes (j til)
		 (princ " Value - " out)
		 (prin-twodec (+ ti j 1) out)
		 (princ " Car[0] " out)
		 (setq a (raypoint  rnow    (+  (caddr state)    (* (cadddr state) (+ j 1) 0.01))))
		 (princ (car a) out)
		 (princ " " out)
		 (princ (cadr a) out)
		 (princ " 0 " out)
		 (princ (carangle state rnow) out)
		 (princ " 0 0 " out)
		 (terpri out)
		 (cond (out2 
			(princ " Value - " out2)
			(prin-twodec (+ ti 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))
		       ))
	
	END (return (list
		     (+ (car state) 
			durn1 
			durn2)
		     (cadr op)
		     (cadr crmod)
		     newvel))
	))



  
;;; Some global selectors
(defun angle       (ray)     (car    ray))
(defun x0          (ray)     (cadr   ray))
(defun y0          (ray)     (caddr  ray))
(defun time        (state)   (car    state))
(defun current-ray (state)   (cadr   state))   ; returns the name of the ray the car is currently following. 
(defun position    (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 roundhund (s)
  (round (* s 100)))


(defun prin-twodec (n out)
;; out is an output-file
  (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)))


(defun velsub (x d vel) (- x (* d (sign vel))))
(defun veladd (x d vel) (+ x (* d (sign vel))))

(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 linoray (name)
;;;; returns a list of (angle x0 y0) for a line
  (let ((s (load-adl (concatenate 'string (docdir) "!" (string name) ".adl"))))
    (princ "linoray trace")
    (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 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 
  (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 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 (car r1)) 
			(negvec (cossin (car 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* ((diff (- psi phi)) 
	 (below (if (< diff 0) (< diff (- pi))
		  ;;else
		  (< diff pi)))) 
    
    ;; end of assign, begin function body
    (princ "Below-angle: ") 
    (princ (list phi psi below))
    (terpri)
    below))


(defun keepsign (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 (car (linoray ray1))
		 (car (linoray ray2))  ))
				    

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








