;; author: ismdeep
;; blog: https://ismdeep.com
;; blog entry: https://ismdeep.com/238
;; doolittle algorithm to decompose a square matrix

(defun mmul (A B)
  (let* ((m (car (array-dimensions A)))
         (n (cadr (array-dimensions A)))
         (l (cadr (array-dimensions B)))
         (C (make-array `(,m ,l) :initial-element 0)))
    (loop for i from 0 to (- m 1) do
              (loop for k from 0 to (- l 1) do
                    (setf (aref C i k)
                          (loop for j from 0 to (- n 1)
                                sum (* (aref A i j)
                                       (aref B j k))))))
    C))
 
;; Using doolittle algorithm to decomposes a square matrix A for LU = A and LDV = A
(defun doolittle-lu (A)
  (let* ((n (car (array-dimensions A))) (tmp 0)
         (L (make-array `(,n ,n) :initial-element 0))
         (U (make-array `(,n ,n) :initial-element 0))
         (D (make-array `(,n ,n) :initial-element 0))
         (V (make-array `(,n ,n) :initial-element 0))
         (lu-mmul 0)
         (ldv-mmul 0))

    (loop for j from 0 to (- n 1) do (setf (aref U 0 j) (aref A 0 j)))
    (loop for i from 0 to (- n 1) do (setf (aref L i i) 1))
    (loop for i from 1 to (- n 1) do 
      ; generate L
      (loop for j from 0 to (- i 1) do
        (setf tmp (aref A i j))
        (loop for k from 0 to (- j 1) do (setf tmp (- tmp (* (aref L i k) (aref U k j)))))
        (setf (aref L i j) (/ tmp (aref U j j)))
        )
      ; generate U
      (loop for j from i to (- n 1) do
        (setf tmp (aref A i j ))
        (loop for k from 0 to (- i 1) do (setf tmp (- tmp (* (aref L i k) (aref U k j)))))
        (setf (aref U i j) tmp)
        )
      )

    ; output A
    (format t "---------- A ------------------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref A i j)))
      (format t "~%"))    

    ; output L
    (format t "~%~%---------- L ------------------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref L i j)))
      (format t "~%"))

    ; output U
    (format t "~%~%---------- U ------------------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref U i j)))
      (format t "~%"))

    ; output L * U
    (setf lu-mmul (mmul L U))
    (format t "~%~%---------- L * U --------------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref lu-mmul i j)))
      (format t "~%"))

    ; generate D
    (loop for i from 0 to (- n 1) do
      (setf (aref D i i) (aref U i i)))

    ; generate V
    (loop for i from 0 to (- n 1) do
      (loop for j from i to (- n 1) do
        (setf (aref V i j) (/ (aref U i j) (aref U i i)))))

    ; output D
    (format t "~%~%---------- D ------------------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref D i j)))
      (format t "~%"))

    ; output V
    (format t "~%~%---------- V ------------------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref V i j)))
      (format t "~%"))


    ; output L * D * V
    (setf ldv-mmul (mmul (mmul L D) V))
    (format t "~%~%---------- L * D * V ----------~%")
    (loop for i from 0 to (- n 1) do
      (loop for j from 0 to (- n 1) do 
        (format t "~4A " (aref ldv-mmul i j)))
      (format t "~%"))


  ;; Return L and U
  (values L U)))


(let ((g 0))
  (setf g (make-array '(4 4) :initial-contents '((2 1 -5 1) (1 -3 0 -6) (0 2 -1 2) (1 4 -7 6)))) 
  (doolittle-lu g))