;; timing test for generalized ref/set!

(define invert-matrix
  (let ((+documentation+ "(invert-matrix matrix b (zero 1.0e-7)) inverts 'matrix'"))
    (lambda* (matrix b (zero 1.0e-7))
      ;; translated from Numerical Recipes (gaussj)
      (call-with-exit
       (lambda (return)
	 (let ((n (vector-dimension matrix 0)))
	   (let ((cols (make-int-vector n))
		 (rows (make-int-vector n))
		 (pivots (make-int-vector n)))
	     (do ((i 0 (+ i 1))
		  (col 0 0)
		  (row 0 0))
		 ((= i n))
	       (do ((biggest 0.0)
		    (j 0 (+ j 1)))
		   ((= j n)
		    (if (< biggest zero) 
			(return #f)))
		 (if (not (= (pivots j) 1))
		     (do ((k 0 (+ k 1)))
			 ((= k n))
		       (if (= (pivots k) 0)
			   (let ((val (abs (matrix j k))))
			     (when (> val biggest)
			       (set! col k)
			       (set! row j)
			       (set! biggest val)))
			   (if (> (pivots k) 1)
			       (return #f))))))
	       (set! (pivots col) (+ (pivots col) 1))
	       (if (not (= row col))
		   (let ((temp (if (sequence? b) (b row) 0.0)))
		     (when (sequence? b)
		       (set! (b row) (b col))
		       (set! (b col) temp))
		     (do ((k 0 (+ k 1)))
			 ((= k n))
		       (set! temp (matrix row k))
		       (set! (matrix row k) (matrix col k))
		       (set! (matrix col k) temp))))
	       (set! (cols i) col)
	       (set! (rows i) row)
	       ;; round-off troubles here
	       (if (< (abs (matrix col col)) zero)
		   (return #f))
	       (let ((inverse-pivot (/ 1.0 (matrix col col))))
		 (set! (matrix col col) 1.0)
		 (do ((k 0 (+ k 1)))
		     ((= k n))
		   (set! (matrix col k) (* inverse-pivot (matrix col k))))
		 (if b (set! (b col) (* inverse-pivot (b col)))))
	       (do ((k 0 (+ k 1)))
		   ((= k n))
		 (if (not (= k col))
		     (let ((scl (matrix k col)))
		       (set! (matrix k col) 0.0)
		       (do ((m 0 (+ 1 m)))
			   ((= m n))
			 (set! (matrix k m) (- (matrix k m) (* scl (matrix col m)))))
		       (if b (set! (b k) (- (b k) (* scl (b col)))))))))
	     (do ((i (- n 1) (- i 1)))
		 ((< i 0))
	       (if (not (= (rows i) (cols i)))
		   (do ((k 0 (+ k 1)))
		       ((= k n))
		     (let ((temp (matrix k (rows i))))
		       (set! (matrix k (rows i)) (matrix k (cols i)))
		       (set! (matrix k (cols i)) temp)))))
	     (list matrix b))))))))


(define (matrix-multiply A B)
  (let ((size (vector-dimension A 0)))
    (do ((C (make-float-vector (list size size)))
         (i 0 (+ i 1)))
	((= i size) C)
      (do ((j 0 (+ j 1)))
	  ((= j size))
	(do ((sum 0.0)
             (k 0 (+ k 1)))
	    ((= k size)
             (set! (C i j) sum))
	  (set! sum (+ sum (* (A i k) (B k j)))))))))


;;; Rosetta scheme code + changes
(define (square-matrix mat)
  (matrix-multiply mat mat))

(define (matrix-exp mat pow)
  (cond ((= pow 0)
	 (let ((m (copy mat)))
	   (fill! m 0.0)
	   (let ((size (car (vector-dimensions mat))))
	     (do ((i 0 (+ i 1)))
		 ((= i size) m)
	       (set! (m i i) 1.0)))))
	  
	  ((= pow 1) 
	   mat)

	  ((negative? pow)
	   (let ((im (invert-matrix (matrix-exp mat (abs pow)))))
	     (and (pair? im)
		  (car im))))

	  ((even? pow)
	   (square-matrix (matrix-exp mat (ash pow -1))))
	  
	  (else
	   (matrix-multiply mat (matrix-exp mat (- pow 1))))))

(define (testm)
  (do ((i 0 (+ i 1)))
      ((= i 10000))
    (let ((v (make-float-vector '(4 4)))
	  (pow (+ 1 (random 10))))
      (do ((k 0 (+ k 1)))
	  ((= k 4))
	(do ((n 0 (+ n 1)))
	    ((= n 4))
	  (set! (v k n) (- (random 50.0) 100.0))))
      (let ((mn (matrix-exp (copy v) (- pow))))
	(if mn
	    (let* ((m1 (matrix-multiply mn (matrix-exp (copy v) pow)))
		   (mx1 (m1 0 0))
		   (mx0 0.0))
	      (do ((k 1 (+ k 1)))
		  ((= k 4))
		(if (> (abs (- (m1 k k) 1.0)) (abs (- mx1 1.0)))
		    (set! mx1 (m1 k k))))
	      (do ((k 0 (+ k 1)))
		  ((= k 4))
		(do ((n 0 (+ n 1)))
		    ((= n 4))
		  (unless (= k n)
		    (set! mx0 (max mx0 (abs (m1 k n)))))))))))))
  
(testm)

;;; --------------------------------------------------------------------------------

(define flarray #r2d((0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20) 
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)))

(define tfl #r2d((0 0 0 0 0 0 0) (1 1 1 1 1 1 1) (2 2 2 2 2 2 2) (3 3 3 3 3 3 3) 
		 (4 4 4 4 4 4 4) (5 5 5 5 5 5 5) (6 6 6 6 6 6 6) (7 7 7 7 7 7 7) 
		 (8 8 8 8 8 8 8) (9 9 9 9 9 9 9) (10 10 10 10 10 10 10) (11 11 11 11 11 11 11)
		 (12 12 12 12 12 12 12) (13 13 13 13 13 13 13) (14 14 14 14 14 14 14) (15 15 15 15 15 15 15)
		 (16 16 16 16 16 16 16) (17 17 17 17 17 17 17) (18 18 18 18 18 18 18) (19 19 19 19 19 19 19) (20 20 20 20 20 20 20)))

(define narray #2d((0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20) 
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)
		     (0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20)))

(define tna #2d((0 0 0 0 0 0 0) (1 1 1 1 1 1 1) (2 2 2 2 2 2 2) (3 3 3 3 3 3 3) 
		 (4 4 4 4 4 4 4) (5 5 5 5 5 5 5) (6 6 6 6 6 6 6) (7 7 7 7 7 7 7) 
		 (8 8 8 8 8 8 8) (9 9 9 9 9 9 9) (10 10 10 10 10 10 10) (11 11 11 11 11 11 11)
		 (12 12 12 12 12 12 12) (13 13 13 13 13 13 13) (14 14 14 14 14 14 14) (15 15 15 15 15 15 15)
		 (16 16 16 16 16 16 16) (17 17 17 17 17 17 17) (18 18 18 18 18 18 18) (19 19 19 19 19 19 19) (20 20 20 20 20 20 20)))

(define tries 10000)
(set! (*s7* 'print-length) 123123)

(define (matrix-transpose mat)
  (let* ((dims (vector-dimensions mat))
	 (new-mat (make-vector (reverse dims)))
	 (end1 (car dims))
	 (end2 (cadr dims)))
    (do ((i 0 (+ i 1)))
	((= i end1) new-mat)
      (do ((j 0 (+ j 1)))
	  ((= j end2))
	(set! (new-mat j i) (mat i j))))))

(unless (equal? (matrix-transpose narray) tna)
  (format *stderr* "tna: ~S~%" (matrix-transpose narray)))

(define (test-transpose)
  (do ((i 0 (+ i 1)))
      ((= i  tries))
    (matrix-transpose narray)))

(test-transpose)


(define (matrix-transpose1 mat)
  (let* ((dims (vector-dimensions mat))
	 (new-mat (make-vector (reverse dims)))
	 (end1 (car dims))
	 (end2 (cadr dims)))
    (do ((i 0 (+ i 1)))
	((= i end1) new-mat)
      (do ((j 0 (+ j 1)))
	  ((= j end2))
	(vector-set! new-mat j i (vector-ref mat i j))))))

(unless (equal? (matrix-transpose1 narray) tna)
  (format *stderr* "tna1: ~S~%" (matrix-transpose1 narray)))

(define (test-transpose1)
  (do ((i 0 (+ i 1)))
      ((= i tries))
    (matrix-transpose1 narray)))

(test-transpose1)


(define (float-matrix-transpose mat)
  (let* ((dims (vector-dimensions mat))
	 (new-mat (make-float-vector (reverse dims)))
	 (end1 (car dims))
	 (end2 (cadr dims)))
    (do ((i 0 (+ i 1)))
	((= i end1) new-mat)
      (do ((j 0 (+ j 1)))
	  ((= j end2))
	(set! (new-mat j i) (mat i j))))))

(unless (equal? (float-matrix-transpose flarray) tfl)
  (format *stderr* "tfl: ~S~%" (float-matrix-transpose flarray)))

(define (test-float-transpose)
  (do ((i 0 (+ i 1)))
      ((= i tries))
    (float-matrix-transpose flarray)))

(test-float-transpose)


(define (float-matrix-transpose1 mat)
  (let* ((dims (vector-dimensions mat))
	 (new-mat (make-float-vector (reverse dims)))
	 (end1 (car dims))
	 (end2 (cadr dims)))
    (do ((i 0 (+ i 1)))
	((= i end1) new-mat)
      (do ((j 0 (+ j 1)))
	  ((= j end2))
	(float-vector-set! new-mat j i (float-vector-ref mat i j))))))

(unless (equal? (float-matrix-transpose1 flarray) tfl)
  (format *stderr* "tfl1: ~S~%" (float-matrix-transpose1 flarray)))

(define (test-float-transpose1)
  (do ((i 0 (+ i 1)))
      ((= i tries))
    (float-matrix-transpose1 flarray)))

(test-float-transpose1)



(when (> (*s7* 'profile) 0)
  (show-profile 200))
(exit)