!
! 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_LC
! SUBROUTINE orthogonal_transformation
! SUBROUTINE normalise_matrix_columns
!
! NAME
!     modal_decomposition_LC
!
! AUTHORS
!     Chris Smartt
!
! DESCRIPTION
!     This subroutine calculates the modal decomposition give the inductance and capacitance matrices L and C
!     for an inhomogeneous, lossless configuration. 
!     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,' 
!
!     Inputs: 1. L, the inductance matrix
!             2. C, the capacitance matrix
! 
!     Outputs 1. TI, TII, TV, TVI, the modal decomposition matrices
!             2. The mode velocities
!             3. The mode impedances
!     
! COMMENTS
!     
!
! HISTORY
!
!     started 20/6/2015 CJS: Looking forward towards inhomogeneous transmission lines
!     8/5/2017          CJS: Include references to Theory_Manual
!
SUBROUTINE modal_decomposition_LC(dim,L,C,TI,TII,TV,TVI,mode_velocity,mode_impedance)

USE type_specifications
USE general_module
USE constants
USE eispack
USE maths

IMPLICIT NONE

! variables passed to the subroutine

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

real(dp),intent(IN)      :: L(dim,dim)  ! inductance matrix
real(dp),intent(IN)      :: C(dim,dim)  ! capacitance matrix

real(dp),intent(OUT)     :: TI(dim,dim)           ! modal decomposition matrix [TI]
real(dp),intent(OUT)     :: TII(dim,dim)          ! modal decomposition matrix inverse of [TI]
real(dp),intent(OUT)     :: TV(dim,dim)           ! modal decomposition matrix [TV]
real(dp),intent(OUT)     :: TVI(dim,dim)          ! modal decomposition matrix inverse of [TV]
real(dp),intent(OUT)     :: mode_velocity(dim)    ! mode velocity list
real(dp),intent(OUT)     :: mode_impedance(dim)   ! mode impedance list

! local variables

! orthonormal diagonalisation of C

real(dp) :: UT(dim,dim)
real(dp) :: thetasqr(dim,dim)
real(dp) :: U(dim,dim)
real(dp) :: theta(dim,dim)

! [M] and the orthonormal diagonalisation of [M]=[theta][UT][L][U][theta]
real(dp) :: M(dim,dim)
real(dp) :: ST(dim,dim)
real(dp) :: gammasqr(dim,dim)
real(dp) :: S(dim,dim)

! un-normalised mode transformation matrix, T
real(dp) :: T(dim,dim)

! normalised mode transformation matrix, Tnorm and the normalisation matrix, alpha
real(dp) :: Tnorm(dim,dim)
real(dp) :: alpha(dim,dim)

! mode inductance and capacitance
real(dp) ::lmode
real(dp) ::cmode

! other matrices
real(dp) :: TM1(dim,dim)
real(dp) :: TM2(dim,dim)

! other variables

integer :: row
integer :: ierr

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

  if(verbose) then
  
    write(*,*)'CALLED modal_decomposition_LC'
    
    write(*,*)'L'
    CALL dwrite_matrix(L,dim,dim,dim,0)
    
    write(*,*)'C'
    CALL dwrite_matrix(C,dim,dim,dim,0)
    
    TM1=matmul(L,C)
    write(*,*)'LC'
    CALL dwrite_matrix(TM1,dim,dim,dim,0)   
   
   end if 
  
! Stage 1: calculate the orthogonal transformation to diagonalise the symmetric real matrix C, [UT][C][U]=[thetasqr] 
! Theory_Manual_Equation 3.37

  CALL orthogonal_transformation(C,UT,thetasqr,U,dim)

! calculate the square root of the diagonal matrix thetasqr
  theta(:,:)=0d0
  do row=1,dim
    theta(row,row)=dsqrt(thetasqr(row,row))
  end do
  
! form the product [M]=[theta][UT][L][U][theta]
! Theory_Manual_Equation 3.39

  TM1=matmul(U,theta) 
  TM2=matmul(L,TM1) 
  TM1=matmul(UT,TM2) 
  M=matmul(theta,TM1) 

! STAGE 2: 
! Theory_Manual_Equation 3.40
  CALL orthogonal_transformation(M,ST,gammasqr,S,dim)

! form the matrix [T]=[U][theta][S]  then normalise the matrix columns
! Theory_Manual_Equation 3.42

  TM1=matmul(theta,S)
  T=matmul(U,TM1)
  
  CALL normalise_matrix_columns(T,Tnorm,alpha,dim)
  
! set the modal decomposition matrices TI,TV,TII,TVI
! Theory_Manual_Equation 3.43,3,44,3.45

  TI=Tnorm
  
  TM1=transpose(Tnorm)
  ierr=0   ! set ierr=0 on input to matrix inverse to cause the program to stop if we have a singular matrix
  CALL dinvert_Gauss_Jordan(TM1,dim,TV,dim,ierr)
  
  TVI=transpose(TI)
  
  TII=transpose(TV)

