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

; This file is for the machine learning (database) programs.

; FRAME MANIPULATION:

; ADDs a new sub-component (at front of list) to a frame.
; NEW-VAL is total subcomponent to be added ( <name> . <data> )
; FRAME is frame from which search starts (can be a subframe of another).
; COMPONENTS is pathname from FRAME to component name whose value is list
;   upon which NEW-VAL is to be pushed.
; Returns list whose CAR is key value (last component to match) of frame into whose
;   first component NEW-VAL is to be inserted.
; CDR of return value is data portion of frame identified by COMPONENTS.
; SECOND of return value is NEW-VAL (the new stuff pushed in).
; CDADR of return value is data portion of new frame pushed in as NEW-VAL.

(defun add-frame (new-val frame &rest components &aux head-cell)
  (setq head-cell (match components (cdr frame)))
  (cond ((null head-cell)
	 (break-error "ADD-FRAME -- Couldn'd find required component."
		      new-val frame components))
	(t (rplacd head-cell (cons new-val (cdr head-cell)))
	   head-cell)))


; SETs the data field (which may be a subframe) of FRAME by searching along pathname
;   specified by COMPONENTS.

; Returns list whose CAR is key value (last component to match) and whose CDR is
;   the new data portion of frame.

(defun set-frame (new-val frame &rest components &aux head-cell)
  (setq head-cell (match components (cdr frame)))
  (cond ((null head-cell)
	 (break-error "SET-FRAME -- Couldn'd find required component."
		      new-val frame components))
	(t (rplacd head-cell new-val)
	   head-cell)))


; GETs the data field (which may be a subframe) of FRAME by searching along pathname
;   specified by COMPONENTS.  Data field is CDR of subframe whose CAR is the KEY.

