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

; ROBOT GRAPHICS

(defun protected-baseline (ox oy nx ny alu-fun &optional (endpt? t))
  (safety-check ox *MAX-SCREEN-X*
		oy *MAX-SCREEN-Y*
		nx *MAX-SCREEN-X*
		ny *MAX-SCREEN-Y*)
  (tv:%draw-line ox oy nx ny alu-fun 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))
	   (ferror nil "Off-screen coordinates for some graphics function.")))))


(defun draw-value (value xcenter ycenter)
  (setq value (xlim (+ value xcenter)))
  (let ((x-left (min xcenter value))
	(x-right (max xcenter value))
	(y-upper (1- ycenter))
	(cross-upper (- ycenter 4.))
	(cross-lower (+ ycenter 3.))
	(cross-left (1- xcenter)))
    (baseline 0. y-upper x-left y-upper TV:ALU-ANDCA)
    (baseline 0. ycenter x-left ycenter TV:ALU-ANDCA)
    (baseline x-right y-upper *MAX-SCREEN-X* y-upper TV:ALU-ANDCA)
    (baseline x-right ycenter *MAX-SCREEN-X* ycenter TV:ALU-ANDCA)
    (baseline x-left y-upper x-right y-upper TV:ALU-IOR)
    (baseline x-left ycenter x-right ycenter TV:ALU-IOR)
    (baseline cross-left cross-upper cross-left cross-lower TV:ALU-IOR)
    (baseline xcenter cross-upper xcenter cross-lower TV:ALU-IOR)))


(defun gwcross (x y alu-fcn)
  (safety-check (- x 2.) *MAX-SCREEN-X* (- y 2.) *MAX-SCREEN-Y*)
  (gwbaseline (- x 2.) y (+ x 2.) y alu-fcn)
  (gwbaseline x (- y 2.) x (+ y 2.) alu-fcn))


(defun fat-gwcross (x y alu-fcn)
  (safety-check (- x 5.) *MAX-SCREEN-X* (- y 5.) *MAX-SCREEN-Y*)
  (gwbaseline (- x 5.) y (+ x 4.) y alu-fcn)
  (gwbaseline (- x 5.) (1- y) (+ x 4.) (1- y) alu-fcn)
  (gwbaseline (1- x) (- y 5.) (1- x) (+ y 4.) alu-fcn)
  (gwbaseline x (- y 5.) x (+ y 4.) alu-fcn))


(defun cross (x y alu-fcn)
  (safety-check (- x 4.) *MAX-SCREEN-X* (- y 4.) *MAX-SCREEN-Y*)
  (baseline (- x 4.) y (+ x 4.) y alu-fcn)
  (baseline x (- y 4.) x (+ y 4.) alu-fcn))


(defun gwsq (x y alu-fcn)
  (let ((xr (+ x 4.))
	(xl (- x 4.))
	(yd (+ y 4.))
	(yu (- y 4.)))
    (gwbaseline xl yu xr yu alu-fcn)
    (gwbaseline xr yu xr yd alu-fcn)
    (gwbaseline xr yd xl yd alu-fcn)
    (gwbaseline xl yd xl yu alu-fcn)))


(defun fat-gwsq (x y alu-fcn)
  (let ((xr (+ x 4.))
	(xrb (+ x 5.))
	(xl (- x 5.))
	(xlb (- x 6.))
	(yd (+ y 4.))
	(ydb (+ y 5.))
	(yu (- y 5.))
	(yub (- y 6.)))
    (gwbaseline xl yu xr yu alu-fcn)
    (gwbaseline xlb yub xrb yub alu-fcn)
    (gwbaseline xr yu xr yd alu-fcn)
    (gwbaseline xrb yub xrb ydb alu-fcn)
    (gwbaseline xr yd xl yd alu-fcn)
    (gwbaseline xrb ydb xlb ydb alu-fcn)
    (gwbaseline xl yd xl yu alu-fcn)
    (gwbaseline xlb ydb xlb yub alu-fcn)))


(defun sq (x y alu-fcn &optional (size 3.))
  (let ((xr (+ x size))
	(xl (- x size))
	(yd (+ y size))
	(yu (- y size)))
    (safety-check xr *MAX-SCREEN-X*
		  xl *MAX-SCREEN-X*
		  yd *MAX-SCREEN-Y*
		  yu *MAX-SCREEN-Y*)
    (baseline xl yu xr yu alu-fcn)
    (baseline xr yu xr yd alu-fcn)
    (baseline xr yd xl yd alu-fcn)
    (baseline xl yd xl yu alu-fcn)))