! calculate the mode inductance and capacitance and hence the mode velocities and impedances  
! Theory_Manual_Equation 3.46,3,47
  do row=1,dim
    lmode=(alpha(row,row)**2)*gammasqr(row,row)
    cmode=1d0/(alpha(row,row)**2)
    mode_velocity(row)=1d0/sqrt(lmode*cmode)
    mode_impedance(row)=sqrt(lmode/cmode)
  end do
  
  if (verbose) then
! do some checks

    write(*,*)'     mode           alpha                gammasqr'
    do row=1,dim
      write(*,*)row,alpha(row,row),gammasqr(row,row)
    end do
  
    write(*,*)'     mode           lmode               cmode             mode_velocity             mode_impedance'
    do row=1,dim
      write(*,*)row,(alpha(row,row)**2)*gammasqr(row,row),(alpha(row,row)**(-2)),mode_velocity(row),mode_impedance(row)
    end do
  
  end if
      
  RETURN

END SUBROUTINE modal_decomposition_LC
!
! __________________________________________________________
!
!
SUBROUTINE orthogonal_transformation(M_in,UT,thetasqr,U,dim)
!
! DESCRIPTION
! calculate an orthogonal transformation which diagonalises the symmetric matrix M_in
! [UT][M_in][U]=[thetasqr] where [thetasqr] is a diagonal matrix.
!
! The subroutine uses the eispack routine rg to calculate the eigenvalues and eigenvectors 
! of a real symmetric matrix.
!     
!     
! COMMENTS
!     The eigenvalues (and eigenvectors) are ordered in ascending order
!
! HISTORY
!
!     20/06/2016 CJS. 
!
USE type_specifications
USE general_module
USE eispack
USE maths

IMPLICIT NONE

! vasriables passed to subroutine
integer,intent(IN)   :: dim               ! matrix dimension
real(dp),intent(IN)  :: M_in(dim,dim)     ! input symmetric matrix 

! output matrices 
real(dp),intent(OUT) :: UT(dim,dim)        
real(dp),intent(OUT) :: thetasqr(dim,dim)
real(dp),intent(OUT) :: U(dim,dim)

! EISPACK variables

real(dp)   :: M(dim,dim)
real(dp)   :: w(dim)  ! vector of eigenvalues
integer(i4)::  matz
integer(i4)::  ierr
  
! local matrices for checks
real(dp) :: UTU(dim,dim)
real(dp) :: Uthetasqr(dim,dim)
real(dp) :: UthetasqrUT(dim,dim)
  
! loop variables
integer :: row,col
  
! START

! copy the M_in matrix 

M(:,:)=M_in(:,:)

! call the eispack routine rs

matz=1    ! calculate both eigenvalues and eigenvectors

CALL rs ( dim, M, w, matz, U, ierr )

if (ierr.NE.0) then
  run_status='ERROR in eispack routine rs called from orthogonal_transformation'
  CALL write_program_status()
  STOP 1
end if

thetasqr(:,:)=0d0
do row=1,dim
  thetasqr(row,row)=w(row)
end do

UT=TRANSPOSE(U)

! Some checks

if (verbose) then

  write(*,*)'Eigenvalues'
  do row=1,dim
    write(*,*)row,w(row)
  end do

  write(*,*)'Eigenvectors:'
  do col=1,dim
    write(*,*)'Eigenvector ',col
    do row=1,dim
      write(*,*)row,UT(row,col)
    end do
  end do

! check that the diagonalisation is orthogonal  
  UTU=matmul(UT,U)
  
  write(*,*)'Check [UT][U]=[I]'
  CALL dwrite_matrix(UTU,dim,dim,dim,0)
  
  write(*,*)'[M]'
  CALL dwrite_matrix(M_in,dim,dim,dim,0)
    
  write(*,*)'Check [U][thetasqr][UT]=[M]'
  
  Uthetasqr=matmul(U,thetasqr)
  UthetasqrUT=matmul(Uthetasqr,UT)
  CALL dwrite_matrix(UthetasqrUT,dim,dim,dim,0)
  
end if ! verbose

END SUBROUTINE orthogonal_transformation

!
! __________________________________________________________
!
!
SUBROUTINE normalise_matrix_columns(T,Tnorm,alpha,dim)
!
! DESCRIPTION
! normalise the columns of matrix T to have length 1. 
! Return the normalised matrix and the diagonal normalisation matrix, alpha
! such that [Tnorm]=[T][alpha]
!     
!     
! COMMENTS
!     
!
! HISTORY
!
!     20/06/2016 CJS. 
!
USE type_specifications
USE maths

IMPLICIT NONE

! vasriables passed to subroutine
integer,intent(IN)  :: dim          ! matrix dimension
real(dp),intent(IN) :: T(dim,dim)   ! input matrix

! output matrices such that [Tnorm]=[T][alpha]
real(dp),intent(OUT) :: Tnorm(dim,dim)
real(dp),intent(OUT) :: alpha(dim,dim)

  
! loop variables
integer :: row,col

real(dp) :: length

! START

alpha(:,:)=0d0

do col=1,dim

  length=0d0
  do row=1,dim
    length=length+T(row,col)*T(row,col)
  end do
  
  length=sqrt(length)
  alpha(col,col)=1d0/length
  
  do row=1,dim
    Tnorm(row,col)=T(row,col)/length
  end do
  
end do

END SUBROUTINE normalise_matrix_columns