;;; matrix.ss (0.1)
;;;
;;; Matrix manipulation routines.
;;;
;;; Winter Quarter 1992
;;; Rose-Hulman Institute of Technology
;;; Todd R. Eigenschink
;;;
;;; My original ideas for which routines to include were borrowed
;;; from a matrix package written by Joe Maruszewski.  I started
;;; from scratch on almost all the routines, writing them, and
;;; then adding every error check I could think of.  I have added
;;; a large number of routines that I thought were missing (or that
;;; I needed at the time) or could be improved.
;;;
;;; The author from whom I swiped some ideas:
;;;     J. Maruszewski (jpm%fluent@dartmouth.edu)
;;;     Fluent Inc.
;;;     10 Cavendish Court
;;;     Lebanon, NH 03766
;;;
;;;
;;; Copyright (C) 1993 Todd R. Eigenschink
;;;
;;; As far as I'm concerned, this is free software.  I don't promise
;;; that any of this is correct or useful, although I think it's at
;;; least correct.  Do what you like with it, but at least keep this
;;; comment block attached.
;;;
;;; Author's address:
;;; Todd R. Eigenschink
;;; 1561 River Highlands Dr.
;;; Oconomowoc, WI   53066
;;; eigenstr@CS.Rose-Hulman.Edu
;;;
;;; Bug reports are appreciated, as well as anything that's not
;;; R4RS-compatible.  (I already know there are some things...)



;;; Routines included:
;;;
;;; (augment-matrix a b)
;;; (back-substitute a rhs)
;;; (choleski-factor a)
;;; (condition-number a)
;;; (copy-matrix a)
;;; (diagonally-dominant? a)
;;; (dot-product a b)
;;; (forward-substitute a rhs)
;;; (gauss-inverse a)
;;; (gauss-seidel a b . tol)
;;; (gauss-solve a rhs)
;;; (generate-latex-matrix a)
;;; (hilbert-matrix n)
;;; (identity-matrix n)
;;; (infinity-norm obj)
;;; (make-matrix row-size col-size fn)
;;; (matrix-* a b)
;;; (matrix-/ a b)
;;; (matrix-+ a b)
;;; (matrix-- a b)
;;; (matrix-col a)
;;; (matrix-cols a)
;;; (matrix-op sym proc a b)
;;; (matrix-ref a i j)
;;; (matrix-row a)
;;; (matrix-rows a)
;;; (matrix-set! a i j value)
;;; (matrix-set-row! a i value)
;;; (matrix? a)
;;; (maximal-element a . av)
;;; (partial-pivot a)
;;; (permute-rows a perm-vec)
;;; (plu-factor m)
;;; (pretty-print-matrix a)
;;; (pretty-print-vector v)
;;; (reduce-matrix a n)
;;; (square? a)
;;; (transpose a)
;;; (tridiagonal-solve a rhs)
;;; (vector->matrix v . v-list)




;;; Principles:
;;;    1) a always refers to a matrix
;;;    2) i always refers to a row subscript
;;;    3) j always refers to a column subscript
;;;    4) Subscripts are (to the user) 1-based, just
;;;       like matrices in mathematics.


;;; You will notice that error checking is rather thorough.
;;; I chose to pay the speed penalty for evaluation so I was
;;; assured that programs crashed at the right time; I would
;;; rather get an error in matrix-* because the matrices are
;;; the wrong size than get an error from vector-ref because
;;; an index is out of range.  It's a lot easier to find
;;; errors when they come from the highest-level possible.
;;;
;;; The one error check that I consistently ignore is making
;;; sure that numeric operations (*, /, etc.) are performed
;;; on numbers; it's perfectly fine to create an arrays of
;;; strings, but just don't try to multiply them!
;;;
;;; After some complaints about speed, I added a matrix??
;;; routine which does the thorough error checks.  These
;;; routines just use matrix?, which doesn't check nearly
;;; as thoroughly, and thus, is quite a bit faster.




;;; Matrix primitives.  These are the only routines that
;;; have anything to do with how the matrices are
;;; defined (i.e., vectors of vectors, lists of lists, etc.).
;;;
;;; Matrices are stored as a pair.  The car of the pair is the
;;; symbol 'matrix, and the cdr is a vectors of vectors.  The
;;; outer vector has length equal to the number of rows in the
;;; matrix.  Its elements are again vectors with length equal
;;; to the number of columns in the matrix.
;;;
;;; There are no restrictions on what can be placed in a
;;; matrix.  In pretty-print-matrix, for example, a matrix
;;; of strings is used.



;;; matrix?
;;; matrix??
;;;
;;; Return #t if the object is a matrix; #f otherwise.
;;;
;;; matrix? does not test very thoroughly.  This is
;;; because the full comparison is a little bit too slow.
;;; All this one does is make sure that the object is a
;;; pair whose car is 'matrix and whose cdr is a vector.
;;;
;;; matrix?? is the thorough one.  First, we make sure
;;; that the outer part is a vector, then make sure that
;;; the first element of that vector is again a vector.
;;; Once both of those are true, make sure that every other
;;; element is a vector with the same length as the the
;;; first one.

