!
! This file is part of SACAMOS, State of the Art CAble MOdels for Spice. 
! It was developed by the University of Nottingham and the Netherlands Aerospace 
! Centre (NLR) for ESA under contract number 4000112765/14/NL/HK.
! 
! Copyright (C) 2016-2018 University of Nottingham
! 
! SACAMOS is free software: you can redistribute it and/or modify it under the 
! terms of the GNU General Public License as published by the Free Software 
! Foundation, either version 3 of the License, or (at your option) any later 
! version.
! 
! SACAMOS is distributed in the hope that it will be useful, but 
! WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
! or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License 
! for more details.
! 
! A copy of the GNU General Public License version 3 can be found in the 
! file GNU_GPL_v3 in the root or at <http://www.gnu.org/licenses/>.
! 
! SACAMOS uses the EISPACK library (in /SRC/EISPACK). EISPACK is subject to 
! the GNU Lesser General Public License. A copy of the GNU Lesser General Public 
! License version can be found in the file GNU_LGPL in the root of EISPACK 
! (/SRC/EISPACK ) or at <http://www.gnu.org/licenses/>.
! 
! The University of Nottingham can be contacted at: ggiemr@nottingham.ac.uk
!
!
! File Contents:
! SUBROUTINE modal_decomposition_global
! SUBROUTINE modal_decomposition_global_ZY
! SUBROUTINE calc_eigenvectors
! SUBROUTINE test_decomposition
!
! NAME
!     modal_decomposition_global
!
! AUTHORS
!     Chris Smartt
!
! DESCRIPTION
!     This subroutine calculates the modal decomposition for the global YZ product
!     The theory is in the Theory Manual_Section 3.3 however there is more detailed discussion in 
!     C. R. Paul,'Analasys of Multiconductor transmission lines,' 
!     
! COMMENTS
!     In practice we operate on the domain based YZ product as it seems to be numerically
!     better in the case of degenerate eigenvalues, then we transform the eigenvector matrix
!     with the domain decomposition matrix MII to give TI. The eigenvaluse are unchanged. 
! i.e.(V_domain)=[MV] (V_global), (I_domain)=[MI] (I_global) 
!     (V_domain)=[Z_domain] (I_domain), (I_domain)=[Y_domain](V_domain)
!     (V_global)=[Z_global] (I_global), (I_global)=[Y_global](V_global)
!     if [TI] diagonalises [Y_global][Z_global] i.e. [Y_global][Z_global]=[TI][gammasqr][TI^-1]
!     and [MPI]diagonalises [Y_domain][Z_domain] i.e. [Y_domain][Z_domain] = [MPI][gammasqr][MPI^-1] then
!     [TI]=[MI^-1][MPI]
!
!     There were originally problems if the system was highly degenerate....
!     This problem has been fudged by applying a small perturbation to the diagonal elements of 
!     the YZ product. Maybe we need to assess the impact of this on mode identificaton for 
!     the propagation correction?
!
! HISTORY
!
!     started 7/12/2015 CJS: STAGE_1 developments
!     April 2016 CJS. Use impedance and admittance matrices for frequency dependent model
!     May 2016 CJS. Attempt to improve the reliability of the solution for degenerate modes (problem with spacewire model)
!     8/5/2017         CJS: Include references to Theory_Manual
!
SUBROUTINE modal_decomposition_global(dim,Z_domain,Y_domain,MV,MVI,MI,MII, &
                               Y,Z,TI,TII,TV,TVI,GAMMA_C,GAMMA_SQR,gamma_r,D,sqrtDI,ZC,YC,Zm,Ym,Zmd,Ymd)

USE type_specifications
USE general_module
USE constants
USE eispack
USE maths

IMPLICIT NONE

! variables passed to the subroutine

integer,intent(IN)         :: dim                ! dimension of matrix system

complex(dp),intent(IN)     :: Z_domain(dim,dim)  ! domain based impedance matrix
complex(dp),intent(IN)     :: Y_domain(dim,dim)  ! domain based admittance matrix