(defun get-frame (frame &rest components &aux body)
  (cond ((null frame) (values 'FRAME-NOT-ALLOCATED nil))
	((null (setq body (match components (cdr frame))))
	 (values 'COMPONENT-NOT-FOUND nil))
	(t (values (cdr body) t))))


; Like GET-FRAME but returns the entire subframe headed by the KEY specified by
;   the last component.

(defun get-subframe (frame &rest components &aux body)
  (cond ((null frame) (values 'FRAME-NOT-ALLOCATED nil))
	((null (setq body (match components (cdr frame))))
	 (values 'COMPONENT-NOT-FOUND nil))
	(t (values body t))))


; Does the work by searching recursively through a frame's tree structure.
; Returns list whose CAR is key (last element of COMPONENTS to match)

(defun match (components subframelist)
  (cond ((null components) nil)
	((atom components) (assoc components subframelist))
	((= (length components) 1.) (assoc (first components) subframelist))
	(t (match (cdr components) (cdr (assoc (first components) subframelist))))))


; COPYITEM works only for tree-structured data (not lattices or circular lists).

(defun copyitem (item new-area &optional (array-depth 0.) &aux new-array len)
  (cond ((null item) nil)
	((stringp item)
	 (let ((DEFAULT-CONS-AREA new-area))
	   (string-append item)))
	((arrayp item)
	 (cond ((= (array-/#-dims item) 1.)
		(setq len (array-length item)
		      new-array (make-array new-area 'ART-Q len))
		(cond ((and (numberp array-depth)
			    (> array-depth 0.))
		       (do ((index 0. (1+ index)))
			   ((>= index len))
			 (aset (copyitem (aref item index) new-area (1- array-depth))
			       new-array index)))))
	       (t (break-error "COPYITEM -- Can't handle multidimensional arrays." item)))
	 new-array)
	((atom item) item)
	((listp item)
	 (setq item (copylist item new-area))
	 (do ((s item (cdr s))
	      (prev nil s))
	     ((null s) item)
	   (rplaca s (copyitem (car s) new-area array-depth))))
	(t (break-error "COPYITEM -- Unknown data-type." item))))


; CONTROL PORTIONS of the HINGE System.

(defun j-torque (r1 r2 &aux joint-veloc)
  (setq joint-veloc (- (ANGLE-VELOC r2) (ANGLE-VELOC r1)))
  (- (range (* 200.0s0 (mouse-read)) 300.0s0)
     (* 100.0s0 (cond ((> joint-veloc 20.0s0) (- joint-veloc 20.0s0))
		      ((< joint-veloc -20.0s0) (+ joint-veloc 20.0s0))
		      (t 0.0s0)))))


(defun j-angle (r1 r2 &aux des-angle joint-angle joint-veloc)
  (setq des-angle (range (* (mouse-read) 2.0s0) 2.95s0)
	joint-angle (pimod (- (ANGLE r2) (ANGLE r1)))
	joint-veloc (- (ANGLE-VELOC r2) (ANGLE-VELOC r1)))
  (- (range (* 500.0s0 (- des-angle joint-angle)) 750.0s0)
     (* 35.0s0 joint-veloc)))


(defun j-veloc (r1 r2 &aux des-veloc joint-veloc)
  (setq des-veloc (range (* 15.0s0 (mouse-read)) 20.0s0)
	joint-veloc (- (ANGLE-VELOC r2) (ANGLE-VELOC r1)))
  (* 35.0s0 (- des-veloc joint-veloc)))


; LINEAR EQUATION SOLVER -- GAUSSIAN ELIMINATION -- LISP VERSION OF SIMQ:

(setq BREAK-ON-INCONSISTENT? nil
      TOLERANCE 0.0s0
      TOLERANCE2 0.01s0)


; N:  length of main list.
; LIM:  amplitude of numerical value.
; N2:   if non-NIL, length of sublist.
; LIM2: if non-NIL, amplitude.
; LIST: if non-NIL, list to RPLACA down; if NIL, create new list.
; Returns:  list of elements or list of lists of elements (creates new, no side-effect).

(defun make-arglist (n lim n2 lim2 &optional list)
  (and (null list) (setq list (make-list DEFAULT-CONS-AREA n)))
  (do ((elem list (cdr elem)))
      ((null elem))
    (rplaca elem (cond ((null n2) (* (small-float lim) (random-number)))
		       (t (make-arglist n2 lim2 nil nil (car elem))))))
  list)


; M1 is list of the ROWS of first matrix,
; M2 is list of the COLUMNS of second matrix.
; OUTPUT: if non-NIL, list to RPLACA down; if NIL, create new list.
; Returns:  list of COLUMNS in product matrix (creates new, no side-effect).

(defun matrix-multiply (m1 m2 &optional output)
  (and (null output) (setq output (make-list DEFAULT-CONS-AREA (length m1))))
  (do ((vector m2 (cdr vector))
       (out output (cdr out)))
      ((null vector))
    (rplaca out (matrix-vector-multiply m1 (car vector) (car out))))
  output)


; A is MATRIX:  list of ROWS.
; B is VECTOR:  list of elements.
; OUTPUT: if non-NIL, list to RPLACA down; if NIL, create new list.
; Returns:  list of elements (COLUMN vector) (creates new, no side-effect).

(defun matrix-vector-multiply (a b &optional output)
  (and (null output) (setq output (make-list DEFAULT-CONS-AREA (length a))))
  (do ((row a (cdr row))
       (out output (cdr out)))
      ((null row))
    (rplaca out (scalar-product (car row) b)))
  output)


; Each arg is a VECTOR:  list of elements.
; Returns:  SCALAR-PRODUCT as a number.
;	    POINTER to rest of unused list (or NIL if both are same length).
; Stops whenever either vector runs out of numbers
;   (ie, runs of list or hits a NIL in either list).

(defun scalar-product (vector1 vector2)
  (do ((v1 vector1 (cdr v1))
       (v2 vector2 (cdr v2))
       (sum 0.0s0))
      (( ))
    (and (null (car v1)) (return sum (car v2)))
    (and (null (car v2)) (return sum (car v1)))
    (setq sum (+ sum (* (car v1) (car v2))))))


; MATRIX:  list of ROWS.
; VECTOR:  list of elements.
; OUTPUT: if non-NIL, list to RPLACA down; if NIL, create new list.
; Returns:  list of ROWS of augmented matrix (creates new, no side-effect).

(defun augmented-matrix (matrix vector &optional output)
  (and (null output) (setq output (make-list DEFAULT-CONS-AREA (length matrix))))
  (do ((out output (cdr out))
       (row matrix (cdr row))
       (vec vector (cdr vec))
       (len (1+ (length vector))))
      ((null row))
    (and (null (car out)) (rplaca out (make-list DEFAULT-CONS-AREA len)))
    (do ((outp (car out) (cdr outp))
	 (elem (car row) (cdr elem)))
	((null elem)
	 (rplaca outp (car vec)))
      (rplaca outp (car elem))))
  output)


; AUGMATRIX:   augmented matrix.
; Returns:   list of elements in solution vector (creates new, no side-effect).
;   MATRIX destroyed (RPLACAd) during computation.

(defun simq (augmatrix &aux output pivot temp biga)
  (do ((matrix augmatrix (cdr matrix))
       (index 0. (1+ index)))
      ((null (cdr matrix)))
    (do ((top (abs (nth index (first matrix))))
	 (test) (biggest)
	 (row (cdr matrix) (cdr row)))
	((null row)
	 (cond (biggest	(setq test (car biggest))
			(rplaca biggest (car matrix))
			(rplaca matrix test))))
      (setq test (abs (nth index (car row))))
      (and (> test top) (setq top test biggest row)))
    (setq pivot (nth index (first matrix)))
    (cond ((> (abs pivot) 0.0s0)
	   (do ((top-row (nthcdr index (first matrix)))
		(rest-rows (cdr matrix) (cdr rest-rows))
		(next-row) (multiplier))
	       ((null rest-rows))
	     (setq next-row (nthcdr index (car rest-rows))
		   multiplier (// (first next-row) pivot))
	     (rplaca next-row 0.0s0)
	     (do ((top-elem (cdr top-row) (cdr top-elem))
		  (new-elem (cdr next-row) (cdr new-elem)))
		 ((null top-elem))
	       (rplaca new-elem (- (car new-elem) (* multiplier (car top-elem)))))))))
  (setq augmatrix (reverse augmatrix))
  (do ((row augmatrix (cdr row)))
      ((null row))
    (rplaca row (reverse (car row))))
  (setq temp (first augmatrix)
	output (make-list TEMPORARY-AREA (length augmatrix)))
  (setq biga (lin-solve (second temp) (first temp) output))
  (do ((row-list (cdr augmatrix) (cdr row-list))
       (outp (cdr output) (cdr outp))
       (constant) (sp) (coef) (row))
      ((null row-list))
    (setq row (car row-list)
	  constant (first row)
	  row (cdr row))
    (multiple-value (sp coef)
      (scalar-product row output))
    (setq temp (lin-solve coef (- constant sp) outp))
    (and (< temp biga) (setq biga temp)))
  (values (reverse output) biga))


; COEF:      LHS coefficient of current unknown.
; CONSTANT:  RHS constant.
; OUTPUT:    Pointer to where in output list to put answer.

(defun lin-solve (coef constant output &aux abscoef)
  (setq abscoef (abs coef))
  (cond ((> abscoef TOLERANCE) (rplaca output (// constant coef)))
	(BREAK-ON-INCONSISTENT?
	 (break-error "LIN-SOLVE -- Inconsistent equation." coef constant SAVED-AUGMATRIX))
	(t (*throw 'INCONSISTENT nil)))
  abscoef)


; LINEAR EQUATION SOLVER -- GAUSSIAN ELIMINATION -- FORTRAN VERSION OF SIMQ:

(defun simq2 (a b &aux temp (n (array-length b)) biga (btemp 1.0s16))
  (do ((jj 0. (+ jj n 1.)) (imax)
       (j 0. (1+ j))
       (jy 1. (1+ jy)))
      ((>= j n))
    (setq biga 0.0s0)
    (do ((i j (1+ i))
	 (ij jj (1+ ij)))
	((>= i n))
      (and (> (abs (setq temp (aref a ij))) biga)
	   (setq biga temp imax i)))
    (cond ((> (abs biga) TOLERANCE2))
	  (BREAK-ON-INCONSISTENT?
	   (break-error "SIMQ2 -- Inconsistent equation." biga a b SAVED-AUGMATRIX))
	  (t (*throw 'INCONSISTENT nil)))
    (do ((i1 (+ j (* n j)) (+ i1 n))
	 (it (- imax j)) (i2) (temp1) (temp2)
	 (k j (1+ k)))
	((>= k n))
      (setq i2 (+ i1 it)
	    temp1 (aref a i1)
	    temp2 (aref a i2))
      (aset temp2 a i1)
      (aset temp1 a i2)
      (aset (// temp2 biga) a i1))
    (setq temp (aref b imax))
    (aset (aref b j) b imax)
    (aset (// temp biga) b j)
    (and (< (setq temp (abs biga)) btemp) (setq btemp temp))
    (cond ((< j (1- n))
	   (do ((ix jy (1+ ix))
		(ixj (+ (* n j) jy) (1+ ixj)))
	       ((>= ix n))
	     (do ((it (- j ix)) (ixjx)
		  (jx jy (1+ jx)))
		 ((>= jx n))
	       (setq ixjx (+ ix (* n jx)))
	       (aset (- (aref a ixjx)
			(* (aref a ixj) (aref a (+ ixjx it))))
		     a ixjx))
	     (aset (- (aref b ix)
		      (* (aref b j) (aref a ixj)))
		   b ix)))))
  (do ((ny (1- n)) (ib)
       (it (1- (* n n)))
       (j 1. (1+ j)))
      ((>= j n))
    (setq ib (- ny j)
	  temp (aref b ib))
    (do ((k 1. (1+ k))
	 (ia (- it j) (- ia n))
	 (ic ny (1- ic)))
	((> k j))
      (setq temp (- temp (* (aref a ia) (aref b ic)))))
    (aset temp b ib))
  (values b btemp))


; SIMQ TESTING FUNCTION:

(defun s (&rest args) (apply #'simqtest args))

(defun simqtest (order &optional (rept 1.) a-matrix x-vect b-vect (lim 1000.0s0)
		 &aux (biga1 1.0s16) (biga2 1.0s16) sum t1 sum2 t2 aarray barray
		 answer aug-matrix line col flush1 flush2 (nn (* order order))
		 btemp1 btemp2 (DEFAULT-CONS-AREA TEMPORARY-AREA))
  (setq t1 0. t2 0. flush1 rept flush2 rept sum 0.0s0 sum2 0.0s0)
  (terpri2) (princ "Count:  ")
  (setq line (linenum) col (charpos))
  (dotimes (count rept)
    (si:reset-temporary-area TEMPORARY-AREA)
    (print-line line col (1+ count))
    (and (or (> rept 1.) (null a-matrix))
	 (setq a-matrix (make-arglist order lim order lim)))
    (and (or (> rept 1.) (null x-vect))
	 (setq x-vect (make-arglist order lim nil nil)))
    (and (or (> rept 1.) (null b-vect))
	 (setq b-vect (matrix-vector-multiply a-matrix x-vect nil)))
    (*catch 'INCONSISTENT
      (progn
	(setq aug-matrix (augmented-matrix a-matrix b-vect nil)
	      SAVED-AUGMATRIX (subst nil nil aug-matrix)
	      t1 (+ t1 (* 16. (- (- (time)
				    (progn (multiple-value (answer btemp1)
					     (simq aug-matrix))
					   (time)))))))
	(and (< btemp1 biga1) (setq biga1 btemp1))
	(do ((inp x-vect (cdr inp))
	     (out answer (cdr out)))
	    ((null inp))
	  (setq sum (+ sum (abs (- (car inp) (car out))))))
	(setq flush1 (1- flush1))))
    (setq aarray (make-array TEMPORARY-AREA 'ART-Q nn)
	  barray (make-array TEMPORARY-AREA 'ART-Q order))
    (do ((coefs a-matrix (cdr coefs))
	 (b-vals b-vect (cdr b-vals))
	 (bindex 0. (1+ bindex)))
	((null coefs))
      (aset (car b-vals) barray bindex)
      (do ((aindex 0. (1+ aindex))
	   (row (car coefs) (cdr row)))
	  ((null row))
	(aset (car row) aarray (+ bindex (* order aindex)))))
    (*catch 'INCONSISTENT
      (progn
	(setq t2 (+ t2 (* 16. (- (- (time)
				    (progn (multiple-value (nil btemp2)
					     (simq2 aarray barray))
					   (time)))))))
	(and (< btemp2 biga2) (setq biga2 btemp2))
	(do ((index 0. (1+ index))
	     (list x-vect (cdr list)))
	    ((null list))
	  (setq sum2 (+ sum2 (abs (- (car list) (aref barray index))))))
	(setq flush2 (1- flush2)))))
  (terpri2) (princ-rest "SIMQ  --  Average Coefficient size:  " (// lim 2.0s0))
  (terpri2) (princ-rest "Average error:  " (// sum order rept) "  per unknown per iter.")
  (terpri) (princ-rest "Total error:    " (// sum rept) "  per iter, all unknowns.")
  (terpri) (princ-rest "Inconsistents:   " flush1)
  (terpri) (princ-rest "Smallest BIGA:  " biga1 "  (not counting inconsistents).")
  (terpri) (princ-rest "Tolerance:      " TOLERANCE)
  (terpri) (princ-rest "Time:           " (// (small-float t1) rept) "  msec per iteration.")
  (terpri2) (princ-rest "SIMQ2  --  Average Coefficient size:  " (// lim 2.0s0))
  (terpri2) (princ-rest "Average error:  " (// sum2 order rept) "  per unknown per iter.")
  (terpri) (princ-rest "Total error:    " (// sum2 rept) "  per iter, all unknowns.")
  (terpri) (princ-rest "Inconsistents:   " flush2)
  (terpri) (princ-rest "Smallest BIGA:  " biga2 "  (not counting inconsistents).")
  (terpri) (princ-rest "Tolerance2:     " TOLERANCE2)
  (terpri) (princ-rest "Time:           " (// (small-float t2) rept) "  msec per iteration.")
  (si:reset-temporary-area TEMPORARY-AREA)
  (terpri) (beep-tune))


; MEMORY MANAGEMENT:

(defun gcollect (&optional AREA-NUMBER)
  (cond ((null AREA-NUMBER))
	((not (fixp AREA-NUMBER)) (^g "GCOLLECT -- Incorrect arg." AREA-NUMBER))
	((= AREA-NUMBER DATA-AREA))
	((= AREA-NUMBER SIGNAL-AREA))
	((= AREA-NUMBER TEMPORARY-AREA))
	(t (^g "GCOLLECT -- Don't garbage-collect area" AREA-NUMBER)))
  (mapatoms
    #'(lambda (symbol &aux symbol-area-number)
	(cond ((and (boundp symbol)
		    (fixp (setq symbol-area-number (%area-number (symeval symbol))))
		    (cond ((fixp AREA-NUMBER) (= symbol-area-number AREA-NUMBER))
			  ((null AREA-NUMBER)
			   (or (= symbol-area-number DATA-AREA)
			       (= symbol-area-number SIGNAL-AREA)
			       (= symbol-area-number TEMPORARY-AREA)))))
	       (print `(SYMBOL ,symbol POINTING INTO AREA ,symbol-area-number SET TO NIL.))
	       (set symbol nil))))
    PACKAGE nil)
  (cond ((null AREA-NUMBER)
	 (si:reset-temporary-area DATA-AREA)
	 (print `(DATA-AREA ,DATA-AREA COLLECTED.))
	 (si:reset-temporary-area SIGNAL-AREA)
	 (print `(SIGNAL-AREA ,SIGNAL-AREA COLLECTED.))
	 (si:reset-temporary-area TEMPORARY-AREA)
	 (print `(TEMPORARY-AREA ,TEMPORARY-AREA COLLECTED.)))
	((= AREA-NUMBER DATA-AREA)
	 (si:reset-temporary-area DATA-AREA)
	 (print `(DATA-AREA ,DATA-AREA COLLECTED.)))
	((= AREA-NUMBER SIGNAL-AREA)
	 (si:reset-temporary-area SIGNAL-AREA)
	 (print `(SIGNAL-AREA ,SIGNAL-AREA COLLECTED.)))
	((= AREA-NUMBER TEMPORARY-AREA)
	 (si:reset-temporary-area TEMPORARY-AREA)
	 (print `(TEMPORARY-AREA ,TEMPORARY-AREA COLLECTED.))))
  DONE)


; Extends a list:  item = last cons of list, or NIL if list not yet allocated.

(defun extend (name item num area &aux extension)
  (setq extension (make-list area num))
  (cond ((null item) (set name extension))
	(t (rplacd item extension)))
  (setq TOTAL-EXTEND-USAGE (+ TOTAL-EXTEND-USAGE num))
  (beep-tune)
  (print-line 58. 0. "EXTEND -- " name " extended (" TOTAL-EXTEND-USAGE " total so far).")
  extension)


; TESTING UTILITIES:

(defun debug-print (index matrix)
  (terpri2) (princ index)
  (listarg matrix))


(defun test-all (&optional (fcn-list TEST-FCN-LIST))
  (do ((return-val)
       (fcn fcn-list (cdr fcn)))
      ((null fcn) (beep-tune) "TEST-ALL done.")
    (terpri2)
    (cond ((ask-yn "Fcn:  " (car fcn) " ->")
	   (setq return-val (funcall (car fcn)))
	   (terpri) (princ "Returned:  ") (funcall PRIN1 return-val)))))


(defun lla (&optional return-arrays? &aux temp return-list)
  (do ((rod-frame-list (get-frame ROBOT-FRAME 'ROD-FRAME-LIST) (cdr rod-frame-list)))
      (( ))
    (clear) (princ-rest "Rod-Array for rod:  " (caar rod-frame-list))
    (setq temp (get-frame (car rod-frame-list) 'ROD-ARRAY))
    (listarg temp ROD-ARRAY-COMPONENTS)
    (terpri)
    (and return-arrays? (push temp return-list))
    (cond ((or (null (cdr rod-frame-list))
	       (progn (terpri) (not (ask-yn "Next ->"))))
	   (return nil))))
  (cond (return-arrays? (reverse return-list))
	(t DONE)))


(defun listarg (arg &optional names (limit 1000000.))
  (terpri1)
  (do ((maxlen (cond ((arrayp arg) (array-length arg))
		     ((listp arg) (length arg))
		     (t (^g "Arg not array or list." arg))))
       (name names (cdr name))
       (value)
       (i 0. (1+ i)))
      ((or (>= i limit)
	   (>= i maxlen))
       DONE)
    (setq value (cond ((arrayp arg) (aref arg i))
		      (t (prog1 (car arg) (setq arg (cdr arg))))))
    (terpri) (princ-rest i "	 ")
    (cond ((numberp value) (princ-rest value))
	  (t (let ((G-PRINENDLINE 2.))
	       (funcall PRIN1 value))))
    (or (null names) (line-print 40. (car name)))))


(defun listarg-all (name-list table-list)
  (do ((varname name-list (cdr varname))
       (table table-list (cdr table)))
      ((null table) DONE)
    (terpri) (print `(VAR ,(car varname) TABLE ,(car table)))
    (listarg (car table))))


;;; End.