!
! 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 findroots(a,roots,poly_order)
! SUBROUTINE rootsort(ordert,roots,rroots,croots,nreal,ncomplex,maxordert)
!
!
! NAME
!    find_roots
!
! DESCRIPTION
!       find all the roots of a polynomial using an eigenvalue method
!
!
! SEE ALSO
!   
!
! HISTORY
!
!     16/11/2017 CJS Include network synthesis process to replace s-domain transfer functions
!
! COMMENTS
! 
! Altered to make arrays dynamic.
! Assume the array roots is already allocated
! 

SUBROUTINE findroots(a,roots,poly_order)

! Modules used
USE constants
USE eispack

IMPLICIT NONE

!Argument list variables

type(polynomial), intent(in) :: a
integer                      :: poly_order
complex(dp)                  :: roots(poly_order)

! Local variables

  integer :: n
  integer :: low
  integer :: igh
  real(dp)  :: h(poly_order,poly_order)
  real(dp)  :: wi(poly_order)
  real(dp)  :: wr(poly_order)
  integer :: ierr
  
  integer   :: i,loop
  real(dp)  :: eps,test1,test2,new_root
  type(polynomial) :: ap
  integer,parameter :: n_newton=5
  
! START


! CALL EISPACK subroutine to calculate the eigenvalues of an upper Hessenberg matrix

! Fill the uppper Hessenberg matrix
  h(1:poly_order,1:poly_order)=0d0
  
  do i=1,poly_order
    h(1,i)=-a%coeff(poly_order-i)/a%coeff(poly_order)
    if (i.ne.poly_order) then
      h(i+1,i)=1d0
    end if
  end do

  n=poly_order
  low=1
  igh=n
  
  CALL hqr ( n, low, igh, h, wr, wi, ierr )
  
  if (ierr.ne.0) then
    write(*,*)'Error in findroots'
    write(*,*)'Not all roots of the polynomial could be found correctly'
    STOP
  end if
    
  do i=1,poly_order
      
    roots(i)=cmplx( wr(i),wi(i) ,kind=dp )

  end do
   
RETURN
END


!
! ______________________________________________________________
!
!
! NAME
!    root_sort
!
! DESCRIPTION
!   
!    Sort real and complex roots
!
! SEE ALSO
!  
!
! HISTORY
!     Based on GGI_TLM software
!     16/11/2017 CJS Include network synthesis process to replace s-domain transfer functions
!
! COMMENTS
!     needs some tidying up...

SUBROUTINE rootsort(ordert,roots,rroots,croots,nreal,ncomplex,maxordert)

! Modules used

USE type_specifications
USE general_module
USE constants

IMPLICIT NONE

!Argument list variables

 integer maxordert
 complex(dp) roots(maxordert)
 complex(dp) rroots(maxordert),croots(maxordert)
 integer ordert,nreal,ncomplex
       
! Local variables

 complex(dp) swap,sum,dif
 real(dp) mag1,mag2,mag_dif
 integer i,roottype(maxordert),fpair
 integer k
 
! START

 do i=1,ordert
   if (abs(roots(i)).eq.0d0) then 
     roottype(i)=-1
   else if (abs(dble((0d0,-1d0)*roots(i))/abs(roots(i))).gt.1d-8) then
!  complex root found
     roottype(i)=1
   else
! real root found
     roottype(i)=-1
   end if  

 end do
!
!  reorder roots, real roots first then complex roots in pairs
!  
 nreal=0
 ncomplex=0
 do i=1,ordert
   if (roottype(i).eq.-1) then
! real root found
     nreal=nreal+1
     rroots(nreal)=roots(i)
     roottype(i)=0
   else if (roottype(i).eq.1) then
! complex root found
     ncomplex=ncomplex+1
     croots(ncomplex)=roots(i)
     roottype(i)=0
   end if
 end do 
!
 ncomplex=ncomplex/2

! pair up complex roots

 do i=1,ncomplex

! index of the pole we are trying to pair up
   fpair=(i-1)*2+1
   mag1=abs(croots(fpair))
   
! loop ove the other complex poles
   do k=fpair+1,ncomplex*2
   
! get the magnitude of the 
     mag2=abs(croots(k))

! get the difference between the initial root and the conjugate of the test root     
     dif=conjg(croots(fpair))-croots(k)
     
     mag_dif=abs(dif)

     if ( (mag_dif/(mag1+mag2)).lt.1D-6 ) then
! conjugate root found so shift next to first root of the pair
       swap=croots(fpair+1)
       croots(fpair+1)=croots(k)
       croots(k)=swap
     end if
   end do

 end do

 do i=1,nreal
   roots(i)=rroots(i)
 end do
 do i=1,ncomplex*2
   roots(nreal+i)=croots(i)
 end do

 RETURN

END