complex(dp),intent(IN)     :: MV(dim,dim)        ! domain voltage decomposition matrix
complex(dp),intent(IN)     :: MVI(dim,dim)       ! inverse domain voltage decomposition matrix
complex(dp),intent(IN)     :: MI(dim,dim)        ! domain current decomposition matrix
complex(dp),intent(IN)     :: MII(dim,dim)       ! inverse domain current decomposition matrix

complex(dp)     :: Z(dim,dim)     ! glabal based impedance matrix
complex(dp)     :: Y(dim,dim)     ! glabal based admittance matrix

complex(dp)     :: GAMMA_SQR(dim)  ! diagonal matrix elements in YZ/ ZY diagonalisation 

complex(dp)     :: TV(dim,dim)     ! modal decomposition matrix            [Z][Y]=[TV][GAMMA_SQR][TVI]
complex(dp)     :: TVI(dim,dim)    ! inverse modal decomposition matrix 

complex(dp)     :: TI(dim,dim)     ! modal decomposition matrix            [Y][Z]=[TI][GAMMA_SQR][TII]
complex(dp)     :: TII(dim,dim)    ! inverse modal decomposition matrix 

complex(dp)     :: Zm(dim,dim)     ! modal characteristic impedance matrix
complex(dp)     :: Ym(dim,dim)     ! modal characteristic admittance matrix
complex(dp)     :: Zmd(dim)        ! modal characteristic impedance list
complex(dp)     :: Ymd(dim)        ! modal characteristic impedance list

complex(dp)     :: GAMMA_C(dim)    ! complex square root of GAMMA_SQR
real(dp)        :: gamma_r(dim)    ! real part of the complex square root of GAMMA_SQR
complex(dp)     :: D(dim,dim)      ! full matrix with GAMMA_C elements on the diagonal
complex(dp)     :: sqrtDI(dim,dim) ! full matrix with 1/sqrt(GAMMA_C) elements on the diagonal

complex(dp)     :: ZC(dim,dim)     ! Characteristic impedance matrix
complex(dp)     :: YC(dim,dim)     ! Characteristic admittance matrix

! local variables

complex(dp)     :: YZ(dim,dim)   ! YZ matrix product to be diagonalised

! temposrary matrices used in the calculations
complex(dp)     :: T1(dim,dim)
complex(dp)     :: T2(dim,dim)

complex(dp)     :: YDZD(dim,dim)
complex(dp)     :: MPI(dim,dim)
complex(dp)     :: TM1(dim,dim)

real(dp)        :: condition_number   ! matrix condition number

! loop variables
integer    :: row,col

! error code for matrix inverse calculation
integer :: ierr

! START
 
! Calculate the product ofthe transmission line impedance (Z) and admittance (Y) matrices

  if(verbose) write(*,*)'CALLED modal_decomposition_global'

  YZ=matmul(Y,Z)
  YDZD=matmul(Y_domain,Z_domain)
  
! perform a modal decomposition on the YZ product Theory_Manual_Eqn 2.36
! using eispack routines to calculate the eigenvalues and eigenvectors of the YZ

! Perturb the diagonal elements very slightly to eliminate the 
! problem with finding all the eigenvectors corresponding to degenerate eigenvalues.
! The perturbation is very small compared with the approximations applied to obtain the 
! original L and C matrices... See Theory_Manual_Section 2.3.1

  do row=1,dim
    YDZD(row,row)=YDZD(row,row)*(1d0+1D-8*row)
  end do
 
! calculate the eigenvectors and eigenvalues of YDZD
  CALL calc_eigenvectors(YDZD,GAMMA_SQR,MPI,dim) 
  
! Calculate the modal decomposition matrix for the global based currents.
  TI=matmul(MII,MPI)
         
  GAMMA_C(:)=sqrt(GAMMA_SQR(:))

  if(verbose) write(*,*)'Invert TI'

  ierr=1   ! set ierr=1 on input to matrix inverse to cause the program to return here if we have a singular matrix
  CALL cinvert_Gauss_Jordan(TI,dim,TII,dim,ierr)
  
  if (ierr.NE.0) then
