! { dg-do run }

program test
  implicit none

  interface check
    procedure check_r4
    procedure check_r8
  end interface check

  real(kind=4) :: x4
  real(kind=8) :: x8

  x8 = 1.9_8 ; x4 = 1.9_4
  call check(bessel_j0 (x8), bessel_j0 (1.9_8))
  call check(bessel_j0 (x4), bessel_j0 (1.9_4))
  call check(bessel_j1 (x8), bessel_j1 (1.9_8))
  call check(bessel_j1 (x4), bessel_j1 (1.9_4))
  call check(bessel_jn (3,x8), bessel_jn (3,1.9_8))
  call check(bessel_jn (3,x4), bessel_jn (3,1.9_4))
  call check(bessel_y0 (x8), bessel_y0 (1.9_8))
  call check(bessel_y0 (x4), bessel_y0 (1.9_4))
  call check(bessel_y1 (x8), bessel_y1 (1.9_8))
  call check(bessel_y1 (x4), bessel_y1 (1.9_4))
  call check(bessel_yn (3,x8), bessel_yn (3,1.9_8))
  call check(bessel_yn (3,x4), bessel_yn (3,1.9_4))

contains
  subroutine check_r4 (a, b)
    real(kind=4), intent(in) :: a, b
    if (abs(a - b) > 1.e-5 * abs(b)) call abort
  end subroutine
  subroutine check_r8 (a, b)
    real(kind=8), intent(in) :: a, b
    if (abs(a - b) > 1.e-7 * abs(b)) call abort
  end subroutine
end program test


!!# FUNCTION MODULE: <STRTIME>
MODULE FUN_STRTIME

!!## PURPOSE
!! Returns a runtime in a human-readable, compact format.

!!## USAGE
!
! WRITE(*,*)STRTIME(dt)
!
!! Note: <STRTIME(dt)> is 7 characters---always.

!!## ACCESS AND IMPLICIT
IMPLICIT NONE
PRIVATE

!!## DETAILS
!! The passed time <dt> (or $\Delta t$) is handled
!! in the following way:
!!
!! @ if $\Delta t < t_{us} $ or $\Delta t >t_{max}$ then use
!!   exponential format with seconds, otherwise
!!
!! @ if $\Delta t >= t_{s}$, then use seconds, otherwise
!!
!! @ if $\Delta t >= t_{ms}$, then use milliseconds, otherwise
!!
!! @ if $\Delta t >= t_{us}$, then use microseconds.
!!

!!## LOCAL VARIABLES
REAL :: tmax=60.
REAL :: ts=1.0
REAL :: tms=1.e-3
REAL :: tus=1.e-6

!!## PUBLIC ACCESS LIST
PUBLIC :: TEST_STRTIME,STRTIME

!!## PROCEDURES
CONTAINS

!!### FUNCTION <STRTIME>
FUNCTION STRTIME(dt)

!!#### REQUIRED INPUT
REAL,INTENT(IN) :: dt

!!#### REQUIRED OUTPUT
CHARACTER(7) :: STRTIME

!!#### LOCAL VARIABLES
CHARACTER(10) :: s

!!--begin--

!use seconds in exponential format
IF( dt>tmax .OR. dt<tus )THEN

 WRITE(s,"(Es10.2)")ABS(dt)
 s = ADJUSTL(s)

 !need both exponent digits
 IF( ABS(dt)<1.e-9 .OR. ABS(dt)>1.e+9 )THEN
  STRTIME = s(1:2)//"E"//s(6:6)//s(7:8)//"s"
 !just need one
 ELSE
  STRTIME = s(1:3)//"E"//s(6:6)//s(8:8)//"s"
 END IF
!use seconds
ELSE IF( dt>=ts )THEN

 WRITE(s,"(f10.4)")ABS(dt)
 s = ADJUSTL(s)
 STRTIME = s(1:6)//"s"

!use millseconds
ELSE IF( dt>=tms )THEN

 WRITE(s,"(f9.3)")ABS(dt*1.E3)
 s = ADJUSTL(s)
 STRTIME = s(1:5)//"ms"

!use micrseconds
ELSE IF( dt>=tus )THEN

 WRITE(s,"(f9.3)")ABS(dt*1.E6)
 s = ADJUSTL(s)
 STRTIME = s(1:5)//"us"

END IF

!!--end--
END FUNCTION

SUBROUTINE TEST_STRTIME()
REAL :: r
INTEGER :: i,j

!!--begin--
WRITE(*,"(a12,2x,a12)")"r (seconds)","STRTIME(r)"
DO i=1,10
 CALL RANDOM_NUMBER(r)
 r=r/1.E7
 DO j=1,10
  r = r*10.
 WRITE(*,"(e12.5,2x,a)")r,STRTIME(r)
END DO

END DO

!!--end--
END SUBROUTINE

END MODULE


module rnd_lib
 implicit none
 interface random
  module procedure d1rnd,z1rnd,d2rnd,z2rnd,d3rnd,z3rnd
 end interface
contains

 subroutine arnd()
  implicit none
  integer s,clock,crate,cmax
  integer,allocatable :: seed(:)
  call random_seed(size=s)
  allocate(seed(s))
  call random_seed(get=seed)
  write(*,*) 'oldseed: ',seed
  call system_clock(clock,crate,cmax)
  !write(*,*)clock,crate,cmax
  seed(1)=clock
  call random_seed(put=seed)
  write(*,*) 'newseed: ',seed
 end subroutine

 double precision function drnd( )
  implicit none
  call random_number(drnd)
 return
 end function

 subroutine d1rnd(d)
  double precision,intent(out)  :: d(:)
  call random_number(d)
 end subroutine
 subroutine z1rnd(z)
  implicit none
  double complex,intent(inout)  :: z(:)
  character(len=*),parameter :: subnam='z1rnd'
  double precision,allocatable :: d(:)
  integer :: n,info
  n=size(z)
  allocate(d(2*n),stat=info)
  if(info.ne.0)then;write(*,*)subnam,': cannot allocate';stop;endif
  call random_number(d)
  call dcopy(2*n,d,1,z,1)
  deallocate(d)
 end subroutine

 subroutine d2rnd(d)
  double precision,intent(out)  :: d(:,:)
  call random_number(d)
 end subroutine
 subroutine z2rnd(z)
  implicit none
  double complex,intent(inout)  :: z(:,:)
  character(len=*),parameter :: subnam='z2rnd'
  double precision,allocatable :: d(:)
  integer :: n,info
  n=size(z)
  allocate(d(2*n),stat=info)
  if(info.ne.0)then;write(*,*)subnam,': cannot allocate';stop;endif
  call random_number(d)
  call dcopy(2*n,d,1,z,1)
  deallocate(d)
 end subroutine

 subroutine d3rnd(d)
  double precision,intent(out)  :: d(:,:,:)
  call random_number(d)
 end subroutine
 subroutine z3rnd(z)
  implicit none
  double complex,intent(inout)  :: z(:,:,:)
  character(len=*),parameter :: subnam='z3rnd'
  double precision,allocatable :: d(:)
  integer :: n,info
  n=size(z)
  allocate(d(2*n),stat=info)
  if(info.ne.0)then;write(*,*)subnam,': cannot allocate';stop;endif
  call random_number(d)
  call dcopy(2*n,d,1,z,1)
  deallocate(d)
 end subroutine

 integer function irnd( maxi )
  integer,intent(in) :: maxi
  double precision :: d
  call random_number(d)
  irnd=int(d*maxi)+1
 return
 end function
 subroutine irand(maxi,ix)
  integer,intent(in)  :: maxi
  integer,intent(out) :: ix(:)
  integer :: i,n
  double precision,allocatable :: d(:)
  n=size(ix)
  allocate(d(n))
  call random_number(d)
  ix=int(d*maxi)+1
  deallocate(d)
 return
 end subroutine

end module


Program testPRNGs

Use Kinds, Only: dp
Use Kinds, Only: id
Use Global, Only: default_seed
Use Global, Only: default_seed_x64
Use PRNGs, Only: MT19937_Type
Use PRNGs, Only: MT19937x64_Type
Use PRNGs, Only: ACORN_Type
Use PRNGs, Only: randos
Use FileIO_Utilities, Only: delta_Time
Use FileIO_Utilities, Only: creturn
Use FileIO_Utilities, Only: slash
Use FileIO_Utilities, Only: max_path_len

Implicit None

Integer, Parameter :: n = 2**20
Integer, Parameter :: m = 100
Integer, Parameter :: nt = 24
Type(MT19937_Type) :: RNG_19937
Type(MT19937x64_Type) :: RNG_19937x64
Type(ACORN_Type) :: RNG_ACORN
Type(ACORN_Type) :: RNG_pACORNs(1:nt)
Real(dp) :: r(1:n),x
Integer :: i,j,unit
Integer(id) :: c
Real(dp) :: t_19937,t_19937x64,t_acorn,t_intr
Integer :: s
Integer, Allocatable :: ss(:)

!seed the generators
Call RNG_19937%seed(default_seed)
Call RNG_19937x64%seed(default_seed_x64)
Call RNG_ACORN%seed(default_seed)
Do j = 1,nt
    Call RNG_pACORNs(j)%seed(default_seed+2*(j-1))
End Do
Call RANDOM_SEED(size = s)
Allocate(ss(1:s))
Call randos(default_seed,s,ss)
Call RANDOM_SEED(put=ss)