(defun rectangle (x y xhalfwidth yhalfwidth alu-fcn)
  (let ((xr (+ x xhalfwidth))
	(xl (- x xhalfwidth))
	(yd (+ y yhalfwidth))
	(yu (- y yhalfwidth)))
    (safety-check xr *MAX-SCREEN-X*
		  xl *MAX-SCREEN-X*
		  yd *MAX-SCREEN-Y*
		  yu *MAX-SCREEN-Y*)
    (baseline xl yu xr yu alu-fcn)
    (baseline xr yu xr yd alu-fcn)
    (baseline xr yd xl yd alu-fcn)
    (baseline xl yd xl yu alu-fcn)))


(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 compute-display (rod-1 rod-2 time display-array
			&aux rod-1-mass rod-2-mass total-mass
			(robot-size (* *ROBOT-SCALE* 2.2559s0))
			(scale-factor (/ *MAX-SCREEN-X* TWOPI)))
  (setf rod-1-mass (ROD-MASS rod-1) 
	rod-2-mass (ROD-MASS rod-2)
	total-mass (+ rod-1-mass rod-2-mass)
	(aref display-array 0.)			; FOOT X
	(xlim-zeroed (round (* robot-size (ROD-LOWER-X rod-1))))
	(aref display-array 1.)			; FOOT Y
	(ylim (- 382. (round (* robot-size (ROD-LOWER-Y rod-1)))))
	(aref display-array 2.)			; KNEE X
	(xlim-zeroed (round (* robot-size 0.5s0
				   (+ (ROD-UPPER-X rod-1)
				      (ROD-LOWER-X rod-2)))))
	(aref display-array 3.)			; KNEE Y
	(ylim (- 382. (round (* robot-size 0.5s0
				(+ (ROD-UPPER-Y rod-1)
				   (ROD-LOWER-Y rod-2))))))
	(aref display-array 4.)			; HIP X
	(xlim-zeroed (round (* robot-size (ROD-UPPER-X rod-2))))
	(aref display-array 5.)			; HIP Y
	(ylim (- 382. (round (* robot-size (ROD-UPPER-Y rod-2)))))
	(aref display-array 6.)			; CENTER-OF-MASS X
	(xlim-zeroed (round (* robot-size
				   (/ (+ (* (ROD-CM-X rod-1)
					    rod-1-mass)
					 (* (ROD-CM-X rod-2)
					    rod-2-mass))
				      total-mass))))
	(aref display-array 7.)			; CENTER-OF-MASS Y
	(ylim (- 382. (round (* robot-size
				(/ (+ (* (ROD-CM-Y rod-1)
					 rod-1-mass)
				      (* (ROD-CM-Y rod-2)
					 rod-2-mass))
				   total-mass)))))
	(aref display-array 8.) time		; you guessed it.
	(aref display-array 9.)
	(xlim-zeroed (round (* scale-factor (ROD-CONTROL-SETPOINT rod-2))))
	(aref display-array 10.)
	(xlim-zeroed (round (* scale-factor (ROD-CONTROL-ACTUAL rod-2))))
	(aref display-array 11.)
	(xlim-zeroed (round (* 0.2s0 (ROD-APPLIED-TORQUE rod-2)))))
  nil)