! we have a singular matrix here 
    run_status='ERROR in modal_decomposition_global. Singular matrix'
    CALL write_program_status()
    STOP 1
  end if
  
  if(verbose) write(*,*)'Done: Invert TI'
  
  GAMMA_SQR(:)=GAMMA_C(:)*GAMMA_C
  gamma_r(:)=sqrt(-dble(GAMMA_SQR(:)))    ! real part of gamma

! get the diagonal matrix with diagonal elements gamma, and its inverse

  D(:,:)=0d0
  sqrtDI(:,:)=0d0
  do row=1,dim
    D(row,row)=GAMMA_C(row)*GAMMA_C(row)
    sqrtDI(row,row)=1d0/GAMMA_C(row)
  end do

! Calculate the voltage transformation matrices based on Theory_Manual_Eqn 2.37
! noting that the transpose of the inverse of a matrix is equal to the inverse of the transpose of a matrix

  TV=transpose(TII)
  
  if(verbose) write(*,*)'Invert TV'
  
  ierr=1   ! set ierr=1 on input to matrix inverse to cause the program to return here if we have a singular matrix
  CALL cinvert_Gauss_Jordan(TV,dim,TVI,dim,ierr)
  
  if (ierr.NE.0) then
! we have a singular matrix here 
    run_status='ERROR in modal_decomposition_global. Singular matrix'
    CALL write_program_status()
    STOP 1
  end if
  if(verbose) write(*,*)'Done: Invert TV'

! Calculate the characteristic impedance matrix Theory_Manual_Eqn 2.41
  T1=matmul(sqrtDI,TII)
  T2=matmul(TI,T1)
  ZC=matmul(Z,T2)
  
  if(verbose) write(*,*)'Invert ZC'
  ierr=1   ! set ierr=1 on input to matrix inverse to cause the program to return here if we have a singular matrix
  
  CALL cinvert_Gauss_Jordan(ZC,dim,YC,dim,ierr)
  
  if (ierr.NE.0) then
! we have a singular matrix here 
    run_status='ERROR in modal_decomposition_global. Singular matrix'
    CALL write_program_status()
    STOP 1
  end if
  if(verbose) write(*,*)'Done: Invert ZC'
  
! Calculate the modal impedance matrix
  T1=matmul(Z,TI)
  Zm=matmul(TVI,T1)

! Calculate the modal admittance matrix
  T1=matmul(Y,TV)
  Ym=matmul(TII,T1)

! Calculate Modal impedance
  do row=1,dim
    Zmd(row)=Zm(row,row)
    Ymd(row)=Ym(row,row)
  end do
  
  if (verbose) then
  
    write(*,*)'Modal decomposition of Global matrix YZ:'
    CALL write_cmatrix_re(YZ,dim,0)
  
    do col=1,dim
    
      write(*,*)
      write(*,*)'Modal decomposition: Mode number',col
      write(*,*)
      write(*,*)'Eigenvalue',GAMMA_SQR(col)
      write(*,*)
      write(*,*)'Eigenvector'
  
      do row=1,dim
        write(*,*)row,TI(row,col)
      end do
  
    end do
    
    CALL c_condition_number(TI,dim,condition_number,dim) 

    write(*,*)'Condition number of TI matrix =',condition_number
    
    CALL test_decomposition(YZ,GAMMA_SQR,TI,dim)
      
  end if
  
  RETURN

END SUBROUTINE modal_decomposition_global
!
! NAME
!     modal_decomposition_global_ZY
!
! AUTHORS
!     Chris Smartt
!
! DESCRIPTION
!     This subroutine calculates the modal decomposition for the global ZY product
!     
! COMMENTS
!     Adapted from modal_decomposition_global
!
! HISTORY
!
!     started 7/12/2015 CJS: STAGE_1 developments
!     April 2016 CJS. Use impedance and admittance matrices for frequency dependent model
!     May 2016 CJS. Attempt to improve the reliability of the solution for degenerate modes (problem with spacewire model)
!     21/4/2017 CJS Adapted from modal_decomposition_global
!
SUBROUTINE modal_decomposition_global_ZY(dim,Z_domain,Y_domain,MV,MVI,MI,MII, &
                               Y,Z,TI,TII,TV,TVI,GAMMA_C,GAMMA_SQR,gamma_r,D,sqrtDI,ZC,YC,Zm,Ym,Zmd,Ymd)

