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

; 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)
	 (cerror t nil nil "Couldn'd find required component."))
	(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)
	 (cerror t nil nil "Couldn'd find required component."))
	(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))
	((= (list-length components) 1.) (assoc (first components) subframelist))
	(t (match (cdr components) (cdr (assoc (first components) subframelist))))))


; CONTROL PORTIONS of the HINGE System.

(defun j-torque (r1 r2 &aux joint-veloc)
  (setq joint-veloc (- (ROD-ANGLE-VELOC r2) (ROD-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 (- (ROD-ANGLE r2) (ROD-ANGLE r1)))
	joint-veloc (- (ROD-ANGLE-VELOC r2) (ROD-ANGLE-VELOC r1)))
  (setf (ROD-CONTROL-SETPOINT r2) des-angle
	(ROD-CONTROL-ACTUAL r2) joint-angle)
  (- (range (* 1500.0s0 (- des-angle joint-angle)) 2000.0s0)
     (* 66.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 (- (ROD-ANGLE-VELOC r2) (ROD-ANGLE-VELOC r1)))
  (* 35.0s0 (- des-veloc joint-veloc)))


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

; 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)
  (when (null output)
    (setq output (make-list (list-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)
  (when (null output)
    (setq output (make-list (list-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))
      (( ))
    (when (null (car v1))
      (return (values sum (car v2))))
    (when (null (car v2))
      (return (values 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)
  (when (null output)
    (setq output (make-list (list-length matrix))))
  (do ((out output (cdr out))
       (row matrix (cdr row))
       (vec vector (cdr vec))
       (len (1+ (list-length vector))))
      ((null row))
    (when (null (car out))
      (rplaca out (make-list 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))))
      (when (> 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 (list-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))
    (when (< 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 0.0s0) (rplaca output (/ constant coef)))
	(t (throw 'INCONSISTENT nil)))
  abscoef)


; TESTING UTILITIES:

(defun lla ( )
  (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))
    (listarg (get-frame (car rod-frame-list) 'ROD-ARRAY)
	     *ROD-ARRAY-COMPONENTS*)
    (terpri)
    (cond ((or (null (cdr rod-frame-list))
	       (progn (terpri) (not (y-or-n-p "Next"))))
	   (return nil))))
  (values))


(defun listarg (arg &optional names (limit 1000000.))
  (terpri2)
  (do ((maxlen (cond ((arrayp arg) (array-length arg))
		     ((listp arg) (list-length arg))
		     (t (ferror nil "Arg not array or list."))))
       (name names (cdr name))
       (value)
       (i 0. (1+ i)))
      ((or (>= i limit)
	   (>= i maxlen))
       t)
    (setq value (cond ((arrayp arg) (aref arg i))
		      (t (prog1 (car arg) (setq arg (cdr arg))))))
    (terprinc i "	 ")
    (cond ((numberp value) (princ value))
	  ((atom value) (prin1 value))
	  (t (princ-rest "Object of type " (type-of value))))
    (unless (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) t)
    (terpri) (print `(VAR ,(car varname) TABLE ,(car table)))
    (listarg (car table))))


;;; End.