;;;---	HSIM >	-*- package: user; mode: lisp; base: 10.; readtable: CL -*-

; This file is for the robot simulation programs.
; Free-body rods with enforced constraint of equal position, velocity, and
;  accelerations of all points pinned together on different rods.


; GENERAL SIMULATOR FUNCTION DEFINITIONS:

; The PROGRAM'S MAIN LOOP:

(defun run-hinge (&optional single-machine-mode?)
  (setq *DEBUG-LIST* nil)
  (cond ((or single-machine-mode? (null *MY-MOBY-HOST*)) (run-single))
	(*COMPUTER-MOBY-HOST* (run-displayer))
	(*DISPLAYER-MOBY-HOST* (run-computer))
	(t (ferror nil "Hosts screwed.")))
  (values))


(defun run-single (&aux (*MOUSE-HORIZ* 0.0s0))
  (unwind-protect
      (catch 'RUNFUN
	(send (car (send *TERMINAL-IO* :blinker-list)) :set-visibility nil)
	(tv:with-mouse-usurped
	  (do ((display-array (make-array 12.)))
	      (( ))
	    (catch 'REPEAT
	      (clear)
	      (initialize-body)
	      (do* ((rod-array-list (get-frame *ROBOT-FRAME* 'ROD-ARRAY-LIST))
		    (rod-1 (first rod-array-list))
		    (rod-2 (second rod-array-list))
		    (time 0. (1+ time)))
		   (( ))
		(when (>= time 512.)
		  (setq time 0.))
		(do ((rod-arrays rod-array-list (cdr rod-arrays)))
		    ((null rod-arrays))
		  (run-1 (car rod-arrays)))
		(get-interaction-forces rod-1 rod-2 rod-array-list)
		(compute-display rod-1 rod-2 time display-array)
		(screen-display-body display-array)
		(when (listen)
		  (user-break (read-char)))
		(do ((rod-arrays rod-array-list (cdr rod-arrays)))
		    ((null rod-arrays))
		  (run-2 (car rod-arrays))))))))
    (cursorset (1- *MAX-LINE*) 0)
    (send (car (send *TERMINAL-IO* :blinker-list)) :set-visibility :blink))
  (values))


(defun run-computer (&aux (*MOUSE-HORIZ* 0.0s0) self-display?)
  (setq self-display? (y-or-n-p "Display on this screen too ?"))
  (let* ((mfs:MLM-AUTOMATICALLY-CREATE-DIRECTORIES t)
	 (root (open (string-append *DISPLAYER-MOBY-HOST-NAME* ":BAR;HINGE-DATA.MBY")
		     :direction :output :moby-mapped t
		     :if-exists :append :if-does-not-exist :create))
	 (root-area (%area-number root))
	 (moby-bucket))
    (cond ((and (null (car root))
		(null (cdr root)))
	   (setq moby-bucket (make-array 12. :area root-area))
	   (rplaca root moby-bucket))
	  ((and (arrayp (car root))
		(null (cdr root)))
	   (setq moby-bucket (car root)))
	  (t (ferror nil "ROOT screwed")))
    (unwind-protect
	(catch 'RUNFUN
	  (when self-display?
	    (send (car (send *TERMINAL-IO* :blinker-list)) :set-visibility nil))
	  (tv:with-mouse-usurped
	    (do ((display-array (make-array 12.)))
		(( ))
	      (catch 'REPEAT
		(when self-display?
		  (clear))
		(initialize-body)
		(do* ((rod-array-list (get-frame *ROBOT-FRAME* 'ROD-ARRAY-LIST))
		      (rod-1 (first rod-array-list))
		      (rod-2 (second rod-array-list))
		      (time 0. (1+ time)))
		     (( ))
		  (do ((rod-arrays rod-array-list (cdr rod-arrays)))
		      ((null rod-arrays))
		    (run-1 (car rod-arrays)))
		  (get-interaction-forces rod-1 rod-2 rod-array-list)
		  (compute-display rod-1 rod-2 time display-array)
		  (when self-display?
		    (screen-display-body display-array))
		  (mfs:moby-wait-for-data moby-bucket)
		  (copy-array-contents display-array moby-bucket)
		  (mfs:moby-give-data-to-host moby-bucket *DISPLAYER-MOBY-HOST*)
		  (when (listen)
		    (user-break (read-char)))
		  (do ((rod-arrays rod-array-list (cdr rod-arrays)))
		      ((null rod-arrays))
		    (run-2 (car rod-arrays))))))))
      (mfs:moby-wait-for-data moby-bucket)
      (setf (aref moby-bucket 0.) nil)
      (mfs:moby-give-data-to-host moby-bucket *DISPLAYER-MOBY-HOST*)
      (mfs:moby-wait-for-data moby-bucket)
      (when self-display?
	(cursorset (1- *MAX-LINE*) 0)
	(send (car (send *TERMINAL-IO* :blinker-list)) :set-visibility :blink))))
  (values))
  

(defun run-displayer (&aux (*MOUSE-HORIZ* 0.0s0))
  (let* ((mfs:MLM-AUTOMATICALLY-CREATE-DIRECTORIES t)
	 (root (open (string-append *MY-MOBY-HOST-NAME* ":BAR;HINGE-DATA.MBY")
		     :direction :output :moby-mapped t
		     :if-exists :append :if-does-not-exist :create))
	 (moby-bucket))
    (setq moby-bucket (car root))
    (unless (arrayp (car root))
      (format t "~%Start COMPUTER side first.")
      (return-from RUN-DISPLAYER nil))
    (unwind-protect
	(catch 'RUNFUN
	  (send (car (send *TERMINAL-IO* :blinker-list)) :set-visibility nil)
	  (tv:with-mouse-usurped
	    (do ((display-array (make-array 12.)))
		(( ))
	      (catch 'REPEAT
		(clear)
		(do ((counter 0. (1+ counter)))
		    (( ))
		  (mfs:moby-wait-for-data moby-bucket)
		  (when (null (aref moby-bucket 0.))
		    (mfs:moby-give-data-to-host moby-bucket *COMPUTER-MOBY-HOST*)
		    (throw 'RUNFUN nil))
		  (copy-array-contents moby-bucket display-array)
		  (mfs:moby-give-data-to-host moby-bucket *COMPUTER-MOBY-HOST*)
		  ;; This is an error-check because of packet screwage.
		  ;; Remove it when things seem to be OK again.
		  (cond ((every #'PACKET-TEST display-array)
			 (screen-display-body display-array))
			(t (push (list counter (listarray display-array))
				 *DEBUG-LIST*)))
		  (when (listen)
		    (user-break (read-char))))))))
      (print-line (- *MAX-LINE* 3) 0 "Return from RUN-DISPLAYER.")
      (send (car (send *TERMINAL-IO* :blinker-list)) :set-visibility :blink)))
  (values))


(defun packet-test (item)
  (and (fixp item)
       (> item -512.)
       (< item 512.)))


(defun run-1 (rod &aux lxf lyf uxf uyf angle-veloc cm-x cm-x-veloc
	      cm-y cm-y-veloc lcm ucm cos-angle sin-angle lower-x lower-x-veloc mass
	      lower-y lower-y-veloc upper-x upper-x-veloc upper-y upper-y-veloc moi)
  
  (setq lcm (ROD-LCM rod)
	ucm (ROD-UCM rod)
	mass (ROD-MASS rod)
	moi (ROD-MOI rod)
	angle-veloc (ROD-ANGLE-VELOC rod)
	cos-angle (ROD-COSANGLE rod)
	sin-angle (ROD-SINANGLE rod))
  
  (setq cm-x (ROD-CM-X rod)
	cm-y (ROD-CM-Y rod)
	cm-x-veloc (ROD-CM-X-VELOC rod)
	cm-y-veloc (ROD-CM-Y-VELOC rod)
	lower-x (- cm-x (* lcm cos-angle))
	lower-y (- cm-y (* lcm sin-angle))
	upper-x (+ cm-x (* ucm cos-angle))
	upper-y (+ cm-y (* ucm sin-angle))
	lower-x-veloc (+ cm-x-veloc (* lcm sin-angle angle-veloc))
	lower-y-veloc (- cm-y-veloc (* lcm cos-angle angle-veloc))
	upper-x-veloc (- cm-x-veloc (* ucm sin-angle angle-veloc))
	upper-y-veloc (+ cm-y-veloc (* ucm cos-angle angle-veloc)))
  
  (setf (ROD-LOWER-X rod) lower-x
	(ROD-LOWER-Y rod) lower-y
	(ROD-UPPER-X rod) upper-x
	(ROD-UPPER-Y rod) upper-y
	(ROD-LOWER-X-VELOC rod) lower-x-veloc
	(ROD-LOWER-Y-VELOC rod) lower-y-veloc
	(ROD-UPPER-X-VELOC rod) upper-x-veloc
	(ROD-UPPER-Y-VELOC rod) upper-y-veloc)
  
  (cond ((> lower-y 0.0s0) (setq lxf 0.0s0 lyf 0.0s0))
	(t (setq lyf (- (+ (* lower-y-veloc *FLOOR-DAMPING*)
			   (* lower-y *FLOOR-ELASTICITY*)))
		 lxf (friction lower-x-veloc lyf))))
  
  (cond ((> upper-y 0.0s0) (setq uxf 0.0s0 uyf 0.0s0))
	(t (setq uyf (- (+ (* upper-y-veloc *FLOOR-DAMPING*)
			   (* upper-y *FLOOR-ELASTICITY*)))
		 uxf (friction upper-x-veloc uyf))))
  
  (setf (ROD-LXF rod) lxf
	(ROD-LYF rod) lyf
	(ROD-UXF rod) uxf
	(ROD-UYF rod) uyf))


(defun get-interaction-forces (r1 r2 rod-array-list &aux at emass sangle1 cangle1 imass1
			       csm1 ccm1 ssm1 xf1 oxf1 yf1 oyf1 sangle2 cangle2 av2 imass2
			       csm2 ccm2 ssm2 xf2 oxf2 yf2 oyf2 solution (iflag t)
			       x-correction y-correction lcm ucm lxf lyf uxf uyf angle-accel
			       cm-x-accel cm-y-accel ltot mass cos-angle sin-angle)
  (setq imass1 (/ 1.0s0 (ROD-MASS r1))
	emass (/ 1.0s0 (ROD-M3 r1))
	sangle1 (ROD-SINANGLE r1)
	cangle1 (ROD-COSANGLE r1)
	csm1 (* emass cangle1 sangle1)
	ccm1 (* emass cangle1 cangle1)
	ssm1 (* emass sangle1 sangle1)
	xf1 (ROD-UXF r1)
	yf1 (ROD-UYF r1)
	oxf1 (ROD-LXF r1)
	oyf1 (ROD-LYF r1))
  (setq imass2 (/ 1.0s0 (ROD-MASS r2))
	emass (/ 1.0s0 (ROD-M1 r2))
	sangle2 (ROD-SINANGLE r2)
	cangle2 (ROD-COSANGLE r2)
	csm2 (* emass cangle2 sangle2)
	ccm2 (* emass cangle2 cangle2)
	ssm2 (* emass sangle2 sangle2)
	xf2 (ROD-LXF r2)
	yf2 (ROD-LYF r2)
	oxf2 (ROD-UXF r2)
	oyf2 (ROD-UYF r2))
  (setq at (funcall *SERVO* r1 r2)
	x-correction (+ (* *JOINT-ELASTICITY* (- (ROD-LOWER-X r2) (ROD-UPPER-X r1)))
			(* *JOINT-DAMPING* (- (ROD-LOWER-X-VELOC r2) (ROD-UPPER-X-VELOC r1))))
	y-correction (+ (* *JOINT-ELASTICITY* (- (ROD-LOWER-Y r2) (ROD-UPPER-Y r1)))
			(* *JOINT-DAMPING* (- (ROD-LOWER-Y-VELOC r2) (ROD-UPPER-Y-VELOC r1)))))
  (setf (ROD-APPLIED-TORQUE r1) (- at)
	(ROD-APPLIED-TORQUE r2) at)
  (catch 'INCONSISTENT
    (progn
      (setq solution
	    (simq (list (list 1.0s0 0.0s0 1.0s0 0.0s0 0.0s0)
			(list 0.0s0 1.0s0 0.0s0 1.0s0 0.0s0)
			(list (+ imass1 ssm1)
			      (- csm1)
			      (- (+ imass2 ssm2))
			      csm2
			      (+ x-correction
				 (* imass2 (+ xf2 oxf2))
				 (- (* imass1 (+ xf1 oxf1)))
				 (* csm1 (- yf1 oyf1))
				 (* csm2 (- oyf2 yf2))
				 (* ssm2 (- xf2 oxf2))
				 (* ssm1 (- oxf1 xf1))
				 (* (ROD-UCM r1) cangle1 (square (ROD-ANGLE-VELOC r1)))
				 (* (ROD-LCM r2) cangle2 (square (ROD-ANGLE-VELOC r2)))
				 (/ (* (ROD-UCM r1) sangle1 (- at)) (ROD-MOI r1))
				 (/ (* (ROD-LCM r2) sangle2 at) (ROD-MOI r2))))
			(list (- csm1)
			      (+ imass1 ccm1)
			      csm2
			      (- (+ imass2 ccm2))
			      (+ y-correction
				 (* imass2 (+ yf2 oyf2))
				 (- (* imass1 (+ yf1 oyf1)))
				 (* csm1 (- xf1 oxf1))
				 (* csm2 (- oxf2 xf2))
				 (* ccm2 (- yf2 oyf2))
				 (* ccm1 (- oyf1 yf1))
				 (* (ROD-UCM r1) sangle1 (square (ROD-ANGLE-VELOC r1)))
				 (* (ROD-LCM r2) sangle2 (square (ROD-ANGLE-VELOC r2)))
				 (- (/ (* (ROD-LCM r2) cangle2 at) (ROD-MOI r2)))
				 (- (/ (* (ROD-UCM r1) cangle1 (- at)) (ROD-MOI r1))))))))
      (setq iflag nil)))
  (when iflag (setq solution '(0.0s0 0.0s0 0.0s0 0.0s0)))
  (setf (ROD-UXF r1) (+ xf1 (first solution))
	(ROD-UYF r1) (+ yf1 (second solution))
	(ROD-LXF r2) (+ xf2 (third solution))
	(ROD-LYF r2) (+ yf2 (fourth solution)))
  (do ((rods rod-array-list (cdr rods)) (rod))
      ((null rods))
    (setq rod (car rods)
	  mass (ROD-MASS rod)
	  ltot (ROD-LTOT rod)
	  lxf (ROD-LXF rod)
	  lyf (ROD-LYF rod)
	  uxf (ROD-UXF rod)
	  uyf (ROD-UYF rod)
	  lcm (ROD-LCM rod)
	  ucm (ROD-UCM rod)
	  av2 (square (ROD-ANGLE-VELOC rod))
	  cos-angle (ROD-COSANGLE rod)
	  sin-angle (ROD-SINANGLE rod)
	  cm-x-accel (/ (+ lxf uxf) mass)
	  cm-y-accel (- (/ (+ lyf uyf) mass) *GRAVITY*)
	  angle-accel (/ (+ (* (- uyf lyf) ltot cos-angle)
			    (* (- lxf uxf) ltot sin-angle)
			    (ROD-APPLIED-TORQUE rod))
			 (ROD-MOI rod)))
    (setf (ROD-LOWER-X-ACCEL rod) (+ cm-x-accel
				     (* lcm sin-angle angle-accel)
				     (* lcm cos-angle av2))
	  (ROD-LOWER-Y-ACCEL rod) (+ cm-y-accel
				     (- (* lcm cos-angle angle-accel))
				     (* lcm sin-angle av2))
	  (ROD-UPPER-X-ACCEL rod) (- cm-x-accel
				     (* ucm sin-angle angle-accel)
				     (* ucm cos-angle av2))
	  (ROD-UPPER-Y-ACCEL rod) (+ cm-y-accel
				     (* ucm cos-angle angle-accel)
				     (- (* ucm sin-angle av2)))
	  (ROD-CM-X-ACCEL rod) cm-x-accel
	  (ROD-CM-Y-ACCEL rod) cm-y-accel
	  (ROD-ANGLE-ACCEL rod) angle-accel)))


(defun run-2 (rod &aux angle)
  
  (setf (ROD-CM-X rod)
	(+ (ROD-CM-X rod)
	   (* 0.005s0				; including TIMESTEP 0.01 sec.
	      (+ (ROD-CM-X-VELOC rod)
		 (setf (ROD-CM-X-VELOC rod)
		       (+ (ROD-CM-X-VELOC rod)
			  (* 0.01s0 (ROD-CM-X-ACCEL rod))))))))
  
  (setf (ROD-CM-Y rod)
	(+ (ROD-CM-Y rod)
	   (* 0.005s0				; including TIMESTEP 0.01 sec.
	      (+ (ROD-CM-Y-VELOC rod)
		 (setf (ROD-CM-Y-VELOC rod)
		       (+ (ROD-CM-Y-VELOC rod)
			  (* 0.01s0 (ROD-CM-Y-ACCEL rod))))))))
  
  (setq angle
	(test-mod (+ (ROD-ANGLE rod)
		     (* 0.005s0			; including TIMESTEP 0.01 sec.
			(+ (ROD-ANGLE-VELOC rod)
			   (setf (ROD-ANGLE-VELOC rod)
				 (+ (ROD-ANGLE-VELOC rod)
				    (* 0.01s0 (ROD-ANGLE-ACCEL rod)))))))
		  TWOPI))
  
  (setf (ROD-ANGLE rod) angle
	(ROD-COSANGLE rod) (cosine angle)
	(ROD-SINANGLE rod) (sine angle)))


(defun friction (velocity normal-force)
  (setq normal-force (max normal-force 0.0s0))
  (cond ((> velocity 1.0s0) (- (* *FRICTION* normal-force)))
	((< velocity -1.0s0) (* *FRICTION* normal-force))
	(t (- (* velocity *FRICTION* normal-force)))))


(defun initialize-body (&aux r-frame)
  (do ((rod-frame-list (get-frame *ROBOT-FRAME* 'ROD-FRAME-LIST) (cdr rod-frame-list)))
      ((null rod-frame-list))
    (initialize-rod (setq r-frame (car rod-frame-list))
		    (get-frame r-frame 'ROD-ARRAY)))
  t)


(defun initialize-rod (r-frame rod &aux cos-angle sin-angle temp)
  (dotimes (i (array-length rod))
    (setf (aref rod i) nil))
  (setf (ROD-LTOT rod) (get-frame r-frame 'LTOT)
	(ROD-M1 rod) (get-frame r-frame 'M1)
	(ROD-M2 rod) (get-frame r-frame 'M2)
	(ROD-M3 rod) (get-frame r-frame 'M3)
	temp (/ (ROD-LTOT rod) (+ (ROD-M1 rod) (ROD-M3 rod)))
	(ROD-LCM rod) (* (ROD-M3 rod) temp)
	(ROD-UCM rod) (* (ROD-M1 rod) temp)
	(ROD-MASS rod) (+ (ROD-M1 rod) (ROD-M2 rod) (ROD-M3 rod))
	(ROD-MOI rod) (+ (* (ROD-M1 rod) (square (ROD-LCM rod)))
			 (* (ROD-M3 rod) (square (ROD-UCM rod))))
	(ROD-ANGLE rod) (get-frame r-frame 'SAVE-ANGLE)
	(ROD-ANGLE-VELOC rod) 0.0s0
	cos-angle (cosine (ROD-ANGLE rod))
	sin-angle (sine (ROD-ANGLE rod))
	(ROD-COSANGLE rod) cos-angle
	(ROD-SINANGLE rod) sin-angle
	(ROD-CM-X rod) (+ (get-frame r-frame 'SAVE-LOWER-X)
			  (* (ROD-LCM rod) cos-angle))
	(ROD-CM-Y rod) (+ (get-frame r-frame 'SAVE-LOWER-Y)
			  (* (ROD-LCM rod) sin-angle))
	(ROD-CM-X-VELOC rod) 0.0s0
	(ROD-CM-Y-VELOC rod) 0.0s0)
  t)

	   
; MOUSE CONTROL:

(defun mouse-read (&aux (horizontal 0.) (vertical 0.))
  (multiple-value-setq (horizontal vertical)
    (tv:mouse-input nil))
  (setq vertical (- vertical))
  (setq *MOUSE-HORIZ* (+ (* MOUSE-SCALE horizontal) *MOUSE-HORIZ*)))


; USER CONTROL OF SYSTEM STATUS:

(defun user-break (ch &optional skip-space? &aux temp throw-tag clear?
		   (blinker (car (send *TERMINAL-IO* :blinker-list))))
  (tv:mouse-input nil)
  (cond
    ((char= ch #\SPACE)
     (or skip-space? (user-break (read-char) t)))
    ((char= ch #\RUBOUT) (setq throw-tag 'RUNFUN))
    
    ((char= ch #\e)
     (print-line 42. 4. "Eval:  ")
     (send blinker :set-visibility :blink)
     (setq temp (eval (read)))
     (send blinker :set-visibility nil)
     (terpri)
     (write-string "Returned:  ")
     (prin1 temp))
    
    ((char= ch #\l)
     (setq clear? t))
    ((char= ch #\r)
     (setq throw-tag 'REPEAT))

    ((char= ch #\v)
     (show)
     (terprinc2 "  SPACE or other key to continue:  ")
     (send blinker :set-visibility :blink)
     (user-break (read-char) t)
     (send blinker :set-visibility nil)
     (setq clear? t))
    ((char= ch #\z)
     (setq *MOUSE-HORIZ* 0.0s0))
    
    (t (clear)
       (princ "	Choices are:

      ?  --  Print this information.

 <space> --  Halt momentarily, then Continue.
<rubout> --  Quit to top-level.

      e  --  EVALUATE single lisp expression.

      l  --  Clear Screen.

      r  --  RESTART current run from beginning.

      v  --  Values shown for System Parameters.
      z  --  Zero Mouse-control.

  SPACE or one of above to continue: ")
       (clear-input)
       (send blinker :set-visibility :blink)
       (user-break (read-char) t)
       (send blinker :set-visibility nil)
       (setq clear? t)))
  
  (when throw-tag (throw throw-tag nil))
  (when clear? (clear))
  (tv:mouse-input nil))


(defun save-starting-configuration ( )
  (do ((rod-frame-list (get-frame *ROBOT-FRAME* 'ROD-FRAME-LIST) (cdr rod-frame-list))
       (rod-array-list (get-frame *ROBOT-FRAME* 'ROD-ARRAY-LIST) (cdr rod-array-list)))
      ((null rod-frame-list))
    (set-frame (ROD-ANGLE (car rod-array-list)) (car rod-frame-list) 'SAVE-ANGLE)
    (set-frame (ROD-LOWER-X (car rod-array-list)) (car rod-frame-list) 'SAVE-LOWER-X)
    (set-frame (ROD-LOWER-Y (car rod-array-list)) (car rod-frame-list) 'SAVE-LOWER-Y))
  "Current configuration saved.")


;;; End.