USE type_specifications
USE general_module
USE constants
USE eispack
USE maths

IMPLICIT NONE

! variables passed to the subroutine

integer,intent(IN)         :: dim                ! dimension of matrix system

complex(dp),intent(IN)     :: Z_domain(dim,dim)  ! domain based impedance matrix
complex(dp),intent(IN)     :: Y_domain(dim,dim)  ! domain based admittance matrix

complex(dp),intent(IN)     :: MV(dim,dim)        ! domain voltage decomposition matrix
complex(dp),intent(IN)     :: MVI(dim,dim)       ! inverse domain voltage decomposition matrix
complex(dp),intent(IN)     :: MI(dim,dim)        ! domain current decomposition matrix
complex(dp),intent(IN)     :: MII(dim,dim)       ! inverse domain current decomposition matrix

complex(dp)     :: Z(dim,dim)     ! glabal based impedance matrix
complex(dp)     :: Y(dim,dim)     ! glabal based admittance matrix

complex(dp)     :: GAMMA_SQR(dim)  ! diagonal matrix elements in YZ/ ZY diagonalisation 

complex(dp)     :: TV(dim,dim)     ! modal decomposition matrix            [Z][Y]=[TV][GAMMA_SQR][TVI]
complex(dp)     :: TVI(dim,dim)    ! inverse modal decomposition matrix 

complex(dp)     :: TI(dim,dim)     ! modal decomposition matrix            [Y][Z]=[TI][GAMMA_SQR][TII]
complex(dp)     :: TII(dim,dim)    ! inverse modal decomposition matrix 

complex(dp)     :: Zm(dim,dim)     ! modal characteristic impedance matrix
complex(dp)     :: Ym(dim,dim)     ! modal characteristic admittance matrix
complex(dp)     :: Zmd(dim)        ! modal characteristic impedance list
complex(dp)     :: Ymd(dim)        ! modal characteristic impedance list

complex(dp)     :: GAMMA_C(dim)    ! complex square root of GAMMA_SQR
real(dp)        :: gamma_r(dim)    ! real part of the complex square root of GAMMA_SQR
complex(dp)     :: D(dim,dim)      ! full matrix with GAMMA_C elements on the diagonal
complex(dp)     :: sqrtDI(dim,dim) ! full matrix with 1/sqrt(GAMMA_C) elements on the diagonal

complex(dp)     :: ZC(dim,dim)     ! Characteristic impedance matrix
complex(dp)     :: YC(dim,dim)     ! Characteristic admittance matrix

! local variables

complex(dp)     :: ZY(dim,dim)   ! YZ matrix product to be diagonalised

! temposrary matrices used in the calculations
complex(dp)     :: T1(dim,dim)
complex(dp)     :: T2(dim,dim)

complex(dp)     :: ZDYD(dim,dim)
complex(dp)     :: MPI(dim,dim)
complex(dp)     :: TM1(dim,dim)

real(dp)        :: condition_number   ! matrix condition number

! loop variables
integer    :: row,col

! error code for matrix inverse calculation
integer :: ierr

! START
 
! Calculate the product ofthe transmission line admittance (Y) and impedance (Z) matrices

  if(verbose) write(*,*)'CALLED modal_decomposition_global_ZY'

  ZY=matmul(Z,Y)
  ZDYD=matmul(Z_domain,Y_domain)
  
