;;;---	HSIGGR >		-*- Mode:LISP; Package:USER; Base:10 -*-

; Signal Processing and Graphical Display functions.

; INITIALIZATIONS:

(or (boundp 'TOP-LINE-ARRAY)
    (setq TOP-LINE-ARRAY (make-array WORKING-STORAGE-AREA ':ART-Q 768.)))


; DISPLAY TEST FUNCTIONS:

(defun size-test (fcn samples &aux temp (maximum -1.0s16) (minimum 1.0s16))
  (do ((x 0 (1+ x)))
      ((>= x samples))
    (do ((y 0. (1+ y)))
	((>= y samples))
      (setq temp (funcall fcn x y samples)
	    maximum (max maximum temp)
	    minimum (min minimum temp))))
  (terpri) (princ-rest "Function:  " fcn)
  (terpri) (princ-rest "Minimum:   " minimum)
  (terpri) (princ-rest "Maximum:   " maximum)
  DONE)

(defun dgf1 (x y samples &aux middle)
  (cond ((or (< x 0.) (< y 0.) (>= x samples) (>= y samples))
	 (break-loop "DGF1 wrong args." x y)))
  (setq middle (// samples 2.)
	x (- (// (* x PI 4.0s0 CYCLES) samples) (* TWOPI CYCLES))
	y (- (// (* y PI 4.0s0 CYCLES) samples) (* TWOPI CYCLES)))
  (* (cond ((zerop x) 1.0s0)
	   (t (// (sine x) x)))
     (cond ((zerop y) 1.0s0)
	   (t (// (sine y) y)))))

(defun dgf2 (x y samples &aux middle)
  (cond ((or (< x 0.) (< y 0.) (>= x samples) (>= y samples))
	 (break-loop "DGF2 wrong args." x y)))
  (setq middle (// samples 2.)
	x (- (// (* x PI 4.0s0 CYCLES) samples) (* TWOPI CYCLES))
	y (- (// (* y PI 4.0s0 CYCLES) samples) (* TWOPI CYCLES)))
  (* (cond ((zerop x) 1.0s0)
	   ((> x 0.0s0) (// (sine x) x))
	   (t (- (// (sine x) x))))
     (cond ((zerop y) 1.0s0)
	   ((> y 0.0s0) (// (sine y) y))
	   (t (- (// (sine y) y))))))

(defun dgf3 (x y samples)
  (cond ((or (< x 0.) (< y 0.) (>= x samples) (>= y samples))
	 (break-loop "DGF3 wrong args." x y)))
  (setq x (- x (// samples 2.))
	y (- y (// samples 2.)))
  (+ (cond ((> (abs x) 10.) 0.0s0)
	   (t (- 10.0s0 (abs x))))
     (cond ((> (abs y) 10.) 0.0s0)
	   (t (- 10.0s0 (abs y))))))

(defun dgf4 (x y samples)
  (cond ((or (< x 0.) (< y 0.) (>= x samples) (>= y samples))
	 (break-loop "DGF4 wrong args." x y)))
  (setq x (- x (// samples 2.))
	y (- y (// samples 2.)))
  (+ (* (cond ((>= y 0.) 2.0s0)
	      (t 0.5s0))
	(cond ((> (abs x) 10.) 0.0s0)
	      (t (- 10.0s0 (abs x)))))
     (* (cond ((>= x 0.) 1.0s0)
	      (t 0.25s0))
	(cond ((> (abs y) 10.) 0.0s0)
	      (t (- 10.0s0 (abs y)))))))

(setq dgf1 (list #'DGF1 "SIN X // X on both axes." -0.21713s0 1.0s0)
      dgf2 (list #'DGF2 "SIN X // X on both axes, with cliffs." -1.0s0 1.0s0)
      dgf3 (list #'DGF3 "Rooftops." 0.0s0 20.0s0)
      dgf4 (list #'DGF4 "Rooftops of varying heights." 0.0s0 20.0s0))


; CONVOLUTION:

(defun convolve (x y samples)
  (do ((sum 0.0s0)
       (weight CONVOLVE-PATTERN)
       (i -1. (1+ i)))
      ((> i 1.) sum)
    (do ((j -1. (1+ j))
	 (jx) (jy))
	((> j 1.))
      (setq jx (+ i x) jy (+ j y))
      (and (>= jx 0.) (< jx samples)
	   (>= jy 0.) (< jy samples)
	   (setq sum (+ sum (* (car weight) (aref CONVOLVE-ARRAY jx jy)))))
      (setq weight (cdr weight)))))


(defun ctest ( &optional (CONVOLVE-ARRAY CONVOLVE-ARRAY) (CONVOLVE-PATTERN CONVOLVE-PATTERN)
	      (zmin -1.0s0) (zmax 1.0s0) (scale 1.0s0))
  (dgf (list #'CONVOLVE "Convolution" zmin zmax) scale
       (car (array-dimensions CONVOLVE-ARRAY))))


; INFORMATION DISPLAY

; In graphing functions, ZMIN > ZMAX inverts Z-axis and negative SCALE rotates
; display 180 degrees about Z-axis.

(defun dgf (graph-fcn &optional (scale 1.0s0) (samples 80.) (label (second graph-fcn)))
  (display-graph (first graph-fcn) (third graph-fcn) (fourth graph-fcn) scale samples label))


(defun display-graph (graph zmin zmax scale samples label
		      &aux xincrement yincrement z0 zscale zcenter)
  (setq scale (small-float scale) zmin (small-float zmin) zmax (small-float zmax)
	yincrement (// (* (abs scale) 215.0s0) samples)
	xincrement (// (* (abs scale) 372.4s0) samples))
  (and (= zmax zmin) (setq zmax (1+ zmin)))
  (setq zscale (// 215.0s0 (- zmax zmin))
	zcenter (// (+ zmin zmax) 2.0s0)
	z0 (* (- zcenter) zscale))
  (clear) (cursorpoint 0. 814.)
  (or (null label) (princ label TERMINAL-IO))
  (princ-rest2 "  Z-: " zmin "  Z+: " zmax " ")
  (sq 383. 383. t 383.) (sq 383. 383. t 382.)
  (do ((i 0. (1+ i))) ((>= i 768.)) (aset 765. TOP-LINE-ARRAY i))
  (do ((ylimit (- (* samples 2.) 2.))
       (ycount 1. (1+ ycount))
       (flag) (x) (y) (z) (xlimit)
       (xpt) (ypt) (xpt0) (ypt0))
      ((>= ycount ylimit) (princ-rest2 " Cmd: " (set-latest)) DONE)
    (setq xlimit (min ycount (- ylimit ycount 1.)))
    (do ((xcount (- xlimit) (1+ xcount))
	 (mark? (< ycount samples) nil)
	 (array-x) (array-y) (old-xpt) (old-ypt))
	((> xcount xlimit))
      (and (< ycount samples) (= xcount xlimit) (setq mark? t))
      (and (= ycount 1.) (= xcount 0.) (setq mark? t))
      (setq flag (oddp (+ xcount ycount))
	    x (* xincrement (- xcount 0.5s0))
	    y (* yincrement (- (cond (flag (1+ ycount))
				     (t (+ ycount 2.)))
			       samples)))
      (multiple-value (array-x array-y)
	(get-adjusted-index xcount ycount samples flag scale))
      (cond ((numberp (setq z (cond ((arrayp graph) (aref graph array-x array-y))
				    (t (funcall graph array-x array-y samples)))))
	     (setq z (* (- z zcenter) zscale)
		   xpt (max 3. (min 764. (+ 383. (round x))))
		   ypt (max 3. (min 764. (- 383. (round (+ z y))))))
	     (and mark? (setq xpt0 (+ 383. (round x))
			      ypt0 (- 383. (round (+ z0 y)))))
	     (and (numberp old-xpt) (top-line old-xpt old-ypt xpt ypt))
	     (cond ((and mark? (> xpt0 2.) (> ypt0 2.) (< xpt0 765.) (< ypt0 765.))
		    (baseline (- xpt 1.) ypt (- xpt0 1.) ypt0)
		    (sq (- xpt0 1.) ypt0 t 1.)
		    (sq (- xpt 1.) ypt t 1.)))
	     (setq old-xpt xpt old-ypt ypt))))))


(defun get-adjusted-index (x y samples flag scale &aux xaux yaux (subtrahend (1- samples)))
  (and flag (setq y (1- y)))
  (setq xaux (- subtrahend (// (- y x) 2.))
	yaux (// (+ x y) 2.))
  (and (< scale 0.0s0) (setq xaux (- subtrahend xaux) yaux (- subtrahend yaux)))
  (values xaux yaux))


(defun top-line (ox oy nx ny &aux flag flag2 oox ooy lgx lgy
		 (xsum 2048.) (ysum 2048.) xstep numsteps xinc yinc)
  (cond ((< oy ny) (swap ox nx) (swap oy ny) (setq flag t)))
  (let ((dx (- nx ox)) (dy (- ny oy)))
    (setq xstep (cond ((< dx 0.) -1.) (t 1.))
	  numsteps (max (setq dx (abs dx)) (setq dy (abs dy))))
    (or (zerop numsteps)
	(setq xinc (// (* 4096. dx) numsteps)
	      yinc (// (* 4096. dy) numsteps)))
    (do ( )
	(( ))
      (cond ((<= oy (aref TOP-LINE-ARRAY ox))
	     (setq flag2 t lgx ox lgy oy)
	     (and (null oox) (setq oox ox ooy oy))
	     (aset (cond ((eq flag 't) 765.)
			 (t oy))
		   TOP-LINE-ARRAY ox))
	    (t (or (null oox) (baseline oox ooy lgx lgy))
	       (setq flag2 nil oox nil ooy nil)))
      (and (eq flag 't) (setq flag 'OFF))
      (cond ((< (setq numsteps (1- numsteps)) 0.)
	     (and flag2 (null flag) (aset 765. TOP-LINE-ARRAY ox))
	     (and (numberp oox) (baseline oox ooy lgx lgy))
	     (return nil)))
      (and (>= (setq xsum (+ xsum xinc)) 4096.)
	   (setq ox (+ ox xstep) xsum (- xsum 4096.)))
      (and (>= (setq ysum (+ ysum yinc)) 4096.)
	   (setq oy (1- oy) ysum (- ysum 4096.))))))


(defun line-graph (xcoord ycoord zcoord scale itemlist
		   &aux (xmin 1.0s16) (xmax -1.0s16) (ymin 1.0s16)
		   (ymax -1.0s16) (zmin 1.0s16) (zmax -1.0s16))
  (*catch 'SKIP-THIS
    (progn
      (do ((nodelist itemlist (cdr nodelist)) (temp) (node))
	  ((null nodelist))
	(setq node (car nodelist))
	(and (< (setq temp (aref node xcoord)) xmin) (setq xmin temp))
	(and (> temp xmax) (setq xmax temp))
	(and (< (setq temp (aref node ycoord)) ymin) (setq ymin temp))
	(and (> temp ymax) (setq ymax temp))
	(cond ((numberp (setq temp (aref node zcoord)))
	       (and (< temp zmin) (setq zmin temp))
	       (and (> temp zmax) (setq zmax temp)))
	      (t (*throw 'SKIP-THIS nil))))
      (and (< 0.0s0 zmin) (setq zmin 0.0s0))
      (and (> 0.0s0 zmax) (setq zmax 0.0s0))
      (clear) (cursorpoint 0. 814.)
      (princ-rest2 "  X-: " xmin "  X+: " xmax "  Y-: " ymin "  Y+: " ymax
		   "  Z-: " zmin "  Z+: " zmax " ")
      (and (= xmax xmin) (setq xmax (1+ xmin)))
      (and (= ymax ymin) (setq ymax (1+ ymin)))
      (and (= zmax zmin) (setq zmax (1+ zmin)))
      (sq 383. 383. t 383.) (sq 383. 383. t 382.)
      (do ((nodelist itemlist (cdr nodelist))
	   (node 'ORIGIN) (oncount 0.) (offcount 0.)
	   (x 0.0s0 (aref node xcoord))
	   (y 0.0s0 (aref node ycoord))
	   (z 0.0s0 (aref node zcoord))
	   (z0) (xpt) (ypt) (xpt0) (ypt0)
	   (xscale (* scale (// 215.0s0 (- xmax xmin))))
	   (yscale (* scale (// 215.0s0 (- ymax ymin))))
	   (zscale (* scale (// 215.0s0 (- zmax zmin))))
	   (xcenter (// (+ xmin xmax) 2.0s0))
	   (ycenter (// (+ ymin ymax) 2.0s0))
	   (zcenter (// (+ zmin zmax) 2.0s0)))
	  (( ))
	(and (eq node 'ORIGIN) (setq z0 (* (- zcenter) zscale)))
	(setq x (* (- x xcenter) xscale)
	      y (* (- y ycenter) yscale)
	      z (* (- z zcenter) zscale)
	      xpt (+ 383. (round (+ (* x 0.86602s0) (* y 0.86602s0))))
	      ypt (- 383. (round (+ (* x -0.5s0) (* y 0.5s0) z)))
	      xpt0 (+ 383. (round (+ (* x 0.86602s0) (* y 0.86602s0))))
	      ypt0 (- 383. (round (+ (* x -0.5s0) (* y 0.5s0) z0))))
	(cond ((and (> xpt 2.) (> xpt0 2.) (> ypt 2.) (> ypt0 2.)
		    (< xpt 765.) (< xpt0 765.) (< ypt 765.) (< ypt0 765.))
	       (cond ((eq node 'ORIGIN) (draw-origin xpt ypt))
		     (t (increment oncount)
			(protected-baseline xpt ypt xpt0 ypt0) (sq xpt0 ypt0 t 1.))))
	      ((neq node 'ORIGIN) (increment offcount)))
	(setq node (car nodelist))
	(cond ((null node)
	       (princ-rest2 " On " oncount "  Off " offcount " ")
	       (return nil)))))))


(defun draw-origin (xorig yorig)
  (sq xorig yorig t 1.)
  (do ((angle-arg '(0.86602s0 -0.5s0 -0.86602s0 0.5s0 0.86602s0 0.5s0 -0.86602s0 -0.5s0
			      0.0s0 1.0s0 0.0s0 -1.0s0)
		  (cddr angle-arg))
       (cos-arg) (sin-arg))
      ((null angle-arg))
    (setq cos-arg (* 4.0s0 (first angle-arg)) sin-arg (* 4.0s0 (second angle-arg)))
    (do ((x (small-float xorig) (+ x cos-arg))
	 (y (small-float yorig) (- y sin-arg))
	 (flag -1. (* flag -1.))
	 (oldx xorig x)
	 (oldy yorig y))
	((or (>= x 765.0s0) (<= x 2.0s0) (>= y 765.0s0) (<= y 2.0s0)))
      (and (> flag 0.)
	   (protected-baseline (round oldx) (round oldy) (round x) (round y) t nil)))))


; STATE-SPACE GRAPHICS:

(defun scopen (vals x-left y-top y-bot &aux (PRINT-MODE 4.))
  (and SCREEN-CLEARED?
       (setq GPOS x-left GRAPH-SIZE  (// (- y-bot y-top) (length DISPLAY-LIST))))
  (and (zerop (\ SIM-TIME (* (// 100. GRATE) DISPLAY-TIME)))
       (display-time-scale x-left y-top y-bot))
  (do ((val vals (cddr val)) (yvalue) (nvalue) (scale)
       (line (+ y-top GRAPH-SIZE) (+ line (* 2. GRAPH-SIZE)))
       (templist SCOPEN-TEMPLIST (cdr templist)))
      ((null val))
    (multiple-value (nvalue scale)
      (normalize-symmetrically (symeval (first val))))
    (setq yvalue (- line (round (* (1- GRAPH-SIZE) nvalue))))
    (TV:PREPARE-SHEET (TERMINAL-IO)
      (cond (SCREEN-CLEARED?
	     (cursorpoint (+ x-left 3.) (- line GRAPH-SIZE))
	     (princ (second val) TERMINAL-IO)
	     (cursorpoint (+ x-left 3.) (+ line GRAPH-SIZE -14.))
	     (princ-rest scale)
	     (protected-baseline x-left (- line GRAPH-SIZE -1.)
				 x-left (+ line GRAPH-SIZE -1.))
	     (safety-check x-left 767. line 883.)
	     (do ((x x-left (+ x 4.)))
		 ((> x 760.))
	       (point x line t)))
	    (t (protected-baseline (- GPOS GRATE) (car templist) GPOS yvalue))))
    (rplaca templist yvalue))
  (setq GPOS (+ GPOS GRATE) SCREEN-CLEARED? nil)
  (cond ((> GPOS 764.)
	 (setq GPOS x-left)
	 (cond ((or FREERUN? (ask-3 "Com: " LATEST ", Run: " RUN-NUMBER))
		(clear))
	       (t (*throw ':DONE nil))))))


(defun state-space-graph (itemlist ytop yincr xleft xincr size &aux item-x item-y x y)
  (do ((ycenter ytop (+ ycenter yincr)))
      ((> ycenter 850.))
    (do ((xcenter xleft (+ xcenter xincr)))
	((> xcenter 750.))
      (setq item-x (cdar itemlist)
	    item-y (caar itemlist)
	    x (round (* (normalize-symmetrically (symeval (second item-x))) size))
	    y (round (* (normalize-symmetrically (symeval (second item-y))) size)))
      (setq x (+ xcenter x) y (- ycenter y))
      (or SCREEN-CLEARED? (protected-baseline (first item-x) (first item-y) x y))
      (rplaca item-x x)
      (rplaca item-y y)
      (setq itemlist (cdr itemlist)))))


(defun preset-graphs (itemlist ytop yincr xleft xincr size xyl yyl xxl yxl)
  (do ((y ytop (+ y yincr)))
      ((> y 850.))
    (do ((x xleft (+ x xincr)))
	((> x 750.))
      (sq x y t size)
      (cross x y t)
      (cursorpoint (- x xyl) (- y yyl))
      (princ (third (caar itemlist)))
      (cursorpoint (+ x xxl) (+ y yxl))
      (princ (third (cdar itemlist)))
      (setq itemlist (cdr itemlist)))))


(defun display-time-scale (x-left y-top y-bot &optional (stime SIM-TIME))
  (safety-check GPOS 767. y-top 883. y-bot 867.)
  (or (= GPOS x-left)
      (do ((y y-top (+ y 4.)))
	  ((> y y-bot))
	(point GPOS y t)))
  (let ((width (* (time-display stime nil) 8.)))
    (cursorpoint (min (max (+ x-left 2.) (- GPOS width))
		      (- 740. width))
		 (- y-bot 2.) ':CLEAR-EOL)
    (time-display stime t)))


; OPTIMIZATION:

; (1.0 < FACTOR < 2.0) (upper limit approximate)
; Can handle ONE symbol only.

(defun optimize (evaluation run-function fact symbol)
  (set-latest) (setq FACTOR (small-float fact))
  ((lambda (&rest arglist)
     (runfun #'OPTIMIZE-INTERNAL arglist))
   evaluation run-function symbol))


; Can handle ONE or TWO symbols.

(defun global-optimize (evaluation run-function num-sides ratio symbol1 &optional symbol2)
  (set-latest)
  ((lambda (&rest arglist)
     (runfun #'GLOBAL-OPTIMIZE-INTERNAL arglist))
   evaluation run-function num-sides ratio symbol1 symbol2))


; CLISTn is list: (SYMBOL val1 val2 val3 ... )
; Can handle ONE or TWO symbols.

(defun global-optimize2 (evaluation run-function clist1 &optional clist2)
  (set-latest)
  ((lambda (&rest arglist)
     (runfun #'GLOBAL-OPTIMIZE2-INTERNAL arglist))
   evaluation run-function clist1 clist2))


(local-declare ((special BEST-COST BV1 BV2))
  
  (defun global-optimize-internal (evaluation run-function num-sides ratio symbol1
				   symbol2 &aux init1 init2 expt-ratio)
    (setq init1 (symeval symbol1) init2 (symeval symbol2)
	  ratio (small-float ratio) expt-ratio (expt ratio num-sides))
    (do ((count (- num-sides) (1+ count))
	 (BEST-COST 1.0s16) (BV1) (BV2)
	 (val1 (// init1 expt-ratio) (* val1 ratio)))
	((> count num-sides)
	 (and (numberp BV1) (set symbol1 BV1))
	 (and (numberp BV2) (set symbol2 BV2)))
      (cond ((null symbol2)
	     (run-test-internal evaluation run-function symbol1 val1))
	    (t (do ((count (- num-sides) (1+ count))
		    (val2 (// init2 expt-ratio) (* val2 ratio)))
		   ((> count num-sides))
		 (run-test-internal evaluation run-function symbol1 val1 symbol2 val2)))))
    (*throw ':DONE nil))
  
  
  (defun global-optimize2-internal (evaluation run-function clist1 clist2)
    (do ((sym1 (car clist1))
	 (BEST-COST 1.0s16) (BV1) (BV2)
	 (val1 (cdr clist1) (cdr val1)))
	((null val1)
	 (and (numberp BV1) (set (car clist1) BV1))
	 (and (numberp BV2) (set (car clist2) BV2)))
      (cond ((null clist2) (run-test-internal evaluation run-function sym1 (car val1)))
	    (t (do ((sym2 (car clist2))
		    (val2 (cdr clist2) (cdr val2)))
		   ((null val2))
		 (run-test-internal
		   evaluation run-function sym1 (car val1) sym2 (car val2))))))
    (*throw ':DONE nil))
  
  
  (defun run-test-internal (evaluation run-function sym1 val1 &optional sym2 val2 &aux cost)
    (setq val1 (small-float val1))
    (and (numberp val2) (setq val2 (small-float val2)))
    (print-line 58. 0. sym1 " " val1 "   " sym2 " " val2)
    (set sym1 val1)
    (and sym2 (set sym2 val2))
    (funcall run-function)
    (setq cost (funcall evaluation))
    (and (< cost BEST-COST) (setq BEST-COST cost BV1 val1 BV2 val2))
    (print-line 59. 0. "Cost: " cost "   Best: " BEST-COST)
    (print-line 60. 0. sym1 " " BV1 "   " sym2 " " BV2)))


(defun optimize-internal (evaluation run-function symbol &aux factor-int)
  (do ((pass 1. (1+ pass))
       (current-val (symeval symbol) new-val)
       (old-val nil current-val) (new-val)
       (cost) (old-cost 1.0s16 cost))
      (( ))
    (funcall run-function)
    (setq cost (funcall evaluation))
    (setq factor-int (cond ((> factor-int 1.0s0) FACTOR)
			   (t (// 1.0s0 FACTOR))))
    (and (> cost old-cost) (setq factor-int (// 1.0s0 factor-int)))
    (setq new-val (* current-val factor-int))
    (print-line 58. 0. "Pass: " pass)
    (print-line 59. 0. "Cost - Old: " old-cost "   New: " cost)
    (print-line 60. 0. "Symbol: " symbol "   Cur. val: " current-val)
    (print-line 61. 0. "Old val: " old-val "   Next: " new-val)
    (set symbol new-val)))


;;; End.