!generate a stream of n real values from each generator
Write(*,*)
Write(*,'(A)') 'Writing real samples to file:'
!Fortran intrinsic
Write(*,'(A,F7.2,A)',ADVANCE='NO') 'Intrinsic:  ',0._dp,'%'
Open(NEWUNIT=unit,FILE='RNGtests'//slash//'randomsIntrinsic.tst',ACTION='WRITE',STATUS='REPLACE')
Do i = 1,n
    If (Mod(i,1000) .EQ. 0) Write(*,'(2A,F7.2,A)',ADVANCE='NO') creturn,'Intrinsic:  ',100._dp*Real(i,dp)/Real(n,dp),'%'
    Call RANDOM_NUMBER(harvest = x)
    Write(unit,'(F0.16)') x
End Do
Close(unit)
Write(*,'(2A,F7.2,A)') creturn,'Intrinsic:  ',100._dp,'%'
!MT19937
Write(*,'(A,F7.2,A)',ADVANCE='NO') 'MT-19937:   ',0._dp,'%'
Open(NEWUNIT=unit,FILE='RNGtests'//slash//'randoms19937.tst',ACTION='WRITE',STATUS='REPLACE')
Do i = 1,n
    If (Mod(i,1000) .EQ. 0) Write(*,'(2A,F7.2,A)',ADVANCE='NO') creturn,'MT-19937:   ',100._dp*Real(i,dp)/Real(n,dp),'%'
    Write(unit,'(F0.16)') RNG_19937%r()
End Do
Close(unit)
Write(*,'(2A,F7.2,A)') creturn,'MT-19937:   ',100._dp,'%'
!MT19937x64
Write(*,'(A,F7.2,A)',ADVANCE='NO') 'MT-19937x64:',0._dp,'%'
Open(NEWUNIT=unit,FILE='RNGtests'//slash//'randoms19937x64.tst',ACTION='WRITE',STATUS='REPLACE')
Do i = 1,n
    If (Mod(i,1000) .EQ. 0) Write(*,'(2A,F7.2,A)',ADVANCE='NO') creturn,'MT-19937x64:',100._dp*Real(i,dp)/Real(n,dp),'%'
    Write(unit,'(F0.16)') RNG_19937x64%r()
End Do
Close(unit)
Write(*,'(2A,F7.2,A)') creturn,'MT-19937x64:',100._dp,'%'
!ACORN
Write(*,'(A,F7.2,A)',ADVANCE='NO') 'ACORN:      ',0._dp,'%'
Open(NEWUNIT=unit,FILE='RNGtests'//slash//'randomsACORN.tst',ACTION='WRITE',STATUS='REPLACE')
Do i = 1,n
    If (Mod(i,1000) .EQ. 0) Write(*,'(2A,F7.2,A)',ADVANCE='NO') creturn,'ACORN:      ',100._dp*Real(i,dp)/Real(n,dp),'%'
    Write(unit,'(F0.16)') RNG_ACORN%r()
End Do
Close(unit)
Write(*,'(2A,F7.2,A)') creturn,'ACORN:      ',100._dp,'%'
!Parallel ACORNs
Write(*,'(A,F7.2,A)',ADVANCE='NO') 'pACORNs:    ',0._dp,'%'
Open(NEWUNIT=unit,FILE='RNGtests'//slash//'randomsACORNs.tst',ACTION='WRITE',STATUS='REPLACE')
Do i = 1,n
    If (Mod(i,1000) .EQ. 0) Write(*,'(2A,F7.2,A)',ADVANCE='NO') creturn,'pACORNs:    ',100._dp*Real(i,dp)/Real(n,dp),'%'
    Do j = 1,nt
        Write(unit,'(F20.16)',ADVANCE='NO') RNG_pACORNs(j)%r()
    End Do
    Write(unit,*)
End Do
Close(unit)
Write(*,'(2A,F7.2,A)') creturn,'pACORNs:    ',100._dp,'%'

!collect timing data
Write(*,*)
Write(*,'(A)') 'Real generation rate:'
!Fortran Intrinsic
Write(*,'(A)',ADVANCE='NO') 'Intrinsic:  '
r = 0._dp
Call SYSTEM_CLOCK(c)
Do j = 1,m
Do i = 1,n
    Call RANDOM_NUMBER(harvest = r(i))
End Do
End Do
t_intr = delta_Time(clock_then=c)
Write(*,'(ES13.6E2,A)') 1._dp / (t_intr / Real(n,dp) / Real(m,dp)),' rand per sec'
!MT19937
Write(*,'(A)',ADVANCE='NO') 'MT-19937:   '
r = 0._dp
Call SYSTEM_CLOCK(c)
Do j = 1,m
Do i = 1,n
    r(i) = RNG_19937%r()
End Do
End Do
t_19937 = delta_Time(clock_then=c)
Write(*,'(ES13.6E2,A)') 1._dp / (t_19937 / Real(n,dp) / Real(m,dp)),' rand per sec'
!MT19937x64
Write(*,'(A)',ADVANCE='NO') 'MT-19937x64:'
r = 0._dp
Call SYSTEM_CLOCK(c)
Do j = 1,m
Do i = 1,n
    r(i) = RNG_19937x64%r()
End Do
End Do
t_19937x64 = delta_Time(clock_then=c)
Write(*,'(ES13.6E2,A)') 1._dp / (t_19937x64 / Real(n,dp) / Real(m,dp)),' rand per sec'
!ACORN
Write(*,'(A)',ADVANCE='NO') 'ACORN:      '
r = 0._dp
Call SYSTEM_CLOCK(c)
Do j = 1,m
Do i = 1,n
    r(i) = RNG_ACORN%r()
End Do
End Do
t_acorn = delta_Time(clock_then=c)
Write(*,'(ES13.6E2,A)') 1._dp / (t_acorn / Real(n,dp) / Real(m,dp)),' rand per sec'

!finish
Write(*,*)
Write(*,'(A)') 'Done.'
Write(*,*)

End Program testPRNGs


!
! This file is part of LIBPFASST.
!
!
!> Sweeper and RHS routines for 1-D advection/diffusion example.
!>     u_t + v*u_x = nu*u_xx
module my_sweeper
  use pf_mod_dtype
  use pf_mod_ndarray
  use pf_mod_imex_sweeper
  use pf_mod_fftpackage
  use pf_mod_solutions
  implicit none

  !>  extend the imex sweeper type with stuff we need to compute rhs
  type, extends(pf_imex_sweeper_t) :: my_sweeper_t
     integer ::     nx   !  Grid size

     !>  FFT and Spectral derivatives
     type(pf_fft_t), pointer :: fft_tool
     complex(pfdp), allocatable :: opE(:) ! Explicit spectral operator
     complex(pfdp), allocatable :: opI(:) ! Implicit spectral operator

   contains

     procedure :: f_eval    !  Computes the advection and diffusion terms
     procedure :: f_comp    !  Does implicit solves
     procedure :: initialize  !  Bypasses base sweeper initialize
     procedure :: destroy     !  Bypasses base sweeper destroy

  end type my_sweeper_t

contains

  !>  Helper function to return sweeper pointer
  function as_my_sweeper(sweeper) result(r)
    class(pf_sweeper_t), intent(inout), target :: sweeper
    class(my_sweeper_t), pointer :: r
    select type(sweeper)
    type is (my_sweeper_t)
       r => sweeper
    class default
       stop
    end select
  end function as_my_sweeper

  !>  Routine to initialize sweeper (bypasses imex sweeper initialize)
  subroutine initialize(this, pf,level_index)
    use probin, only:  imex_stat, v ,nu
    class(my_sweeper_t), intent(inout) :: this
    type(pf_pfasst_t),   intent(inout),target :: pf
    integer,             intent(in)    :: level_index

    complex(pfdp), allocatable :: lap(:) ! Lapclacian operator
    complex(pfdp), allocatable :: ddx(:) ! First derivative operator

    integer     :: nx

    !  Call the imex sweeper initialize
    call this%imex_initialize(pf,level_index)

    !>  Set variables for explicit and implicit parts
    if (imex_stat .eq. 0 ) then
       this%explicit=.TRUE.
       this%implicit=.FALSE.
    elseif (imex_stat .eq. 1 ) then
       this%implicit=.TRUE.
       this%explicit=.FALSE.
    else
       this%implicit=.TRUE.
       this%explicit=.TRUE.
    end if

    nx=pf%levels(level_index)%lev_shape(1)  !  local convenient grid size

    !>  Set up the FFT
    allocate(this%fft_tool)
    call this%fft_tool%fft_setup([nx],1)

    !>  Define spectral derivatitive operators
    allocate(lap(nx))
    allocate(ddx(nx))
    call this%fft_tool%make_lap(lap)
    call this%fft_tool%make_deriv(ddx)

    !> Allocate operators for implicit and explicit parts
    allocate(this%opE(nx))
    allocate(this%opI(nx))

    !>  Choose the explicit and implicit operators depending on imex_stat
    select case (imex_stat)
    case (0)  ! Fully Explicit
       this%opE = -v*ddx + nu*lap
       this%opI = 0.0_pfdp
    case (1)  ! Fully Implicit
       this%opE = 0.0_pfdp
       this%opI = -v*ddx + nu * lap
    case (2)  ! IMEX
       this%opE = -v*ddx
       this%opI =  nu*lap
    case DEFAULT
       print *,'Bad case for imex_stat in f_eval ', imex_stat
       call exit(0)
    end select

    !>  Clean up locals
    deallocate(lap)
    deallocate(ddx)

  end subroutine initialize

  !>  Destroy sweeper (bypasses base sweeper destroy)
  subroutine destroy(this,pf,level_index)
    class(my_sweeper_t), intent(inout) :: this
    type(pf_pfasst_t),  target, intent(inout) :: pf
    integer,              intent(in)    :: level_index

    !>  Call base sweeper destroy
    call this%imex_destroy(pf,level_index)

    !> Nuke the FFT operators
    deallocate(this%opE)
    deallocate(this%opI)

    !>  Free up FFT stuff
    call this%fft_tool%fft_destroy()
    deallocate(this%fft_tool)

  end subroutine destroy

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! These routines must be provided for the sweeper
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! Evaluate the explicit function at y, t.
  subroutine f_eval(this, y, t, level_index, f, piece)
    use probin, only:  imex_stat ,nu, v
    class(my_sweeper_t), intent(inout) :: this
    class(pf_encap_t),   intent(in   ) :: y
    class(pf_encap_t),   intent(inout) :: f
    real(pfdp),          intent(in   ) :: t
    integer,             intent(in   ) :: level_index
    integer,             intent(in   ) :: piece  !  Which piece to solve for

    real(pfdp),      pointer :: yvec(:), fvec(:)
    type(pf_fft_t),     pointer :: fft

    !  Grab the arrays from the encap
    yvec  => get_array1d(y)
    fvec => get_array1d(f)
    fft => this%fft_tool

    ! Apply spectral operators using the FFT convolution function
    select case (piece)
    case (1)  ! Explicit piece
       call fft%conv(yvec,this%opE,fvec)
    case (2)  ! Implicit piece
       call fft%conv(yvec,this%opI,fvec)
    case DEFAULT
       print *,'Bad case for piece in f_eval ', piece
       call exit(0)
    end select

  end subroutine f_eval

  ! Solve for y and return f2 also
  !   y-dtq*f(y,t) = rhs
  subroutine f_comp(this, y, t, dtq, rhs, level_index, f,piece)
    use probin, only:  imex_stat ,nu,v
    class(my_sweeper_t), intent(inout) :: this
    class(pf_encap_t),   intent(inout) :: y       !  The solution we seek
    real(pfdp),          intent(in   ) :: t       !  Equation time of implicit solve
    real(pfdp),          intent(in   ) :: dtq     !  The
    class(pf_encap_t),   intent(in   ) :: rhs     !  The right hand side of the solve
    integer,             intent(in   ) :: level_index !  Which level this is
    class(pf_encap_t),   intent(inout) :: f       !  The function value
    integer,             intent(in   ) :: piece   !  Designates which piece to solve for (here implicit)

    real(pfdp),      pointer :: yvec(:), rhsvec(:), fvec(:)
    type(pf_fft_t),     pointer :: fft

    !  Grab the arrays from the encaps
    yvec  => get_array1d(y)
    rhsvec => get_array1d(rhs)
    fvec => get_array1d(f)

    if (imex_stat .eq. 0)  then
       print *,'We should not be calling fcomp for fully explicit'
       yvec=rhsvec
       fvec=0.0_pfdp
       return
    endif

    ! Grab the fft workspace
    fft => this%fft_tool

    if (piece == 2) then
       ! Apply the inverse operator with the FFT convolution
       call fft%conv(rhsvec,1.0_pfdp/(1.0_pfdp - dtq*this%opI),yvec)

       !  The function is easy to derive
       fvec = (yvec - rhsvec) / dtq
    else
       print *,'Bad piece in f_comp ',piece
       call exit(0)
    end if
  end subroutine f_comp

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!>  Here are some extra routines which are problem dependent
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !> Routine to set initial condition.
  subroutine initial_sol(y_0)
    type(pf_ndarray_t), intent(inout) :: y_0
    call exact(0.0_pfdp, y_0%flatarray)
  end subroutine initial_sol

  !> Routine to return the exact solution
  subroutine exact(t, yex)
    use probin, only: nu, v,kfreq,Lx,ic_type
    real(pfdp), intent(in)  :: t
    real(pfdp), intent(out) :: yex(:)

    !  Call exact solution from Libpfasst for ad problem
    if (ic_type .eq. 1) then
       call exact_ad_cos(t,yex,nu,v,kfreq,Lx)  ! Cosine wave
    else
       call exact_ad_exp(t,yex,nu,v,Lx)   !  Exponential
    endif
  end subroutine exact

end module my_sweeper


subroutine one_element(p1,p2,s1,s2,cp1,cp2,cs1,cs2,np1,np2,ns1,ns2,val)
  USE variables
  implicit none

  integer i,j,k,l,m,n,o
  integer np1,np2,ns1,ns2
  real*8 cp1(np1),cp2(np2),cs1(ns1),cs2(ns2),val
  integer p1(np1,n_el),p2(np2,n_el),s1(ns1,n_el),s2(ns2,n_el)
  real*8 v,val1,val2,nm,norm_aa1,norm_aa2,norm_bb1,norm_bb2,over
!--------------------------------------------------------------------------------
! This subroutines compute
!  part <PHI(i)|H|PHI(j)>  where
! PHI(i)=phi_11(i)*alpha_1 + phi_12(i)*alpha_2
! PHI(i)=phi_11(j)*alpha_1 + phi_12(j)*alpha_2
! and <alpha_1|alpha_2> = 0 i.e orthogonal
!--------------------------------------------------------------------------------

!write(6,*)s1(ns1,:)
!write(6,*)np1,ns1,np2,ns2

! do i=1,np1
!  write(6,*)cp1(i),p1(i,:)
! end do

 norm_aa1=0.d0;
 norm_aa2=0.d0;

! Computing norm square of phi_11(i)

 do i=1,np1
  do k=1,np1
!  write(6,*)p1(i,:),p1(k,:)
!  write(6,*)cp1(i),cp1(k)
  call norm1(p1(i,:),p1(k,:),nm)
  norm_aa1=norm_aa1+cp1(i)*cp1(k)*nm
  end do
 end do

!write(6,*) norm_aa1

! Computing norm square of phi_11(j)

 do i=1,ns1
  do k=1,ns1
  call norm1(s1(i,:),s1(k,:),nm)
  norm_aa2=norm_aa2+cs1(i)*cs1(k)*nm
  end do
 end do

! do i=1,ns1
! write(6,*)cs1(i),s1(i,:)
! end do
! write(6,*) norm_aa2

!stop
! Computing the <phi_11(i)|H|phi_11(j)>

val1=0
 do i=1,np1
   do j=1,ns1
       call ham(p1(i,:),s1(j,:),v)
       val1=val1+cp1(i)*cs1(j)*v
   end do
 end do

! Computing norm square of phi_12(i)

 do i=1,np2
  do k=1,np2
   call norm1(p2(i,:),p2(k,:),nm)
   norm_bb1=norm_bb1+cp2(i)*cp2(k)*nm
  end do
 end do

! Computing norm square of phi_12(j)

 do i=1,ns2
  do k=1,ns2
  call norm1(s2(i,:),s2(k,:),nm)
  norm_bb2=norm_bb2+cs2(i)*cs2(k)*nm
  end do
 end do

! Computing <phi_12(i)|H|phi_12(j)>

val2=0;
  do i=1,np2
   do j=1,ns2
       call ham(p2(i,:),s2(j,:),v)
       val2=val2+cp2(i)*cs2(j)*v
   end do
 end do

 over=0.d0
 ! Computing overlap between psi_i and psi_j
    do i=1,np1
      do k=1,ns1
         call norm1(p1(i,:),s1(k,:),nm)
!         write(6,*) p1(i,:)
!         write(6,*) s1(k,:)
!         write(6,*) cp1(i)
!         write(6,*) cs1(k)
         over=over+cp1(i)*cs1(k)*nm
!         write(6,*) cp1(i)*cs1(k)*nm
      end do
    end do
!     write(6,*) over
!     over = 0.d0

    do i=1,np2
      do k=1,ns2
         call norm1(p2(i,:),s2(k,:),nm)
!         write(6,*) p2(i,:)
!         write(6,*) s2(k,:)
!         write(6,*) cp2(i)
!         write(6,*) cs2(k)
         over=over+cp2(i)*cs2(k)*nm
!        write(6,*) cp2(i)*cs2(k)*nm
      end do
    end do

!         write(6,*) over
! This is more crucial step
!
!1. This assuming the alpha1 and alpha2 are already orthonormal and normalizing it with norms
!   of individual part
!
   val= (val1+val2)/(sqrt((norm_aa1+norm_bb1))*sqrt((norm_aa2+norm_bb2)))
!   write (6,*) val,"val"
!2. This assuming the alpha1 and alpha2 are already orthonormal and using the direct expression
!   the book by Ruben Paunz
!
! val=(val1+val2)/(6.d0)

!3. This assuming the alpha1 and alpha2 are not normalized and get constructed from conjugate young
!   tabulae
!   alpha1= sqrt(3)[beta*alpha*alpha - alpha*alpha*beta]
!   alpha2= [2*alpha*alpha*beta - beta*lpha*alpha - alpha*beta*alpha ]
!   <alpha1|alpha1> = 6 ; <alpha2|alpha2>=6
!
!  val= (val1*6.d0+val2*3.d0)/(sqrt((norm_aa1*6.d0+norm_bb1*3.d0))*sqrt((norm_aa2*6.d0+norm_bb2*3.d0)))

end subroutine one_element


C++***************************************************************
C Program ARCSMOOTH
C Smoothing, gradient, laplacian along arcs within angular sectors
C
C JLP Version 30-08-93
C--***************************************************************
	PROGRAM ARCSMOOTH
	PARAMETER (IDIM=600,IDIM1=360,IDIM2=1000)
	REAL*4 IMAGE1(IDIM,IDIM),IMAGE2(IDIM,IDIM)
	REAL*4 PROF1(IDIM2,IDIM1),PROF2(IDIM2,IDIM1)
	REAL*4 PROF(IDIM2,IDIM1),LIMITS(IDIM1)
        REAL*4 PI
	CHARACTER IN_NAME*40,IN_COMMENTS*80
	CHARACTER OUT_NAME*40,OUT_COMMENTS*80
	COMMON /CONSTANT/PI
	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA
	PI=3.14159265358979323846

10	FORMAT(A)

	WRITE(6,867)
867	FORMAT(' PROGRAMME ARCSMOOTH : Version 30-08-93',/)

C**** Input of the file **********************
        CALL JLP_BEGIN
	CALL JLP_INQUIFMT
        WRITE(6,*) 'Input file: '
        READ(5,10) IN_NAME
	CALL JLP_READIMAG(IMAGE1,NPL,NL,IDIM,IN_NAME,IN_COMMENTS)

C Input of parameters:
        PRINT *,' COORDINATES OF THE CENTER OF THE GALAXY : X0,Y0 '
        READ(5,*) X0,Y0
        PRINT *,' Starting increment and ratio between two successive increments '
        PRINT *,' in radius for the profile (pixel units, real) '
        READ(5,*) XINC,XRATIO
        PRINT *,' Number of sectors (integer) <360'
        READ(5,*) NL2
        DTHETA=360./FLOAT(NL2)
        WRITE(6,34) X0,Y0,XINC,NL2,DTHETA
34      FORMAT('Profiles1/ Xcent =',F10.3,' Ycent =',F10.3,
     1         ' Delta R =',F10.3,/,' Number of sectors =',I4,' Delta Theta =',F10.4)

C Computing NPL2 with the four corners of the image
        X1=1.-X0
        Y1=1.-Y0
        RAD1=SQRT(X1*X1+Y1*Y1)

        X2=1.-X0
        Y2=FLOAT(NL)-Y0
        RAD2=SQRT(X2*X2+Y2*Y2)

        X3=FLOAT(NPL)-X0
        Y3=1.-Y0
        RAD3=SQRT(X3*X3+Y3*Y3)

        X4=FLOAT(NPL)-X0
        Y4=FLOAT(NL)-Y0
        RAD4=SQRT(X4*X4+Y4*Y4)

        RADMAX=AMAX1(RAD1,RAD2,RAD4)
        NPL2=1+INT(RADMAX/XINC)

C Computing LIMITS, i.e. the limits in radius for each annulus:
        CALL COMPUTE_IND(LIMITS)

C**** Generation of the profile ******************
	CALL PROFILE(PROF1,IMAGE1,LIMITS)
	PRINT *,'NPL2,NL2',NPL2,NL2

C**** "Smoothing" of the profiles separately
	CALL SMOOTHPRO1(PROF1,PROF2)

C**** "Smoothing" of the profiles along circular annuli
	CALL SMOOTHPRO2(PROF2,PROF)

800	PRINT 801
801	FORMAT(' MENU :',/,' 1: OUTPUT OF THE PROFILE',/,
     1	' 2: SMOOTHED IMAGE',/,
     1	' 3: GRADIENT IMAGE',/,
     1	' 4: LAPLACIAN IMAGE',/,
     1	' 10: EXIT',/,
     1	' Enter the option you want : ',$)
	READ(5,*) IOPT

C Output of all the profiles in an "image"
	IF(IOPT.EQ.1)THEN
C**** Output of the profiles ******************
	PRINT *,' OUTPUT PROFILE:'
        READ(5,10) OUT_NAME
        WRITE(OUT_COMMENTS,35) IN_NAME
35      FORMAT('Concentric profiles from',A)
	CALL JLP_WRITEIMAG(PROF,NPL2,NL2,IDIM2,OUT_NAME,OUT_COMMENTS)
	GO TO 800
	ENDIF

C Output of a smoothed image
	IF(IOPT.EQ.2)THEN

C**** Generation of a synthetic image **************
	CALL SYNTHESIS(PROF,IMAGE1,IMAGE2,LIMITS)

C**** Output of the file ******************
	PRINT *,' OUTPUT SMOOTHED IMAGE:'
        READ(5,10) OUT_NAME
        WRITE(OUT_COMMENTS,36) IN_NAME
36      FORMAT('Arcsmooth from',A)
	CALL JLP_WRITEIMAG(IMAGE2,NPL,NL,IDIM,OUT_NAME,OUT_COMMENTS)

	GO TO 800
	ENDIF

C Output of a gradient image
	IF(IOPT.EQ.3)THEN
C**** "Slope" of the profiles *****************
	CALL SLOPE(PROF,PROF1)

C**** Generation of a synthetic image **************
	CALL SYNTHESIS(PROF,IMAGE1,IMAGE2,LIMITS)

C**** Output of the file ******************
	PRINT *,' OUTPUT GRADIENT IMAGE:'
        READ(5,10) OUT_NAME
        WRITE(OUT_COMMENTS,37) IN_NAME
37      FORMAT('Radial gradient from',A)
	CALL JLP_WRITEIMAG(IMAGE2,NPL,NL,IDIM,OUT_NAME,OUT_COMMENTS)

	GO TO 800
	ENDIF

C Output of a laplacian image
	IF(IOPT.EQ.4)THEN
C**** "Laplacian" of the profiles *****************
	CALL SLOPE(PROF,PROF1)
	CALL SLOPE(PROF1,PROF2)

C**** Generation of a synthetic image **************
	CALL SYNTHESIS(PROF,IMAGE1,IMAGE2,LIMITS)

C**** Output of the file ******************
	PRINT *,' OUTPUT LAPLACIAN IMAGE:'
        READ(5,10) OUT_NAME
        WRITE(OUT_COMMENTS,38) IN_NAME
38      FORMAT('Radial laplacian from',A)
	CALL JLP_WRITEIMAG(IMAGE2,NPL,NL,IDIM,OUT_NAME,OUT_COMMENTS)

	GO TO 800
	ENDIF

        CALL JLP_END
	STOP
	END
C**************************************************************
C Subroutine PROFILE
C Generates an array PROF of the profile
C***************************************************************
	SUBROUTINE PROFILE(PROF,IMAGE1,LIMITS)
	PARAMETER (IDIM=600,IDIM1=360,IDIM2=1000)
	REAL*4 IMAGE1(IDIM,*),PROF(IDIM2,*),LIMITS(*)
	INTEGER NBER(IDIM2,IDIM1)
	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

C Main loop
	DO J=1,NL
	  Y1=FLOAT(J)-Y0
	    DO I=1,NPL
	      X1=FLOAT(I)-X0
	      CALL POLARD(X1,Y1,ANGL,RAD,IPRO,LIMITS)
	      JPRO=1+INT(ANGL/DTHETA)
	      PROF(IPRO,JPRO)=PROF(IPRO,JPRO)+IMAGE1(I,J)
	      NBER(IPRO,JPRO)=NBER(IPRO,JPRO)+1
	    END DO
	END DO

C Computing the mean
	DO IPRO=1,NPL2
	  DO JPRO=1,NL2
	    NBR=NBER(IPRO,JPRO)
	      IF(NBR.NE.0)THEN
	        PROF(IPRO,JPRO)=PROF(IPRO,JPRO)/NBR
	      ENDIF
	  END DO
	END DO

	RETURN
	END
C***********************************************************************
C Subroutine POLARD
C Input:
C  X, Y: coordinates relative to center
C
C Output:
C ANGL: polar angle in degrees between 0. and 360.)
C INDEX: index for profile (with increasing steps...)
C***********************************************************************
	SUBROUTINE POLARD(X,Y,ANGL,RADIUS,INDEX,LIMITS)
        REAL*4 LIMITS(*)
	REAL*4 X,Y,ANGL,RADIUS,COSINUS,PI
        INTEGER*4 INDEX
	COMMON /CONSTANT/PI
	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

	RADIUS=SQRT(X*X+Y*Y)
	IF(RADIUS.EQ.0.) RADIUS=0.00001
	    COSINUS=X/RADIUS
	    ANGL=ACOS(COSINUS)
	    ANGL=ANGL*180./PI
	IF(Y.LT.0.) ANGL=360.-ANGL

C Now index:
        DO I=1,NPL2
          IF(LIMITS(I).GT.RADIUS)THEN
           INDEX = I-1
           GOTO 99
          ENDIF
        END DO

99	RETURN
	END
C**************************************************************
C Subroutine SYNTHESIS to synthesize an image from PROF
C**************************************************************
	SUBROUTINE SYNTHESIS(PROF,IMAGE1,IMAGE2,LIMITS)
	PARAMETER (IDIM=600,IDIM1=360,IDIM2=1000)
	REAL*4 IMAGE1(IDIM,*),IMAGE2(IDIM,*),LIMITS(*),PROF(IDIM2,IDIM1)
        REAL*4 W1,W2,VAL1,VAL2
	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

C Main loop
	DO J=1,NL
	  Y1=FLOAT(J)-Y0
	    DO I=1,NPL
	      X1=FLOAT(I)-X0
	      CALL POLARD(X1,Y1,ANGL,RAD,IPRO,LIMITS)
C Leaving the central pixels unsmoothed:
              IF(RAD.LT.8.)THEN
	        IMAGE2(I,J)=IMAGE1(I,J)
              ELSE
	        JPRO=1+INT(ANGL/DTHETA)
C Interpolation between the two values in radius:
                W1 = (RAD-LIMITS(IPRO))/(LIMITS(IPRO+1)-LIMITS(IPRO))
	        VAL1= PROF(IPRO,JPRO)+W1*(PROF(IPRO+1,JPRO)-PROF(IPRO,JPRO))
C Same quantity for next sector:
                JPRO1=JPRO+1
                IF(JPRO.EQ.NL2)JPRO1=1
	        VAL2= PROF(IPRO,JPRO1)+W1*(PROF(IPRO+1,JPRO1)-PROF(IPRO,JPRO1))
C Now interpolation between the sectors:
                W2 = (ANGL-DTHETA*FLOAT(JPRO-1))/DTHETA
                IF(W2.LT.0)W2=-W2
	        IMAGE2(I,J)= VAL1+W2*(VAL2-VAL1)
              ENDIF

	    END DO
	END DO

	RETURN
	END
C--------------------------------------------------------------------------
C**** "Smoothing" of the profiles separately
C--------------------------------------------------------------------------
	SUBROUTINE SMOOTHPRO1(PROF1,PROF2)
	PARAMETER (IDIM1=360,IDIM2=1000)
	REAL*4 PROF1(IDIM2,IDIM1),PROF2(IDIM2,IDIM1)

	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

	DO J=1,NL2
	  DO I=2,NPL2-1
	    IF(PROF1(I-1,J).NE.0.)THEN
	      PROF2(I,J)=0.25*(PROF1(I-1,J)+PROF1(I+1,J))+
     1	0.5*PROF1(I+1,J)
	    ELSE
	      PROF2(I,J)=PROF1(I,J)
	    ENDIF
	  END DO
	END DO

C And then the edges :
	DO J=1,NL2
	  PROF2(1,J)=PROF2(2,J)
	  PROF2(NPL2,J)=PROF2(NPL2,J)
	END DO

	RETURN
	END
C-----------------------------------------------------------------------------
C**** "Smoothing" of the profiles along cicular annuli
	SUBROUTINE SMOOTHPRO2(PROF1,PROF2)
	PARAMETER (IDIM1=360,IDIM2=1000)
	REAL*4 PROF1(IDIM2,IDIM1),PROF2(IDIM2,IDIM1)

	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

	DO I=1,NPL2
	  DO J=2,NL2-1
	    IF(PROF1(I,J-1).NE.0.)THEN
	      PROF2(I,J)=0.25*(PROF1(I,J-1)+PROF1(I,J+1))+
     1	0.5*PROF1(I,J)
	    ELSE
	      PROF2(I,J)=PROF1(I,J)
	    ENDIF
	  END DO
	END DO

C And then the edges :
	DO I=1,NPL2
	  PROF2(I,1)=0.25*(PROF1(I,2)+PROF1(I,NL2))+
     1	0.5*PROF1(I,1)

	  PROF2(I,NL2)=0.25*(PROF1(I,NL2-1)+PROF1(I,1))+
     1	0.5*PROF1(I,NL2)
	END DO

	RETURN
	END
C-----------------------------------------------------------------------------
C**** Gradients of the profiles *****************
	SUBROUTINE SLOPE(PROF1,PROF2)
	PARAMETER (IDIM1=360,IDIM2=1000)
	REAL*4 PROF1(IDIM2,IDIM1),PROF2(IDIM2,IDIM1)

	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

	DO J=1,NL2
	  DO I=2,NPL2-1
	    IF(PROF1(I-1,J).NE.0.)THEN
	      PROF2(I,J)=PROF1(I-1,J)-PROF1(I+1,J)
	    ELSE
	      PROF2(I,J)=0.
	    ENDIF
	  END DO
	END DO

C And then the edges :
	DO J=1,NL2
	  PROF2(1,J)=PROF2(2,J)
	  PROF2(NPL2,J)=PROF2(NPL2,J)
	END DO

	RETURN
	END
C*****************************************************************
        SUBROUTINE COMPUTE_IND(LIMITS)
        REAL*4 LIMITS(*),XINC1
        INTEGER*4 I,NPL2
	COMMON /ACS_PARAM/NPL,NL,NPL2,NL2,X0,Y0,XINC,XRATIO,DTHETA

        XINC1 = XINC
        LIMITS(1)=0.
        LIMITS(2)=XINC1
        DO I=3,NPL2
          XINC1 = XINC1 * XRATIO
          LIMITS(I) = LIMITS(I-1) + XINC1
        END DO
        RETURN
        END


!============================================================================!
        subroutine filter_x( nx, ny, f )
!
!  Filter a 2-D field in x
!
!============================================================================!
        implicit none

        integer:: nx, ny
        real :: f(nx,ny), rhs(nx,ny)
        real, allocatable :: lhs(:,:,:)

        real, parameter :: alpha = 0.1
        real, parameter :: beta  = 0.0
        real, parameter :: d     = (6.0*beta-1.0)/16.0
        real, parameter :: a     = (2.0+3.0*alpha)/4.0
        real, parameter :: b     = (9.0 + 16.0*alpha + 10.0*beta)/16.0
        real, parameter :: c     = (alpha + 4.0*beta)/4.0

        integer :: iter
!============================================================================!

!.... setup RHS of filter in x

        rhs(1,:) = (15.0 * f(1,:) + 4.0 * f(2,:) - 6.0 * f(3,:) + &
                   4.0 * f(4,:) - f(5,:))/16.0
        rhs(2,:) = 3.0/4.0 * f(2,:) + (f(1,:) + 6.0 * f(3,:) - &
                   4.0 * f(4,:) + f(5,:))/16.0

        rhs(3,:) = 5.0/8.0 * f(3,:) + (-f(1,:) + 4.0 * f(2,:) + &
                   4.0 * f(4,:) - f(5,:))/16.0
        rhs(4:nx-3,:) = a * f(4:nx-3,:) + &
                        b/2.0 * ( f(5:nx-2,:) + f(3:nx-4,:) ) + &
                        c/2.0 * ( f(6:nx-1,:) + f(2:nx-5,:) ) + &
                        d/2.0 * ( f(7:nx,:)   + f(1:nx-6,:) )
        rhs(nx-2,:) = 5.0/8.0 * f(nx-2,:) + (-f(nx-4,:) + 4.0 * f(nx-3,:) + &
                      4.0 * f(nx-1,:) - f(nx,:))/16.0

        rhs(nx-1,:) = 3.0/4.0 * f(nx-1,:) + (f(nx,:) + 6.0 * f(nx-2,:) - &
                      4.0 * f(nx-3,:) + f(nx-4,:))/16.0
        rhs(nx,:) = (15.0 * f(nx,:) + 4.0 * f(nx-1,:) - 6.0 * f(nx-2,:) + &
                    4.0 * f(nx-3,:) - f(nx-4,:))/16.0

!.... setup LHS of filter in x

        if (beta.eq.0.0) then           !.... tridiagonal LHS

          allocate ( lhs(nx,ny,3) )

          lhs(:,:,1) = alpha
          lhs(:,:,2) = 1.0
          lhs(:,:,3) = alpha

!.... correct for the boundaries

          lhs(1,:,:) = 0.0
          lhs(1,:,2) = 1.0
          lhs(2,:,:) = 0.0
          lhs(2,:,2) = 1.0
          lhs(3,:,:) = 0.0
          lhs(3,:,2) = 1.0

          lhs(nx-2,:,:) = 0.0
          lhs(nx-2,:,2) = 1.0
          lhs(nx-1,:,:) = 0.0
          lhs(nx-1,:,2) = 1.0
          lhs(nx,:,:)   = 0.0
          lhs(nx,:,2)   = 1.0

          call solve2( nx, ny, 1, lhs, rhs )

          deallocate ( lhs )

        else
          write(*,*) 'Error:  only tridiagonal LHS is currently supported'
          call exit(1)
        end if

        f(:,:) = rhs(:,:)

        return
        end

!============================================================================!
        subroutine filter_y( nx, ny, f )
!
!  Filter a 2-D field in y
!
!============================================================================!
        implicit none

        integer:: nx, ny
        real :: f(nx,ny), rhs(nx,ny)
        real, allocatable :: lhs(:,:,:)

        real, parameter :: alpha = 0.1
        real, parameter :: beta  = 0.0
        real, parameter :: d     = (6.0*beta-1.0)/16.0
        real, parameter :: a     = (2.0+3.0*alpha)/4.0
        real, parameter :: b     = (9.0 + 16.0*alpha + 10.0*beta)/16.0
        real, parameter :: c     = (alpha + 4.0*beta)/4.0

        integer :: iter
!============================================================================!

!.... setup RHS of filter in y

        rhs(:,1) = (15.0 * f(:,1) + 4.0 * f(:,2) - 6.0 * f(:,3) + &
                   4.0 * f(:,4) - f(:,5))/16.0
        rhs(:,2) = 3.0/4.0 * f(:,2) + (f(:,1) + 6.0 * f(:,3) - &
                   4.0 * f(:,4) + f(:,5))/16.0
        rhs(:,3) = 5.0/8.0 * f(:,3) + (-f(:,1) + 4.0 * f(:,2) + &
                   4.0 * f(:,4) - f(:,5))/16.0
        rhs(:,4:ny-3) = a * f(:,4:ny-3) + &
                        b/2.0 * ( f(:,5:ny-2) + f(:,3:ny-4) ) + &
                        c/2.0 * ( f(:,6:ny-1) + f(:,2:ny-5) ) + &
                        d/2.0 * ( f(:,7:ny)   + f(:,1:ny-6) )
        rhs(:,ny-2) = 5.0/8.0 * f(:,ny-2) + (-f(:,ny-4) + 4.0 * f(:,ny-3) + &
                      4.0 * f(:,ny-1) - f(:,ny))/16.0
        rhs(:,ny-1) = 3.0/4.0 * f(:,ny-1) + (f(:,ny) + 6.0 * f(:,ny-2) - &
                      4.0 * f(:,ny-3) + f(:,ny-4))/16.0
        rhs(:,ny) = (15.0 * f(:,ny) + 4.0 * f(:,ny-1) - 6.0 * f(:,ny-2) + &
                    4.0 * f(:,ny-3) - f(:,ny-4))/16.0

!.... setup LHS of filter in y

        if (beta.eq.0.0) then           !.... tridiagonal LHS

          allocate ( lhs(nx,ny,3) )

          lhs(:,:,1) = alpha
          lhs(:,:,2) = 1.0
          lhs(:,:,3) = alpha

!.... correct for the boundaries

          lhs(:,1,:) = 0.0
          lhs(:,1,2) = 1.0
          lhs(:,2,:) = 0.0
          lhs(:,2,2) = 1.0
          lhs(:,3,:) = 0.0
          lhs(:,3,2) = 1.0

          lhs(:,ny-2,:) = 0.0
          lhs(:,ny-2,2) = 1.0
          lhs(:,ny-1,:) = 0.0
          lhs(:,ny-1,2) = 1.0
          lhs(:,ny,:)   = 0.0
          lhs(:,ny,2)   = 1.0

          call solve1( nx, ny, 1, lhs, rhs )

          deallocate ( lhs )

        else
          write(*,*) 'Error:  only tridiagonal LHS is currently supported'
          call exit(1)
        end if

        f(:,:) = rhs(:,:)

        return
        end

!=============================================================================!
        subroutine solve1( nsys, n, nblk, mat, rhs )
!=============================================================================!
!
!  Solves a block tridiagonal system of equations without pivoting.
!  Vectorized over the first index.
!
!  WARNING: Since pivoting is not done, this algorithm could fail
!
!  input:
!    nsys       : number of systems to vectorize over
!    n          : number of rows in the systems
!    nblk       : dimension of the blocks in each row
!
!  inout:
!    mat        : the block tridiagonal matrix to be solved
!    rhs        : the right hand side
!=============================================================================!
        implicit none

        integer, intent(in) :: n, nblk, nsys
        real, intent(inout) :: mat(nsys,n,nblk,nblk*3), rhs(nsys,n,nblk)

!.... useful constants

        real, parameter :: zero = 0.0, one = 1.0

!.... local variables

        integer :: nc, nr, i, j, l, m, p, q
        real    :: mult(nsys), fact(nsys)

!=============================================================================!
!                     F O R W A R D   E L I M I N A T I O N
!=============================================================================!
        do i = 1, n - 1

!.... do the first four columns

          do nc = 1, nblk - 1

            j = nblk + nc

            do m = nc, nblk - 1
              l = m + 1
              mult(:) = -mat(:,i,l,j) / mat(:,i,nc,j)
              do q = nc + nblk + 1, nblk * 3
                mat(:,i,l,q) = mat(:,i,l,q) + mult(:) * mat(:,i,nc,q)
              end do
              rhs(:,i,l) = rhs(:,i,l) + mult(:) * rhs(:,i,nc)
            end do

          end do        ! end of loop on nc

!.... for all columns

          do nc = 1, nblk

            p  = i + 1

            j = nc + nblk
            fact(:) = one / mat(:,i,nc,j)

            do m = 1, nblk
              mult(:) = -mat(:,p,m,nc) * fact(:)
              do q = nc + 1, nblk * 2
                l = q + nblk
                mat(:,p,m,q) = mat(:,p,m,q) + mult(:) * mat(:,i,nc,l)
              end do
              rhs(:,p,m) = rhs(:,p,m) + mult(:) * rhs(:,i,nc)
            end do

          end do        ! end of loop on nc

        end do          ! end of loop on i

!.... do the last row

        i = n

        do nc = 1, nblk - 1

          j = nblk + nc

          do m = nc, nblk - 1
            l = m + 1
            mult(:) = -mat(:,i,l,j) / mat(:,i,nc,j)
            do q = nc + nblk + 1, nblk * 2
              mat(:,i,l,q) = mat(:,i,l,q) + mult(:) * mat(:,i,nc,q)
            end do
            rhs(:,i,l) = rhs(:,i,l) + mult(:) * rhs(:,i,nc)
          end do

        end do  ! end of loop on nc

!=============================================================================!
!                   B A C K W A R D   S U B S T I T U T I O N
!=============================================================================!

!.... do the last row first

        i = n

        do m = nblk, 1, -1
          j = m + nblk
          do q = m+1, nblk
            l = q + nblk
            rhs(:,i,m) = rhs(:,i,m) - rhs(:,i,q) * mat(:,i,m,l)
          end do
          rhs(:,i,m) = rhs(:,i,m) / mat(:,i,m,j)
        end do

!.... now do the rest of the rows in reverse order

        do i = n - 1, 1, -1

          p = i + 1

          do m = nblk, 1, -1
            j = m + nblk
            do q = m+1, nblk
              l = q + nblk
              rhs(:,i,m) = rhs(:,i,m) - rhs(:,i,q) * mat(:,i,m,l)
            end do
            do q = 1, nblk
              l = q + nblk * 2
              rhs(:,i,m) = rhs(:,i,m) - rhs(:,p,q) * mat(:,i,m,l)
            end do
            rhs(:,i,m) = rhs(:,i,m) / mat(:,i,m,j)
          end do

        end do
!=============================================================================!
        return
        end

!=============================================================================!
        subroutine solve2( n, nsys, nblk, mat, rhs )
!=============================================================================!
!
!  Solves a block tridiagonal system of equations without pivoting.
!  Vectorized over the second index.
!
!  WARNING: Since pivoting is not done, this algorithm could fail
!
!  input:
!    nsys       : number of systems to vectorize over
!    n          : number of rows in the systems
!    nblk       : dimension of the blocks in each row
!
!  inout:
!    mat        : the block tridiagonal matrix to be solved
!    rhs        : the right hand side
!=============================================================================!
        implicit none

        integer, intent(in) :: n, nblk, nsys
        real, intent(inout) :: mat(n,nsys,nblk,nblk*3), rhs(n,nsys,nblk)

!.... useful constants

        real, parameter :: zero = 0.0, one = 1.0

!.... local variables

        integer :: nc, nr, i, j, l, m, p, q
        real    :: mult(nsys), fact(nsys)

!=============================================================================!
!                     F O R W A R D   E L I M I N A T I O N
!=============================================================================!
        do i = 1, n - 1

!.... do the first four columns

          do nc = 1, nblk - 1

            j = nblk + nc

            do m = nc, nblk - 1
              l = m + 1
              mult(:) = -mat(i,:,l,j) / mat(i,:,nc,j)
              do q = nc + nblk + 1, nblk * 3
                mat(i,:,l,q) = mat(i,:,l,q) + mult(:) * mat(i,:,nc,q)
              end do
              rhs(i,:,l) = rhs(i,:,l) + mult(:) * rhs(i,:,nc)
            end do

          end do        ! end of loop on nc

!.... for all columns

          do nc = 1, nblk

            p  = i + 1

            j = nc + nblk
            fact(:) = one / mat(i,:,nc,j)

            do m = 1, nblk
              mult(:) = -mat(p,:,m,nc) * fact(:)
              do q = nc + 1, nblk * 2
                l = q + nblk
                mat(p,:,m,q) = mat(p,:,m,q) + mult(:) * mat(i,:,nc,l)
              end do
              rhs(p,:,m) = rhs(p,:,m) + mult(:) * rhs(i,:,nc)
            end do

          end do        ! end of loop on nc

        end do          ! end of loop on i

!.... do the last row

        i = n

        do nc = 1, nblk - 1

          j = nblk + nc

          do m = nc, nblk - 1
            l = m + 1
            mult(:) = -mat(i,:,l,j) / mat(i,:,nc,j)
            do q = nc + nblk + 1, nblk * 2
              mat(i,:,l,q) = mat(i,:,l,q) + mult(:) * mat(i,:,nc,q)
            end do
            rhs(i,:,l) = rhs(i,:,l) + mult(:) * rhs(i,:,nc)
          end do

        end do  ! end of loop on nc

!=============================================================================!
!                   B A C K W A R D   S U B S T I T U T I O N
!=============================================================================!

!.... do the last row first

        i = n

        do m = nblk, 1, -1
          j = m + nblk
          do q = m+1, nblk
            l = q + nblk
            rhs(i,:,m) = rhs(i,:,m) - rhs(i,:,q) * mat(i,:,m,l)
          end do
          rhs(i,:,m) = rhs(i,:,m) / mat(i,:,m,j)
        end do

!.... now do the rest of the rows in reverse order

        do i = n - 1, 1, -1

          p = i + 1

          do m = nblk, 1, -1
            j = m + nblk
            do q = m+1, nblk
              l = q + nblk
              rhs(i,:,m) = rhs(i,:,m) - rhs(i,:,q) * mat(i,:,m,l)
            end do
            do q = 1, nblk
              l = q + nblk * 2
              rhs(i,:,m) = rhs(i,:,m) - rhs(p,:,q) * mat(i,:,m,l)
            end do
            rhs(i,:,m) = rhs(i,:,m) / mat(i,:,m,j)
          end do

        end do
!=============================================================================!
        return
        end


      SUBROUTINE ALMNAC
     &          (year,month,day,UT,colat,xlon)

c***********************************************************************
c                   subroutine almnac
c***********************************************************************

c  Program Source:  Naval Ocean Systems Center - Code 542

c  Date:
c     Oct 27, 1989

c  Function:
c     Returns the sub-solar point (SSP) for a specific date and UT

c  Parameters passed:
c     year          [i] year
c     month         [i] month number, Jan is 1
c     day           [i] day of the month
c     UT            [i] Universal time; hours

c  Parameters returned:
c     colat         [r] co-latitude    of the SSP; radians
c     xlon          [r] West longitude of the SSP; radians

c  Common blocks referenced:

c  Functions and subroutines referenced:
c     cos
c     mod
c     sin

c  References:

c  Change History:

c*******************!***************************************************

      implicit real*8 (a-h,o-z)

      real     *  8 mdays(12)
      real     *  4 UT,colat,xlon
      integer       year,month,day
      dimension     a(6),b(5),c(5),d(5)
      data          mdays/  0.d0, 31.d0, 59.d0, 90.d0,120.d0,151.d0,
     &                    181.d0,212.d0,243.d0,273.d0,304.d0,334.d0/,
     &              a0/3.798d-1/,
     &              a/-2.30009d1,-3.802d-1,-1.55d-1,-7.6d-3,-2.5d-3,
     &                -4.d-4/,
     &              b/3.5354d0,3.02d-2,7.28d-2,3.2d-3,2.0d-3/,
     &              c/5.965d-1,-2.9502d0,-6.53d-2,-1.248d-1,-1.03d-2/,
     &              d/-7.3435d0,-9.4847d0,-3.083d-1,-1.747d-1,-1.59d-2/

      kyr=MOD(year,1900)
      kk=MOD(kyr,4)
      days=365*kk+.0078d0*(kyr-68)
      if (kk .ne. 0) days=days+1.d0
      date=days+mdays(month)+day+UT/24.d0
c     x=twopi*date/365.25
      x=date*.017202424d0
      cx=COS(x)
      sx=SIN(x)
      twocx=2.d0*cx
      c2x=twocx*cx-1.d0
      s2x=twocx*sx
      c3x=twocx*c2x-cx
      s3x=twocx*s2x-sx
      c4x=twocx*c3x-c2x
      s4x=twocx*s3x-s2x
      c5x=twocx*c4x-c3x
      s5x=twocx*s4x-s3x
      c6x=twocx*c5x-c4x
      dec =a0
     &    +a(1)*cx+a(2)*c2x+a(3)*c3x+a(4)*c4x+a(5)*c5x+a(6)*c6x
     &    +b(1)*sx+b(2)*s2x+b(3)*s3x+b(4)*s4x+b(5)*s5x
      eqnt=c(1)*cx+c(2)*c2x+c(3)*c3x+c(4)*c4x+c(5)*c5x
     &    +d(1)*sx+d(2)*s2x+d(3)*s3x+d(4)*s4x+d(5)*s5x

c     Convert to colat and longitude
      colat=(90.-dec)*1.745329252519949d-2
      xlon=((UT+eqnt/60.d0)*15.d0-180.d0)*1.745329252519949d-2

      RETURN
      END      ! ALMNAC


      subroutine impeo(elevm,eo)
c
c     + + + PURPOSE + + +
c
c     SR IMPEO calculates the evaporation from bare soil in
c     an impoundment.  The amount of potential evaporation is
c     computed as if all the soil were bare (EO). Calculations
c     are performed using Ritchie's ET model.
c
c     This routine is based upon SR EVAP, simplified for
c     impoundments, and now in SI (meters).
c
c     Called from: SR WSHIMP
c     Author(s): Baffaut
c     Reference in User Guide:
c
c     Chapter 7.   See also EPIC model documentation (Sept. 1990)
c     and "Microclimate, the Biological Environment" by Rosenberg
c     et al., 1983.
c
c     + + + KEYWORDS + + +
c
c     + + + PARAMETERS + + +
c
      include 'pmxpln.inc'
      include 'pmxnsl.inc'
      include 'pmxhil.inc'
      include 'pmxtls.inc'
      include 'pmxelm.inc'
c
c     + + + ARGUMENT DECLARATIONS + + +
c
      real elevm,eo
c
c     + + + ARGUMENT DEFINITIONS + + +
c
c     elevm - elevation of climate station in meters
c     eo - calculated potential evaporation
c
c     + + + COMMON BLOCKS + + +
c
      include 'cangie.inc'
c     read: radpot
c
      include 'cclim.inc'
c     read: tave, radly, tdpt, iwind
c     modify: vwind
c
      include 'pmxtil.inc'
c
c     + + + LOCAL VARIABLES + + +
c
      real tk, delta, gma, alb, xx, dlt, ed, ee, fwv, pb, ra,
     1    ralb1, rbo, rhd, rn, rso, xl
      integer ievap
c
c     + + + LOCAL DEFINITIONS + + +
c
c     tk     - daily average air temperature (degrees Kelvin)
c     delta  - slope of the saturated vapor pressure
c     gma    - the second part of Priestly Taylor equation
c     alb    - soil albedo (fraction)
c     xx     - soil evaporation (potential of stage 1 and 2)
c     dlt    - slope of the saturation vapor pressure curve
c     ed     - vapor pressure
c     ee     - saturation vapor pressure
c     fwv    - mean daily wind speed
c     pb     - baromatic pressure
c     ra     - radiation
c     ralb1  - radiation
c     rbo    - net outgoing longwave radiation
c     rhd    - relative humidity
c     rn     - net radiation
c     rso    -
c     xl     - latent heat of vaporization
c     ievap  - defined below
c
c     + + + DATA INITIALIZATIONS + + +
c
c     + + + END SPECIFICATIONS + + +
c
c     convert average daily temp to degrees Kelvin
c
      tk = tave + 273.0
c
c     compute vapor pressure slope (svp) (WEPP Equation 7.2.4
c     or EPIC Equation 2.53)
c
      delta = exp(21.255-5304.0/tk) * 5304.0 / tk ** 2
      gma = delta / (delta+0.68)
c
c     compute corrected soil albedo (alb) assuming lai = 0
c
      alb = 0.25
c
c     compute potential evaporation from bare soil (eo)
c
c     ievap - flag that determines which equation is used for
c     PET calculations
c
c     ievap = 2 for PRIESTLEY-TAYLOR  (wind data not available)
c     ievap = 3 for PENMAN (wind data available)
c
c     if wind data not available use the PRIESTLEY-TAYLOR equation
c
      if (iwind.eq.1) ievap = 2
c
c     if wind data available use the PENMAN equation
c
      if (iwind.eq.0) ievap = 3
c
c     priestley-taylor equation
c
      if (ievap.eq.2) then
c
        eo = 0.00128 * (radly*(1.0-alb)/58.3) * gma
c
      else
c
c       penman equation
c
        ra = radly / 23.9
c
c       estimate latent heat of vaporization (xl)
c       (EPIC Equation 2.38)
c
        xl = 2.501 - 0.0022 * tave
c
c       estimate saturation vapor pressure (ee) using the average
c       temperature in degrees Kelvin (tk) (EPIC Equation 2.39)
c
        ee = 0.1 * exp(54.879-5.029*alog(tk)-6790.5/tk)
c
c       calculate relative humidity (rhd) fraction from dewpoint
c       temperature (tdpt) and average temperature (tave)
c       (Rosenberg et al. (1983), Equations 5.17 & 5.18)
c
        rhd = exp((17.27*tdpt)/(237.3+tdpt)) /
     1      exp((17.27*tave)/(237.3+tave))
c
        if (rhd.gt.1.0) rhd = 1.0
c
c       calculate the vapor pressure (ed) from the saturation vapor
c       pressure (ee) and the relative humidity (rhd)
c       (EPIC Equation 2.40)
c
        ed = ee * rhd
        ralb1 = ra * (1-alb)
c
c       estimate the slope of the saturation vapor pressure
c       curve (dlt)  (EPIC Equation 2.41)
c
        dlt = ee * (6790.5/tk-5.029) / tk
c
c       compute barometric pressure in kpa (pb)
c       (Approximation of EPIC Equation 2.43)
c
        pb = 101.3 - (0.01055*elevm)
c
c       compute psychrometric constant kpa/c (gma)
c       (EPIC Equation 2.42)
c
        gma = 0.00066 * pb
        xx = dlt + gma
        rso = radpot / 23.9
c
c       calculate mean daily wind speed (vwind)
c       (Modified EPIC Equation 2.50)
c
c       the value 1.63 is used below because the wind generated
c       by CLIGEN is for a 10 meter height (not for a 2 meter
c       height as in EPIC equation 2.50)
c
        fwv = 2.7 + 1.63 * vwind
c
c       estimate the net outgoing longwave radiation (rbo)
c       (EPIC Equation 2.46)
c
        rbo = (0.34-0.14*sqrt(ed)) * 4.9e-9 * tk ** 4
c
c       calculate net radiation (rn)
c       (EPIC Equation 2.45)
c
        rn = ralb1 - rbo * (0.9*ra/rso+0.1)
        if (rn.le.0.0) rn = 0.0
c
c       compute potential evaporation (eo)
c
c       (approximation of EPIC Equation 2.37, assuming the
c       soil flux is close to zero, and thus negligible)
c
        eo = ((dlt*rn)/xl+gma*fwv*(ee-ed)) * 0.001 / xx
c
      end if
c
      return
      end


      SUBROUTINE MFFTIM(C,IMSX,IVSX,IESX,NMX,NVX,NEX,TABLES,IERR)

*   PURPOSE:
*       THIS SUBROUTINE PERFORMS AN INVERSE FOURIER TRANSFORM ALONG
*       THE SECOND DIMENSION OF A 3-DIMENSIONAL MATRIX, USING THE
*       COOLEY-TUKEY ALGORITHM.
*       THE INPUT SEQUENCE IS ASSUMED TO HAVE BEEN SUBJECTED TO A
*       "BIT REVERSAL" PERMUTATION, THROUGH A CALL TO MFFTOM, OR
*       BECAUSE IT IS THE OUTPUT OF MFFTDM.
*       THE SEQUENCE TO BE TRANSFORMED IS C[IMSX,NMX], WHOSE COMPONENTS
*       ARE THE 2-VECTORS C(M)[IVSX,NVX [IESX,NEX]].
*       SEE REF.[1] FOR NOTATIONS.
*  EXAMPLE:
*       LET C BE A 3-D MATRIX C(N1,N2,N3), DECLARED VIA
*              DIMENSION C(NN1,N2,N3)
*       WITH NN1.GE.N1
*       THEN ITS IDFT ALONG THE SECOND DIMENSION IS OBTAINED BY
*              CALL MFFTIM(C,NN1,NN1*N2,1,N2,N3,N1,TABLES,IERR)
*  IMPLEMENTATION:
*       THE TRANSFORMATION IS IMPLEMENTED THROUGH REPEATED CALLS TO THE
*       "BUTTERFLY" TRANSFORMATION MFFT?7; PARAMETERS OF THE "BUTTERFLY"
*       ARE COMMUNICATED THROUGH THE COMMON BLOCK MFFTPA.
*  ARGUMENTS:
*    INPUT  :
*       C : ARRAY TO BE TRANSFORMED.
*       IMSX,IVSX,IESX,NMX,NVX,NEX: THESE ARGUMENTS DEFINE THE STRUCTURE
*           OF C ACCORDING TO THE DEFINITIONS ABOVE. THEY ARE UNCHANGED
*           ON OUTPUT
*       TABLES : ARRAY PREPARED BY MFFTP. IT  IS NOT CHANGED ON OUTPUT.
*                IT SHOULD BE DECLARED INTEGER TABLES(4*NM+14);
*                IT MUST BE INITIALIZED BY MFFTP BEFORE USAGE.
*    OUTPUT:
*       C : TRANSFORM OF THE ORIGINAL ARRAY;
*       IERR : ERROR CODE : =0 : SUCCESSFUL
*                         : =3 :  'TABLES' NOT CORRECTLY INITIALIZED
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DOUBLE COMPLEX C(*)
      INTEGER TABLES(-14:*)
      INTEGER IDERR,FACERR,TBERR
      PARAMETER (IDERR=1,FACERR=2,TBERR=3)
      COMMON /MFFTPA/  IMS,IVS,IES,NM,NV,NE,MX,LX,MLIM,MSTEP,LLIM,LSTEP,
     $ NUSTEP,IVLIM,ILIM,MD2LIM,LD2LIM
*  LOADING THE COMMON BLOCK : CONSTANTS
      IMS=IMSX
      IVS=IVSX
      IES=IESX
      NM=NMX
      NV=NVX
      NE=NEX
      IVLIM=(NV-1)*IVS
      ILIM=(NE-1)*IES
      LSTEP=IMS
*  LOADING THE COMMON BLOCK : ITERATION-DEPENDENT QUANTITIES: INITIALIZA
      LX=1
      MX=NM
      IFAC=TABLES(-1)
      IF(IFAC.GT.3)THEN
        IERR=TBERR
        RETURN
      ENDIF
*..   RADIX 2 LOOP
200   CONTINUE
      DO 210 IM=1,TABLES(-14)
        MX=MX/2
        NUSTEP=LX*LSTEP
        LLIM=NUSTEP-LSTEP
        LD2LIM=NUSTEP/2-LSTEP
        MSTEP=NUSTEP*2
        MLIM=NM*LSTEP-MSTEP
        CALL MFFTA7(C,TABLES(0))
        LX=LX+LX
210   CONTINUE
      IF(IFAC.EQ.1)RETURN
*..   RADIX 3 LOOP
300   CONTINUE
      DO 310 IM=1,TABLES(-13)
        MX=MX/3
        NUSTEP=LX*LSTEP
        LLIM=NUSTEP-LSTEP
        MSTEP=NUSTEP*3
        MLIM=NM*LSTEP-MSTEP
        CALL MFFTB7(C,TABLES(0))
        LX=LX*3
310   CONTINUE
      IF(IFAC.EQ.2)RETURN
*..   RADIX 5 LOOP
500   CONTINUE
      DO 510 IM=1,TABLES(-12)
        MX=MX/5
        NUSTEP=LX*LSTEP
        LLIM=NUSTEP-LSTEP
        MSTEP=NUSTEP*5
        MLIM=NM*LSTEP-MSTEP
        CALL MFFTC7(C,TABLES(0))
        LX=LX*5
510   CONTINUE
      END


function interp (x, y, kfmin, kfmax, kfstep, kmax, xval)
!----- GPL ---------------------------------------------------------------------
!
!  Copyright (C)  Stichting Deltares, 2011-2016.
!
!  This program 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 version 3.
!
!  This program 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.
!
!  You should have received a copy of the GNU General Public License
!  along with this program.  If not, see <http://www.gnu.org/licenses/>.
!
!  contact: delft3d.support@deltares.nl
!  Stichting Deltares
!  P.O. Box 177
!  2600 MH Delft, The Netherlands
!
!  All indications and logos of, and references to, "Delft3D" and "Deltares"
!  are registered trademarks of Stichting Deltares, and remain the property of
!  Stichting Deltares. All rights reserved.
!
!-------------------------------------------------------------------------------
!  $Id: interp.f90 5717 2016-01-12 11:35:24Z mourits $
!  $HeadURL: https://svn.oss.deltares.nl/repos/delft3d/tags/6686/src/engines_gpl/flow2d3d/packages/kernel/src/compute/interp.f90 $
!!--description-----------------------------------------------------------------
!
! interpolate y values for xval in the range of x array
!
!!--pseudo code and references--------------------------------------------------
! NONE
!!--declarations----------------------------------------------------------------
    use precision
    !
    implicit none
!
! Function result
    real(fp) :: interp
!
!
! Global variables
!
    integer                       , intent(in) :: kfmin
    integer                       , intent(in) :: kfmax
    integer                       , intent(in) :: kfstep
    integer                       , intent(in) :: kmax
    real(fp)                      , intent(in) :: xval
    real(fp), dimension (0:kmax+1), intent(in) :: x
    real(fp), dimension (kmax)    , intent(in) :: y
!
! Local variables
!
    integer :: k
!
!! executable statements -------------------------------------------------------
!
    interp = -999.0_fp
    if (xval < x(0)) then
       !
       ! outside extrapolation region lower side
       !
       interp = 0.0_fp
    elseif (xval <= x(kfmin)) then
       !
       ! inside extrapolation region lower side
       !
       interp = y(kfmin)
    elseif (xval > x(kfmax+1)) then
       !
       ! outside extrapolation region upper side
       !
       interp = 0.0_fp
    elseif (xval >= x(kfmax)) then
       !
       ! inside extrapolation region upper side
       !
       interp = y(kfmax)
    else
       !
       ! interpolation
       !
       do k = kfmin+kfstep, kfmax, kfstep
          if (xval >= x(k-kfstep) .and. xval <= x(k)) then
             interp = y(k-kfstep) + ((xval - x(k-kfstep))/(x(k) - x(k-kfstep))) &
                    &               * (y(k) - y(k-kfstep))
             exit
          endif
       enddo
    endif
end function interp


      SUBROUTINE SB10LD( N, M, NP, NCON, NMEAS, A, LDA, B, LDB, C, LDC,
     $                   D, LDD, AK, LDAK, BK, LDBK, CK, LDCK, DK, LDDK,
     $                   AC, LDAC, BC, LDBC, CC, LDCC, DC, LDDC, IWORK,
     $                   DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To compute the matrices of the closed-loop system
C
C              | AC | BC |
C          G = |----|----|,
C              | CC | DC |
C
C     from the matrices of the open-loop system
C
C               | A | B |
C           P = |---|---|
C               | C | D |
C
C     and the matrices of the controller
C
C              | AK | BK |
C          K = |----|----|.
C              | CK | DK |
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the system.  N >= 0.
C
C     M       (input) INTEGER
C             The column size of the matrix B.  M >= 0.
C
C     NP      (input) INTEGER
C             The row size of the matrix C.  NP >= 0.
C
C     NCON    (input) INTEGER
C             The number of control inputs (M2).  M >= NCON >= 0.
C             NP-NMEAS >= NCON.
C
C     NMEAS   (input) INTEGER
C             The number of measurements (NP2).  NP >= NMEAS >= 0.
C             M-NCON >= NMEAS.
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N part of this array must contain the
C             system state matrix A.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= max(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain the
C             system input matrix B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= max(1,N).
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading NP-by-N part of this array must contain the
C             system output matrix C.
C
C     LDC     INTEGER
C             The leading dimension of the array C.  LDC >= max(1,NP).
C
C     D       (input) DOUBLE PRECISION array, dimension (LDD,M)
C             The leading NP-by-M part of this array must contain the
C             system input/output matrix D.
C
C     LDD     INTEGER
C             The leading dimension of the array D.  LDD >= max(1,NP).
C
C     AK      (input) DOUBLE PRECISION array, dimension (LDAK,N)
C             The leading N-by-N part of this array must contain the
C             controller state matrix AK.
C
C     LDAK    INTEGER
C             The leading dimension of the array AK.  LDAK >= max(1,N).
C
C     BK      (input) DOUBLE PRECISION array, dimension (LDBK,NMEAS)
C             The leading N-by-NMEAS part of this array must contain the
C             controller input matrix BK.
C
C     LDBK    INTEGER
C             The leading dimension of the array BK.  LDBK >= max(1,N).
C
C     CK      (input) DOUBLE PRECISION array, dimension (LDCK,N)
C             The leading NCON-by-N part of this array must contain the
C             controller output matrix CK.
C
C     LDCK    INTEGER
C             The leading dimension of the array CK.
C             LDCK >= max(1,NCON).
C
C     DK      (input) DOUBLE PRECISION array, dimension (LDDK,NMEAS)
C             The leading NCON-by-NMEAS part of this array must contain
C             the controller input/output matrix DK.
C
C     LDDK    INTEGER
C             The leading dimension of the array DK.
C             LDDK >= max(1,NCON).
C
C     AC      (output) DOUBLE PRECISION array, dimension (LDAC,2*N)
C             The leading 2*N-by-2*N part of this array contains the
C             closed-loop system state matrix AC.
C
C     LDAC    INTEGER
C             The leading dimension of the array AC.
C             LDAC >= max(1,2*N).
C
C     BC      (output) DOUBLE PRECISION array, dimension (LDBC,M-NCON)
C             The leading 2*N-by-(M-NCON) part of this array contains
C             the closed-loop system input matrix BC.
C
C     LDBC    INTEGER
C             The leading dimension of the array BC.
C             LDBC >= max(1,2*N).
C
C     CC      (output) DOUBLE PRECISION array, dimension (LDCC,2*N)
C             The leading (NP-NMEAS)-by-2*N part of this array contains
C             the closed-loop system output matrix CC.
C
C     LDCC    INTEGER
C             The leading dimension of the array CC.
C             LDCC >= max(1,NP-NMEAS).
C
C     DC      (output) DOUBLE PRECISION array, dimension (LDDC,M-NCON)
C             The leading (NP-NMEAS)-by-(M-NCON) part of this array
C             contains the closed-loop system input/output matrix DC.
C
C     LDDC    INTEGER
C             The leading dimension of the array DC.
C             LDDC >= max(1,NP-NMEAS).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (2*max(NCON,NMEAS))
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) contains the optimal
C             LDWORK.
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= 2*M*M+NP*NP+2*M*N+M*NP+2*N*NP.
C             For good performance, LDWORK must generally be larger.
C
C     Error Indicactor
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if the matrix Inp2 - D22*DK is singular to working
C                   precision;
C             = 2:  if the matrix Im2 - DK*D22 is singular to working
C                   precision.
C
C     METHOD
C
C     The routine implements the formulas given in [1].
C
C     REFERENCES
C
C     [1] Balas, G.J., Doyle, J.C., Glover, K., Packard, A., and
C         Smith, R.
C         mu-Analysis and Synthesis Toolbox.
C         The MathWorks Inc., Natick, Mass., 1995.
C
C     NUMERICAL ASPECTS
C
C     The accuracy of the result depends on the condition numbers of the
C     matrices  Inp2 - D22*DK  and  Im2 - DK*D22.
C
C     CONTRIBUTORS
C
C     P.Hr. Petkov, D.W. Gu and M.M. Konstantinov, October 1998.
C
C     REVISIONS
C
C     V. Sima, Research Institute for Informatics, Bucharest, May 1999.
C     A. Markovski, Technical University, Sofia, April, 2003.
C
C     KEYWORDS
C
C     Closed loop systems, feedback control, robust control.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
C
C     .. Scalar Arguments ..
      INTEGER            INFO, LDA, LDAC, LDAK, LDB, LDBC, LDBK, LDC,
     $                   LDCC, LDCK, LDD, LDDC, LDDK, LDWORK, M, N,
     $                   NCON, NMEAS, NP
C     ..
C     .. Array Arguments ..
      INTEGER            IWORK( * )
      DOUBLE PRECISION   A( LDA, * ), AC( LDAC, * ), AK( LDAK, * ),
     $                   B( LDB, * ), BC( LDBC, * ), BK( LDBK, * ),
     $                   C( LDC, * ), CC( LDCC, * ), CK( LDCK, * ),
     $                   D( LDD, * ), DC( LDDC, * ), DK( LDDK, * ),
     $                   DWORK( * )
C     ..
C     .. Local Scalars ..
      INTEGER            INFO2, IW2, IW3, IW4, IW5, IW6, IW7, IW8, IWRK,
     $                   LWAMAX, M1, M2, MINWRK, N2, NP1, NP2
      DOUBLE PRECISION   ANORM, EPS, RCOND
C     ..
C     .. External Functions ..
      DOUBLE PRECISION   DLAMCH, DLANGE
      EXTERNAL           DLAMCH, DLANGE
C     ..
C     .. External Subroutines ..
      EXTERNAL           DGECON, DGEMM, DGETRF, DGETRI, DLACPY, DLASET,
     $                   XERBLA
C     ..
C     .. Executable Statements ..
C
C     Decode and Test input parameters.
C
      N2  = 2*N
      M1  = M - NCON
      M2  = NCON
      NP1 = NP - NMEAS
      NP2 = NMEAS
C
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( M.LT.0 ) THEN
         INFO = -2
      ELSE IF( NP.LT.0 ) THEN
         INFO = -3
      ELSE IF( NCON.LT.0 .OR. M1.LT.0 .OR. M2.GT.NP1 ) THEN
         INFO = -4
      ELSE IF( NMEAS.LT.0 .OR. NP1.LT.0 .OR. NP2.GT.M1 ) THEN
         INFO = -5
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -9
      ELSE IF( LDC.LT.MAX( 1, NP ) ) THEN
         INFO = -11
      ELSE IF( LDD.LT.MAX( 1, NP ) ) THEN
         INFO = -13
      ELSE IF( LDAK.LT.MAX( 1, N ) ) THEN
         INFO = -15
      ELSE IF( LDBK.LT.MAX( 1, N ) ) THEN
         INFO = -17
      ELSE IF( LDCK.LT.MAX( 1, M2 ) ) THEN
         INFO = -19
      ELSE IF( LDDK.LT.MAX( 1, M2 ) ) THEN
         INFO = -21
      ELSE IF( LDAC.LT.MAX( 1, N2 ) ) THEN
         INFO = -23
      ELSE IF( LDBC.LT.MAX( 1, N2 ) ) THEN
         INFO = -25
      ELSE IF( LDCC.LT.MAX( 1, NP1 ) ) THEN
         INFO = -27
      ELSE IF( LDDC.LT.MAX( 1, NP1 ) ) THEN
         INFO = -29
      ELSE
C
C        Compute workspace.
C
         MINWRK = 2*M*M + NP*NP + 2*M*N + M*NP + 2*N*NP
         IF( LDWORK.LT.MINWRK )
     $      INFO = -32
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SB10LD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF( N.EQ.0 .OR. M.EQ.0 .OR. NP.EQ.0 .OR. M1.EQ.0 .OR. M2.EQ.0
     $    .OR. NP1.EQ.0 .OR. NP2.EQ.0 ) THEN
         DWORK( 1 ) = ONE
         RETURN
      END IF
C
C     Get the machine precision.
C
      EPS = DLAMCH( 'Epsilon' )
C
C     Workspace usage.
C
      IW2 = NP2*NP2 + 1
      IW3 = IW2 + M2*M2
      IW4 = IW3 + NP2*N
      IW5 = IW4 + M2*N
      IW6 = IW5 + NP2*M1
      IW7 = IW6 + M2*M1
      IW8 = IW7 + M2*N
      IWRK = IW8 + NP2*N
C
C     Compute inv(Inp2 - D22*DK) .
C
      CALL DLASET( 'Full', NP2, NP2, ZERO, ONE, DWORK, NP2 )
      CALL DGEMM( 'N', 'N', NP2, NP2, M2, -ONE, D( NP1+1, M1+1 ),
     $            LDD, DK, LDDK, ONE, DWORK, NP2 )
      ANORM = DLANGE( '1', NP2, NP2, DWORK, NP2, DWORK( IWRK ) )
      CALL DGETRF( NP2, NP2, DWORK, NP2, IWORK, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = 1
         RETURN
      END IF
      CALL DGECON( '1', NP2, DWORK, NP2, ANORM, RCOND, DWORK( IWRK ),
     $             IWORK( NP2+1 ), INFO )
      LWAMAX = INT( DWORK( IWRK ) ) + IWRK - 1
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND.LT.EPS ) THEN
         INFO = 1
         RETURN
      END IF
      CALL DGETRI( NP2, DWORK, NP2, IWORK, DWORK( IWRK ), LDWORK-IWRK+1,
     $             INFO2 )
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
C     Compute inv(Im2 - DK*D22) .
C
      CALL DLASET( 'Full', M2, M2, ZERO, ONE, DWORK( IW2 ), M2 )
      CALL DGEMM( 'N', 'N', M2, M2, NP2, -ONE, DK, LDDK,
     $            D( NP1+1, M1+1 ), LDD, ONE, DWORK( IW2 ), M2 )
      ANORM = DLANGE( '1', M2, M2, DWORK( IW2 ), M2, DWORK( IWRK ) )
      CALL DGETRF( M2, M2, DWORK( IW2 ), M2, IWORK, INFO2 )
      IF( INFO2.GT.0 ) THEN
         INFO = 2
         RETURN
      END IF
      CALL DGECON( '1', M2, DWORK( IW2 ), M2, ANORM, RCOND,
     $             DWORK( IWRK ), IWORK( M2+1 ), INFO )
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
C     Return if the matrix is singular to working precision.
C
      IF( RCOND.LT.EPS ) THEN
         INFO = 2
         RETURN
      END IF
      CALL DGETRI( M2, DWORK( IW2 ), M2, IWORK, DWORK( IWRK ),
     $             LDWORK-IWRK+1, INFO2 )
      LWAMAX = MAX( INT( DWORK( IWRK ) ) + IWRK - 1, LWAMAX )
C
C     Compute inv(Inp2 - D22*DK)*C2 .
C
      CALL DGEMM( 'N', 'N', NP2, N, NP2, ONE, DWORK, NP2, C( NP1+1, 1 ),
     $            LDC, ZERO, DWORK( IW3 ), NP2 )
C
C     Compute DK*inv(Inp2 - D22*DK)*C2 .
C
      CALL DGEMM( 'N', 'N', M2, N, NP2, ONE, DK, LDDK,  DWORK( IW3 ),
     $            NP2, ZERO, DWORK( IW4 ), M2 )
C
C     Compute inv(Inp2 - D22*DK)*D21 .
C
      CALL DGEMM( 'N', 'N', NP2, M1, NP2, ONE, DWORK, NP2,
     $            D( NP1+1, 1 ), LDD, ZERO, DWORK( IW5 ), NP2 )
C
C     Compute DK*inv(Inp2 - D22*DK)*D21 .
C
      CALL DGEMM( 'N', 'N', M2, M1, NP2, ONE, DK, LDDK, DWORK( IW5 ),
     $            NP2, ZERO, DWORK( IW6 ), M2 )
C
C     Compute inv(Im2 - DK*D22)*CK .
C
      CALL DGEMM( 'N', 'N', M2, N, M2, ONE, DWORK( IW2 ), M2, CK, LDCK,
     $            ZERO, DWORK( IW7 ), M2 )
C
C     Compute D22*inv(Im2 - DK*D22)*CK .
C
      CALL DGEMM( 'N', 'N', NP2, N, M2, ONE, D( NP1+1, M1+1 ), LDD,
     $            DWORK( IW7 ), M2, ZERO, DWORK( IW8 ), NP2 )
C
C     Compute AC .
C
      CALL DLACPY( 'Full', N, N, A, LDA, AC, LDAC )
      CALL DGEMM( 'N', 'N', N, N, M2, ONE, B( 1, M1+1 ), LDB,
     $            DWORK( IW4 ), M2, ONE, AC, LDAC )
      CALL DGEMM( 'N', 'N', N, N, M2, ONE, B( 1, M1+1 ), LDB,
     $            DWORK( IW7 ), M2, ZERO, AC( 1, N+1 ), LDAC )
      CALL DGEMM( 'N', 'N', N, N, NP2, ONE, BK, LDBK, DWORK( IW3 ), NP2,
     $            ZERO, AC( N+1, 1 ), LDAC )
      CALL DLACPY( 'Full', N, N, AK, LDAK, AC( N+1, N+1 ), LDAC )
      CALL DGEMM( 'N', 'N', N, N, NP2, ONE, BK, LDBK, DWORK( IW8 ), NP2,
     $            ONE, AC( N+1, N+1 ), LDAC )
C
C     Compute BC .
C
      CALL DLACPY( 'Full', N, M1, B, LDB, BC, LDBC )
      CALL DGEMM( 'N', 'N', N, M1, M2, ONE, B( 1, M1+1 ), LDB,
     $            DWORK( IW6 ), M2, ONE, BC, LDBC )
      CALL DGEMM( 'N', 'N', N, M1, NP2, ONE, BK, LDBK, DWORK( IW5 ),
     $            NP2, ZERO, BC( N+1, 1 ), LDBC )
C
C     Compute CC .
C
      CALL DLACPY( 'Full', NP1, N, C, LDC, CC, LDCC )
      CALL DGEMM( 'N', 'N', NP1, N, M2, ONE, D( 1, M1+1 ), LDD,
     $            DWORK( IW4 ), M2, ONE, CC, LDCC )
      CALL DGEMM( 'N', 'N', NP1, N, M2, ONE, D( 1, M1+1 ), LDD,
     $            DWORK( IW7 ), M2, ZERO, CC( 1, N+1 ), LDCC )
C
C     Compute DC .
C
      CALL DLACPY( 'Full', NP1, M1, D, LDD, DC, LDDC )
      CALL DGEMM( 'N', 'N', NP1, M1, M2, ONE, D( 1, M1+1 ), LDD,
     $            DWORK( IW6 ), M2, ONE, DC, LDDC )
C
      RETURN
C *** Last line of SB10LD ***
      END