! perform a modal decomposition on the ZY product (equation 2.8)
! using eispack routines to calculate the eigenvalues and eigenvectors of the ZY
! In practice we operate on the domain based ZY product as it seems to be numerically
! better in the case of degenerate eigenvalues, then we transform the eigenvector matrix
! with the domain decomposition matrix MVI to give TV The eigenvaluse are unchanged. 
! i.e.(V_domain)=[MV] (V_global), (I_domain)=[MI] (I_global) 
!     (V_domain)=[Z_domain] (I_domain), (I_domain)=[Y_domain](V_domain)
!     (V_global)=[Z_global] (I_global), (I_global)=[Y_global](V_global)
!     if [TV] diagonalises [Z_global][Y_global] i.e. [Z_global][Y_global]=[TV][gammasqr][TV^-1]
!     and [MPI]diagonalises [D_domain][Y_domain] i.e. [Z_domain][Y_domain] = [MPI][gammasqr][MPI^-1] then
!     [TV]=[MV^-1][MPI]

! Perturb the diagonal elements very slightly to eliminate the 
! problem with finding all the eigenvectors corresponding to degenerate eigenvalues.
! The perturbation is very small compared with the approximations applied to obtain the 
! original L and C matrices... See Theory_Manual_Section 2.3.1

  do row=1,dim
    ZDYD(row,row)=ZDYD(row,row)*(1d0+1D-8*row)
  end do
 
! calculate the eigenvectors and eigenvalues of YDZD
  CALL calc_eigenvectors(ZDYD,GAMMA_SQR,MPI,dim) 

  TV=matmul(MVI,MPI)
         
  GAMMA_C(:)=sqrt(GAMMA_SQR(:))

  if(verbose) write(*,*)'Invert TV'

  ierr=1   ! set ierr=1 on input to matrix inverse to cause the program to return here if we have a singular matrix
  CALL cinvert_Gauss_Jordan(TV,dim,TVI,dim,ierr)
  
  if (ierr.NE.0) then
! we have a singular matrix here 
    run_status='ERROR in modal_decomposition_global_ZY. Singular matrix'
    CALL write_program_status()
    STOP 1
  end if
  
  if(verbose) write(*,*)'Done: Invert TV'
  
  GAMMA_SQR(:)=GAMMA_C(:)*GAMMA_C
  gamma_r(:)=sqrt(-dble(GAMMA_SQR(:)))

! get the diagonal matrix 

  D(:,:)=0d0
  sqrtDI(:,:)=0d0
  do row=1,dim
    D(row,row)=GAMMA_C(row)*GAMMA_C(row)
    sqrtDI(row,row)=1d0/GAMMA_C(row)
  end do

! Calculate the voltage transformation matrices based on Theory_Manual_Eqn 2.37

  TII=transpose(TV)
  
  if(verbose) write(*,*)'Invert TII'
  
  ierr=1   ! set ierr=1 on input to matrix inverse to cause the program to return here if we have a singular matrix
  CALL cinvert_Gauss_Jordan(TII,dim,TI,dim,ierr)
  
  if (ierr.NE.0) then
! we have a singular matrix here 
    run_status='ERROR in modal_decomposition_global_ZY. Singular matrix'
    CALL write_program_status()
    STOP 1
  end if
  if(verbose) write(*,*)'Done: Invert TII'

! Calculate the characteristic impedance matrix Zc=Tv gamma^-1 TVI Z (derived in a mannaer similar to Theory_Manual_Eqn 2.41)
  T1=matmul(sqrtDI,TVI)
  T2=matmul(TV,T1)
  ZC=matmul(T2,Z)
  
  if(verbose) write(*,*)'Invert ZC'
  ierr=1   ! set ierr=1 on input to matrix inverse to cause the program to return here if we have a singular matrix
  CALL cinvert_Gauss_Jordan(ZC,dim,YC,dim,ierr)
  if (ierr.NE.0) then
! we have a singular matrix here 
    run_status='ERROR in modal_decomposition_global_ZY. Singular matrix'
    CALL write_program_status()
    STOP 1
  end if
  if(verbose) write(*,*)'Done: Invert ZC'
  
! Calculate the modal impedance matrix
  T1=matmul(Z,TI)
  Zm=matmul(TVI,T1)

