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


; INITIALIZATIONS:

(setq *PRINT-BASE* 10.
      *READ-BASE* 10.
      *PRINT-CIRCLE* t
      ZUNDERFLOW t)

(defvar *MAX-SCREEN-X*)
(defvar *MAX-SCREEN-Y*)
(defvar *MAX-COL*)
(defvar *MAX-LINE*)

(multiple-value-bind (nil nil x y)
    (send tv:main-screen :edges)
  (setq *MAX-SCREEN-X* (- x 16.)
	*MAX-SCREEN-Y* (- y 30.)
	*MAX-COL* (floor *MAX-SCREEN-X* 8.)
	*MAX-LINE* (floor *MAX-SCREEN-Y* 14.)))


; DECLARATIONS:

(defconstant ONEPI 3.1415926535s0)
(defconstant TWOPI (* 2.0s0 ONEPI))
(defconstant HALFPI (/ ONEPI 2.0s0))
(defconstant MOUSE-SCALE 0.0015s0)


; General System Variables:
(defvar *DEBUG-LIST*)
(defvar *GRAPHICS-WINDOW*)
(defvar *GRAPHICS-WINDOW-ARRAY*)
(defvar *SCREEN-ARRAY* (send *TERMINAL-IO* :screen-array))
(defvar *ROD-ARRAY-COMPONENTS*)


; Hosts:
(defvar *MY-MOBY-HOST* nil)
(defvar *MY-MOBY-HOST-NAME* nil)
(defvar *COMPUTER-MOBY-HOST* nil)
(defvar *DISPLAYER-MOBY-HOST* nil)
(defvar *DISPLAYER-MOBY-HOST-NAME* nil)


; Frames:
(defvar *ROBOT-FRAME*)
(defvar *ROD1*)
(defvar *ROD2*)


; Control Parameters:
(defvar *FLOOR-ELASTICITY*)
(defvar *FLOOR-DAMPING*)
(defvar *SCREEN-CLEARED?* nil)
(defvar *ROBOT-SCALE*)
(defvar *JOINT-ELASTICITY*)
(defvar *JOINT-DAMPING*)
(defvar *SERVO*)


; General Simulator Variables:
(defvar *FRICTION*)
(defvar *GRAVITY*)
(defvar *MOUSE-HORIZ*)


; 3-D Graphics System Variables:
(defvar *TOP-LINE-ARRAY* (make-array *MAX-SCREEN-X*))
(defvar *CONVOLVE-ARRAY*)
(defvar *CONVOLVE-PATTERN*)
(defvar *CYCLES*)


; DEFSTRUCT DEFINITIONS:

(defstruct (ROD (:conc-name ROD-) (:alterant nil))
  CM-X CM-X-VELOC CM-X-ACCEL
  CM-Y CM-Y-VELOC CM-Y-ACCEL
  ANGLE ANGLE-VELOC ANGLE-ACCEL
  LOWER-X LOWER-X-VELOC LOWER-X-ACCEL
  LOWER-Y LOWER-Y-VELOC LOWER-Y-ACCEL
  UPPER-X UPPER-X-VELOC UPPER-X-ACCEL
  UPPER-Y UPPER-Y-VELOC UPPER-Y-ACCEL
  COSANGLE SINANGLE LXF LYF UXF UYF
  CONTROL-SETPOINT CONTROL-ACTUAL APPLIED-TORQUE
  M1 M2 M3 MASS MOI LCM UCM LTOT)


; DEFSUBSTS:

(defsubst terpri1 ( )
  (send *TERMINAL-IO* ':FRESH-LINE))

(defsubst terpri2 ( )
  (send *TERMINAL-IO* ':FRESH-LINE)
  (terpri))

(defsubst not= (x y)
  (not (= x y)))

(defsubst range (wanted limit)
  (max (min wanted limit)
       (- limit)))

(defsubst radians (y)
  (* (small-float y)
     0.01745329252))

(defsubst degrees (y)
  (* (small-float y)
     57.29577953s0))

(defsubst square (x)
  (* x x))

(defsubst square-root (n)
  (small-float (sqrt (small-float n))))

(defsubst distance (ox oy nx ny)
  (square-root (+ (square (- nx ox))
		  (square (- ny oy)))))

(defsubst polar-m (x y)
  (square-root (+ (square x)
		  (square y))))

(defsubst vector-cross-product (x-radial y-radial x-velocity y-velocity)
  (- (* x-radial y-velocity)
     (* x-velocity y-radial)))

(defsubst cosine (x)
  (sine (+ x 1.570796326s0)))

(defsubst baseline (ox oy nx ny alu-fun)
  (tv:%draw-line ox oy nx ny alu-fun t *TERMINAL-IO*))

(defsubst fat-baseline (ox oy nx ny alu-fun)
  (let ((ox-1 (1- ox))
	(oy-1 (1- oy))
	(nx-1 (1- nx))
	(ny-1 (1- ny)))
    (tv:%draw-line ox oy nx ny alu-fun t *TERMINAL-IO*)
    (tv:%draw-line ox-1 oy nx-1 ny alu-fun t *TERMINAL-IO*)
    (tv:%draw-line ox oy-1 nx ny-1 alu-fun t *TERMINAL-IO*)
    (tv:%draw-line ox-1 oy-1 nx-1 ny-1 alu-fun t *TERMINAL-IO*)))

(defsubst gwpoint (x y val)
  (aset val *GRAPHICS-WINDOW-ARRAY* x y))

(defsubst point (x y val)
  (aset val *SCREEN-ARRAY* x y))

(defsubst gwbaseline (ox oy nx ny alu-fun)
  (tv:%draw-line ox oy nx ny alu-fun t *GRAPHICS-WINDOW*))

(defsubst fat-gwbaseline (ox oy nx ny alu-fun)
  (let ((ox-1 (1- ox))
	(oy-1 (1- oy))
	(nx-1 (1- nx))
	(ny-1 (1- ny)))
    (tv:%draw-line ox oy nx ny alu-fun t *GRAPHICS-WINDOW*)
    (tv:%draw-line ox-1 oy nx-1 ny alu-fun t *GRAPHICS-WINDOW*)
    (tv:%draw-line ox oy-1 nx ny-1 alu-fun t *GRAPHICS-WINDOW*)
    (tv:%draw-line ox-1 oy-1 nx-1 ny-1 alu-fun t *GRAPHICS-WINDOW*)))

(defsubst xlim-zeroed (x)
  (max -511. (min x 511.)))

(defsubst xlim (x)
  (max 20. (min (- *MAX-SCREEN-X* 8.) x)))

(defsubst ylim (y)
  (max 20. (min 450. y)))


; DEFMACROS:

(defmacro swap (x y)
  `(setq ,x (prog1 ,y (setq ,y ,x))))


;;; End.