(defun screen-display-body (display-array &aux (xcenter (floor *MAX-SCREEN-X* 2)))
  (when *SCREEN-CLEARED?*
    (print-line 33. 16. "Time:")
    (line-print 64. "Control: " *SERVO*)
    (cursorpoint (+ xcenter 8.) 490.)
    (write-string "Joint Angle Setpoint")
    (cursorpoint (+ xcenter 8.) 520.)
    (write-string "Actual Joint Angle")
    (cursorpoint (+ xcenter 8.) 550.)
    (write-string "Applied Torque"))
  (soft-print 33. 23. (aref display-array 8.))
  (let ((fx (xlim (+ xcenter (aref display-array 0.))))
	(fy (aref display-array 1.))
	(kx (xlim (+ xcenter (aref display-array 2.))))
	(ky (aref display-array 3.))
	(hx (xlim (+ xcenter (aref display-array 4.))))
	(hy (aref display-array 5.))
	(cmx (xlim (+ xcenter (aref display-array 6.))))
	(cmy (aref display-array 7.))
	trampoline-list)
    (when (> fy 382.)
      (push (cons fx (+ fy 8.)) trampoline-list))
    (when (> ky 382.)
      (push (cons kx (+ ky 8.)) trampoline-list))
    (when (> hy 382.)
      (push (cons hx (+ hy 8.)) trampoline-list))
    (when trampoline-list
      (setq trampoline-list (sort trampoline-list #'< :key #'CAR)))
    (tv:prepare-sheet (*GRAPHICS-WINDOW*)
      (tv:sheet-clear *GRAPHICS-WINDOW*)
      (cond ((null trampoline-list)
	     (fat-gwbaseline 22. 390. (- *MAX-SCREEN-X* 10.) 390. TV:ALU-IOR))
	    ((= (list-length trampoline-list) 1.)
	     (let ((x1 (caar trampoline-list))
		   (y1 (cdar trampoline-list)))
	       (fat-gwbaseline 22. 390. x1 y1 TV:ALU-IOR)
	       (fat-gwbaseline x1 y1 (- *MAX-SCREEN-X* 10.) 390. TV:ALU-IOR)))
	    ((= (list-length trampoline-list) 2.)
	     (let ((x1 (car (first trampoline-list)))
		   (y1 (cdr (first trampoline-list)))
		   (x2 (car (second trampoline-list)))
		   (y2 (cdr (second trampoline-list))))
	       (fat-gwbaseline 22. 390. x1 y1 TV:ALU-IOR)
	       (fat-gwbaseline x1 y1 x2 y2 TV:ALU-IOR)
	       (fat-gwbaseline x2 y2 (- *MAX-SCREEN-X* 10.) 390. TV:ALU-IOR)))
	    (t (let ((x1 (car (first trampoline-list)))
		     (y1 (cdr (first trampoline-list)))
		     (x2 (car (second trampoline-list)))
		     (y2 (cdr (second trampoline-list)))
		     (x3 (car (third trampoline-list)))
		     (y3 (cdr (third trampoline-list))))
		 (fat-gwbaseline 22. 390. x1 y1 TV:ALU-IOR)
		 (fat-gwbaseline x1 y1 x2 y2 TV:ALU-IOR)
		 (fat-gwbaseline x2 y2 x3 y3 TV:ALU-IOR)
		 (fat-gwbaseline x3 y3 (- *MAX-SCREEN-X* 10.) 390. TV:ALU-IOR))))
      (fat-gwbaseline 12. 12. *MAX-SCREEN-X* 12. TV:ALU-IOR)
      (fat-gwbaseline 12. 12. 12. 460. TV:ALU-IOR)
      (fat-gwbaseline 12. 460. *MAX-SCREEN-X* 460. TV:ALU-IOR)
      (fat-gwbaseline *MAX-SCREEN-X* 12. *MAX-SCREEN-X* 460. TV:ALU-IOR)
      (fat-gwsq fx fy TV:ALU-IOR)
      (fat-gwbaseline fx fy kx ky TV:ALU-IOR)
      (fat-gwsq kx ky TV:ALU-IOR)
      (fat-gwbaseline kx ky hx hy TV:ALU-IOR)
      (fat-gwsq hx hy TV:ALU-IOR)
      (fat-gwcross cmx cmy TV:ALU-IOR)
      (let ((width (1+ *MAX-SCREEN-X*)))
	(bitblt TV:ALU-SETA
		width 461.
		*GRAPHICS-WINDOW-ARRAY* 0. 0.
		*SCREEN-ARRAY* 0. 0.))))
  (tv:prepare-sheet (*TERMINAL-IO*)
    (draw-value (aref display-array 9.) xcenter 490.)
    (draw-value (aref display-array 10.) xcenter 520.)
    (draw-value (aref display-array 11.) xcenter 550.))
  (setq *SCREEN-CLEARED?* nil))


; MISCELLANEOUS UTILITY FUNCTIONS:

(defun show ( )
  (clear)
  (princ-rest "		        TimeStep:  10 Milliseconds") (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)
  (terpri)
  (princ-rest "		        Friction:  " *FRICTION*) (terpri)
  t)


;;; End.