! Calculate the modal admittance matrix
  T1=matmul(Y,TV)
  Ym=matmul(TII,T1)

! Calculate Modal impedance
  do row=1,dim
    Zmd(row)=Zm(row,row)
    Ymd(row)=Ym(row,row)
  end do
  
  if (verbose) then
  
    write(*,*)'Modal decomposition of Global matrix ZY:'
    CALL write_cmatrix_re(ZY,dim,0)
  
    do col=1,dim
    
      write(*,*)
      write(*,*)'Modal decomposition: Mode number',col
      write(*,*)
      write(*,*)'Eigenvalue',GAMMA_SQR(col)
      write(*,*)
      write(*,*)'Eigenvector'
  
      do row=1,dim
        write(*,*)row,TV(row,col)
      end do
  
    end do
    
    CALL c_condition_number(TV,dim,condition_number,dim) 

    write(*,*)'Condition number of TV matrix =',condition_number
    
    CALL test_decomposition(ZY,GAMMA_SQR,TV,dim)
      
  end if
  
  RETURN

END SUBROUTINE modal_decomposition_global_ZY
!
! NAME SUBROUTINE calc_eigenvectors
!
! wrapper for the eispack codes cg and rg
!
! DESCRIPTION
!     Calculate the eigenvalues and eigenvectors of a general complex matrix.
!     We automatically detect if the system is real and use SUBROUTINE rg from eispack
!     otherwise we use SUBROUTINE cg from eispack
!     A small perturbation is applied to the matrix to prevent a problem with degenerate
!     systems where the eispack subroutine would occasionally return the same eigenvector 
!     for more than one of the degenerate eigenvalues (i.e. the diagonalised system
!     would be singular which causes problems in the subsequent analysis.)
!     
! COMMENTS
!     Order the eigenvalues (and eigenvectors) with largest first. 
!
! HISTORY
!
!     17/06/2016 CJS. Use the eispack routine rg for real matrices to try and improve robustness
!
SUBROUTINE calc_eigenvectors(M,gamma,P,dim)

USE type_specifications
USE general_module
USE eispack
USE maths

IMPLICIT NONE

integer,intent(IN)         :: dim        ! matrix dimension
complex(dp),intent(IN)     :: M(dim,dim) ! input matrix 
complex(dp),intent(OUT)    :: gamma(dim) ! vector of eigenvalues to be returned
complex(dp),intent(OUT)    :: P(dim,dim) ! matrix of eigenvectors to be returned, eigenvectors in columns

! local variables

complex(dp)     :: lambda
complex(dp)    :: AML(dim,dim)

complex(dp) c1,c2
complex(dp) x1,x2

! EISPACK variables (see eispack routines rg,cg for their meanings)
integer(i4)  n

real(dp) :: ai(dim,dim)
real(dp) :: ar(dim,dim)
integer(i4) :: ierr
integer(i4)::  matz
real(dp) :: wi(dim)
real(dp) :: wr(dim)
real(dp) :: zi(dim,dim)
real(dp) :: zr(dim,dim)
  
! normalisation variables
real(dp) norm

real(dp) max_value
integer row,col
  
complex(dp) :: largest_element
real(dp)    :: mag_largest_element

! variables for testing whether we have a real or complex system
real(dp) :: max_re,max_im,re_test

! variables for sorting eigenvalues and eigenvectors

real(dp)    :: min_eigenvalue
integer     :: min_pos
complex(dp) :: swap
complex(dp) :: vswap(1:dim)

! condition number calculation
real(dp) :: condition_number

! eigenvalue loop variable
integer :: ie
  
! START

! copy variables into the format required for EISPACK subroutines
n=dim

ar(:,:)=dble(M(:,:))
ai(:,:)=aimag(M(:,:))

! test to see whether we have a real matrix

max_re=0d0
max_im=0d0

do row=1,dim
  do col=1,dim
    max_re=max(abs(ar(row,col)),max_re)
    max_im=max(abs(ai(row,col)),max_im)
  end do
end do

if (max_im.GT.0d0) then
  re_test=max_re/max_im
