;; some functions for matrix manipulation, gauss-algorithm


(defun matrix-random (max m n)

  (let ((res (make-array (list m n))))
				 
    (dotimes (i m res) 

      (dotimes (j n) 

        (setf (aref res i j)

              (random max))))))


(defun matrix-copy (a)

  (let* ((m (array-dimension a 0))

         (n (array-dimension a 1)) 

         (res (make-array (list m n)))) 

    (dotimes (i m res) 

      (dotimes (j n) 

        (setf (aref res i j) (aref a i j))))))






(defun matrix-make-triangular (a)

  (let ((m (array-dimension a 0)))
                                    
    (loop for j upfrom 0

          while (< j (min m (array-dimension a 1)))
          
          finally (return a)

          do 

          (setf a (lower-to-zero a j)))))

(defun lower-to-zero (a col) 

  (let ((m (array-dimension a 0))

           (a (do-pivot-swap a col)))

        (if (/= 0 (aref a col col)) 

            (loop for i upfrom (1+ col)

                  while (< i m) 

                  finally (return a) 

                  do 

                  (setf a (add-to-zero a col i))) a)))


(defun do-pivot-swap (a col)

  (let* ((m (array-dimension a 0))

         (n (array-dimension a 1))

         (i0 (loop for i upfrom col

	           while (and (< i m) (= 0 (aref a i col)))

	           finally (return i))))

    (if (/= 0 (aref a i0 col))

        (loop for j upfrom col

	      while (< j n)

	      finally (return a)
              
              do

	      (rotatef (aref a col j) (aref a i0 j))) a)))




(defun add-to-zero (a col i)

  (let ((n (array-dimension a 1)) 

        (mult (/ (aref a i col) (aref a col col))))

    (loop for j upfrom col

          while (< j n)

          do 

          (setf (aref a i j) (- (aref a i j) (* mult (aref a col j))))

          finally (return a))))


(setf *arr1* (make-array '(2 3) :initial-contents '((1 1 1) (2 1 3))))

(setf *arr2* (make-array '(3 3) :initial-contents '((2 1 5) (2 1 3) (3 3 6))))






(defun gauss-solve (a)

  ( let ((m (array-dimension a 0)) 

         (n (array-dimension a 1)))

    (if (> n m) 

        (let ((res (make-array (list m (- n m)))))

          (loop for col upfrom 0 to (- n m 1)

                finally (return res)

                do

                (setf res (gauss-solve-column a res col)))))))


(defun gauss-solve-column (a res col)

  (let ((m (array-dimension a 0)))

    (loop with s = 0

          for i downfrom (- m 1) to 0
          
          do

          (setf s

                (loop with s1 = 0 

                      for j upfrom (+ i 1) to (- m 1)

	              sum (* (aref a i j) (aref res j col )) into s1

	              finally (return s1))) 

          (setf (aref res i col) (/ (- (aref a i (+ col m)) s) (aref a i i)))

          finally (return res))))


(defun matrix-concat-horizontally (a b)

  (let* ((m (min (array-dimension a 0) (array-dimension b 0)))

         (na (array-dimension a 1))

         (nb (array-dimension b 1))

         (res (make-array (list m (+ na nb)))))

    (dotimes (i m res)

      (dotimes (j na)
                   
        (setf (aref res i j) (aref a i j)))

      (dotimes (j nb)

        (setf (aref res i (+ j na)) (aref b i j))))))



(defun matrix-multiply (a b)

  (let* ((m (array-dimension a 0))

         (n (array-dimension b 1))

         (p (min (array-dimension a 1) (array-dimension b 0)))

         (res (make-array (list m n))))

    (dotimes (i m res)

      (dotimes (j n)

        (setf (aref res i j )

	      (loop for k upfrom 0 to (- p 1) 

                    sum (* (aref a i k) (aref b k j)) ))))))


(defun matrix-diagonal (diagvec)

  (let* ((n (length diagvec))

         (res (make-array (list n n) :initial-element 0)))

    (dotimes (i n res)

      (setf (aref res i i) (first diagvec))

      (setf diagvec (rest diagvec)))))



(defun test (n)

  (let (a1 diag ee a1ee a1inv)

    (setf a1 (matrix-random 1.0D1 n n))

    (setf diag (make-list n :initial-element 1))

    (setf ee (matrix-diagonal diag))

    (setf a1ee (matrix-concat-horizontally a1 ee))

    (setf a1ee (matrix-make-triangular a1ee))

    (setf a1inv (gauss-solve a1ee))

    (matrix-multiply a1 a1inv)))