;; -*-theme-d-*-

;; Copyright (C) 2014  Tommi Höynälänmaa
;; Distributed under GNU Lesser General Public License version 3,
;; see file doc/LGPL-3.


(define-body (standard-library matrix)


  (import (standard-library list-utilities))

  
  (define-param-proc matrix (%number)
		     (((elements (:uniform-list (:uniform-list %number))))
		      (:matrix %number)
		      (force-pure))
    (let ((rows (length elements)))
      (assert (> rows 0))
      (let ((columns (length (gen-car elements))))
	(assert (and-map?
		 (lambda (((row (:uniform-list %number))) <boolean> (pure))
		   (= (length row) columns))
		 (gen-cdr elements)))
	(let ((result (make-matrix rows columns (zero %number))))
	  (do ((i-row <integer> 0 (+ i-row 1))
	       (cur-elements (:uniform-list (:uniform-list %number))
			     elements (gen-cdr cur-elements)))
	      ((>= i-row rows))
	    (do ((i-column <integer> 0 (+ i-column 1))
		 (cur-elements2 (:uniform-list %number)
		  (gen-car cur-elements) (gen-cdr cur-elements2)))
		((>= i-column columns))
	      (matrix-set! result i-row i-column (gen-car cur-elements2))))
	  result))))


  (define-param-proc make-matrix (%number)
		     (((rows <integer>) (columns <integer>) (element %number))
		      (:matrix %number)
		      (pure))
    (make (:matrix %number) rows columns
	  (make-mutable-value-vector %number (* rows columns) element)))


  (define-param-proc column-vector (%number)
		     (((elements (:uniform-list %number)))
		      (:matrix %number)
		      (force-pure))
    (let* ((len (length elements))
	   (result (make-matrix len 1 (zero %number)))
	   (contents (field-ref result 'contents)))
      (do ((i <integer> 0 (+ i 1))
	   (cur (:uniform-list %number) elements (gen-cdr cur)))
	  ((>= i len))
	(mutable-value-vector-set! contents i (gen-car cur)))
      result))


  (define-param-proc make-column-vector (%number)
		     (((len <integer>) (element %number))
		      (:matrix %number)
		      (pure))
    (make-matrix len 1 element))


  (define-param-proc row-vector (%number)
		     (((elements (:uniform-list %number)))
		      (:matrix %number)
		      (force-pure))
    (let* ((len (length elements))
	   (result (make-matrix 1 len (zero %number)))
	   (contents (field-ref result 'contents)))
      (do ((i <integer> 0 (+ i 1))
	   (cur (:uniform-list %number) elements (gen-cdr cur)))
	  ((>= i len))
	(mutable-value-vector-set! contents i (gen-car cur)))
      result))


  (define-param-proc make-row-vector (%number)
		     (((len <integer>) (element %number))
		      (:matrix %number)
		      (pure))
    (make-matrix 1 len element))


  (define-param-proc diagonal-matrix (%number)
		     (((elements (:uniform-list %number)))
		      (:diagonal-matrix %number)
		      (force-pure))
    (let ((len (length elements)))
      (if (>= len 0)
	  (let* ((result (make-diagonal-matrix len (zero %number)))
		 (contents (field-ref result 'contents)))
	    (do ((i <integer> 0 (+ i 1)) 
		 (cur (:uniform-list %number) elements (gen-cdr cur)))
		((>= i len))
	      (mutable-value-vector-set! contents i (gen-car cur)))
	    result)
	  (raise-simple 'diagonal-matrix:invalid-size))))


  (define-param-proc make-diagonal-matrix (%number)
		     (((len <integer>)
		       (element %number))
		      (:diagonal-matrix %number)
		      (force-pure))
    (make (:diagonal-matrix %number) len
	  (make-mutable-value-vector %number len element)))


  (define-param-proc matrix-copy (%number)
		     (((mx (:matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (let* ((rows (field-ref mx 'rows))
	   (columns (field-ref mx 'columns))
	   (raw-len (* rows columns))
	   (source-contents (field-ref mx 'contents))
	   (contents (make-mutable-value-vector
		      %number raw-len (zero %number)))
	   (result (make (:matrix %number) rows columns contents)))
      (do ((i <integer> 0 (+ i 1)))
	  ((>= i raw-len))
	(mutable-value-vector-set!
	 contents i
	 (mutable-value-vector-ref source-contents i)))
      result))


  (define-param-proc diagonal-matrix-copy (%number)
		     (((mx (:diagonal-matrix %number)))
		      (:diagonal-matrix %number)
		      (force-pure))
    (let* ((len (field-ref mx 'len))
	   (source-contents (field-ref mx 'contents))
	   (contents (make-mutable-value-vector
		      %number len (zero %number)))
	   (result (make (:diagonal-matrix %number) len contents)))
      (do ((i <integer> 0 (+ i 1)))
	  ((>= i len))
	(mutable-value-vector-set!
	 contents i
	 (mutable-value-vector-ref source-contents i)))
      result))


  (define-param-method number-of-columns (%number)
		       (((mx (:matrix %number)))
			<integer>
			(pure))
    (field-ref mx 'columns))


  (define-param-method number-of-columns (%number)
		       (((mx (:diagonal-matrix %number)))
			<integer>
			(pure))
    (field-ref mx 'len))


  (define-param-method number-of-rows (%number)
		       (((mx (:matrix %number)))
			<integer>
			(pure))
    (field-ref mx 'rows))


  (define-param-method number-of-rows (%number)
		       (((mx (:diagonal-matrix %number)))
			<integer>
			(pure))
    (field-ref mx 'len))


  (define-param-method matrix-ref (%number)
		       (((mx (:matrix %number))
			 (i-row <integer>)
			 (i-column <integer>))
			%number
			(pure))
    (let ((rows (field-ref mx 'rows))
	  (columns (field-ref mx 'columns)))
      (assert (and (>= i-row 0) (< i-row rows)))
      (assert (and (>= i-column 0) (< i-column columns)))
      (mutable-value-vector-ref
       (field-ref mx 'contents)
       (+ (* i-row columns) i-column))))


  (define-param-method matrix-ref (%number)
		       (((mx (:diagonal-matrix %number))
			 (i-row <integer>)
			 (i-column <integer>))
			%number
			(pure))
    (let ((len (field-ref mx 'len)))
      (assert (and (>= i-row 0) (< i-row len)))
      (assert (and (>= i-column 0) (< i-column len)))
      (if (= i-row i-column)
	  (mutable-value-vector-ref
	   (field-ref mx 'contents)
	   i-row)
	  (zero %number))))


  (define-param-method matrix-set! (%number)
		       (((mx (:matrix %number))
			 (i-row <integer>)
			 (i-column <integer>)
			 (element %number))
			<none>
			(nonpure))
    (let ((rows (field-ref mx 'rows))
	  (columns (field-ref mx 'columns)))
      (assert (and (>= i-row 0) (< i-row rows)))
      (assert (and (>= i-column 0) (< i-column columns)))
      (mutable-value-vector-set!
       (field-ref mx 'contents)
       (+ (* i-row columns) i-column) element)))


  (define-param-method matrix-set! (%number)
		       (((mx (:diagonal-matrix %number))
			 (i-row <integer>)
			 (i-column <integer>)
			 (element %number))
			<none>
			(nonpure))
    (let ((len (field-ref mx 'len)))
      (assert (and (>= i-row 0) (< i-row len)))
      (assert (= i-row i-column))
      (mutable-value-vector-set! (field-ref mx 'contents)
				 i-row element)))


  (define-param-proc diagonal-matrix-ref (%number)
		     (((mx (:diagonal-matrix %number))
		       (i <integer>))
		      %number
		      (pure))
      (assert (and (>= i 0) (< i (field-ref mx 'len))))
      (mutable-value-vector-ref (field-ref mx 'contents) i))


  (define-param-proc diagonal-matrix-set! (%number)
		     (((mx (:diagonal-matrix %number))
		       (i <integer>)
		       (element %number))
		      <none>
		      (nonpure))
      (assert (and (>= i 0) (< i (field-ref mx 'len))))
      (mutable-value-vector-set!
       (field-ref mx 'contents)
       i element))


  (define-param-proc matrix= (%number)
		     (((mx1 (:matrix %number)) (mx2 (:matrix %number)))
		      <boolean> pure)
    (if (equal-objects? mx1 mx2)
	#t
	(let ((i-rows (field-ref mx1 'rows))
	      (i-columns (field-ref mx1 'columns)))
	  (and
	   (integer= i-rows (field-ref mx2 'rows))
	   (integer= i-columns (field-ref mx2 'columns))
	   (let* ((v-contents1 (field-ref mx1 'contents))
		  (v-contents2 (field-ref mx2 'contents))
		  (i-len1 (mutable-value-vector-length v-contents1))
		  (i-len2 (mutable-value-vector-length v-contents2)))
	     (assert (integer= i-len1 i-len2))
	     (let-mutable ((all-eq? <boolean> #t))
	       (do ((i <integer> 0 (+ i 1)))
		   ((or (>= i i-len1) (not all-eq?)) all-eq?)
		 (if (not (= (mutable-value-vector-ref v-contents1 i)
			     (mutable-value-vector-ref v-contents2 i)))
		     (set! all-eq? #f)))))))))

  
  (define-param-proc diagonal-matrix= (%number)
		     (((mx1 (:diagonal-matrix %number))
		       (mx2 (:diagonal-matrix %number)))
		      <boolean> pure)
    (if (equal-objects? mx1 mx2)
	#t
	(let ((i-len1 (field-ref mx1 'len))
	      (i-len2 (field-ref mx2 'len)))
	  (and (integer= i-len1 i-len2)
	       (begin
		 (assert (integer= i-len1 i-len2))
		 (let ((v-contents1 (field-ref mx1 'contents))
		       (v-contents2 (field-ref mx2 'contents)))
		   (let-mutable ((all-eq? <boolean> #t))
		     (do ((i <integer> 0 (+ i 1)))
			 ((or (>= i i-len1) (not all-eq?)) all-eq?)
		       (if (not (= (mutable-value-vector-ref v-contents1 i)
				   (mutable-value-vector-ref v-contents2 i)))
			   (set! all-eq? #f))))))))))

  
  (define-param-proc matrix-diagonal-matrix= (%number)
		     (((mx1 (:matrix %number))
		       (mx2 (:diagonal-matrix %number)))
		      <boolean> pure)
    (let ((i-rows1 (field-ref mx1 'rows))
	  (i-columns1 (field-ref mx1 'columns))
	  (i-len2 (field-ref mx2 'len))
	  (v-contents2 (field-ref mx2 'contents))
	  (x-zero (zero %number)))
      (and
       (integer= i-rows1 i-len2)
       (integer= i-columns1 i-len2)
       (let-mutable ((all-eq1? <boolean> #t))
	 (do ((i <integer> 0 (+ i 1)))
	     ((or (>= i i-len2) (not all-eq1?)) all-eq1?)
	   (if (not (= (matrix-ref mx1 i i)
		       (mutable-value-vector-ref v-contents2 i)))
	       (set! all-eq1? #f))))
       (let-mutable ((all-eq2? <boolean> #t))
	 (do ((i1 <integer> 0 (+ i1 1)))
	     ((or (>= i1 i-len2) (not all-eq2?)) all-eq2?)
	   (do ((i2 <integer> 0 (+ i2 1)))
	       ((or (>= i2 i-len2) (not all-eq2?)))
	     (if (and (not (integer= i1 i2))
		      (not (= (matrix-ref mx1 i1 i2) x-zero)))
		 (set! all-eq2? #f))))))))

  
  (define-param-proc diagonal-matrix-matrix= (%number)
		     (((mx1 (:diagonal-matrix %number))
		       (mx2 (:matrix %number)))
		      <boolean> pure)
    (let ((i-rows2 (field-ref mx2 'rows))
	  (i-columns2 (field-ref mx2 'columns))
	  (i-len1 (field-ref mx1 'len))
	  (v-contents1 (field-ref mx1 'contents))
	  (x-zero (zero %number)))
      (and
       (integer= i-rows2 i-len1)
       (integer= i-columns2 i-len1)
       (let-mutable ((all-eq1? <boolean> #t))
	 (do ((i <integer> 0 (+ i 1)))
	     ((or (>= i i-len1) (not all-eq1?)) all-eq1?)
	   (if (not (= (matrix-ref mx2 i i)
		       (mutable-value-vector-ref v-contents1 i)))
	       (set! all-eq1? #f))))
       (let-mutable ((all-eq2? <boolean> #t))
	 (do ((i1 <integer> 0 (+ i1 1)))
	     ((or (>= i1 i-len1) (not all-eq2?)) all-eq2?)
	   (do ((i2 <integer> 0 (+ i2 1)))
	       ((or (>= i2 i-len1) (not all-eq2?)))
	     (if (and (not (integer= i1 i2))
		      (not (= (matrix-ref mx2 i1 i2) x-zero)))
		 (set! all-eq2? #f))))))))


  (add-method = matrix=)


  (add-method = diagonal-matrix=)

  
  (add-method = matrix-diagonal-matrix=)

  
  (add-method = diagonal-matrix-matrix=)

  
  (define-param-proc matrix* (%number)
		     (((mx1 (:matrix %number)) (mx2 (:matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (let ((rows1 (field-ref mx1 'rows))
	  (columns1 (field-ref mx1 'columns))
	  (rows2 (field-ref mx2 'rows))
	  (columns2 (field-ref mx2 'columns)))
      (assert (= columns1 rows2))
      (let ((result (make-matrix rows1 columns2 (zero %number))))
	(do ((i1 <integer> 0 (+ i1 1))) ((>= i1 rows1))
	  (do ((i2 <integer> 0 (+ i2 1))) ((>= i2 columns2))
	    (let-mutable ((sum %number (zero %number)))
	      (do ((i3 <integer> 0 (+ i3 1))) ((>= i3 columns1))
		(set! sum
		      (+ sum
			 (* (matrix-ref mx1 i1 i3)
			    (matrix-ref mx2 i3 i2)))))
	      (matrix-set! result i1 i2 sum))))
	result)))


  (define-param-proc diagonal-matrix* (%number)
		     (((mx1 (:diagonal-matrix %number))
		       (mx2 (:diagonal-matrix %number)))
		      (:diagonal-matrix %number)
		      (force-pure))
    (let ((len1 (field-ref mx1 'len))
	  (len2 (field-ref mx2 'len)))
      (assert (= len1 len2))
      (let* ((result (make-diagonal-matrix len1 (zero %number)))
	     (vec1 (field-ref mx1 'contents))
	     (vec2 (field-ref mx2 'contents))
	     (vec-r (field-ref result 'contents)))
	(do ((i <integer> 0 (+ i 1))) ((>= i len1))
	  (mutable-value-vector-set! vec-r i
				     (* (mutable-value-vector-ref vec1 i)
					(mutable-value-vector-ref vec2 i))))
	result)))


  (add-method * matrix*)


  (add-method * diagonal-matrix*)


  (define-param-method * (%number)
		       (((nr %number) (mx (:matrix %number)))
			(:matrix %number)
			(force-pure))
    (let* ((rows (field-ref mx 'rows))
	   (columns (field-ref mx 'columns))
	   (result (make-matrix rows columns (zero %number)))
	   (vec-r (field-ref result 'contents))
	   (vec-s (field-ref mx 'contents))
	   (raw-len (mutable-value-vector-length vec-s)))
      (assert (= (mutable-value-vector-length vec-r) raw-len))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set!
	 vec-r i
	 (* nr (mutable-value-vector-ref vec-s i))))
      result))


  (define-param-method * (%number)
		       (((mx (:matrix %number)) (nr %number))
			(:matrix %number)
			(force-pure))
    (let* ((rows (field-ref mx 'rows))
	   (columns (field-ref mx 'columns))
	   (result (make-matrix rows columns (zero %number)))
	   (vec-r (field-ref result 'contents))
	   (vec-s (field-ref mx 'contents))
	   (raw-len (mutable-value-vector-length vec-s)))
      (assert (= (mutable-value-vector-length vec-r) raw-len))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set!
	 vec-r i
	 (* nr (mutable-value-vector-ref vec-s i))))
      result))


  (define-param-method * (%number)
		       (((nr %number) (mx (:diagonal-matrix %number)))
			(:diagonal-matrix %number)
			(force-pure))
    (let* ((len (field-ref mx 'len))
	   (result (make-diagonal-matrix len (zero %number)))
	   (vec-r (field-ref result 'contents))
	   (vec-s (field-ref mx 'contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(mutable-value-vector-set!
	 vec-r i
	 (* nr (mutable-value-vector-ref vec-s i))))
      result))


  (define-param-method * (%number)
		       (((mx (:diagonal-matrix %number)) (nr %number))
			(:diagonal-matrix %number)
			(force-pure))
    (let* ((len (field-ref mx 'len))
	   (result (make-diagonal-matrix len (zero %number)))
	   (vec-r (field-ref result 'contents))
	   (vec-s (field-ref mx 'contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(mutable-value-vector-set!
	 vec-r i
	 (* nr (mutable-value-vector-ref vec-s i))))
      result))


  (define-param-method * (%number)
		       (((mx1 (:matrix %number))
			 (mx2 (:diagonal-matrix %number)))
			(:matrix %number)
			(force-pure))
    (let* ((rows1 (field-ref mx1 'rows))
	   (columns1 (field-ref mx1 'columns))
	   (len2 (field-ref mx2 'len)))
      (assert (= columns1 len2))
      (let ((result (make-matrix rows1 columns1 (zero %number))))
	(do ((i <integer> 0 (+ i 1))) ((>= i rows1))
	  (do ((j <integer> 0 (+ j 1))) ((>= j columns1))
	    (matrix-set! result i j (* (matrix-ref mx1 i j)
				       (diagonal-matrix-ref mx2 j)))))
	result)))


  (define-param-method * (%number)
		       (((mx1 (:diagonal-matrix %number))
			 (mx2 (:matrix %number)))
			(:matrix %number)
			(force-pure))
    (let* ((len1 (field-ref mx1 'len))
	   (rows2 (field-ref mx2 'rows))
	   (columns2 (field-ref mx2 'columns)))
      (assert (= len1 rows2))
      (let ((result (make-matrix rows2 columns2 (zero %number))))
	(do ((i <integer> 0 (+ i 1))) ((>= i rows2))
	  (do ((j <integer> 0 (+ j 1))) ((>= j columns2))
	    (matrix-set! result i j (* (diagonal-matrix-ref mx1 i)
				       (matrix-ref mx2 i j)))))
	result)))


  (define-param-method / (%number)
		       (((mx (:matrix %number)) (nr %number))
			(:matrix %number)
			(force-pure))
    (let* ((rows (field-ref mx 'rows))
	   (columns (field-ref mx 'columns))
	   (result (make-matrix rows columns (zero %number)))
	   (vec-r (field-ref result 'contents))
	   (vec-s (field-ref mx 'contents))
	   (raw-len (mutable-value-vector-length vec-s)))
      (assert (= (mutable-value-vector-length vec-r) raw-len))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set!
	 vec-r i
	 (/ (mutable-value-vector-ref vec-s i) nr)))
      result))


  (define-param-method / (%number)
		       (((mx (:diagonal-matrix %number)) (nr %number))
			(:diagonal-matrix %number)
			(force-pure))
    (let* ((len (field-ref mx 'len))
	   (result (make-diagonal-matrix len (zero %number)))
	   (vec-r (field-ref result 'contents))
	   (vec-s (field-ref mx 'contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(mutable-value-vector-set!
	 vec-r i
	 (/ (mutable-value-vector-ref vec-s i) nr)))
      result))


  (define-param-proc matrix+ (%number)
		     (((mx1 (:matrix %number)) (mx2 (:matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (assert (= (field-ref mx1 'rows) (field-ref mx2 'rows)))
    (assert (= (field-ref mx1 'columns) (field-ref mx2 'columns)))
    (let* ((rows (field-ref mx1 'rows))
	   (columns (field-ref mx1 'columns))
	   (result (make-matrix rows columns (zero %number)))
	   (contents (field-ref result 'contents))
	   (c1 (field-ref mx1 'contents))
	   (c2 (field-ref mx2 'contents))
	   (raw-len (mutable-value-vector-length contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set! contents i
				   (+ (mutable-value-vector-ref c1 i)
				      (mutable-value-vector-ref c2 i))))
      result))


  (define-param-proc diagonal-matrix+ (%number)
		     (((mx1 (:diagonal-matrix %number))
		       (mx2 (:diagonal-matrix %number)))
		      (:diagonal-matrix %number)
		      (force-pure))
    (let ((len1 (field-ref mx1 'len))
	  (len2 (field-ref mx2 'len)))
      (assert (= len1 len2))
      (let* ((result (make-diagonal-matrix len1 (zero %number)))
	     (contents (field-ref result 'contents))
	     (c1 (field-ref mx1 'contents))
	     (c2 (field-ref mx2 'contents)))
	(do ((i <integer> 0 (+ i 1))) ((>= i len1))
	  (mutable-value-vector-set!
	   contents
	   i
	   (+ (mutable-value-vector-ref c1 i)
	      (mutable-value-vector-ref c2 i))))
	result)))


  (add-method + matrix+)
  (add-method + diagonal-matrix+)


  (define-param-method + (%number)
		       (((mx1 (:matrix %number)) (mx2 (:diagonal-matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (assert (= (field-ref mx1 'rows) (field-ref mx2 'len)))
    (assert (= (field-ref mx1 'columns) (field-ref mx2 'len)))
    (let ((len (field-ref mx2 'len))
	  (result (matrix-copy mx1)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(matrix-set! result i i
		     (+ (matrix-ref mx1 i i)
			(diagonal-matrix-ref mx2 i))))
      result))


  (define-param-method + (%number)
		       (((mx1 (:diagonal-matrix %number))
			 (mx2 (:matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (assert (= (field-ref mx1 'len) (field-ref mx2 'rows)))
    (assert (= (field-ref mx1 'len) (field-ref mx2 'columns)))
    (let ((len (field-ref mx1 'len))
	  (result (matrix-copy mx2)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(matrix-set! result i i
		     (+ (diagonal-matrix-ref mx1 i)
			(matrix-ref mx2 i i))))
      result))


  (define-param-proc matrix- (%number)
		     (((mx1 (:matrix %number)) (mx2 (:matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (assert (= (field-ref mx1 'rows) (field-ref mx2 'rows)))
    (assert (= (field-ref mx1 'columns) (field-ref mx2 'columns)))
    (let* ((rows (field-ref mx1 'rows))
	   (columns (field-ref mx1 'columns))
	   (result (make-matrix rows columns (zero %number)))
	   (contents (field-ref result 'contents))
	   (c1 (field-ref mx1 'contents))
	   (c2 (field-ref mx2 'contents))
	   (raw-len (mutable-value-vector-length contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set! contents i
				   (- (mutable-value-vector-ref c1 i)
				      (mutable-value-vector-ref c2 i))))
      result))


  (define-param-proc diagonal-matrix- (%number)
		     (((mx1 (:diagonal-matrix %number))
		       (mx2 (:diagonal-matrix %number)))
		      (:diagonal-matrix %number)
		      (force-pure))
    (let ((len1 (field-ref mx1 'len))
	  (len2 (field-ref mx2 'len)))
      (assert (= len1 len2))
      (let* ((result (make-diagonal-matrix len1 (zero %number)))
	     (contents (field-ref result 'contents))
	     (c1 (field-ref mx1 'contents))
	     (c2 (field-ref mx2 'contents)))
	(do ((i <integer> 0 (+ i 1))) ((>= i len1))
	  (mutable-value-vector-set!
	   contents i
	   (- (mutable-value-vector-ref c1 i)
	      (mutable-value-vector-ref c2 i))))
	result)))


  (add-method - matrix-)
  (add-method - diagonal-matrix-)


  (define-param-method - (%number)
		       (((mx1 (:matrix %number)) (mx2 (:diagonal-matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (assert (= (field-ref mx1 'rows) (field-ref mx2 'len)))
    (assert (= (field-ref mx1 'columns) (field-ref mx2 'len)))
    (let ((len (field-ref mx2 'len))
	  (result (matrix-copy mx1)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(matrix-set! result i i
		     (- (matrix-ref mx1 i i)
			(diagonal-matrix-ref mx2 i))))
      result))


  (define-param-method - (%number)
		       (((mx1 (:diagonal-matrix %number))
			 (mx2 (:matrix %number)))
		      (:matrix %number)
		      (force-pure))
    (assert (= (field-ref mx1 'len) (field-ref mx2 'rows)))
    (assert (= (field-ref mx1 'len) (field-ref mx2 'columns)))
    (let ((len (field-ref mx1 'len))
	  (result (- mx2)))
      (do ((i <integer> 0 (+ i 1))) ((>= i len))
	(matrix-set! result i i
		     (+ (diagonal-matrix-ref mx1 i)
			(matrix-ref result i i))))
      result))


  (define-param-method - (%number)
		       (((mx (:matrix %number)))
			(:matrix %number)
			(force-pure))
    (let* ((rows (field-ref mx 'rows))
	   (columns (field-ref mx 'columns))
	   (result (make-matrix rows columns (zero %number)))
	   (contents (field-ref result 'contents))
	   (source-contents (field-ref mx 'contents))
	   (raw-len (mutable-value-vector-length contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set!
	 contents i
	 (- (mutable-value-vector-ref source-contents i))))
      result))


  (define-param-method - (%number)
		       (((mx (:diagonal-matrix %number)))
			(:diagonal-matrix %number)
			(force-pure))
    (let* ((len (field-ref mx 'len))
	   (result (make-diagonal-matrix len (zero %number)))
	   (contents (field-ref result 'contents))
	   (source-contents (field-ref mx 'contents))
	   (raw-len (mutable-value-vector-length contents)))
      (do ((i <integer> 0 (+ i 1))) ((>= i raw-len))
	(mutable-value-vector-set!
	 contents i
	 (- (mutable-value-vector-ref source-contents i))))
      result)))