else
! maximum imaginary part is equal to zero so use the real eigen solver from eispack
  re_test=1d13
end if

matz=1 ! eigenvalues and eigenvectors required

! call the eispack routine

if (re_test.GT.1D12) then   ! This tolerance should be set in constants
! Assume that this is a real problem so call the EISPACK routine for real matrices

  CALL rg ( n, ar, wr, wi, matz, zr, ierr )

  if (ierr.NE.0) then
    run_status='ERROR in eispack routine rg called from calc_eigenvectors'
    CALL write_program_status()
    STOP 1
  end if
  
  gamma(:)=cmplx( wr(:),wi(:), dp )

! assemble the complex eigenvector matrix. See header for eispack subroutine cg for explanation  
  ie=1
  do while (ie.LE.dim)
    if (wi(ie).NE.0d0) then
! complex eigenvalue

      P(1:dim,ie)  =cmplx( zr(1:dim,ie), zr(1:dim,ie+1), dp )
      P(1:dim,ie+1)=cmplx( zr(1:dim,ie),-zr(1:dim,ie+1), dp )
      ie=ie+2

    else
! real eigenvalue    

      P(1:dim,ie)=cmplx( zr(1:dim,ie),0d0, dp )
      ie=ie+1

    end if
  
 end do ! next eigenvector
  
else
! Assume that we have a complex problem so call the EISPACK routine for the general complex form 

  CALL cg ( n, ar, ai, wr, wi, matz, zr, zi, ierr )

  if (ierr.NE.0) then
    run_status='ERROR in eispack routine cg called from calc_eigenvectors'
    CALL write_program_status()
    STOP 1
  end if
  
  gamma(:)=cmplx( wr(:),wi(:), dp )

  P(:,:)=cmplx( zr(:,:),zi(:,:), dp )

end if  ! real or complex problem

! multiply the eigenvectors such that the largest element is real and positive

do col=1,dim

  mag_largest_element=0d0
  
  do row=1,dim
    if (abs(P(row,col)).GT.mag_largest_element) then
      mag_largest_element=abs(P(row,col))
      largest_element=P(row,col)
    end if
  end do
  
  do row=1,dim
    P(row,col)=P(row,col)/largest_element
  end do
  
end do 

! normalise the eigenvectors to a length of 1
do col=1,dim

  norm=0d0
  
  do row=1,dim
    norm=norm+abs(P(row,col))**2
  end do
  
  norm=sqrt(norm)
  
  do row=1,dim
    P(row,col)=P(row,col)/norm
  end do
  
end do 

! At this stage we have all the eigenvalues and eigenvectors though the order is uncertain

! Put the eigenvalues and the corresponding eigenvectors into order of magnitude, smallest first
! This corresponds to the process for the lossless homogeneous case in SUBROUTINE modal_decomposition_LC

do ie=1,dim-1

  min_eigenvalue=abs(gamma(ie))
  min_pos=ie
  
  do row=ie+1,dim
  
    if (abs(gamma(row)).LT.min_eigenvalue) then
! this eigenvalue is the smallest so far
    
      min_eigenvalue=abs(gamma(row))
      min_pos=row
   
    end if
    
    if (min_pos.NE.ie) then
! swap the eigenvalue and eigenvector into the ie-th position
    
      swap=gamma(ie)
      gamma(ie)=gamma(min_pos)
      gamma(min_pos)=swap
      
      vswap(1:dim)=P(1:dim,ie)
      P(1:dim,ie)=P(1:dim,min_pos)
      P(1:dim,min_pos)=vswap(1:dim)
      
    end if ! we need to swap the eigenvalue and eigenvector position
    
  end do ! next eigenvalue to check

end do ! next eigenvalue to put in order
 
RETURN

END SUBROUTINE calc_eigenvectors
!
! NAME SUBROUTINE test_decomposition
!
! DESCRIPTION
!    check the results of a modal decomposition to see whether it is accurate...
! Checks are:
!     1. [Pinverse][M][P]=[gamma]
!     2. evaluate [M](x)-gamma (x) and calculate the maximum residual over all of the eigenvalues,(x)
!     3. evaluate [P][gamma][PI]-[M] and check the error
!
! COMMENTS
!     
!
! HISTORY
!
!     
!
SUBROUTINE test_decomposition(M,gamma,P,dim)