(define (matrix? a)
  (and (pair? a)
       (eq? (car a) 'matrix)
       (vector? (cdr a))))

(define (matrix?? a)
  (and (matrix? a)
       (vector? (vector-ref (cdr a) 0))
       (let ((l (vector-length (vector-ref (cdr a) 0))))
	 (= l (apply + (map (lambda (x)
			      (and (vector? x)
				   (if (= (vector-length x) l) 1 0)))
			    (vector->list (cdr a))))))))


;;; make-matrix
;;;
;;; Creates a new matrix with the given dimensions.
;;; The element for each position is the result of
;;; the application of fn to the row and column
;;; values.  If fn is not a procedure, the value of
;;; fn is placed directly into the matrix.

(define (make-matrix row-size col-size fn)
  (cond
   ((or (not (integer? row-size)) (< row-size 1))
    (error 'make-matrix "row-size must be a positive integer"))
   ((or (not (integer? col-size)) (< col-size 1))
    (error 'make-matrix "col-size must be a positive integer"))
   (else
    (do ((a (make-vector row-size))
	 (r (1- row-size) (1- r)))
	((= r -1) (cons 'matrix a))
      (vector-set! a r (do ((v (make-vector col-size))
			    (c (1- col-size) (1- c)))
			   ((= c -1) v)
			 (vector-set!
			  v c
			  (if (procedure? fn)
			      (fn (1+ r) (1+ c))
			      fn))))))))


;;; matrix-ref
;;;
;;; Returns the element a_ij.

(define (matrix-ref a i j)
  (cond
   ((not (matrix? a))
    (error 'matrix-ref "~s is not a matrix" a))
   ((or (< i 1) (> i (matrix-rows a)))
    (error 'matrix-ref "row subscript ~s is out of range for matrix ~s" i a))
   ((or (< j 1) (> j (matrix-cols a)))
    (error 'matrix-ref
	   "column subscript ~s is out of range for matrix ~s" j a))
   (else
    (vector-ref (vector-ref (cdr a) (1- i)) (1- j)))))


;;; matrix-set!
;;;
;;; Set!'s the a_ij element of the matrix to value.

(define (matrix-set! a i j value)
  (cond
   ((not (matrix? a))
    (error 'matrix-set! "~s is not a matrix" a))
   ((or (< i 1) (> i (matrix-rows a)))
    (error 'matrix-ref "row subscript ~s is out of range for array ~s" i a))
   ((or (< j 1) (> j (matrix-cols a)))
    (error 'matrix-ref "column subscript ~s is out of range for array ~s" j a))
   (else
    (vector-set! (vector-ref (cdr a) (1- i)) (1- j) value))))


;;; matrix-rows
;;;
;;; Returns the number of rows in the matrix.

(define (matrix-rows a)
  (cond
   ((not (matrix? a))
    (error 'matrix-rows "~s is not a matrix" a))
   (else
    (vector-length (cdr a)))))


;;; matrix-cols
;;;
;;; Returns the number of columns in the matrix.

(define (matrix-cols a)
  (cond
   ((not (matrix? a))
    (error 'matrix-cols "~s is not a matrix" a))
   (else
    (vector-length (vector-ref (cdr a) 0)))))


;;; matrix-row
;;;
;;; Returns the ith row of matrix a as a vector.
;;;
;;; Note: The return vector is 0-based!

(define (matrix-row a i)
  (cond
   ((not (matrix? a))
    (error 'matrix-row "~s is not a matrix" a))
   ((or (< i 1) (> i (matrix-rows a)))
    (error 'matrix-row "row subscript is out of range for array ~s" a))
   (else
    (vector-ref (cdr a) (1- i)))))


;;; matrix-col
;;;
;;; Returns the jth column of matrix a as a vector.
;;;
;;; Note: The vector is 0-based!

(define (matrix-col a j)
  (cond
   ((not (matrix? a))
    (error 'matrix-col "~s is not a matrix" a))
   ((or (< j 1) (> j (matrix-cols a)))
    (error 'matrix-col "column subscript is out of range for array ~s" a))
   (else
    (list->vector (map (lambda (v) (vector-ref v (1- j)))
		       (vector->list (cdr a)))))))


;;; matrix-set-row!
;;;
;;; Sets the given row of the given matrix
;;; to the given value.

(define (matrix-set-row! a i value)
  (cond
   ((not (matrix? a))
    (error 'matrix-set-row! "~s is not a matrix" a))
   ((or (< i 1) (> i (matrix-rows a)))
    (error 'matrix-set-row! "row subscript ~s is out of range" i))
   ((not (= (matrix-cols a) (vector-length value)))
    (error 'matrix-set-row! "vector ~s is not of length ~s"
	   value (matrix-cols a)))
   (else
    (vector-set! (cdr a) (1- i) value))))


;;; vector->matrix
;;;
;;; Converts vector(s) to a matrix.  The vectors
;;; given (at least 1) are treated as column can
;;; be used with these procedures.  Any number
;;; of vectors can be included; these are treated
;;; as column vectors and turned into the columns
;;; of the matrix.

(define (vector->matrix v . v-list)
  (cond
   ((not (vector? v))
    (error 'vector->matrix "~s is not a vector" v))
   (else
    (let ((andmap (lambda (fn l)
		    (letrec ((ck (lambda (x)
				   (cond
				    ((null? x) #t)
				    ((not (car x)) #f)
				    (else (ck (cdr x)))))))
		      (ck (map fn l))))))
      (cond
       ((not (andmap (lambda (x)
		       (and (vector? x)
			    (= (vector-length x)
			       (vector-length v))))
		     v-list))
	(error 'vector->matrix "vectors are not of the correct lengths"))
       (else
	(transpose (cons 'matrix (list->vector (cons v v-list))))))))))





;;; Other matrix routines
;;;
;;; These are not dependent on the structure of the matrices.
;;;
;;; All the routines below should use only the routines above
;;; for information about the matrix itself.  Nothing should
;;; be added that depends on the implementation of the above
;;; procedures.



;;; square?
;;;
;;; Returns #t if the given item is
;;; both a matrix and square, and #f if not.

(define (square? a)
  (and (matrix? a)
       (= (matrix-rows a ) (matrix-cols a))))


;;; copy-matrix
;;;
;;; Returns an identical copy of the given matrix.

(define (copy-matrix a)
  (cond
   ((not (matrix? a))
    (error 'copy-matrix "~s is not a matrix" a))
   (else (make-matrix (matrix-rows a)
		      (matrix-cols a)
		      (lambda (i j) (matrix-ref a i j))))))


;;; transpose
;;;
;;; Returns the transpose of the given matrix.

(define (transpose a)
  (cond
   ((not (matrix? a))
    (error 'transpose "~s is not a matrix" a))
   (else
    (make-matrix (matrix-cols a)
		 (matrix-rows a)
		 (lambda (i j) (matrix-ref a j i))))))


;;; permute-rows
;;;
;;; Permutes the rows of matrix according to
;;; the permutation vector given.

(define (permute-rows a perm-vec)
  (cond
   ((not (matrix? a))
    (error 'permute-rows "~s is not a matrix" a))
   ((not (vector? perm-vec))
    (error 'permute-rows "~s is not a vector" perm-vec))
   ((not (= (vector-length perm-vec) (1+ (matrix-rows a))))
    (error 'permute-rows "permutation vector ~s is the wrong length" perm-vec))
   (else
    (let ((new-a (copy-matrix a)))
      (do ((i 1 (1+ i))) ((> i (matrix-rows a)))
	(matrix-set-row! new-a i (matrix-row a (vector-ref perm-vec i))))
      new-a))))


;;; hilbert-matrix
;;;
;;; Returns an n x n Hilbert matrix.

(define (hilbert-matrix n)
  (cond
   ((< n 0)
    (error 'hilbert-matrix "matrix must have positive size"))
   (else
    (make-matrix n n (lambda (i j) (/ 1 (+ i j -1)))))))


;;; indentity-matrix
;;;
;;; Returns an n x n identity matrix.

(define (identity-matrix n)
  (cond
   ((< n 0)
    (error 'identity-matrix "matrix must have positive size"))
   (else
    (make-matrix n n (lambda (i j) (if (= i j) 1 0))))))


;;; augment-matrix
;;;
;;; Augments matrix a by adding the columns of matrix
;;; b to its right side.  The two matrices must obviously
;;; have the same number of rows.  This is intended for
;;; use in Gaussian elimination, where multiple right-hand
;;; side vectors can be solved for at once.
;;;
;;; This was 'roxed almost completely from jpm's original
;;; code.  I just basically just added error-checking.

(define (augment-matrix a b)
  (cond
   ((not (matrix? a))
    (error 'augment-matrix "~s is not a matrix" a))
   ((not (matrix? b))
    (error 'augment-matrix "~s is not a matrix" b))
   ((not (= (matrix-rows a) (matrix-rows b)))
    (error 'augment-matrix "matrices have differing numbers of rows"))
   (else
    (make-matrix (matrix-rows a)
		 (+ (matrix-cols a) (matrix-cols b))
		 (lambda (i j)
		   (if (<= j (matrix-cols a))
		       (matrix-ref a i j)
		       (matrix-ref b i (- j (matrix-cols a)))))))))


;;; gauss-inverse
;;;
;;; Computes the inverse of matrix a by using
;;; Gaussian elimination.  Of course, the matrix
;;; must be square.  The solution is solved by
;;; setting Ax = I, where I is the n x n identity
;;; matrix.

(define (gauss-inverse a)
  (cond
   ((not (matrix? a))
    (error 'gauss-inverse "~s is not a matrix" a))
   ((not (square? a))
    (error 'gauss-inverse "cannot invert non-square matrix ~s" a))
   (else
    (gauss-solve a (identity-matrix (matrix-rows a))))))


;;; gauss-solve
;;;
;;; Solves a matrix equation using Gaussian elimination.
;;; There can be multiple right-hand side vectors; they
;;; must be in the form of a matrix with the same number
;;; of rows as the left-hand side matrix.
;;;
;;; The method used is simple; LUx = b, so set Ux = v, and
;;; then Lv = b.  Solve for v first using forward
;;; substitution, and then use backward substitution to get x.

(define (gauss-solve a rhs)
  (cond
   ((not (matrix? a))
    (error 'gauss-solve "~s is not a matrix" a))
   ((not (matrix? rhs))
    (error 'gauss-solve "~s is not a matrix" rhs))
   ((not (= (matrix-rows a) (matrix-rows rhs)))
    (error 'gauss-solve "matrices have differing numbers of rows"))
   (else
    (let* ((PLU (PLU-factor a))
	   (P (car PLU))
	   (L (cadr PLU))
	   (U (cddr PLU)))
      (back-substitute U (forward-substitute L (permute-rows rhs P)))))))


;;; back-substitute
;;;
;;; Performs back substitution on the (assumed
;;; upper-triangular) matrix using the right-hand side
;;; vectors (in matrix form).

(define (back-substitute a rhs)
  (cond
   ((not (matrix? a))
    (error 'back-substitute "~s is not a matrix" a))
   ((not (matrix? rhs))
    (error 'back-substitute "~s is not a matrix" rhs))
   (else
    (let* ((rows (matrix-rows rhs))
	   (cols (matrix-cols rhs))
	   (m (make-matrix rows cols (lambda (i j) 0)))
	   (term
	    (lambda (i vec-n)
	      (if (= i rows)
		  (/ (matrix-ref rhs rows vec-n) (matrix-ref a rows rows))
		  (/ (- (matrix-ref rhs i vec-n)
			(let loop ((sum 0) (j (1+ i)))
			  (cond
			   ((> j (matrix-cols a)) sum)
			   (else (loop (+ sum (* (matrix-ref a i j)
						 (matrix-ref m j vec-n)))
				       (1+ j))))))
		     (matrix-ref a i i))))))

      (do ((i rows (1- i))) ((= i 0))
	(do ((j 1 (1+ j))) ((> j cols))
	  (matrix-set! m i j (term i j))))

      m))))


;;; forward-substitute
;;;
;;; Performs forward substitution on the (assumed
;;; upper-triangular) matrix and the righthand side vectors
;;; (in matrix form).  The method used is simple; just use
;;; backward substitution after mirroring the matrices.  The
;;; matrix itself has to be sort of flipped along the diagonal;
;;; the matrix of right-hand side vectors just has to be
;;; flipped upside-down.  Of course, once the set of solution
;;; vectors is found, it must be flipped back over.

(define (forward-substitute a rhs)
  (cond
   ((not (matrix? a))
    (error 'forward-substitute "~s is not a matrix" a))
   ((not (matrix? rhs))
    (error 'forward-substitute "~s is not a matrix" rhs))
   ((not (= (matrix-rows a) (matrix-rows rhs)))
    (error 'forward-substitute "matrices have differing numbers of rows"))
   (else
    (let* ((rows (matrix-rows a))
	   (cols (matrix-cols a))
	   
	   ;; mirror-matrix 
	   (mm (lambda (a)
		 (make-matrix rows cols
			      (lambda (i j)
				(matrix-ref
				 a (- rows i -1) (- cols j -1))))))
	   ;; mirror-vector-matrix
	   (mvm (lambda (a)
		  (make-matrix rows (matrix-cols rhs)
			       (lambda (i j)
				 (matrix-ref a (- rows i -1) j))))))
      
      (mvm (back-substitute (mm a) (mvm rhs)))))))


;;; partial-pivot
;;;
;;; Performs partial pivoting, and returns the permutation
;;; matrix consed onto the new matrix.

(define (partial-pivot m)
  (let* ((a (copy-matrix m))
	 (rows (matrix-rows a))
	 (cols (matrix-cols a))
	 (P (make-vector (1+ rows)))
	 (magic-d
	  (lambda (i j)
	    (let* ((row (vector->list (matrix-row a i))))
	      (/ (abs (matrix-ref a i j))
		 (apply max (map abs row))))))
	 (pivot
	  (lambda ()
	    (do ((level 1 (1+ level))) ((= level rows))
	      (let loop ((row (1+ level))
			 (max-row level)
			 (max-d (magic-d level level)))
		(cond
		 ((> row (matrix-rows a))
		  (if (not (= max-row level))
		      (let ((temp-row (matrix-row a max-row))
			    (temp-n (vector-ref P max-row)))
			(matrix-set-row! a max-row (matrix-row a level))
			(matrix-set-row! a level temp-row)
			(vector-set! P max-row level)
			(vector-set! P level temp-n))))
		 ((> (magic-d row level) max-d)
		  (loop (1+ row) row (matrix-ref a row level)))
		 (else
		  (loop (1+ row) max-row max-d))))))))

    ;; Create initial permutation vector
    (do ((i 1 (1+ i))) ((> i rows))
      (vector-set! P i i))
    ;; Actually perform the pivoting
    (pivot)
    ;; Create the return value
    (cons P a)))


;;; plu-factor
;;;
;;; Returns the PLU factorization of the matrix.
;;; The permutation vector is consed onto the results
;;; of consing L onto U.
;;;
;;; Gaussian elimination with partial pivoting (rows only)
;;; is used to performs the factorization.  The results of
;;; switching rows are kept in a permutation vector which
;;; is returned as P in the PLU factorization.

(define (plu-factor m)
  (cond
   ((not (matrix? m))
    (error 'plu-factor "~s is not a matrix" m))
   ((not (= (matrix-rows m) (matrix-cols m)))
    (error 'plu-factor "cannot factor a non-square matrix"))
   (else
    (let* ((PM (partial-pivot m))
	   (a (cdr PM))
	   (P (car PM))
	   (rows (matrix-rows a))
	   (cols (matrix-cols a)))

      ;; Main loop.  Goes top to bottom and left to right,
      ;; creating zeros as it goes.

      (do ((k 1 (1+ k))) ((= k rows))
	(do ((i (1+ k) (1+ i))) ((> i rows))
	  (let ((m_ik (/ (matrix-ref a i k) (matrix-ref a k k))))
	    (matrix-set! a i k m_ik)
	    (do ((j (1+ k) (1+ j))) ((> j cols))
	      (matrix-set! a i j
			   (- (matrix-ref a i j)
			      (* m_ik (matrix-ref a k j))))))))

      ;; Create the return value.  This returns separate L and U
      ;; matrices by pulling the appropriate values out of a.
      (cons P (cons (make-matrix rows cols
				 (lambda (i j)
				   (cond
				    ((= i j) 1)
				    ((> i j) (matrix-ref a i j))
				    (else 0))))
		    (make-matrix rows cols
				 (lambda (i j)
				   (if (<= i j)
				       (matrix-ref a i j) 0)))))))))


;;; doolittle-factor
;;;
;;; Factors the matrix into LU by using Doolittle's
;;; method.  Returns L consed onto U.

(define (doolittle-factor a)
  (cond
   ((not (matrix? a))
    (error 'doolittle-factor "~s is not a matrix" a))
   ((not (square? a))
    (error 'doolittle-factor "matrix is not square"))
   (else
    (let* ((n (matrix-rows a))
	   (L (make-matrix
	       n n (lambda (i j) (if (= j 1) (matrix-ref a i 1) 0))))
	   (U (make-matrix
	       n n (lambda (i j) (if (= i 1)
				     (/ (matrix-ref a 1 j)
					(matrix-ref L 1 1))
				     0)))))

      (do ((j 2 (1+ j))) ((> j n))
	(do ((i j (1+ i))) ((> i n))
	  (matrix-set!
	   L i j
	   (- (matrix-ref a i j)
	      (do ((k 1 (1+ k)) (sum 0))
		  ((= j k) sum)
		(set! sum (+ sum (* (matrix-ref L i k)
				    (matrix-ref U k j))))))))

	(matrix-set! U j j 1)

	(do ((i (1+ j) (1+ i)))
	    ((> i n))
	  (matrix-set!
	   U j i
	   (/ (- (matrix-ref a j i)
		 (do ((k 1 (1+ k)) (sum 0))
		     ((= k j) sum)
		   (set! sum (+ sum (* (matrix-ref L j k)
				       (matrix-ref U k i))))))
	      (matrix-ref L j j)))))

      (cons L U)))))


;;; choleski-factor
;;;
;;; Factors the matrix into LU by using Choleski's
;;; method.  The matrix, of course, must be square.
;;; Returns the lower-triangular matrix (L) consed
;;; onto the upper-triangular (U).  U is just the
;;; transpose of L.

(define (choleski-factor a)
  (cond
   ((not (matrix? a))
    (error 'choleski-factor "~s is not a matrix" a))
   ((not (square? a))
    (error 'choleski-factor "matrix is not square"))
   (else
    (let* ((n (matrix-rows a))
	   (L (make-matrix n n (lambda (i j) 0))))

      (matrix-set! L 1 1 (sqrt (matrix-ref a 1 1)))

      (do ((j 2 (1+ j))) ((> j n))
	(matrix-set! L j 1 (/ (matrix-ref a j 1) (matrix-ref L 1 1))))

      (do ((i 2 (1+ i))) ((= i n))
	(matrix-set!
	 L i i
	 (sqrt (- (matrix-ref a i i)
		  (let loop ((k 1) (sum 0))
		    (cond
		     ((= k i) sum)
		     (else
		      (loop (1+ k) (+ sum (expt (matrix-ref L i k) 2)))))))))
	
	(do ((j (1+ i) (1+ j))) ((> j n))
	  (matrix-set!
	   L j i
	   (/ (- (matrix-ref a j i)
		 (let loop ((k 1) (sum 0))
		   (cond
		    ((= k i) sum)
		    (else
		     (loop (1+ k) (+ sum (* (matrix-ref L j k)
					    (matrix-ref L i k))))))))
	      (matrix-ref L i i)))))

      (matrix-set!
       L n n
       (sqrt (- (matrix-ref a n n)
		(let loop ((k 1) (sum 0))
		  (cond
		   ((= k n) sum)
		   (else
		    (loop (1+ k) (+ sum (expt (matrix-ref L n k) 2)))))))))

      (cons L (transpose L))))))


;;; matrix-*
;;;
;;; Multiplies a matrix by something.  If by a matrix,
;;; the number of columns in the left-hand matrix must
;;; be the same as the number of rows in the right-side
;;; matrix.  This operation is defined in terms of the
;;; dot product, which makes the algorithm trivial.
;;;
;;; You can also multiply a matrix by a constant, in which
;;; case, the matrix is just multiplied term-wise by the
;;; constant.
;;;
;;; If you multiply by a vector, it is assumed that it is
;;; a column-vector, and is interpreted as a 1-column
;;; matrix.

(define (matrix-* a b)
  (cond
   ((not (matrix? a))
    (error 'matrix-* "~s is not a matrix" a))
   ((vector? b)
    (cond
     ((not (= (vector-length b) (matrix-rows a)))
      (error 'matrix-* "vector is the wrong length"))
     (else
      (matrix-* a (vector->matrix b)))))
   ((number? b)
    (make-matrix (matrix-rows a)
		 (matrix-cols a)
		 (lambda (i j)
		   (* b (matrix-ref a i j)))))
   ((not (matrix? b))
    (error 'matrix-* "~s is not a matrix" b))
   ((not (= (matrix-cols a) (matrix-rows b)))
    (error 'matrix-* "# colums in A do not match # rows in B"))
   (else
    (make-matrix (matrix-rows a)
		 (matrix-cols b)
		 (lambda (i j)
		   (dot-product (matrix-col b j)
				(matrix-row a i)))))))


;;; matrix-/
;;;
;;; Divides a matrix by a constant.  Uses matrix-* with
;;; the inverse of the number.

(define (matrix-/ a b)
  (cond
   ((not (matrix? a))
    (error 'matrix-/ "~s is not a matrix" a))
   ((not (number? b))
    (error 'matrix-/ "~s is not a number" b))
   (else
    (matrix-* a (/ 1 b)))))


;;; matrix-op
;;;
;;; Performs some operation on corresponding elements in
;;; two same-sized matrices, and returns a new matrix
;;; composed of the results of the operation.

(define (matrix-op sym proc a b)
  (cond
   ((not (matrix? a))
    (error sym "~s is not a matrix" a))
   ((not (matrix? b))
    (error sym "~s is not a matrix" b))
   ((not (= (matrix-cols a) (matrix-cols b)))
    (error sym "matrices have differing numbers of columns"))
   ((not (= (matrix-rows a) (matrix-rows b)))
    (error sym "matrices have differing numbers of rows"))
   (else
    (make-matrix (matrix-rows a)
		 (matrix-cols a)
		 (lambda (i j)
		   (proc (matrix-ref a i j) (matrix-ref b i j)))))))


;;; matrix--
;;;
;;; Subtracts two matrices.  They must be of the same
;;; size, of course.  This trivially uses matrix-op.

(define (matrix-- a b)
  (matrix-op 'matrix-- - a b))


;;; matrix-+
;;;
;;; Adds two matrices.  They must be of the same
;;; size, of course.  This trivially uses matrix-op.

(define (matrix-+ a b)
  (matrix-op 'matrix-+ + a b))


;;; tridiagonal-solve
;;;
;;; Solves a tridiagonal matrix.  The right-hand side
;;; can be a matrix, which is interpreted as multiple
;;; column-vectors.

(define (tridiagonal-solve a rhs)
  (cond
   ((not (matrix? a))
    (error 'solve-tridiagonal "~s is not a matrix" a))
   ((not (matrix? rhs))
    (error 'solve-tridiagonal "~s is not a matrix" rhs))
   ((not (square? a))
    (error 'solve-tridiagonal "matrix is not square"))
   (else
    (let* ((a (copy-matrix a))
	   (rhs (copy-matrix rhs))
	   (n (matrix-rows a))
	   (get-a (lambda (k) (if (= k 1) 0 (matrix-ref a k (1- k)))))
	   (get-d (lambda (k) (matrix-ref a k k)))
	   (get-c (lambda (k) (if (= k n) 0 (matrix-ref a k (1+ k)))))
	   (set-d (lambda (k v) (matrix-set! a k k v)))
	   (x (make-matrix (matrix-rows rhs)
			   (matrix-cols rhs)
			   (lambda (i j) 0))))

      (do ((k 2 (1+ k))) ((> k n))

	(if (= (matrix-ref a (1- k) (1- k)) 0)
	    (error 'solve-tridiagonal "matrix cannot be solved"))

	(let ((m (/ (matrix-ref a k (1- k))
		    (matrix-ref a (1- k) (1- k)))))

	  (set-d k (- (get-d k) (* m (get-c (1- k)))))

	  (do ((j 1 (1+ j))) ((> j (matrix-cols rhs)))
	    (matrix-set!
	     rhs k j
	     (- (matrix-ref rhs k j) (* m (matrix-ref rhs (1- k) j)))))))

      (if (= (matrix-ref a n n) 0)
	  (error 'solve-tridiagonal "matrix cannot be solved"))

      (do ((j 1 (1+ j))) ((> j (matrix-cols rhs)))
	(matrix-set! x n j (/ (matrix-ref rhs n j) (get-d n))))

      (do ((k (1- n) (1- k))) ((= k 0))
	(do ((j 1 (1+ j))) ((> j (matrix-cols rhs)))
	  (matrix-set!
	   x k j
	   (/ (- (matrix-ref rhs k j) (* (get-c k) (matrix-ref x (1+ k) j)))
	      (get-d k)))))

      x))))


;;; pretty-print-vector
;;;
;;; Prints a vector in a decent format (brackets on either
;;; side).

(define (pretty-print-vector v)
  (cond
   ((not (vector? v))
    (error 'pretty-print-vector "~s is not a vector" v))
   (else
    (display "[ ")
    (do ((i 0 (1+ i))) ((= i (vector-length v)))
      (display (vector-ref v i))
      (display " "))
    (display "]")
    (newline))))
      

;;; pretty-print-matrix
;;;
;;; Prints a matrix in a decent format, with vertical
;;; bars on either side.

(define (pretty-print-matrix a)
  (cond
   ((not (matrix? a))
    (error 'pretty-print-matrix "~s is not a matrix" a))
   (else
    (let* ((rows (matrix-rows a))
	   (cols (matrix-cols a))
	   (width-vec (make-vector (1+ cols)))
	   (s (make-matrix rows cols
			   (lambda (i j)
			     (format "~a" (matrix-ref a i j))))))

      ;; Find the greatest width in each column
      (do ((i 1 (1+ i))) ((> i rows))
	(do ((j 1 (1+ j))) ((> j cols))
	  (let ((len (string-length (matrix-ref s i j))))
	    (if (> len (vector-ref width-vec j))
		(vector-set! width-vec j len)))))
      
      ;; Now that the widths are computed, display the matrix,
      ;; spacing as needed.
      (do ((i 1 (1+ i))) ((> i rows))
	(display "|")
	(do ((j 1 (1+ j))) ((> j cols))
	  (let* ((obj (matrix-ref s i j))
		 (len (string-length obj)))
	    (display " ")
	    (display (make-string (- (vector-ref width-vec j) len) #\ ))
	    (display obj)
	    (display " ")))
	(display "|")
	(newline))))))


;;; generate-latex-matrix
;;;
;;; Generates LaTeX code for the matrix passed in
;;; which can be pasted into a LaTeX document.  This
;;; does not pay attention to how large the matrix is;
;;; it might be necessary to add LaTeX code to change
;;; the font size if the matrix spills off the right
;;; side of the page.

(define (generate-latex-matrix a)
  (cond
   ((vector? a)
    (generate-latex-matrix (vector->matrix a)))
   ((not (matrix? a))
    (error 'generate-latex-matrix "~s is not a matrix" a))
   (else
    (let ((cols (matrix-cols a))
	  (rows (matrix-rows a)))

      ;; Turn on displayed math
      (display "\\[ \\left[ \\begin{array}{")
      (display (make-string cols #\c))
      (display "}")
      (newline)

      (do ((i 1 (1+ i))) ((> i rows))
	(do ((j 1 (1+ j))) ((> j cols))

	  (let ((el (matrix-ref a i j)))

	    (cond
	     ;; Handles integers or inexact (floating-point) numbers
	     ((or (inexact? el) (and (exact? el) (integer? el)))
	      (display el))
	     ;; Handles fractions
	     (else
	      (display "\\frac{")
	      (display (numerator el))
	      (display "}{")
	      (display (denominator el))))

	    ;; Inter-column spacing
	    (if (< j cols) (display " & "))

	    ;; End-of row command to go to new line,
	    ;; but not after the last row
	    (if (and (< i rows) (= j cols)) (display " \\\\"))

	    ;; Pop in a newline after the last column in each row
	    (if (= j cols) (newline)))))

	;; Wrap it up
	(display "\\end{array} \\right] \\]")
	(newline)))))


;;; dot-product
;;;
;;; Returns the dot product of two vectors.

(define (dot-product x y)
  (cond
   ((not (vector? x))
    (error 'dot-product "~s is not a vector" x))
   ((not (vector? y))
    (error 'dot-product "~s is not a vector" y))
   ((not (= (vector-length x) (vector-length y)))
    (error 'dot-product "vectors are of differing lengths"))
   (else
    (let loop ((r 0) (i 0))
      (cond
       ((= i (vector-length x)) r)
       (else (loop (+ r (* (vector-ref x i) (vector-ref y i))) (1+ i))))))))


;;; infinity-norm
;;;
;;; Computes the infinity-norm of either a matrix or
;;; a vector.

(define (infinity-norm obj)
  (cond
   ((vector? obj)
    (apply max (map abs (vector->list obj))))
   ((not (matrix? obj))
    (error 'infinity-norm "~s is not a matrix or vector" obj))
   (else
    (let loop ((row 1) (max-sum 0))
      (if (> row (matrix-rows obj))
	  max-sum
	  (loop (1+ row)
		(max max-sum
		     (apply + (map abs (vector->list
					(matrix-row obj row)))))))))))


;;; condition-number
;;;
;;; Computes the condition number of a matrix.

(define (condition-number a)
  (cond
   ((not (matrix? a))
    (error 'condition-number "~s is not a matrix" a))
   (else
    (* (infinity-norm a) (infinity-norm (gauss-inverse a))))))


;;; diagonally-dominant?
;;;
;;; Returns #t if the given matrix is diagonally dominant,
;;; and #f if not.

(define (diagonally-dominant? a)
  (cond
   ((not (matrix? a))
    (error 'diagonally-dominant? "~s is not a matrix" a))
   (else
    (let loop ((n 1))
      (cond
       ((> n (matrix-rows a)) #t)
       (else
	(let* ((r (matrix-row a n))
	       (el (abs (vector-ref r (1- n)))))
	  (if (> el (- (apply + (map abs (vector->list r))) el))
	      (loop (1+ n))
	      #f))))))))


;;; maximal-element
;;;
;;; Returns the maximal element in the vector or matrix.
;;; If an optional extra parameter is #t, the elements
;;; are taken in absolute value.

(define (maximal-element a . av)
  (let ((fn (if (or (null? av) (not (car av)))
		(lambda (x) x)
		abs)))
    (cond
     ((vector? a)
      (apply max (map fn (vector->list a))))
     ((not (matrix? a))
      (error 'maximal-element "~s is not a vector or matrix" a))
     (else
      (let loop ((i 1) (j 1) (m (fn (matrix-ref a 1 1))))
	(cond
	 ((> i (matrix-rows a)) m)
	 ((> j (matrix-cols a))
	  (loop (1+ i) 1 m))
	 (else
	  (loop i (1+ j) (max m (fn (matrix-ref a i j)))))))))))


;;; reduce-matrix
;;;
;;; Returns a new matrix, which is the original matrix
;;; without the given column or row.

(define (reduce-matrix a n)
  (cond
   ((not (matrix? a))
    (error 'reduce-matrix "~s is not a matrix" a))
   ((> n (matrix-rows a))
    (error 'reduce-matrix "row subscript ~s is out of range for matrix" n))
   ((> n (matrix-cols a))
    (error 'reduce-matrix "column subscript ~s is out of range for matrix" n))
   (else
    (make-matrix (1- (matrix-rows a))
		 (1- (matrix-cols a))
		 (lambda (i j)
		   (matrix-ref a (if (>= i n) (1+ i) i)
			         (if (>= j n) (1+ j) j)))))))


;;; gauss-seidel
;;;
;;; Attempts to find a solution vector for the matrix
;;; using Gauss-Seidel iteration.  The matrix must be
;;; diagonally dominant.  That is, the element on the
;;; diagonal must be at least equal to the sum of the
;;; other elements in the same row.
;;;
;;; The optional extra parameter specified a tolerance
;;; to be used; iteration will cease when all values
;;; are within this tolerance of the last value.  If no
;;; tolerance is specified, a value of 1e-8 will be
;;; used.

(define (gauss-seidel a b . tol)
  (cond
   ((not (matrix? a))
    (error 'gauss-seidel "~s is not a matrix" a))
   ((not (matrix? b))
    (error 'gauss-seidel "~s is not a matrix" b))
   ((not (= (matrix-rows a) (matrix-rows b)))
    (error 'gauss-seidel "matrices have differing numbers of rows"))
   ((not (diagonally-dominant? a))
    (error 'gauss-seidel "matrix is not diagonally dominant"))
   (else
    (let* ((epsilon (if (not (null? tol)) (car tol) 1e-8))
	   (n (matrix-rows a))
	   (guess (make-matrix n (matrix-cols b) 0))
	   (last (make-matrix n (matrix-cols b) 1)))

      (do ((iter 1 (1+ iter)))
	  ((< (maximal-element (matrix-- last guess) #t) epsilon) guess)

	(set! last (copy-matrix guess))

	(do ((k 1 (1+ k))) ((> k (matrix-cols b)) guess)
	  
	  (do ((i 1 (1+ i))) ((> i n))
	    
	    (matrix-set!
	     guess i k
	     (- (/ (matrix-ref b i k) (matrix-ref a i i))
		(let loop ((j 1) (sum 0))
		  (cond
		   ((> j n) sum)
		   ((= j i) (loop (1+ j) sum))
		   (else
		    (loop (1+ j)
			  (+ sum (* (/ (matrix-ref a i j)
				       (matrix-ref a i i))
				    (matrix-ref guess j k))))))))))))))))