;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; Copyright (c) 2000 The Regents of the University of California.
;;; All rights reserved. 
;;; 
;;; Permission is hereby granted, without written agreement and without
;;; license or royalty fees, to use, copy, modify, and distribute this
;;; software and its documentation for any purpose, provided that the
;;; above copyright notice and the following two paragraphs appear in all
;;; copies of this software.
;;; 
;;; IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY
;;; FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
;;; ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF
;;; THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
;;; SUCH DAMAGE.
;;;
;;; THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
;;; INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
;;; MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
;;; PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
;;; CALIFORNIA HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,
;;; ENHANCEMENTS, OR MODIFICATIONS.
;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; Originally written by Raymond Toy for Matlisp
;;; 
;;; Modfied by Jørn Inge Vestgården
;;; 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;; An issue with this code is the following: it creates output matrices 
;;; of default kind, ie. matrix-dge and matrix-zge. It should create based 
;;; on input in stead.

(in-package :lisplab)

(defmethod eigenvectors ((a matrix-foreign-dge))
  (if cl-user::*lisplab-liblapack-path*
      (destructuring-bind (evals vl-mat vr-mat)
	  (dgeev (copy a) nil (mcreate a 0))
	(list evals vr-mat))
      (call-next-method)))

(defmethod eigenvalues ((a matrix-foreign-dge))
  (if cl-user::*lisplab-liblapack-path*
      (destructuring-bind (evals vl-mat vr-mat)
	  (dgeev (copy a) nil nil)
	evals)
      (call-next-method)))

(defgeneric rearrange-eigenvector-matrix (v p))

(defmethod rearrange-eigenvector-matrix (v p)
  p)

(defmethod rearrange-eigenvector-matrix ((evals matrix-foreign-zge) (p matrix-foreign-dge)) 
  (let* ((n (size evals))
	 (evec (znew 0 n n))) ; TODO aggregate input type
    (do ((col 0 (incf col)))
	((>= col n))
      (if (zerop (imagpart (vref evals col )))
	  (dotimes (row n)
	    (setf (mref evec row col) 
		  (mref p row col)))
	  (progn 
	    (dotimes (row n)
	      (let ((c (complex (mref p row col) (mref p row (1+ col)))))
		(setf (mref evec row col) c
		      (mref evec row (1+ col)) (conjugate c))))
	    (incf col))))
    evec))

(defun combine-ri-vectors (wr wi)
  (let* ((n (size wr))
	 (wr2 (make-instance 'matrix-dge :dim (list n 1) :store wr))
	 (wi2 (make-instance 'matrix-dge :dim (list n 1) :store wi)))
    (if (.= wi2 0)
	wr2
	(.+ wr2 (.* %i (convert wi2 'matrix-zge))))))

(defun dgeev-workspace-size (n lv? rv?)
  ;; Ask geev how much space it wants for the work array
  (let* ((work (allocate-real-store 1)))
    (multiple-value-bind (store-a store-wr store-wi store-vl store-vr
				  work info)
	(f77::dgeev
	 (if lv? "V" "N")
	 (if rv? "V" "N")
	 n			; N
	 work			; A
	 n			; LDA
	 work			; WR
	 work			; WI
	 work			; VL
	 (if lv? n 1)          	; LDVL
	 work			; VR
	 (if rv? n 1)           ; LDVR
	 work			; WORK
	 -1			; LWORK
	 0 )			; INFO
      (declare (ignore store-a store-wr store-wi store-vl store-vr))
      (assert (zerop info))
      (ceiling (realpart (aref work 0))))))

(defun dgeev (a vl-mat vr-mat)
  "Wrapper for f77:dgeev. Potentially destructive."
  (let* ((n (rows a))
	 (xxx (allocate-real-store 1))
	 (wr (allocate-real-store n))
	 (wi (allocate-real-store n))
	 (vl (if vl-mat (vector-store vl-mat) xxx))
	 (vr (if vr-mat (vector-store vr-mat) xxx))
	 (lwork (dgeev-workspace-size n (if vl-mat t nil) (if vr-mat t nil) ))
	 (work (allocate-real-store lwork)))
    (multiple-value-bind (a wr wi vl vr work info)
	(f77::dgeev 
	 
	 (if vl-mat "V" "N")    ; JOBVL
	 (if vr-mat "V" "N")    ; JOBVR
	 n			; N
	 (vector-store a)	; A
	 n			; LDA
	 wr			; WR
	 wi			; WI
	 vl             	; VL
	 (if vl-mat n 1)       	; LDVL
	 vr		        ; VR
	 (if vr-mat n 1)        ; LDVR
	 work			; WORK
	 lwork			; LWORK
	 0 )			; INFO
      (let* ((evec (combine-ri-vectors wr wi))
	     (vl-mat2 (rearrange-eigenvector-matrix evec vl-mat))
	     (vr-mat2 (rearrange-eigenvector-matrix evec vr-mat)))
	(list evec vl-mat2 vr-mat2)))))

(defmethod eigenvectors ((a matrix-zge))
  (if cl-user::*lisplab-liblapack-path*
      (destructuring-bind (evals vl-mat vr-mat)
	  (zgeev (copy a) nil (mcreate a 0))
	(list evals vr-mat))
      (call-next-method)))

(defmethod eigenvalues ((a matrix-zge))
  (if cl-user::*lisplab-liblapack-path*
      (destructuring-bind (evals vl-mat vr-mat)
	  (zgeev (copy a) nil nil)
	evals)
      (call-next-method)))

(defun zgeev-workspace-size (n lv? rv?)
  ;; Ask geev how much space it wants for the work array
  (let* ((work (allocate-real-store 1)))
    (f77::zgeev 
     (if lv? "V" "N")
     (if rv? "V" "N")
     n		  	        ; N
     work			; A
     n			        ; LDA
     work			; W
     work			; VL
     (if lv? n 1)		; LDVL
     work			; VR
     (if rv? n 1)	        ; LDVR
     work			; WORK
     -1			        ; LWORK
     work			; RWORK
     0 )			; INFO
    ;; The desired size in in work[0], which we convert to an
    ;; integer.
    (ceiling (aref work 0))))

(defun zgeev (a vl-mat vr-mat)
  "Wrapper for f77:zgeev. Potentially destructive."
  (let* ((n (rows a))
	 (2n (* 2 n))
	 (xxx (allocate-real-store 2))
	 (w  (znew 0 n 1)) ; TODO aggregate type
	 (vl (if vl-mat (vector-store vl-mat) xxx))
	 (vr (if vr-mat (vector-store vr-mat) xxx))
	 (lwork (zgeev-workspace-size n (if vl-mat t nil) (if vr-mat t nil)))
	 (work (allocate-real-store lwork))
	 (rwork (allocate-real-store 2n)))
    ;; TODO get info for error
    (f77::zgeev 
     (if vl-mat "V" "N") ;; JOBVL
     (if vr-mat "V" "N") ;; JOBVR
     n                   ;; N
     (vector-store a)    ;; A
     n                   ;; LDA
     (vector-store w)    ;; W
     vl                  ;; VL
     (if vl-mat n 1)     ;; LDVL
     vr                  ;; VR
     (if vr-mat n 1)     ;; LDVR
     work                ;; WORK
     lwork               ;; LWORK
     rwork               ;; RWORK
     0 )                 ;; INFO
    (let* ((vl-mat2 (rearrange-eigenvector-matrix w vl-mat))
	   (vr-mat2 (rearrange-eigenvector-matrix w vr-mat)))
      (list w vl-mat2 vr-mat2))))