USE type_specifications
USE eispack
USE maths

IMPLICIT NONE

integer,intent(IN)        :: dim        ! matrix dimension
complex(dp),intent(IN)    :: M(dim,dim) ! input matrix 
complex(dp),intent(IN)    :: gamma(dim) ! vector of eigenvalues to be returned
complex(dp),intent(IN)    :: P(dim,dim) ! matrix of eigenvectors to be returned, eigenvectors in columns

! Local variables

complex(dp)    :: gamma2(dim,dim)
complex(dp)    :: PI(dim,dim)

complex(dp)    :: T1(dim,dim)
complex(dp)    :: PH(dim,dim)
complex(dp)    :: PgammaPI(dim,dim)

complex(dp)    :: x(dim),M_x(dim),gamma_x(dim)

real(dp)       :: norm
real(dp)       :: diag_diff
real(dp)       :: err
real(dp)       :: max_off_diag
real(dp)       :: max_residual
real(dp)       :: residual

complex(dp)    :: v1v2

integer        :: row,col,eigenvalue,e1,e2
integer        :: n_eigenvectors
integer        :: ierr
! START

  write(*,*)'Test modal decomposition'
  
  ierr=0
  CALL cinvert_Gauss_Jordan(P,dim,PI,dim,ierr)

  T1=matmul(PI,M)
  gamma2=matmul(t1,P)
  
! check the difference between gamma and gamma2

  norm=0d0
  diag_diff=0d0
  
  do row=1,dim
    norm=norm+abs(gamma(row))**2
    diag_diff=diag_diff+abs(gamma(row)-gamma2(row,row))**2
  end do
  
  norm=sqrt(norm)
  diag_diff=diag_diff/norm

  max_off_diag=0d0
  
  do row=1,dim
    do col=1,dim
    
      if (row.NE.col) then
        max_off_diag=max( max_off_diag,ABS(gamma2(row,col)) )
      end if
      
    end do
  end do
  
! evaluate [M](x)-gamma (x) and calculate the maximum residual over all of the eigenvalues

  max_residual=0d0
  
  do eigenvalue=1,dim
  
    x(1:dim)=P(1:dim,eigenvalue)
    
    M_x=matmul(M,x)
    gamma_x(:)=gamma(eigenvalue)*x(:)
      
    residual=0d0
    do row=1,dim
      residual=residual+abs(M_x(row)-gamma_x(row))**2
    end do
    residual=sqrt(residual)/norm
    max_residual=max(residual,max_residual)
    
  end do
  
! evaluate PgammaPI-M and check the error

  T1=matmul(P,gamma2)
  PgammaPI=matmul(T1,PI)
  
  err=0d0
  norm=0d0
  
  do row=1,dim
    do col=1,dim
    
      err=err+abs(M(row,col)-PgammaPI(row,col))
      norm=norm+abs(M(row,col))**2
      
    end do
  end do
  
  err=err/sqrt(norm)
  
  Write(*,8100)'Diagonalisation check: diag_diff=',diag_diff,' max_off_diag=',&
             max_off_diag,' max_residual=',max_residual,' max_error=',err
8100 format(A,ES12.4,A,ES12.4,A,ES12.4,A,ES12.4)
  
  
    write(*,*)'M'
    CALL write_cmatrix(M,dim,0)
  
    write(*,*)'P'
    CALL write_cmatrix(P,dim,0)
    
    write(*,*)'Gamma'
    do row=1,dim
      write(*,*)row,gamma(row),abs(gamma(row))
    end do
   
    write(*,*)'PI'
    CALL write_cmatrix(PI,dim,0)
    
    write(*,*)'PgammaPi'
    CALL write_cmatrix(PgammaPI,dim,0)
   
  RETURN
  
END SUBROUTINE test_decomposition