;;;---	HCON >		-*- package: user; mode: lisp; base: 10. -*-

; ROBOT GRAPHICS

(defun protected-baseline (ox oy nx ny &optional (alufn 't) (endpt? t))
  (safety-check ox 767. oy 883. nx 767. ny 883.)
  (tv:%draw-line ox oy nx ny (cond ((null alufn) TV:ALU-ANDCA)
				   ((eq alufn 't) TV:ALU-IOR)
				   (t TV:ALU-XOR))
		 endpt? TERMINAL-IO))


(defun safety-check (&rest arguments)
  (do ((values arguments (cddr values))
       (value) (limit))
      ((null values))
    (setq value (first values) limit (second values))
    (cond ((or (< value 0.) (> value limit))
	   (break-error "Off-screen coordinates for some graphics function." arguments)
	   (^g "This is an unproceedable error.")))))


(defun gwcross (x y)
  (safety-check (- x 2.) 752. (- y 2.) (- GRAPHICS-SCREEN-HEIGHT 2.))
  (gwbaseline (- x 2.) y (+ x 2.) y)
  (gwbaseline x (- y 2.) x (+ y 2.)))


(defun cross (x y flag)
  (safety-check (- x 2.) 756. (- y 2.) 879.)
  (cond ((null flag)
	 (baseline-erase (- x 2.) y (+ x 2.) y) (baseline-erase x (- y 2.) x (+ y 2.)))
	((eq flag 't)
	 (baseline (- x 2.) y (+ x 2.) y) (baseline x (- y 2.) x (+ y 2.)))
	(t (baseline-xor (- x 2.) y (+ x 2.) y) (baseline-xor x (- y 2.) x (+ y 2.)))))


(defun gwsq (x y)
  (let ((xr (+ x 3.)) (xl (- x 3.)) (yd (+ y 3.)) (yu (- y 3.)))
    (tv:prepare-sheet (graphics-window)
      (gwbaseline xl yu xr yu) (gwbaseline xr yu xr yd)
      (gwbaseline xr yd xl yd) (gwbaseline xl yd xl yu))))


(defun sq (x y flag &optional (size 2.))
  (let ((xr (+ x size)) (xl (- x size)) (yd (+ y size)) (yu (- y size)))
    (safety-check xr 767. xl 767. yd 883. yu 883.)
    (cond ((eq flag 't)
	   (baseline xl yu xr yu) (baseline xr yu xr yd)
	   (baseline xr yd xl yd) (baseline xl yd xl yu))
	  ((null flag)
	   (baseline-erase xl yu xr yu) (baseline-erase xr yu xr yd)
	   (baseline-erase xr yd xl yd) (baseline-erase xl yd xl yu))
	  (t (baseline-xor xl yu xr yu) (baseline-xor xr yu xr yd)
	     (baseline-xor xr yd xl yd) (baseline-xor xl yd xl yu)))))


(defun rectangle (x y xhalfwidth yhalfwidth flag)
  (let ((xr (+ x xhalfwidth)) (xl (- x xhalfwidth))
	(yd (+ y yhalfwidth)) (yu (- y yhalfwidth)))
    (safety-check xr 767. xl 767. yd 883. yu 883.)
    (cond ((eq flag 't)
	   (baseline xl yu xr yu) (baseline xr yu xr yd)
	   (baseline xr yd xl yd) (baseline xl yd xl yu))
	  ((null flag)
	   (baseline-erase xl yu xr yu) (baseline-erase xr yu xr yd)
	   (baseline-erase xr yd xl yd) (baseline-erase xl yd xl yu))
	  (t (baseline-xor xl yu xr yu) (baseline-xor xr yu xr yd)
	     (baseline-xor xr yd xl yd) (baseline-xor xl yd xl yu)))))


(defun draw-value (value label ypos &aux nvalue scale xpos (PRINT-MODE 4.))
  (multiple-value (nvalue scale)
    (normalize-symmetrically value))
  (setq xpos (min (max (+ 383. (round (* 363.0s0 nvalue)))
		       20.)
		  747.))
  (cond (SCREEN-CLEARED?
	 (cursorpoint (- 368. (* 8. (flatc label))) (+ ypos 2.) ':CLEAR-EOL)
	 (princ-rest label "   " scale)))
  (cond (PRINT-VAL? (cursorpoint 391. (+ ypos 2.) ':CLEAR-EOL)
		    (princ-rest value))
	(t (tv:prepare-sheet (terminal-io)
	     (baseline-erase 20. ypos 747. ypos)
	     (baseline 383. ypos xpos ypos)
	     (baseline 383. (- ypos 2.) 383. (+ ypos 2.))))))


(defun normalize-symmetrically (value &aux list absval scale max)
  (setq list (car NORMALIZATION-LIST)
	max (first list)
	scale (second list)
	absval (abs value)
	max (cond ((> absval max) (setf (first list) absval) absval)
		  (t max)))
  (rplaca NORMALIZATION-LIST (cddr list))
  (and (null scale) (setf (second list) (setq scale max)))
  (values (cond ((zerop scale) 0.0s0)
		(t (range (// value scale) 1.0s0)))
	  scale))


(defun set-normalization (reset?)
  (or (numberp (second NORMALIZATION-LIST)) (setq reset? t))
  (do ((max (cdr NORMALIZATION-LIST) (cddr max))
       (scale (cddr NORMALIZATION-LIST) (cddr scale)))
      ((null max) "Normalization complete.")
    (cond (reset? (rplaca max 0.0s0) (rplaca scale nil))
	  (t (rplaca scale (round-up (car max)))))))


(defun round-up (x)
  (cond ((zerop x) 0.0s0)
	(t (do ((val 16. (1- val))
		(temp))
	       ((< (setq temp (expt 10.0s0 val)) x)
		(cond ((>= (setq temp (* temp 2.0s0)) x) temp)
		      ((>= (setq temp (* temp 2.5s0)) x) temp)
		      (t (* temp 2.0s0))))))))


(defun screen-display-body (rod-array-list windforce &aux (limit (// 166.0s0 ROBOT-SCALE))
			    (robot-size (* ROBOT-SCALE 2.2559s0))
			    (screen-c-y (- GRAPHICS-SCREEN-HEIGHT 10.))
			    (gscreen-bot (// (+ GRAPHICS-SCREEN-HEIGHT 4.) 14.)))
  (cond (DEBUGGING?)
	(t (and SCREEN-CLEARED?
		(dstat))
	   (rplaca NORMALIZATION-LIST (cdr NORMALIZATION-LIST))
	   (cond ((or STEPPING? (zerop (\ SIM-TIME 100.)))
		  (cursorset (1+ gscreen-bot) 15. (time-display SIM-TIME nil) 'CHARS)
		  (time-display SIM-TIME t)))
	   (and (or (zerop ERASE-COUNT)
		    (zerop (\ (// SIM-TIME DISPLAY-TIME) ERASE-COUNT)))
		(tv:sheet-clear GRAPHICS-WINDOW))
	   (tv:prepare-sheet (graphics-window)
	     (do ((rods rod-array-list (cdr rods))
		  (rod) (lx) (ly) (ux) (uy) (*lx) (*ly) (*ux) (*uy))
		 ((null rods))
	       (setq rod (car rods) lx (LOWER-X rod)
		     ly (LOWER-Y rod) ux (UPPER-X rod) uy (UPPER-Y rod))
	       (cond ((>= (- (max lx ux) HCENTER) limit)
		      (setq HCENTER (1+ HCENTER)) (label-axis))
		     ((<= (- (min lx ux) HCENTER) (- limit))
		      (setq HCENTER (1- HCENTER)) (label-axis)))
	       (setq *lx (xlim (+ 383. (round (* robot-size (- lx HCENTER)))))
		     *ly (ylim (- screen-c-y (round (* robot-size ly))))
		     *ux (xlim (+ 383. (round (* robot-size (- ux HCENTER)))))
		     *uy (ylim (- screen-c-y (round (* robot-size uy)))))
	       (gwsq *lx *ly) (gwbaseline *lx *ly *ux *uy) (gwsq *ux *uy))
	     (gwbaseline 12. (+ screen-c-y 4.) 750. (+ screen-c-y 4.))
	     (gwcross (xlim (+ 383. (round (* robot-size (- MOUSE-HORIZ HCENTER)))))
		    (ylim (- screen-c-y (round (* robot-size MOUSE-VERT)))))
	     (bitblt 5. 768. GRAPHICS-SCREEN-HEIGHT GRAPHICS-WINDOW-ARRAY
		     0. 0. SCREEN-ARRAY 0. 0.))
	   (draw-value windforce "Wind" (+ (* gscreen-bot 14.) 46.))
	   ;; Put any fcns which test SCREEN-CLEARED? before SCOPEN.
	   (cond ((= DISPLAY-MODE 1.)
		  (scopen DISPLAY-LIST 0. SCOPE-TOP 802.))
		 ((= DISPLAY-MODE 2.)
		  (miscellaneous-draw-values MDV-LIST1) (setq SCREEN-CLEARED? nil))
		 ((= DISPLAY-MODE 3.)
		  (miscellaneous-draw-values MDV-LIST2)
		  (scopen DISPLAY-LIST 0. SCOPE-TOP 802.))
		 (t (setq SCREEN-CLEARED? nil)))
	   (cursorset 62. 95.)))
  t)


(defun miscellaneous-draw-values (list)
  (do ((items list (cdddr items)))
      ((null items))
    (draw-value (symeval (first items)) (second items) (third items))))


(defun dstat ( )
  (tv:prepare-sheet (terminal-io)
    (baseline 0. 0. 767. 0.)
    (baseline 0. 0. 0. (+ GRAPHICS-SCREEN-HEIGHT 4.))
    (baseline 0. (+ GRAPHICS-SCREEN-HEIGHT 4.) 767. (+ GRAPHICS-SCREEN-HEIGHT 4.))
    (baseline 767. 0. 767. (+ GRAPHICS-SCREEN-HEIGHT 4.))
    (label-axis)))


; MISCELLANEOUS UTILITY FUNCTIONS:

(defun show ( )
  (clear)
  (princ-rest "		        TimeStep:   " TIMESTEP " msec.") (terpri)
  (princ-rest "		    Display-Time:   " DISPLAY-TIME " msec.")
  (terpri) (terpri)
  (princ-rest "		Joint-Elasticity:  " JOINT-ELASTICITY) (terpri)
  (princ-rest "		   Joint-Damping:  " JOINT-DAMPING)
  (terpri) (terpri)
  (princ-rest "		Floor-Elasticity:  " FLOOR-ELASTICITY) (terpri)
  (princ-rest "		   Floor-Damping:  " FLOOR-DAMPING) (terpri)
  (princ-rest "		        Friction:  " FRICTION) (terpri)
  (show-misc)
  DONE)


(defun show-misc (&optional (items show-list) &aux thing)
  (do ((print-list items (cdr print-list)))
      ((null print-list))
    (terpri)
    (setq thing (car print-list))
    (skip-to (- 32. (flatsize thing)))
    (funcall PRIN1 thing)
    (princ ":   ")
    (funcall PRIN1 (cond ((or (not (symbolp thing)) (boundp thing)) (eval thing))
			 (t "Unbound."))))
  (terpri)
  DONE)


(defun time-display (tim write? &aux left middle right)
  (setq left (// tim 1000.)
	right (// (\ tim 1000.) 10.)
	middle (cond ((< right 10.) "0")
		     (t "")))
  (let ((*nopoint t))
    (cond (write? (princ-rest2 left "." middle right))
	  (t (+ (flatc left) 3.)))))


(defun label-axis (&aux (line (1+ (// (+ GRAPHICS-SCREEN-HEIGHT 4.) 14.))))
  (print-line line 1. (- HCENTER 2.))
  (line-print2 8. "Time:  ") (time-display SIM-TIME t)
  (line-print2 31. "Page: " (cond (FREERUN? "Free") (t "Sync")))
  (line-print2 48. "Control: " SERVO)
  (line-print2 90. (+ HCENTER 2.))
  (print-line (1+ line) 8. "Horiz: ")
  (time-display (// (* 100. DISPLAY-TIME) GRATE) t)
  (princ " sec//in" TERMINAL-IO)
  (line-print2 31. "DM: " DISPLAY-MODE)
  (cursorset 62. 95.)
  (+ line 2.))					; Returns next free line number.


;;; End.