!===============================================================================
! One of Andy Nowacki's Fortran utility modules for dealing with seismic
! anisotropy and other problems.
!
! Andy Nowacki <andy.nowacki@bristol.ac.uk>
!
! See the file LICENCE for licence details.
!===============================================================================
!
! FFFTW - Fastest (Fortran) Fourier Transform in the West
!
!===============================================================================
!
! FFFTW is a module interface to the FFTW3 C DFT library.  It provides
! a simple wrapper to call FFTW's routines from Fortran programs.  It requires
! a F2003-aware compiler in order to use the ISO_C_BINDING intrinsic module,
! but there are no requirements other than that.
!
! The module requires FFTW3 to be installed and probably compiled with the same
! type of compiler (e.g., GNU, Intel, etc.) as used to compile the module.
!
! Module procedures allow one to call just a forward or reverse FFT: just use
! the array you want to be transformed and the module will automatically choose
! the correct FFTW routine.
!
! E.g., to transform a 1D single-precision real array:
!     real :: time_domain_array(100)
!     complex, allocatable :: freq_domain_array(:)
!     call FFFTW_fwd(time_domain_array, freq_domain_array)
!
! To then transform back:
!     call FFFTW_rev(freq_domain_array, time_domain_array)
!
! (N.B.: freq_domain_array is destroyed in the process; to save it, copy the
! array to another variable before the reverse transform.)
!
! FFFTW computes the required size of the FFTed complex trace automatically and
! (re)allocates it as necessary.  Hence the FFT array must be declared
! allocatable.  The compiler should flag any attempt to do otherwise, but note
! that the error message may not say explicitly so--instead, you may get an
! undefined reference to the generic overloaded procedure FFFTW_{fwd,rev}.
!===============================================================================
module FFFTW
!===============================================================================

   use, intrinsic :: iso_c_binding

   implicit none

   ! Only allow explicitly public variables and routines to be visible
!    private

   include 'fftw3.f03'

   ! Precision selectors
   integer, parameter, private :: r4 = selected_real_kind(6,37), &
                                  r8 = selected_real_kind(15,307)
   integer, parameter, private :: c4 = r4, &
                                  c8 = r8

   ! Current state of FFFTW: The first time we're called, make sure that the
   ! Fortran and C types match.  Otherwise we're doomed.
   logical, private :: FFFTW_initialised = .false.

   ! Can call each type of transform with the same call regardless of using
   ! real(4) or real(8) precision.
   interface FFFTW_fwd
      module procedure :: FFFTW_fwd_r2c_1d_real4, &
                          FFFTW_fwd_r2c_1d_real8, &
                          FFFTW_fwd_c2c_1d_complex4, &
                          FFFTW_fwd_c2c_1d_complex8
   end interface FFFTW_fwd

   interface FFFTW_rev
      module procedure :: FFFTW_rev_c2r_1d_real4, &
                          FFFTW_rev_c2r_1d_real8, &
                          FFFTW_rev_c2c_1d_complex4, &
                          FFFTW_rev_c2c_1d_complex8
   end interface FFFTW_rev

   interface FFFTW_allocate
      module procedure :: FFFTW_allocate_real4, &
                          FFFTW_allocate_real8, &
                          FFFTW_allocate_complex4, &
                          FFFTW_allocate_complex8
   end interface FFFTW_allocate

   private :: FFFTW_allocate_real4, &
              FFFTW_allocate_real8, &
              FFFTW_allocate_complex4, &
              FFFTW_allocate_complex8, &
              FFFTW_err

contains

!===============================================================================
subroutine FFFTW_fwd_r2c_1d_real4(real_array, complex_fft, plan_type)
!===============================================================================
!  FFFTW_fwd_*() computes the FT for a 1d array.  This is done out-of-place, so
!  the input array is preserved.  complex_fft
   implicit none
   real(r4), intent(inout), dimension(:) :: real_array
   complex(c4), intent(inout), allocatable, dimension(:) :: complex_fft
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc

   call FFFTW_initialise

   ! Size of real and complex FFT traces
   nr = size(real_array)
   nc = nr/2 + 1

   ! Allocate memory for the FFT trace if necessary
   call FFFTW_allocate(complex_fft, nc)

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftwf_plan_dft_r2c_1d(int(nr, kind=C_INT), &
                              real_array, &
                              complex_fft, &
                              fftw_plan_type)
   call fftwf_execute_dft_r2c(plan, real_array, complex_fft)
   call fftwf_destroy_plan(plan)

end subroutine FFFTW_fwd_r2c_1d_real4
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_fwd_r2c_1d_real8(real_array, complex_fft, plan_type)
!===============================================================================
   implicit none
   real(r8), intent(inout), dimension(:) :: real_array
   complex(c8), intent(inout), allocatable, dimension(:) :: complex_fft
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc

   call FFFTW_initialise

   ! Size of real and complex FFT traces
   nr = size(real_array)
   nc = nr/2 + 1

   ! Allocate memory for the FFT trace if necessary
   call FFFTW_allocate(complex_fft, nc)

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftw_plan_dft_r2c_1d(int(nr, kind=C_INT), &
                              real_array, &
                              complex_fft, &
                              fftw_plan_type)
   call fftw_execute_dft_r2c(plan, real_array, complex_fft)
   call fftw_destroy_plan(plan)

end subroutine FFFTW_fwd_r2c_1d_real8
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_fwd_c2c_1d_complex4(complex_array, complex_fft, plan_type)
!===============================================================================
   implicit none
   complex(c4), intent(inout), dimension(:) :: complex_array
   complex(c4), intent(inout), allocatable, dimension(:) :: complex_fft
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc

   call FFFTW_initialise

   ! Size of real and complex FFT traces
   nr = size(complex_array)
   nc = nr

   ! Allocate memory for the FFT trace if necessary
   call FFFTW_allocate(complex_fft, nc)

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftwf_plan_dft_1d(int(nr, kind=C_INT), &
                          complex_array, &
                          complex_fft, &
                          FFTW_FORWARD, &
                          fftw_plan_type)
   call fftwf_execute_dft(plan, complex_array, complex_fft)
   call fftwf_destroy_plan(plan)

end subroutine FFFTW_fwd_c2c_1d_complex4
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_fwd_c2c_1d_complex8(complex_array, complex_fft, plan_type)
!===============================================================================
   implicit none
   complex(c8), intent(inout), dimension(:) :: complex_array
   complex(c8), intent(inout), allocatable, dimension(:) :: complex_fft
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc

   call FFFTW_initialise

   ! Size of real and complex FFT traces
   nr = size(complex_array)
   nc = nr

   ! Allocate memory for the FFT trace if necessary
   call FFFTW_allocate(complex_fft, nc)

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftw_plan_dft_1d(int(nr, kind=C_INT), &
                          complex_array, &
                          complex_fft, &
                          FFTW_FORWARD, &
                          fftw_plan_type)
   call fftw_execute_dft(plan, complex_array, complex_fft)
   call fftw_destroy_plan(plan)

end subroutine FFFTW_fwd_c2c_1d_complex8
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_rev_c2r_1d_real4(complex_fft, real_array, plan_type)
!===============================================================================
   implicit none
   complex(c4), intent(inout), allocatable, dimension(:) :: complex_fft
   real(r4), intent(inout), dimension(:) :: real_array
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc

   call FFFTW_initialise

   ! Check array sizes
   nc = size(complex_fft)
   nr = size(real_array)
   if (nc /= nr/2 + 1) &
      call FFFTW_err('FFFTW_rev_c2r_1d_real4: Array sizes of FFT and T-domain' &
         // ' trace not as expected')

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftwf_plan_dft_c2r_1d(int(nr, kind=C_INT), &
                              complex_fft, &
                              real_array, &
                              fftw_plan_type)
   call fftwf_execute_dft_c2r(plan, complex_fft, real_array)
   call fftwf_destroy_plan(plan)

   ! Normalise values
   real_array = real_array/real(nr, kind=r4)

end subroutine FFFTW_rev_c2r_1d_real4
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_rev_c2r_1d_real8(complex_fft, real_array, plan_type)
!===============================================================================
   implicit none
   complex(c8), intent(inout), allocatable, dimension(:) :: complex_fft
   real(r8), intent(inout), dimension(:) :: real_array
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc

   call FFFTW_initialise

   ! Check array sizes
   nc = size(complex_fft)
   nr = size(real_array)
   if (nc /= nr/2 + 1) &
      call FFFTW_err('FFFTW_rev_c2r_1d_real8: Array sizes of FFT and T-domain' &
         // ' trace not as expected')

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftw_plan_dft_c2r_1d(int(nr, kind=C_INT), &
                              complex_fft, &
                              real_array, &
                              fftw_plan_type)
   call fftw_execute_dft_c2r(plan, complex_fft, real_array)
   call fftw_destroy_plan(plan)

   ! Normalise values
   real_array = real_array/real(nr, kind=r4)

end subroutine FFFTW_rev_c2r_1d_real8
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_rev_c2c_1d_complex4(complex_fft, complex_array, plan_type)
!===============================================================================
   implicit none
   complex(c4), intent(inout), allocatable, dimension(:) :: complex_fft
   complex(c4), intent(inout), dimension(:) :: complex_array
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc
   complex(c4) :: norm

   call FFFTW_initialise

   ! Check array sizes
   nc = size(complex_fft)
   nr = size(complex_array)
   if (nc /= nr) &
      call FFFTW_err('FFFTW_rev_c2c_1d_complex4: Array sizes of FFT and T-domain' &
         // ' trace not as expected')

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftwf_plan_dft_1d(int(nr, kind=C_INT), &
                          complex_fft, &
                          complex_array, &
                          FFTW_BACKWARD, &
                          fftw_plan_type)
   call fftwf_execute_dft(plan, complex_fft, complex_array)
   call fftwf_destroy_plan(plan)

   ! Normalise values
   norm = real(nr, kind=r4)
   complex_array = complex_array/norm

end subroutine FFFTW_rev_c2c_1d_complex4
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_rev_c2c_1d_complex8(complex_fft, complex_array, plan_type)
!===============================================================================
   implicit none
   complex(c8), intent(inout), allocatable, dimension(:) :: complex_fft
   complex(c8), intent(inout), dimension(:) :: complex_array
   integer(C_INT), intent(in), optional :: plan_type
   integer(C_INT) :: fftw_plan_type
   type(C_PTR) :: plan
   integer :: nr, nc
   complex(c8) :: norm

   call FFFTW_initialise

   ! Check array sizes
   nc = size(complex_fft)
   nr = size(complex_array)
   if (nc /= nr) &
      call FFFTW_err('FFFTW_rev_c2c_1d_complex4: Array sizes of FFT and T-domain' &
         // ' trace not as expected')

   fftw_plan_type = FFTW_ESTIMATE
   if (present(plan_type)) fftw_plan_type = int(plan_type, kind=C_INT)

   plan = fftw_plan_dft_1d(int(nr, kind=C_INT), &
                          complex_fft, &
                          complex_array, &
                          FFTW_BACKWARD, &
                          fftw_plan_type)
   call fftw_execute_dft(plan, complex_fft, complex_array)
   call fftw_destroy_plan(plan)

   ! Normalise values
   norm = real(nr, kind=r8)
   complex_array = complex_array/norm

end subroutine FFFTW_rev_c2c_1d_complex8
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_allocate_real4(a,n)
!===============================================================================
!  FFFTW_allocate_*() allocates memory to an array.  If that array is already
!  allocated but is of different length to n, it is reallocated.  If it is the
!  same length as n, it is not touched.
   implicit none
   real(r4), dimension(:), allocatable, intent(inout) :: a
   integer, intent(in) :: n
   if (allocated(a)) then
      if (size(a) /= n) allocate(a(n))
   else
      allocate(a(n))
   endif
end subroutine FFFTW_allocate_real4
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_allocate_real8(a,n)
!===============================================================================
   implicit none
   real(r8), dimension(:), allocatable, intent(inout) :: a
   integer, intent(in) :: n
   if (allocated(a)) then
      if (size(a) /= n) allocate(a(n))
   else
      allocate(a(n))
   endif
end subroutine FFFTW_allocate_real8
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_allocate_complex4(a,n)
!===============================================================================
   implicit none
   complex(c4), dimension(:), allocatable, intent(inout) :: a
   integer, intent(in) :: n
   if (allocated(a)) then
      if (size(a) /= n) allocate(a(n))
   else
      allocate(a(n))
   endif
end subroutine FFFTW_allocate_complex4
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_allocate_complex8(a,n)
!===============================================================================
   implicit none
   complex(c8), dimension(:), allocatable, intent(inout) :: a
   integer, intent(in) :: n
   if (allocated(a)) then
      if (size(a) /= n) allocate(a(n))
   else
      allocate(a(n))
   endif
end subroutine FFFTW_allocate_complex8
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_initialise()
!===============================================================================
!  Perform initialisation procedures.
   implicit none
   if (.not.FFFTW_initialised) then
      ! Test for equivalence of Fortran selected kinds and C kinds.
      ! The Fortran kinds are used by the compiler to pick the correct
      ! subroutines when calling the generic procedures (FFFTW_fwd(), etc.),
      ! therefore as long as they are equal to the C kinds we can pick the
      ! correct subroutine and call the FFTW functions safely.
      if (r4 /= C_FLOAT .or. r8 /= C_DOUBLE .or. c4 /= C_FLOAT_COMPLEX .or. &
          c8 /= C_DOUBLE_COMPLEX) &
          call FFFTW_err('FFFTW_initialise: Fortran kinds do not match C types')
      FFFTW_initialised = .true.
   endif
end subroutine FFFTW_initialise
!-------------------------------------------------------------------------------

!===============================================================================
subroutine FFFTW_err(string)
!===============================================================================
!  Private utility routine to write a message and stop the program
   implicit none
   character(len=*), intent(in) :: string
   write(0,'(a)') 'FFFTW: Error: ' // trim(string)
   stop
end subroutine FFFTW_err
!-------------------------------------------------------------------------------

end module FFFTW
!-------------------------------------------------------------------------------


      INCLUDE 'VICMAIN_FOR'

      SUBROUTINE MAIN44

      REAL*4 A(4,4), C(4,4)
      REAL*4 B(4), Z(4), D
      INTEGER*4 N

C     Initialize an array with a determinant of 1

      DATA A/5,7,6,5,  7,10,8,7,  6,8,10,9,  5,7,9,10/

C       INITIALIZE AN ARRAY TO COMPARE WITH THE 'C' VERSION

      C(1,1) = 6
      C(1,2) = 4
      C(1,3) = 6
      C(1,4) = 5

      C(2,1) = 7
      C(2,2) = 2
      C(2,3) = 3
      C(2,4) = 4

      C(3,1) = 4
      C(3,2) = 2
      C(3,3) = 1
      C(3,4) = 5

      C(4,1) = 6
      C(4,2) = 6
      C(4,3) = 6
      C(4,4) = 6

      N = 4
      CALL XVMESSAGE('FORTRAN TEST',' ')
      CALL XVMESSAGE('MATRIX WITH DETERMINANT = 1',' ')
      CALL TMINVMAT(A,N)
      CALL MINV(A,N,D,B,Z)
      CALL TMINVPRT (A,N,D)

      CALL XVMESSAGE('NOW INVERT BACK TO ORIGINAL',' ')
      CALL MINV(A,N,D,B,Z)
      CALL TMINVPRT (A,N,D)

      CALL XVMESSAGE('MATRIX FOR C BRIDGE COMPARE',' ')
      CALL TMINVMAT(C,N)
      CALL MINV(C,N,D,B,Z)
      CALL TMINVPRT (C,N,D)

      CALL XVMESSAGE('NOW INVERT BACK TO ORIGINAL',' ')
      CALL MINV(C,N,D,B,Z)
      CALL TMINVPRT (C,N,D)

      CALL XVMESSAGE('TEST THE C INTERFACE zminv',' ')
      CALL TZMINV

      RETURN
      END

C*******************************************************************C
C  THIS SUBROUTINE IS USED TO PRINT THE RESULTS OF THE TESTS.       C
C*******************************************************************C

      SUBROUTINE TMINVPRT (A,N,D)

      REAL*4 A(4,4), D
      INTEGER*4 N
      INTEGER*2 I, J
      CHARACTER*80 MSG

      CALL XVMESSAGE(' ',' ')
      CALL XVMESSAGE('LOWER TRIANGLE OF INVERSE',' ')

      DO 340 I=1,N
         WRITE (MSG,99994) (A(I,J),J=1,I)
         CALL XVMESSAGE(MSG,' ')
  340 CONTINUE

      CALL XVMESSAGE(' ',' ')
      WRITE (MSG,99993) D
      CALL XVMESSAGE(MSG,' ')

99994 FORMAT (' ', 4F10.6)
99993 FORMAT ('VALUE OF DETERMINANT = ',F14.8)
      END
C*******************************************************************C
C  THIS SUBROUTINE IS USED TO PRINT THE MATRIX.			    C
C*******************************************************************C

      SUBROUTINE TMINVMAT (A,N)

      REAL*4 A(4,4)
      INTEGER*4 N
      INTEGER*2 I, J
      CHARACTER*80 MSG

      DO 540 I=1,N
         WRITE (MSG,99994) (A(I,J),J=1,N)
         CALL XVMESSAGE(MSG,' ')
  540 CONTINUE
      CALL XVMESSAGE(' ',' ')

99994 FORMAT (' ', 4F10.6)
      END


!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
!                          Futility Development Group                          !
!                             All rights reserved.                             !
!                                                                              !
! Futility is a jointly-maintained, open-source project between the University !
! of Michigan and Oak Ridge National Laboratory.  The copyright and license    !
! can be found in LICENSE.txt in the head directory of this repository.        !
!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
!> @brief A Fortran 2003 module defining "planes" for use in geometry.
!>
!> This module provides a derived data types for a "plane" which must exist in
!> 3-D space. The plane is defined by a point and a normal vector. The point
!> also indicates the minimnum extents of the plane so it is semi-finite.
!> The module also provides methods for constructing a plane, deallocating the
!> plane, and getting the intersection of a plane and line.
!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
MODULE Geom_Plane
USE IntrType
USE Geom_Points
USE Geom_Line

IMPLICIT NONE
PRIVATE !Default contents of module to private
!
! List of Public items
PUBLIC :: PlaneType
PUBLIC :: OPERATOR(==)

!> @brief Type for a Plane defined by a normal vector @c n and a point @c v0.
TYPE :: PlaneType
  !> The vector that is normal to the plane's surface
  REAL(SRK),DIMENSION(3) :: n=0._SRK
  !> The point that defines the plane, it must also have the minimum values
  !> of the plane in x, y, and z.
  TYPE(PointType) :: v0
!
!List of type bound procedures
  CONTAINS
    !> @copybrief GeomPlane::set_PlaneType
    !> @copydetails GeomPlane::set_PlaneType
    PROCEDURE,PASS :: set => set_PlaneType
    !> @copybrief GeomPlane::clear_PlaneType
    !> @copydetails GeomPlane::clear_PlaneType
    PROCEDURE,PASS :: clear => clear_PlaneType
    !> @copybrief GeomPlane::intersect_PlaneType_and_LineType
    !> @copydetails GeomPlane::intersect_PlaneType_and_LineType
    PROCEDURE,PASS,PRIVATE :: intersectLine => intersect_PlaneType_and_LineType
    GENERIC :: intersect => intersectLine
ENDTYPE PlaneType

!> @brief Generic interface for 'is equal to' operator (==)
!>
!> Adds 'is equal to' capability for Plane types
INTERFACE OPERATOR(==)
  !> @copybrief Geom_Plane::isequal_PlaneType
  !> @copydetails Geom_Plane::isequal_PlaneType
  MODULE PROCEDURE isequal_PlaneType
ENDINTERFACE
!
!===============================================================================
CONTAINS
!
!-------------------------------------------------------------------------------
!> @brief Sets value for PlaneType
!> @param plane the plane type to set the value of
!> @param n_in a 3-dimensional vector representing the direction normal to the
!>        plane's surface
!> @param v0 a point lying in the plane, also the minimum extent of the plane
!>
PURE SUBROUTINE set_PlaneType(plane,n_in,v0)
  CLASS(PlaneType),INTENT(INOUT) :: plane
  REAL(SRK),INTENT(IN) :: n_in(3)
  TYPE(PointType),INTENT(IN) :: v0
  REAL(SRK) :: magn,n(3)

  CALL clear_PlaneType(plane)
  !Make sure the point v0 has data
  IF(v0%dim == 3 .AND. ALLOCATED(v0%coord)) THEN
    !Make sure n is normalized to 1
    magn=SQRT(n_in(1)*n_in(1)+n_in(2)*n_in(2)+n_in(3)*n_in(3))
    n=n_in
    IF(magn > 1.0_SRK) n=n_in/magn

    plane%n=n
    plane%v0=v0
  ENDIF
ENDSUBROUTINE set_PlaneType
!
!-------------------------------------------------------------------------------
!> @brief Clears the PlaneType object
!> @param plane the plane to clear
ELEMENTAL SUBROUTINE clear_PlaneType(plane)
  CLASS(PlaneType),INTENT(INOUT) :: plane
  plane%n=0._SRK
  CALL plane%v0%clear()
ENDSUBROUTINE clear_PlaneType
!
!-------------------------------------------------------------------------------
!> @brief Finds the intersection between a plane and line segment (if it exists)
!> @param plane the plane to test for intersection
!> @param line the segment to test for intersection
!> @returns @c point the point of intersection (if it exists)
!> @note a return code is assigned to point%dim indicating the type of
!> intersection. @n
!>   -3: there is no intersection (disjoint) @n
!>   -2: the line segment lies within the plane @n
!>   -1: problem with dummy arguments passed to routine @n
!>  > 0: success; an intersection point was found @n
ELEMENTAL FUNCTION intersect_PlaneType_and_LineType(plane,line) RESULT(point)
  CLASS(PlaneType),INTENT(IN) :: plane
  TYPE(LineType),INTENT(IN) :: line
  TYPE(PointType) :: point
  REAL(SRK) :: d,n,s
  REAL(SRK),DIMENSION(3) :: u,w
  point%dim=-1
  !Verify dummy arguments have data
  IF((plane%v0%dim == 3 .AND. ALLOCATED(plane%v0%coord)) .AND. &
      (line%p1%dim == 3 .AND. ALLOCATED(line%p1%coord)) .AND. &
      (line%p2%dim == 3 .AND. ALLOCATED(line%p2%coord))) THEN
    u(1)=line%p2%coord(1)-line%p1%coord(1)
    u(2)=line%p2%coord(2)-line%p1%coord(2)
    u(3)=line%p2%coord(3)-line%p1%coord(3)
    w(1)=line%p1%coord(1)-plane%v0%coord(1)
    w(2)=line%p1%coord(2)-plane%v0%coord(2)
    w(3)=line%p1%coord(3)-plane%v0%coord(3)
    d=plane%n(1)*u(1)+plane%n(2)*u(2)+plane%n(3)*u(3)
    n=-(plane%n(1)*w(1)+plane%n(2)*w(2)+plane%n(3)*w(3))
    IF(ABS(d) < EPSREAL) THEN
      IF(n .APPROXEQA. 0._SRK) THEN
        point%dim=-2 !collinear
      ELSE
        point%dim=-3 !parallel
      ENDIF
    ELSE
      s=n/d
      IF(0._SRK <= s .AND. s <= 1._SRK) THEN
        !Compute the intersection
        point%dim=line%p1%dim
        ALLOCATE(point%coord(point%dim))
        point%coord(1)=line%p1%coord(1)+s*u(1)
        point%coord(2)=line%p1%coord(2)+s*u(2)
        point%coord(3)=line%p1%coord(3)+s*u(3)
      ELSE
        point%dim=-3 !would intersect if infinite segment length
      ENDIF
    ENDIF
  ENDIF
ENDFUNCTION intersect_PlaneType_and_LineType
!
!-------------------------------------------------------------------------------
!> @brief Defines the 'is equal to' operation between two planes e.g. @c
!>        plane1 == plane2
!> @param p0 the first plane
!> @param p1 the second plane
!> @returns @c bool the boolean result of the operation
!>
!> Function is elemental so it can be used on an array of planes.
ELEMENTAL FUNCTION isequal_PlaneType(p0,p1) RESULT(bool)
  TYPE(PlaneType),INTENT(IN) :: p0,p1
  LOGICAL(SBK) :: bool
  bool=.FALSE.
  IF(ALL(p0%n .APPROXEQA. p1%n)) bool=(p0%v0 == p1%v0)
ENDFUNCTION isequal_PlaneType
!
ENDMODULE Geom_Plane


program psmma_testcauchy

  implicit none
  include  'mpif.h'

  integer, parameter :: BLACSCTXTSIZE=9

  integer :: descA(BLACSCTXTSIZE), descXB(BLACSCTXTSIZE)
  integer :: n
  integer :: nb
  integer :: locr, locc
  integer :: i, j, ii, jj
  integer :: ierr, dummy
  integer :: myid, np
  integer :: myrow, mycol, nprow, npcol
  integer :: ictxt, lwork, NTAC
  logical :: REDIST
  double precision :: xnorm, xnorm0, gap, sa, bb, time1,time2
!
  double precision, allocatable :: A(:), B(:), X(:), X1(:), WORK(:)
  double precision, allocatable :: D(:),F(:),U(:),V(:), TA(:,:)
  double precision,parameter :: one=1.0D+0, zero=0.0D+0

  integer, parameter :: IONE = 1, INONE=-1, IZERO=0

  integer, external :: indxl2g, numroc

  !n= 8192  ! Size of the problem
  !n= 16384  ! Size of the problem
  !n= 32768   ! Size of the problem
  n = 768

  allocate( D(N),F(N),U(N),V(N),TA(N,4) )

  sa = 0.1D0
  bb = 9.0D0
  gap = (bb-sa)/ (2*N)
  DO I = 1, N
      F(i) = sa + (2*I-1)*gap
      D(i) = F(i) + gap
  END DO
  call random_number(U)
  call random_number(V)
  !U = 2.0E+0
  !V = one

  NTAC = 4
  TA(1:N,1) = D
  TA(1:N,2) = F
  TA(1:N,3) = U(1:N)
  TA(1:N,4) = V(1:N)

  ! Initialize MPI
  call MPI_Init(ierr)
  call MPI_Comm_rank(MPI_COMM_WORLD,myid,ierr)
  call MPI_Comm_size(MPI_COMM_WORLD,np,ierr)

  ! Initialize the BLACS grid
  !nprow=floor(sqrt(real(np)))
  !npcol=np/nprow

  do nprow = NINT(SQRT(REAL(np))),2,-1
      if(mod(np,nprow) == 0 ) exit
  enddo
  ! at the end of the above loop, nprocs is always divisible by np_cols
   npcol = np/nprow

!  do npcol = NINT(SQRT(REAL(np))),2,-1
!      if(mod(np,npcol) == 0 ) exit
!  enddo
!  ! at the end of the above loop, nprocs is always divisible by np_cols
!   nprow = np/npcol

   call blacs_get(IZERO,IZERO,ictxt)
   call blacs_gridinit(ictxt,'R',nprow,npcol)
   call blacs_gridinfo(ictxt,nprow,npcol,myrow,mycol)

   !nb = n/nprow
   nb= 64   ! Blocksize of the 2D block-cyclic distribution
   if(myrow.eq.0 .and. mycol.eq.0) write(*,*) "n,nprow,npcol,nb=", n, nprow,npcol,nb

  ! A is a dense n x n distributed Toeplitz matrix
  if(myid<nprow*npcol) then
    locr=numroc(n,nb,myrow,IZERO,nprow)
    locc=numroc(n,nb,mycol,IZERO,npcol)
    allocate(A(locr*locc))
    A=0
    dummy=max(1,locr)
    call descinit(descA,n,n,nb,nb,IZERO,IZERO,ictxt,dummy,ierr)

!    write(*,*) 'myid=', myid, myrow, mycol, locr

    do j=1,locc
      jj=indxl2g(j,nb,mycol,IZERO,npcol)
      do i=1,locr
        ii=indxl2g(i,nb,myrow,IZERO,nprow)
        ! Cauchy-like matrix
          A(locr*(j-1)+i)= U(ii)*V(jj) / ( D(ii)-F(jj) )
      end do
    end do
  else
    call descset(descA,n,n,nb,nb,IZERO,IZERO,INONE,IONE)
  end if

  ! Set the random matrix
  if(myid<nprow*npcol) then
    locr=numroc(n,nb,myrow,IZERO,nprow)
    locc=numroc(n,nb,mycol,IZERO,npcol)
    allocate( B(locr*locc) )
    dummy=max(1,locr)
    call descinit(descXB,n,n,nb,nb,IZERO,IZERO,ictxt,dummy,ierr)
!    B=1D0
    allocate( X(locr*locc) )
    allocate( X1(locr*locc) )
  else
    call descset(descXB,n,n,nb,nb,IZERO,IZERO,INONE,IONE)
  end if

!  write(*,*) "Before call pdmma", N, nb, nprow, npcol
!  write(*,*) "myrow, mycol",myrow, mycol, locr, locc, ictxt

  call random_number(B)
  ! /* Original matrix-matrix mulitplication */
  time1 = MPI_Wtime()
  call PDGEMM( 'N','N',n,n,n,one,B,1,1,descA,A,1,1,descA,zero,X1,1,1,descA )
  time1 = MPI_Wtime()-time1
  IF(myid .eq. 0) WRITE(*,*) "PDGEMM finishes and costs ", time1

  time1 = MPI_Wtime()
  call PDGEMM( 'N','N',n,n,n,one,B,1,1,descA,A,1,1,descA,zero,X1,1,1,descA )
  time1 = MPI_Wtime()-time1
  IF(myid .eq. 0) WRITE(*,*) "PDGEMM finishes and costs ", time1

!  WRITE(*,*) "PDGEMM finishes"

  !REDIST = .true.
  REDIST = .false.
  LWORK = 6*locr*locc
  allocate( WORK(LWORK) )
  ! /* Matrix-matrix multiplication */
  time2 = MPI_Wtime()
  call psmma_cauchy( n,n,one,B,1,1,desca,TA,n,zero,X,1,1,desca,WORK,REDIST )
  time2 = MPI_Wtime()-time2
  IF(myid .eq. 0) WRITE(*,*) "PDMMMA finishes and costs ", time2, time1/time2

  time2 = MPI_Wtime()
  call psmma_cauchy( n,n,one,B,1,1,desca,TA,n,zero,X,1,1,desca,WORK,REDIST )
  time2 = MPI_Wtime()-time2
  IF(myid .eq. 0) WRITE(*,*) "PDMMMA finishes and costs ", time2, time1/time2

  ! Check the accuracy of computed results
  if(myid < nprow*npcol) then
     call pdnrm2( n, xnorm0, X, 1, 1, descXB, 1)
     locr=numroc(n,nb,myrow,IZERO,nprow)
     locc=numroc(n,nb,mycol,IZERO,npcol)
     do i =1, locr*locc
       X1(i) = X1(i) -X(i)
     end do
     call pdnrm2( n, xnorm, X1, 1, 1, descXB, 1)
  end if
  if(myid .eq. 0 ) write(*,*) 'Error norm is ', xnorm/xnorm0, xnorm0

  ! Clean-up
  if( myid < nprow*npcol ) then
    deallocate(A,TA,D,F,U,V)
    deallocate(B)
    deallocate(X)
    deallocate(X1)
    deallocate(WORK)
  end if

  ! The end
  call MPI_Finalize(ierr)

end program psmma_testcauchy


subroutine SHBias(Shh, lwin, incspectra, ldata, outcspectra, save_cg, &
                  exitstatus)
!------------------------------------------------------------------------------
!
!   This subroutine will compute the expected windowed (cross-)power spectra
!   given the power spectrum of an arbitrary window, and the "known" input
!   (cross-)power spectra. Note that this routine makes the assumption that
!   the known spectra can be described as a random variable.
!
!   Calling Parameteters
!
!       IN
!           Shh         Window power spectrum.
!           lwin        Maximum spherical harmonic degree of the window.
!           incspectra  Known input (cross-)power spectrum as a function of
!                       degree.
!           ldata       Maximum degree of incspectra. Beyong this degree
!                       incspectra is assumed to be zero.
!
!       OUT
!           outcspectra Biased estimate of the windowed
!                       power spectra. Maximum degree calculated is equal
!                       to lwin + ldata, or the dimension of outcspectra.
!
!       IN, OPTIONAL
!           save_cg     If 1, the Clebsch-Gordon coefficients will be calculated
!                       and saved, and then used in all subsequent calls
!                       (if lwin and ldata are not changed).
!                       If -1, the allocated memory for these terms will be
!                       deallocated.
!
!       OPTIONAL (OUT)
!           exitstatus  If present, instead of executing a STOP when an error
!                       is encountered, the variable exitstatus will be
!                       returned describing the error.
!                       0 = No errors;
!                       1 = Improper dimensions of input array;
!                       2 = Improper bounds for input variable;
!                       3 = Error allocating memory;
!                       4 = File IO error.
!
!   Copyright (c) 2005-2019, SHTOOLS
!   All rights reserved.
!
!------------------------------------------------------------------------------
    use SHTOOLS, only: Wigner3j
    use ftypes

    implicit none

    real(dp), intent(in) :: Shh(:), incspectra(:)
    real(dp), intent(out) :: outcspectra(:)
    integer(int32), intent(in) :: lwin, ldata
    integer(int32), intent(in), optional :: save_cg
    integer(int32), intent(out), optional :: exitstatus
    integer(int32) :: l, i, j, lmax, imin, imax, astat
    real(dp) :: wig(2*lwin+ldata+1)
    real(dp), allocatable, save :: cg2(:,:,:)

!$OMP   threadprivate(cg2)

    if (present(exitstatus)) exitstatus = 0

    lmax = ldata + lwin
    outcspectra = 0.0_dp

    if (size(Shh) < lwin+1) then
        print*, "Error --- SHBias"
        print*, "SHH must be dimensioned as (LWIN+1) where LWIN is ", lwin
        print*, "Input array is dimensioned as ", size(Shh)
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    else if (size(incspectra) < ldata+1) then
        print*, "Error --- SHBias"
        print*, "INCSPECTRA must be dimensioned as (LDATA+1) " // &
                "where LDATA is ", ldata
        print*, "Input array is dimensioned as ", size(incspectra)
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    end if

    if (present(save_cg)) then
        if (save_cg /= 1 .and. save_cg /= -1 .and. save_cg /= 0) then
            print*, "Error --- SHBias"
            print*, "SAVE_CG must be 1 (to save the Clebsch-Gordon " // &
                    "coefficients), -1 (to deallocate the memory), " // &
                    "or 0 (to do nothing)."
            print*, "Input value is ", save_cg
            if (present(exitstatus)) then
                exitstatus = 2
                return
            else
                stop
            end if

        end if

    end if

    !--------------------------------------------------------------------------
    !
    !   Calculate the biased power spectrum
    !
    !--------------------------------------------------------------------------
    if (present(save_cg)) then
        if (save_cg == -1) then
            if (allocated(cg2)) deallocate(cg2)
            return

        else if (save_cg == 0) then
            do l = 0, min(lmax, size(outcspectra)-1)
                do j = 0, lwin
                    if (present(exitstatus)) then
                        call Wigner3j(wig, imin, imax, j, l, 0, 0, 0, &
                                      exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call Wigner3j(wig, imin, imax, j, l, 0, 0, 0)
                    end if

                    do i = imin, min(imax, ldata), 2
                        outcspectra(l+1) = outcspectra(l+1) + Shh(j+1) &
                                           * incspectra(i+1) * (2.0_dp*l+1.0_dp) &
                                           * wig(i-imin+1)**2
                    end do

                end do

            end do

            return

        end if

        if (allocated(cg2) .and. (size(cg2(:,1,1)) /= lmax+1 .or. &
                size(cg2(1,:,1)) /= lwin+1 .or. &
                size(cg2(1,1,:)) /= lmax+lwin+1) ) deallocate(cg2)

        if (.not. allocated(cg2)) then
            allocate (cg2(lmax+1, lwin+1, lmax+lwin+1), stat = astat)

            if (astat /= 0) then
                print*, "Error --- SHBias"
                print*, "Problem allocating internal array CG2"
                if (present(exitstatus)) then
                    exitstatus = 3
                    return
                else
                    stop
                end if

            end if

            cg2 = 0.0_dp

            do l = 0, lmax
                do j = 0, lwin
                    if (present(exitstatus)) then
                        call Wigner3j(wig, imin, imax, j, l, 0, 0, 0, &
                                      exitstatus = exitstatus)
                        if (exitstatus /= 0) return
                    else
                        call Wigner3j(wig, imin, imax, j, l, 0, 0, 0)
                    end if

                    cg2(l+1,j+1,1:imax-imin+1) = (2.0_dp*l+1.0_dp) &
                                                 * wig(1:imax-imin+1)**2
                end do
            end do

        end if

        do l = 0, min(lmax, size(outcspectra)-1)
            do j = 0, lwin
                imin = abs(j-l)
                imax = j + l

                do i = imin, min(imax, ldata), 2
                    outcspectra(l+1) = outcspectra(l+1) + Shh(j+1) &
                                       * incspectra(i+1) &
                                       * cg2(l+1,j+1,i-imin+1)
                end do

            end do

        end do

    else
        do l = 0, min(lmax, size(outcspectra)-1)
            do j = 0, lwin
                if (present(exitstatus)) then
                    call Wigner3j(wig, imin, imax, j, l, 0, 0, 0, &
                                    exitstatus = exitstatus)
                    if (exitstatus /= 0) return
                else
                    call Wigner3j(wig, imin, imax, j, l, 0, 0, 0)
                end if

                do i = imin, min(imax, ldata), 2
                    outcspectra(l+1) = outcspectra(l+1) + Shh(j+1) &
                                       * incspectra(i+1) * (2.0_dp*l+1.0_dp) &
                                       * wig(i-imin+1)**2
                end do

            end do

        end do

    end if

end subroutine SHBias


      SUBROUTINE DGBTRF( M, N, KL, KU, AB, LDAB, IPIV, INFO )
*
*  -- LAPACK routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      INTEGER            INFO, KL, KU, LDAB, M, N
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * )
      DOUBLE PRECISION   AB( LDAB, * )
*     ..
*
*  Purpose
*  =======
*
*  DGBTRF computes an LU factorization of a real m-by-n band matrix A
*  using partial pivoting with row interchanges.
*
*  This is the blocked version of the algorithm, calling Level 3 BLAS.
*
*  Arguments
*  =========
*
*  M       (input) INTEGER
*          The number of rows of the matrix A.  M >= 0.
*
*  N       (input) INTEGER
*          The number of columns of the matrix A.  N >= 0.
*
*  KL      (input) INTEGER
*          The number of subdiagonals within the band of A.  KL >= 0.
*
*  KU      (input) INTEGER
*          The number of superdiagonals within the band of A.  KU >= 0.
*
*  AB      (input/output) DOUBLE PRECISION array, dimension (LDAB,N)
*          On entry, the matrix A in band storage, in rows KL+1 to
*          2*KL+KU+1; rows 1 to KL of the array need not be set.
*          The j-th column of A is stored in the j-th column of the
*          array AB as follows:
*          AB(kl+ku+1+i-j,j) = A(i,j) for max(1,j-ku)<=i<=min(m,j+kl)
*
*          On exit, details of the factorization: U is stored as an
*          upper triangular band matrix with KL+KU superdiagonals in
*          rows 1 to KL+KU+1, and the multipliers used during the
*          factorization are stored in rows KL+KU+2 to 2*KL+KU+1.
*          See below for further details.
*
*  LDAB    (input) INTEGER
*          The leading dimension of the array AB.  LDAB >= 2*KL+KU+1.
*
*  IPIV    (output) INTEGER array, dimension (min(M,N))
*          The pivot indices; for 1 <= i <= min(M,N), row i of the
*          matrix was interchanged with row IPIV(i).
*
*  INFO    (output) INTEGER
*          = 0: successful exit
*          < 0: if INFO = -i, the i-th argument had an illegal value
*          > 0: if INFO = +i, U(i,i) is exactly zero. The factorization
*               has been completed, but the factor U is exactly
*               singular, and division by zero will occur if it is used
*               to solve a system of equations.
*
*  Further Details
*  ===============
*
*  The band storage scheme is illustrated by the following example, when
*  M = N = 6, KL = 2, KU = 1:
*
*  On entry:                       On exit:
*
*      *    *    *    +    +    +       *    *    *   u14  u25  u36
*      *    *    +    +    +    +       *    *   u13  u24  u35  u46
*      *   a12  a23  a34  a45  a56      *   u12  u23  u34  u45  u56
*     a11  a22  a33  a44  a55  a66     u11  u22  u33  u44  u55  u66
*     a21  a32  a43  a54  a65   *      m21  m32  m43  m54  m65   *
*     a31  a42  a53  a64   *    *      m31  m42  m53  m64   *    *
*
*  Array elements marked * are not used by the routine; elements marked
*  + need not be set on entry, but are required by the routine to store
*  elements of U because of fill-in resulting from the row interchanges.
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D+0, ZERO = 0.0D+0 )
      INTEGER            NBMAX, LDWORK
      PARAMETER          ( NBMAX = 64, LDWORK = NBMAX+1 )
*     ..
*     .. Local Scalars ..
      INTEGER            I, I2, I3, II, IP, J, J2, J3, JB, JJ, JM, JP,
     $                   JU, K2, KM, KV, NB, NW
      DOUBLE PRECISION   TEMP
*     ..
*     .. Local Arrays ..
      DOUBLE PRECISION   WORK13( LDWORK, NBMAX ),
     $                   WORK31( LDWORK, NBMAX )
*     ..
*     .. External Functions ..
      INTEGER            IDAMAX, ILAENV
      EXTERNAL           IDAMAX, ILAENV
*     ..
*     .. External Subroutines ..
      EXTERNAL           DCOPY, DGBTF2, DGEMM, DGER, DLASWP, DSCAL,
     $                   DSWAP, DTRSM, XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     KV is the number of superdiagonals in the factor U, allowing for
*     fill-in
*
      KV = KU + KL
*
*     Test the input parameters.
*
      INFO = 0
      IF( M.LT.0 ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( KL.LT.0 ) THEN
         INFO = -3
      ELSE IF( KU.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDAB.LT.KL+KV+1 ) THEN
         INFO = -6
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DGBTRF', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( M.EQ.0 .OR. N.EQ.0 )
     $   RETURN
*
*     Determine the block size for this environment
*
      NB = ILAENV( 1, 'DGBTRF', ' ', M, N, KL, KU )
*
*     The block size must not exceed the limit set by the size of the
*     local arrays WORK13 and WORK31.
*
      NB = MIN( NB, NBMAX )
*
      IF( NB.LE.1 .OR. NB.GT.KL ) THEN
*
*        Use unblocked code
*
         CALL DGBTF2( M, N, KL, KU, AB, LDAB, IPIV, INFO )
      ELSE
*
*        Use blocked code
*
*        Zero the superdiagonal elements of the work array WORK13
*
         DO 20 J = 1, NB
            DO 10 I = 1, J - 1
               WORK13( I, J ) = ZERO
   10       CONTINUE
   20    CONTINUE
*
*        Zero the subdiagonal elements of the work array WORK31
*
         DO 40 J = 1, NB
            DO 30 I = J + 1, NB
               WORK31( I, J ) = ZERO
   30       CONTINUE
   40    CONTINUE
*
*        Gaussian elimination with partial pivoting
*
*        Set fill-in elements in columns KU+2 to KV to zero
*
         DO 60 J = KU + 2, MIN( KV, N )
            DO 50 I = KV - J + 2, KL
               AB( I, J ) = ZERO
   50       CONTINUE
   60    CONTINUE
*
*        JU is the index of the last column affected by the current
*        stage of the factorization
*
         JU = 1
*
         DO 180 J = 1, MIN( M, N ), NB
            JB = MIN( NB, MIN( M, N )-J+1 )
*
*           The active part of the matrix is partitioned
*
*              A11   A12   A13
*              A21   A22   A23
*              A31   A32   A33
*
*           Here A11, A21 and A31 denote the current block of JB columns
*           which is about to be factorized. The number of rows in the
*           partitioning are JB, I2, I3 respectively, and the numbers
*           of columns are JB, J2, J3. The superdiagonal elements of A13
*           and the subdiagonal elements of A31 lie outside the band.
*
            I2 = MIN( KL-JB, M-J-JB+1 )
            I3 = MIN( JB, M-J-KL+1 )
*
*           J2 and J3 are computed after JU has been updated.
*
*           Factorize the current block of JB columns
*
            DO 80 JJ = J, J + JB - 1
*
*              Set fill-in elements in column JJ+KV to zero
*
               IF( JJ+KV.LE.N ) THEN
                  DO 70 I = 1, KL
                     AB( I, JJ+KV ) = ZERO
   70             CONTINUE
               END IF
*
*              Find pivot and test for singularity. KM is the number of
*              subdiagonal elements in the current column.
*
               KM = MIN( KL, M-JJ )
               JP = IDAMAX( KM+1, AB( KV+1, JJ ), 1 )
               IPIV( JJ ) = JP + JJ - J
               IF( AB( KV+JP, JJ ).NE.ZERO ) THEN
                  JU = MAX( JU, MIN( JJ+KU+JP-1, N ) )
                  IF( JP.NE.1 ) THEN
*
*                    Apply interchange to columns J to J+JB-1
*
                     IF( JP+JJ-1.LT.J+KL ) THEN
*
                        CALL DSWAP( JB, AB( KV+1+JJ-J, J ), LDAB-1,
     $                              AB( KV+JP+JJ-J, J ), LDAB-1 )
                     ELSE
*
*                       The interchange affects columns J to JJ-1 of A31
*                       which are stored in the work array WORK31
*
                        CALL DSWAP( JJ-J, AB( KV+1+JJ-J, J ), LDAB-1,
     $                              WORK31( JP+JJ-J-KL, 1 ), LDWORK )
                        CALL DSWAP( J+JB-JJ, AB( KV+1, JJ ), LDAB-1,
     $                              AB( KV+JP, JJ ), LDAB-1 )
                     END IF
                  END IF
*
*                 Compute multipliers
*
                  CALL DSCAL( KM, ONE / AB( KV+1, JJ ), AB( KV+2, JJ ),
     $                        1 )
*
*                 Update trailing submatrix within the band and within
*                 the current block. JM is the index of the last column
*                 which needs to be updated.
*
                  JM = MIN( JU, J+JB-1 )
                  IF( JM.GT.JJ )
     $               CALL DGER( KM, JM-JJ, -ONE, AB( KV+2, JJ ), 1,
     $                          AB( KV, JJ+1 ), LDAB-1,
     $                          AB( KV+1, JJ+1 ), LDAB-1 )
               ELSE
*
*                 If pivot is zero, set INFO to the index of the pivot
*                 unless a zero pivot has already been found.
*
                  IF( INFO.EQ.0 )
     $               INFO = JJ
               END IF
*
*              Copy current column of A31 into the work array WORK31
*
               NW = MIN( JJ-J+1, I3 )
               IF( NW.GT.0 )
     $            CALL DCOPY( NW, AB( KV+KL+1-JJ+J, JJ ), 1,
     $                        WORK31( 1, JJ-J+1 ), 1 )
   80       CONTINUE
            IF( J+JB.LE.N ) THEN
*
*              Apply the row interchanges to the other blocks.
*
               J2 = MIN( JU-J+1, KV ) - JB
               J3 = MAX( 0, JU-J-KV+1 )
*
*              Use DLASWP to apply the row interchanges to A12, A22, and
*              A32.
*
               CALL DLASWP( J2, AB( KV+1-JB, J+JB ), LDAB-1, 1, JB,
     $                      IPIV( J ), 1 )
*
*              Adjust the pivot indices.
*
               DO 90 I = J, J + JB - 1
                  IPIV( I ) = IPIV( I ) + J - 1
   90          CONTINUE
*
*              Apply the row interchanges to A13, A23, and A33
*              columnwise.
*
               K2 = J - 1 + JB + J2
               DO 110 I = 1, J3
                  JJ = K2 + I
                  DO 100 II = J + I - 1, J + JB - 1
                     IP = IPIV( II )
                     IF( IP.NE.II ) THEN
                        TEMP = AB( KV+1+II-JJ, JJ )
                        AB( KV+1+II-JJ, JJ ) = AB( KV+1+IP-JJ, JJ )
                        AB( KV+1+IP-JJ, JJ ) = TEMP
                     END IF
  100             CONTINUE
  110          CONTINUE
*
*              Update the relevant part of the trailing submatrix
*
               IF( J2.GT.0 ) THEN
*
*                 Update A12
*
                  CALL DTRSM( 'Left', 'Lower', 'No transpose', 'Unit',
     $                        JB, J2, ONE, AB( KV+1, J ), LDAB-1,
     $                        AB( KV+1-JB, J+JB ), LDAB-1 )
*
                  IF( I2.GT.0 ) THEN
*
*                    Update A22
*
                     CALL DGEMM( 'No transpose', 'No transpose', I2, J2,
     $                           JB, -ONE, AB( KV+1+JB, J ), LDAB-1,
     $                           AB( KV+1-JB, J+JB ), LDAB-1, ONE,
     $                           AB( KV+1, J+JB ), LDAB-1 )
                  END IF
*
                  IF( I3.GT.0 ) THEN
*
*                    Update A32
*
                     CALL DGEMM( 'No transpose', 'No transpose', I3, J2,
     $                           JB, -ONE, WORK31, LDWORK,
     $                           AB( KV+1-JB, J+JB ), LDAB-1, ONE,
     $                           AB( KV+KL+1-JB, J+JB ), LDAB-1 )
                  END IF
               END IF
*
               IF( J3.GT.0 ) THEN
*
*                 Copy the lower triangle of A13 into the work array
*                 WORK13
*
                  DO 130 JJ = 1, J3
                     DO 120 II = JJ, JB
                        WORK13( II, JJ ) = AB( II-JJ+1, JJ+J+KV-1 )
  120                CONTINUE
  130             CONTINUE
*
*                 Update A13 in the work array
*
                  CALL DTRSM( 'Left', 'Lower', 'No transpose', 'Unit',
     $                        JB, J3, ONE, AB( KV+1, J ), LDAB-1,
     $                        WORK13, LDWORK )
*
                  IF( I2.GT.0 ) THEN
*
*                    Update A23
*
                     CALL DGEMM( 'No transpose', 'No transpose', I2, J3,
     $                           JB, -ONE, AB( KV+1+JB, J ), LDAB-1,
     $                           WORK13, LDWORK, ONE, AB( 1+JB, J+KV ),
     $                           LDAB-1 )
                  END IF
*
                  IF( I3.GT.0 ) THEN
*
*                    Update A33
*
                     CALL DGEMM( 'No transpose', 'No transpose', I3, J3,
     $                           JB, -ONE, WORK31, LDWORK, WORK13,
     $                           LDWORK, ONE, AB( 1+KL, J+KV ), LDAB-1 )
                  END IF
*
*                 Copy the lower triangle of A13 back into place
*
                  DO 150 JJ = 1, J3
                     DO 140 II = JJ, JB
                        AB( II-JJ+1, JJ+J+KV-1 ) = WORK13( II, JJ )
  140                CONTINUE
  150             CONTINUE
               END IF
            ELSE
*
*              Adjust the pivot indices.
*
               DO 160 I = J, J + JB - 1
                  IPIV( I ) = IPIV( I ) + J - 1
  160          CONTINUE
            END IF
*
*           Partially undo the interchanges in the current block to
*           restore the upper triangular form of A31 and copy the upper
*           triangle of A31 back into place
*
            DO 170 JJ = J + JB - 1, J, -1
               JP = IPIV( JJ ) - JJ + 1
               IF( JP.NE.1 ) THEN
*
*                 Apply interchange to columns J to JJ-1
*
                  IF( JP+JJ-1.LT.J+KL ) THEN
*
*                    The interchange does not affect A31
*
                     CALL DSWAP( JJ-J, AB( KV+1+JJ-J, J ), LDAB-1,
     $                           AB( KV+JP+JJ-J, J ), LDAB-1 )
                  ELSE
*
*                    The interchange does affect A31
*
                     CALL DSWAP( JJ-J, AB( KV+1+JJ-J, J ), LDAB-1,
     $                           WORK31( JP+JJ-J-KL, 1 ), LDWORK )
                  END IF
               END IF
*
*              Copy the current column of A31 back into place
*
               NW = MIN( I3, JJ-J+1 )
               IF( NW.GT.0 )
     $            CALL DCOPY( NW, WORK31( 1, JJ-J+1 ), 1,
     $                        AB( KV+KL+1-JJ+J, JJ ), 1 )
  170       CONTINUE
  180    CONTINUE
      END IF
*
      RETURN
*
*     End of DGBTRF
*
      END

      SUBROUTINE DGBCON( NORM, N, KL, KU, AB, LDAB, IPIV, ANORM, RCOND,
     $                   WORK, IWORK, INFO )
*
*  -- LAPACK routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     Modified to call DLACN2 in place of DLACON, 5 Feb 03, SJH.
*
*     .. Scalar Arguments ..
      CHARACTER          NORM
      INTEGER            INFO, KL, KU, LDAB, N
      DOUBLE PRECISION   ANORM, RCOND
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * ), IWORK( * )
      DOUBLE PRECISION   AB( LDAB, * ), WORK( * )
*     ..
*
*  Purpose
*  =======
*
*  DGBCON estimates the reciprocal of the condition number of a real
*  general band matrix A, in either the 1-norm or the infinity-norm,
*  using the LU factorization computed by DGBTRF.
*
*  An estimate is obtained for norm(inv(A)), and the reciprocal of the
*  condition number is computed as
*     RCOND = 1 / ( norm(A) * norm(inv(A)) ).
*
*  Arguments
*  =========
*
*  NORM    (input) CHARACTER*1
*          Specifies whether the 1-norm condition number or the
*          infinity-norm condition number is required:
*          = '1' or 'O':  1-norm;
*          = 'I':         Infinity-norm.
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  KL      (input) INTEGER
*          The number of subdiagonals within the band of A.  KL >= 0.
*
*  KU      (input) INTEGER
*          The number of superdiagonals within the band of A.  KU >= 0.
*
*  AB      (input) DOUBLE PRECISION array, dimension (LDAB,N)
*          Details of the LU factorization of the band matrix A, as
*          computed by DGBTRF.  U is stored as an upper triangular band
*          matrix with KL+KU superdiagonals in rows 1 to KL+KU+1, and
*          the multipliers used during the factorization are stored in
*          rows KL+KU+2 to 2*KL+KU+1.
*
*  LDAB    (input) INTEGER
*          The leading dimension of the array AB.  LDAB >= 2*KL+KU+1.
*
*  IPIV    (input) INTEGER array, dimension (N)
*          The pivot indices; for 1 <= i <= N, row i of the matrix was
*          interchanged with row IPIV(i).
*
*  ANORM   (input) DOUBLE PRECISION
*          If NORM = '1' or 'O', the 1-norm of the original matrix A.
*          If NORM = 'I', the infinity-norm of the original matrix A.
*
*  RCOND   (output) DOUBLE PRECISION
*          The reciprocal of the condition number of the matrix A,
*          computed as RCOND = 1/(norm(A) * norm(inv(A))).
*
*  WORK    (workspace) DOUBLE PRECISION array, dimension (3*N)
*
*  IWORK   (workspace) INTEGER array, dimension (N)
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0: if INFO = -i, the i-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D+0, ZERO = 0.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            LNOTI, ONENRM
      CHARACTER          NORMIN
      INTEGER            IX, J, JP, KASE, KASE1, KD, LM
      DOUBLE PRECISION   AINVNM, SCALE, SMLNUM, T
*     ..
*     .. Local Arrays ..
      INTEGER            ISAVE( 3 )
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      INTEGER            IDAMAX
      DOUBLE PRECISION   DDOT, DLAMCH
      EXTERNAL           LSAME, IDAMAX, DDOT, DLAMCH
*     ..
*     .. External Subroutines ..
      EXTERNAL           DAXPY, DLACN2, DLATBS, DRSCL, XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      ONENRM = NORM.EQ.'1' .OR. LSAME( NORM, 'O' )
      IF( .NOT.ONENRM .AND. .NOT.LSAME( NORM, 'I' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( KL.LT.0 ) THEN
         INFO = -3
      ELSE IF( KU.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDAB.LT.2*KL+KU+1 ) THEN
         INFO = -6
      ELSE IF( ANORM.LT.ZERO ) THEN
         INFO = -8
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DGBCON', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      RCOND = ZERO
      IF( N.EQ.0 ) THEN
         RCOND = ONE
         RETURN
      ELSE IF( ANORM.EQ.ZERO ) THEN
         RETURN
      END IF
*
      SMLNUM = DLAMCH( 'Safe minimum' )
*
*     Estimate the norm of inv(A).
*
      AINVNM = ZERO
      NORMIN = 'N'
      IF( ONENRM ) THEN
         KASE1 = 1
      ELSE
         KASE1 = 2
      END IF
      KD = KL + KU + 1
      LNOTI = KL.GT.0
      KASE = 0
   10 CONTINUE
      CALL DLACN2( N, WORK( N+1 ), WORK, IWORK, AINVNM, KASE, ISAVE )
      IF( KASE.NE.0 ) THEN
         IF( KASE.EQ.KASE1 ) THEN
*
*           Multiply by inv(L).
*
            IF( LNOTI ) THEN
               DO 20 J = 1, N - 1
                  LM = MIN( KL, N-J )
                  JP = IPIV( J )
                  T = WORK( JP )
                  IF( JP.NE.J ) THEN
                     WORK( JP ) = WORK( J )
                     WORK( J ) = T
                  END IF
                  CALL DAXPY( LM, -T, AB( KD+1, J ), 1, WORK( J+1 ), 1 )
   20          CONTINUE
            END IF
*
*           Multiply by inv(U).
*
            CALL DLATBS( 'Upper', 'No transpose', 'Non-unit', NORMIN, N,
     $                   KL+KU, AB, LDAB, WORK, SCALE, WORK( 2*N+1 ),
     $                   INFO )
         ELSE
*
*           Multiply by inv(U').
*
            CALL DLATBS( 'Upper', 'Transpose', 'Non-unit', NORMIN, N,
     $                   KL+KU, AB, LDAB, WORK, SCALE, WORK( 2*N+1 ),
     $                   INFO )
*
*           Multiply by inv(L').
*
            IF( LNOTI ) THEN
               DO 30 J = N - 1, 1, -1
                  LM = MIN( KL, N-J )
                  WORK( J ) = WORK( J ) - DDOT( LM, AB( KD+1, J ), 1,
     $                        WORK( J+1 ), 1 )
                  JP = IPIV( J )
                  IF( JP.NE.J ) THEN
                     T = WORK( JP )
                     WORK( JP ) = WORK( J )
                     WORK( J ) = T
                  END IF
   30          CONTINUE
            END IF
         END IF
*
*        Divide X by 1/SCALE if doing so will not cause overflow.
*
         NORMIN = 'Y'
         IF( SCALE.NE.ONE ) THEN
            IX = IDAMAX( N, WORK, 1 )
            IF( SCALE.LT.ABS( WORK( IX ) )*SMLNUM .OR. SCALE.EQ.ZERO )
     $         GO TO 40
            CALL DRSCL( N, SCALE, WORK, 1 )
         END IF
         GO TO 10
      END IF
*
*     Compute the estimate of the reciprocal condition number.
*
      IF( AINVNM.NE.ZERO )
     $   RCOND = ( ONE / AINVNM ) / ANORM
*
   40 CONTINUE
      RETURN
*
*     End of DGBCON
*
      END

      SUBROUTINE DGBTRS( TRANS, N, KL, KU, NRHS, AB, LDAB, IPIV, B, LDB,
     $                   INFO )
*
*  -- LAPACK routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          TRANS
      INTEGER            INFO, KL, KU, LDAB, LDB, N, NRHS
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * )
      DOUBLE PRECISION   AB( LDAB, * ), B( LDB, * )
*     ..
*
*  Purpose
*  =======
*
*  DGBTRS solves a system of linear equations
*     A * X = B  or  A' * X = B
*  with a general band matrix A using the LU factorization computed
*  by DGBTRF.
*
*  Arguments
*  =========
*
*  TRANS   (input) CHARACTER*1
*          Specifies the form of the system of equations.
*          = 'N':  A * X = B  (No transpose)
*          = 'T':  A'* X = B  (Transpose)
*          = 'C':  A'* X = B  (Conjugate transpose = Transpose)
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  KL      (input) INTEGER
*          The number of subdiagonals within the band of A.  KL >= 0.
*
*  KU      (input) INTEGER
*          The number of superdiagonals within the band of A.  KU >= 0.
*
*  NRHS    (input) INTEGER
*          The number of right hand sides, i.e., the number of columns
*          of the matrix B.  NRHS >= 0.
*
*  AB      (input) DOUBLE PRECISION array, dimension (LDAB,N)
*          Details of the LU factorization of the band matrix A, as
*          computed by DGBTRF.  U is stored as an upper triangular band
*          matrix with KL+KU superdiagonals in rows 1 to KL+KU+1, and
*          the multipliers used during the factorization are stored in
*          rows KL+KU+2 to 2*KL+KU+1.
*
*  LDAB    (input) INTEGER
*          The leading dimension of the array AB.  LDAB >= 2*KL+KU+1.
*
*  IPIV    (input) INTEGER array, dimension (N)
*          The pivot indices; for 1 <= i <= N, row i of the matrix was
*          interchanged with row IPIV(i).
*
*  B       (input/output) DOUBLE PRECISION array, dimension (LDB,NRHS)
*          On entry, the right hand side matrix B.
*          On exit, the solution matrix X.
*
*  LDB     (input) INTEGER
*          The leading dimension of the array B.  LDB >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0: if INFO = -i, the i-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            LNOTI, NOTRAN
      INTEGER            I, J, KD, L, LM
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*     ..
*     .. External Subroutines ..
      EXTERNAL           DGEMV, DGER, DSWAP, DTBSV, XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      NOTRAN = LSAME( TRANS, 'N' )
      IF( .NOT.NOTRAN .AND. .NOT.LSAME( TRANS, 'T' ) .AND. .NOT.
     $    LSAME( TRANS, 'C' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( KL.LT.0 ) THEN
         INFO = -3
      ELSE IF( KU.LT.0 ) THEN
         INFO = -4
      ELSE IF( NRHS.LT.0 ) THEN
         INFO = -5
      ELSE IF( LDAB.LT.( 2*KL+KU+1 ) ) THEN
         INFO = -7
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -10
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DGBTRS', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 .OR. NRHS.EQ.0 )
     $   RETURN
*
      KD = KU + KL + 1
      LNOTI = KL.GT.0
*
      IF( NOTRAN ) THEN
*
*        Solve  A*X = B.
*
*        Solve L*X = B, overwriting B with X.
*
*        L is represented as a product of permutations and unit lower
*        triangular matrices L = P(1) * L(1) * ... * P(n-1) * L(n-1),
*        where each transformation L(i) is a rank-one modification of
*        the identity matrix.
*
         IF( LNOTI ) THEN
            DO 10 J = 1, N - 1
               LM = MIN( KL, N-J )
               L = IPIV( J )
               IF( L.NE.J )
     $            CALL DSWAP( NRHS, B( L, 1 ), LDB, B( J, 1 ), LDB )
               CALL DGER( LM, NRHS, -ONE, AB( KD+1, J ), 1, B( J, 1 ),
     $                    LDB, B( J+1, 1 ), LDB )
   10       CONTINUE
         END IF
*
         DO 20 I = 1, NRHS
*
*           Solve U*X = B, overwriting B with X.
*
            CALL DTBSV( 'Upper', 'No transpose', 'Non-unit', N, KL+KU,
     $                  AB, LDAB, B( 1, I ), 1 )
   20    CONTINUE
*
      ELSE
*
*        Solve A'*X = B.
*
         DO 30 I = 1, NRHS
*
*           Solve U'*X = B, overwriting B with X.
*
            CALL DTBSV( 'Upper', 'Transpose', 'Non-unit', N, KL+KU, AB,
     $                  LDAB, B( 1, I ), 1 )
   30    CONTINUE
*
*        Solve L'*X = B, overwriting B with X.
*
         IF( LNOTI ) THEN
            DO 40 J = N - 1, 1, -1
               LM = MIN( KL, N-J )
               CALL DGEMV( 'Transpose', LM, NRHS, -ONE, B( J+1, 1 ),
     $                     LDB, AB( KD+1, J ), 1, ONE, B( J, 1 ), LDB )
               L = IPIV( J )
               IF( L.NE.J )
     $            CALL DSWAP( NRHS, B( L, 1 ), LDB, B( J, 1 ), LDB )
   40       CONTINUE
         END IF
      END IF
      RETURN
*
*     End of DGBTRS
*
      END

      SUBROUTINE DGECON( NORM, N, A, LDA, ANORM, RCOND, WORK, IWORK,
     $                   INFO )
*
*  -- LAPACK routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     Modified to call DLACN2 in place of DLACON, 5 Feb 03, SJH.
*
*     .. Scalar Arguments ..
      CHARACTER          NORM
      INTEGER            INFO, LDA, N
      DOUBLE PRECISION   ANORM, RCOND
*     ..
*     .. Array Arguments ..
      INTEGER            IWORK( * )
      DOUBLE PRECISION   A( LDA, * ), WORK( * )
*     ..
*
*  Purpose
*  =======
*
*  DGECON estimates the reciprocal of the condition number of a general
*  real matrix A, in either the 1-norm or the infinity-norm, using
*  the LU factorization computed by DGETRF.
*
*  An estimate is obtained for norm(inv(A)), and the reciprocal of the
*  condition number is computed as
*     RCOND = 1 / ( norm(A) * norm(inv(A)) ).
*
*  Arguments
*  =========
*
*  NORM    (input) CHARACTER*1
*          Specifies whether the 1-norm condition number or the
*          infinity-norm condition number is required:
*          = '1' or 'O':  1-norm;
*          = 'I':         Infinity-norm.
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  A       (input) DOUBLE PRECISION array, dimension (LDA,N)
*          The factors L and U from the factorization A = P*L*U
*          as computed by DGETRF.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  ANORM   (input) DOUBLE PRECISION
*          If NORM = '1' or 'O', the 1-norm of the original matrix A.
*          If NORM = 'I', the infinity-norm of the original matrix A.
*
*  RCOND   (output) DOUBLE PRECISION
*          The reciprocal of the condition number of the matrix A,
*          computed as RCOND = 1/(norm(A) * norm(inv(A))).
*
*  WORK    (workspace) DOUBLE PRECISION array, dimension (4*N)
*
*  IWORK   (workspace) INTEGER array, dimension (N)
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D+0, ZERO = 0.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            ONENRM
      CHARACTER          NORMIN
      INTEGER            IX, KASE, KASE1
      DOUBLE PRECISION   AINVNM, SCALE, SL, SMLNUM, SU
*     ..
*     .. Local Arrays ..
      INTEGER            ISAVE( 3 )
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      INTEGER            IDAMAX
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           LSAME, IDAMAX, DLAMCH
*     ..
*     .. External Subroutines ..
      EXTERNAL           DLACN2, DLATRS, DRSCL, XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      ONENRM = NORM.EQ.'1' .OR. LSAME( NORM, 'O' )
      IF( .NOT.ONENRM .AND. .NOT.LSAME( NORM, 'I' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -4
      ELSE IF( ANORM.LT.ZERO ) THEN
         INFO = -5
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DGECON', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      RCOND = ZERO
      IF( N.EQ.0 ) THEN
         RCOND = ONE
         RETURN
      ELSE IF( ANORM.EQ.ZERO ) THEN
         RETURN
      END IF
*
      SMLNUM = DLAMCH( 'Safe minimum' )
*
*     Estimate the norm of inv(A).
*
      AINVNM = ZERO
      NORMIN = 'N'
      IF( ONENRM ) THEN
         KASE1 = 1
      ELSE
         KASE1 = 2
      END IF
      KASE = 0
   10 CONTINUE
      CALL DLACN2( N, WORK( N+1 ), WORK, IWORK, AINVNM, KASE, ISAVE )
      IF( KASE.NE.0 ) THEN
         IF( KASE.EQ.KASE1 ) THEN
*
*           Multiply by inv(L).
*
            CALL DLATRS( 'Lower', 'No transpose', 'Unit', NORMIN, N, A,
     $                   LDA, WORK, SL, WORK( 2*N+1 ), INFO )
*
*           Multiply by inv(U).
*
            CALL DLATRS( 'Upper', 'No transpose', 'Non-unit', NORMIN, N,
     $                   A, LDA, WORK, SU, WORK( 3*N+1 ), INFO )
         ELSE
*
*           Multiply by inv(U').
*
            CALL DLATRS( 'Upper', 'Transpose', 'Non-unit', NORMIN, N, A,
     $                   LDA, WORK, SU, WORK( 3*N+1 ), INFO )
*
*           Multiply by inv(L').
*
            CALL DLATRS( 'Lower', 'Transpose', 'Unit', NORMIN, N, A,
     $                   LDA, WORK, SL, WORK( 2*N+1 ), INFO )
         END IF
*
*        Divide X by 1/(SL*SU) if doing so will not cause overflow.
*
         SCALE = SL*SU
         NORMIN = 'Y'
         IF( SCALE.NE.ONE ) THEN
            IX = IDAMAX( N, WORK, 1 )
            IF( SCALE.LT.ABS( WORK( IX ) )*SMLNUM .OR. SCALE.EQ.ZERO )
     $         GO TO 20
            CALL DRSCL( N, SCALE, WORK, 1 )
         END IF
         GO TO 10
      END IF
*
*     Compute the estimate of the reciprocal condition number.
*
      IF( AINVNM.NE.ZERO )
     $   RCOND = ( ONE / AINVNM ) / ANORM
*
   20 CONTINUE
      RETURN
*
*     End of DGECON
*
      END

      SUBROUTINE DGETRF( M, N, A, LDA, IPIV, INFO )
*
*  -- LAPACK routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      INTEGER            INFO, LDA, M, N
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * )
      DOUBLE PRECISION   A( LDA, * )
*     ..
*
*  Purpose
*  =======
*
*  DGETRF computes an LU factorization of a general M-by-N matrix A
*  using partial pivoting with row interchanges.
*
*  The factorization has the form
*     A = P * L * U
*  where P is a permutation matrix, L is lower triangular with unit
*  diagonal elements (lower trapezoidal if m > n), and U is upper
*  triangular (upper trapezoidal if m < n).
*
*  This is the right-looking Level 3 BLAS version of the algorithm.
*
*  Arguments
*  =========
*
*  M       (input) INTEGER
*          The number of rows of the matrix A.  M >= 0.
*
*  N       (input) INTEGER
*          The number of columns of the matrix A.  N >= 0.
*
*  A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
*          On entry, the M-by-N matrix to be factored.
*          On exit, the factors L and U from the factorization
*          A = P*L*U; the unit diagonal elements of L are not stored.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,M).
*
*  IPIV    (output) INTEGER array, dimension (min(M,N))
*          The pivot indices; for 1 <= i <= min(M,N), row i of the
*          matrix was interchanged with row IPIV(i).
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*          > 0:  if INFO = i, U(i,i) is exactly zero. The factorization
*                has been completed, but the factor U is exactly
*                singular, and division by zero will occur if it is used
*                to solve a system of equations.
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      INTEGER            I, IINFO, J, JB, NB
*     ..
*     .. External Subroutines ..
      EXTERNAL           DGEMM, DGETF2, DLASWP, DTRSM, XERBLA
*     ..
*     .. External Functions ..
      INTEGER            ILAENV
      EXTERNAL           ILAENV
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX, MIN
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      IF( M.LT.0 ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( LDA.LT.MAX( 1, M ) ) THEN
         INFO = -4
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DGETRF', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( M.EQ.0 .OR. N.EQ.0 )
     $   RETURN
*
*     Determine the block size for this environment.
*
      NB = ILAENV( 1, 'DGETRF', ' ', M, N, -1, -1 )
      IF( NB.LE.1 .OR. NB.GE.MIN( M, N ) ) THEN
*
*        Use unblocked code.
*
         CALL DGETF2( M, N, A, LDA, IPIV, INFO )
      ELSE
*
*        Use blocked code.
*
         DO 20 J = 1, MIN( M, N ), NB
            JB = MIN( MIN( M, N )-J+1, NB )
*
*           Factor diagonal and subdiagonal blocks and test for exact
*           singularity.
*
            CALL DGETF2( M-J+1, JB, A( J, J ), LDA, IPIV( J ), IINFO )
*
*           Adjust INFO and the pivot indices.
*
            IF( INFO.EQ.0 .AND. IINFO.GT.0 )
     $         INFO = IINFO + J - 1
            DO 10 I = J, MIN( M, J+JB-1 )
               IPIV( I ) = J - 1 + IPIV( I )
   10       CONTINUE
*
*           Apply interchanges to columns 1:J-1.
*
            CALL DLASWP( J-1, A, LDA, J, J+JB-1, IPIV, 1 )
*
            IF( J+JB.LE.N ) THEN
*
*              Apply interchanges to columns J+JB:N.
*
               CALL DLASWP( N-J-JB+1, A( 1, J+JB ), LDA, J, J+JB-1,
     $                      IPIV, 1 )
*
*              Compute block row of U.
*
               CALL DTRSM( 'Left', 'Lower', 'No transpose', 'Unit', JB,
     $                     N-J-JB+1, ONE, A( J, J ), LDA, A( J, J+JB ),
     $                     LDA )
               IF( J+JB.LE.M ) THEN
*
*                 Update trailing submatrix.
*
                  CALL DGEMM( 'No transpose', 'No transpose', M-J-JB+1,
     $                        N-J-JB+1, JB, -ONE, A( J+JB, J ), LDA,
     $                        A( J, J+JB ), LDA, ONE, A( J+JB, J+JB ),
     $                        LDA )
               END IF
            END IF
   20    CONTINUE
      END IF
      RETURN
*
*     End of DGETRF
*
      END

      SUBROUTINE DGETRS( TRANS, N, NRHS, A, LDA, IPIV, B, LDB, INFO )
*
*  -- LAPACK routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          TRANS
      INTEGER            INFO, LDA, LDB, N, NRHS
*     ..
*     .. Array Arguments ..
      INTEGER            IPIV( * )
      DOUBLE PRECISION   A( LDA, * ), B( LDB, * )
*     ..
*
*  Purpose
*  =======
*
*  DGETRS solves a system of linear equations
*     A * X = B  or  A' * X = B
*  with a general N-by-N matrix A using the LU factorization computed
*  by DGETRF.
*
*  Arguments
*  =========
*
*  TRANS   (input) CHARACTER*1
*          Specifies the form of the system of equations:
*          = 'N':  A * X = B  (No transpose)
*          = 'T':  A'* X = B  (Transpose)
*          = 'C':  A'* X = B  (Conjugate transpose = Transpose)
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.
*
*  NRHS    (input) INTEGER
*          The number of right hand sides, i.e., the number of columns
*          of the matrix B.  NRHS >= 0.
*
*  A       (input) DOUBLE PRECISION array, dimension (LDA,N)
*          The factors L and U from the factorization A = P*L*U
*          as computed by DGETRF.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(1,N).
*
*  IPIV    (input) INTEGER array, dimension (N)
*          The pivot indices from DGETRF; for 1<=i<=N, row i of the
*          matrix was interchanged with row IPIV(i).
*
*  B       (input/output) DOUBLE PRECISION array, dimension (LDB,NRHS)
*          On entry, the right hand side matrix B.
*          On exit, the solution matrix X.
*
*  LDB     (input) INTEGER
*          The leading dimension of the array B.  LDB >= max(1,N).
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE
      PARAMETER          ( ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            NOTRAN
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*     ..
*     .. External Subroutines ..
      EXTERNAL           DLASWP, DTRSM, XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          MAX
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters.
*
      INFO = 0
      NOTRAN = LSAME( TRANS, 'N' )
      IF( .NOT.NOTRAN .AND. .NOT.LSAME( TRANS, 'T' ) .AND. .NOT.
     $    LSAME( TRANS, 'C' ) ) THEN
         INFO = -1
      ELSE IF( N.LT.0 ) THEN
         INFO = -2
      ELSE IF( NRHS.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -5
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -8
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DGETRS', -INFO )
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 .OR. NRHS.EQ.0 )
     $   RETURN
*
      IF( NOTRAN ) THEN
*
*        Solve A * X = B.
*
*        Apply row interchanges to the right hand sides.
*
         CALL DLASWP( NRHS, B, LDB, 1, N, IPIV, 1 )
*
*        Solve L*X = B, overwriting B with X.
*
         CALL DTRSM( 'Left', 'Lower', 'No transpose', 'Unit', N, NRHS,
     $               ONE, A, LDA, B, LDB )
*
*        Solve U*X = B, overwriting B with X.
*
         CALL DTRSM( 'Left', 'Upper', 'No transpose', 'Non-unit', N,
     $               NRHS, ONE, A, LDA, B, LDB )
      ELSE
*
*        Solve A' * X = B.
*
*        Solve U'*X = B, overwriting B with X.
*
         CALL DTRSM( 'Left', 'Upper', 'Transpose', 'Non-unit', N, NRHS,
     $               ONE, A, LDA, B, LDB )
*
*        Solve L'*X = B, overwriting B with X.
*
         CALL DTRSM( 'Left', 'Lower', 'Transpose', 'Unit', N, NRHS, ONE,
     $               A, LDA, B, LDB )
*
*        Apply row interchanges to the solution vectors.
*
         CALL DLASWP( NRHS, B, LDB, 1, N, IPIV, -1 )
      END IF
*
      RETURN
*
*     End of DGETRS
*
      END

      DOUBLE PRECISION FUNCTION DLANGB( NORM, N, KL, KU, AB, LDAB,
     $                 WORK )
*
*  -- LAPACK auxiliary routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          NORM
      INTEGER            KL, KU, LDAB, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   AB( LDAB, * ), WORK( * )
*     ..
*
*  Purpose
*  =======
*
*  DLANGB  returns the value of the one norm,  or the Frobenius norm, or
*  the  infinity norm,  or the element of  largest absolute value  of an
*  n by n band matrix  A,  with kl sub-diagonals and ku super-diagonals.
*
*  Description
*  ===========
*
*  DLANGB returns the value
*
*     DLANGB = ( max(abs(A(i,j))), NORM = 'M' or 'm'
*              (
*              ( norm1(A),         NORM = '1', 'O' or 'o'
*              (
*              ( normI(A),         NORM = 'I' or 'i'
*              (
*              ( normF(A),         NORM = 'F', 'f', 'E' or 'e'
*
*  where  norm1  denotes the  one norm of a matrix (maximum column sum),
*  normI  denotes the  infinity norm  of a matrix  (maximum row sum) and
*  normF  denotes the  Frobenius norm of a matrix (square root of sum of
*  squares).  Note that  max(abs(A(i,j)))  is not a consistent matrix norm.
*
*  Arguments
*  =========
*
*  NORM    (input) CHARACTER*1
*          Specifies the value to be returned in DLANGB as described
*          above.
*
*  N       (input) INTEGER
*          The order of the matrix A.  N >= 0.  When N = 0, DLANGB is
*          set to zero.
*
*  KL      (input) INTEGER
*          The number of sub-diagonals of the matrix A.  KL >= 0.
*
*  KU      (input) INTEGER
*          The number of super-diagonals of the matrix A.  KU >= 0.
*
*  AB      (input) DOUBLE PRECISION array, dimension (LDAB,N)
*          The band matrix A, stored in rows 1 to KL+KU+1.  The j-th
*          column of A is stored in the j-th column of the array AB as
*          follows:
*          AB(ku+1+i-j,j) = A(i,j) for max(1,j-ku)<=i<=min(n,j+kl).
*
*  LDAB    (input) INTEGER
*          The leading dimension of the array AB.  LDAB >= KL+KU+1.
*
*  WORK    (workspace) DOUBLE PRECISION array, dimension (MAX(1,LWORK)),
*          where LWORK >= N when NORM = 'I'; otherwise, WORK is not
*          referenced.
*
* =====================================================================
*
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D+0, ZERO = 0.0D+0 )
*     ..
*     .. Local Scalars ..
      INTEGER            I, J, K, L
      DOUBLE PRECISION   SCALE, SUM, VALUE
*     ..
*     .. External Subroutines ..
      EXTERNAL           DLASSQ
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX, MIN, SQRT
*     ..
*     .. Executable Statements ..
*
      IF( N.EQ.0 ) THEN
         VALUE = ZERO
      ELSE IF( LSAME( NORM, 'M' ) ) THEN
*
*        Find max(abs(A(i,j))).
*
         VALUE = ZERO
         DO 20 J = 1, N
            DO 10 I = MAX( KU+2-J, 1 ), MIN( N+KU+1-J, KL+KU+1 )
               VALUE = MAX( VALUE, ABS( AB( I, J ) ) )
   10       CONTINUE
   20    CONTINUE
      ELSE IF( ( LSAME( NORM, 'O' ) ) .OR. ( NORM.EQ.'1' ) ) THEN
*
*        Find norm1(A).
*
         VALUE = ZERO
         DO 40 J = 1, N
            SUM = ZERO
            DO 30 I = MAX( KU+2-J, 1 ), MIN( N+KU+1-J, KL+KU+1 )
               SUM = SUM + ABS( AB( I, J ) )
   30       CONTINUE
            VALUE = MAX( VALUE, SUM )
   40    CONTINUE
      ELSE IF( LSAME( NORM, 'I' ) ) THEN
*
*        Find normI(A).
*
         DO 50 I = 1, N
            WORK( I ) = ZERO
   50    CONTINUE
         DO 70 J = 1, N
            K = KU + 1 - J
            DO 60 I = MAX( 1, J-KU ), MIN( N, J+KL )
               WORK( I ) = WORK( I ) + ABS( AB( K+I, J ) )
   60       CONTINUE
   70    CONTINUE
         VALUE = ZERO
         DO 80 I = 1, N
            VALUE = MAX( VALUE, WORK( I ) )
   80    CONTINUE
      ELSE IF( ( LSAME( NORM, 'F' ) ) .OR. ( LSAME( NORM, 'E' ) ) ) THEN
*
*        Find normF(A).
*
         SCALE = ZERO
         SUM = ONE
         DO 90 J = 1, N
            L = MAX( 1, J-KU )
            K = KU + 1 - J + L
            CALL DLASSQ( MIN( N, J+KL )-L+1, AB( K, J ), 1, SCALE, SUM )
   90    CONTINUE
         VALUE = SCALE*SQRT( SUM )
      END IF
*
      DLANGB = VALUE
      RETURN
*
*     End of DLANGB
*
      END

      DOUBLE PRECISION FUNCTION DLANGE( NORM, M, N, A, LDA, WORK )
*
*  -- LAPACK auxiliary routine (version 3.1) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          NORM
      INTEGER            LDA, M, N
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   A( LDA, * ), WORK( * )
*     ..
*
*  Purpose
*  =======
*
*  DLANGE  returns the value of the one norm,  or the Frobenius norm, or
*  the  infinity norm,  or the  element of  largest absolute value  of a
*  real matrix A.
*
*  Description
*  ===========
*
*  DLANGE returns the value
*
*     DLANGE = ( max(abs(A(i,j))), NORM = 'M' or 'm'
*              (
*              ( norm1(A),         NORM = '1', 'O' or 'o'
*              (
*              ( normI(A),         NORM = 'I' or 'i'
*              (
*              ( normF(A),         NORM = 'F', 'f', 'E' or 'e'
*
*  where  norm1  denotes the  one norm of a matrix (maximum column sum),
*  normI  denotes the  infinity norm  of a matrix  (maximum row sum) and
*  normF  denotes the  Frobenius norm of a matrix (square root of sum of
*  squares).  Note that  max(abs(A(i,j)))  is not a consistent matrix norm.
*
*  Arguments
*  =========
*
*  NORM    (input) CHARACTER*1
*          Specifies the value to be returned in DLANGE as described
*          above.
*
*  M       (input) INTEGER
*          The number of rows of the matrix A.  M >= 0.  When M = 0,
*          DLANGE is set to zero.
*
*  N       (input) INTEGER
*          The number of columns of the matrix A.  N >= 0.  When N = 0,
*          DLANGE is set to zero.
*
*  A       (input) DOUBLE PRECISION array, dimension (LDA,N)
*          The m by n matrix A.
*
*  LDA     (input) INTEGER
*          The leading dimension of the array A.  LDA >= max(M,1).
*
*  WORK    (workspace) DOUBLE PRECISION array, dimension (MAX(1,LWORK)),
*          where LWORK >= M when NORM = 'I'; otherwise, WORK is not
*          referenced.
*
* =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ONE, ZERO
      PARAMETER          ( ONE = 1.0D+0, ZERO = 0.0D+0 )
*     ..
*     .. Local Scalars ..
      INTEGER            I, J
      DOUBLE PRECISION   SCALE, SUM, VALUE
*     ..
*     .. External Subroutines ..
      EXTERNAL           DLASSQ
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      EXTERNAL           LSAME
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX, MIN, SQRT
*     ..
*     .. Executable Statements ..
*
      IF( MIN( M, N ).EQ.0 ) THEN
         VALUE = ZERO
      ELSE IF( LSAME( NORM, 'M' ) ) THEN
*
*        Find max(abs(A(i,j))).
*
         VALUE = ZERO
         DO 20 J = 1, N
            DO 10 I = 1, M
               VALUE = MAX( VALUE, ABS( A( I, J ) ) )
   10       CONTINUE
   20    CONTINUE
      ELSE IF( ( LSAME( NORM, 'O' ) ) .OR. ( NORM.EQ.'1' ) ) THEN
*
*        Find norm1(A).
*
         VALUE = ZERO
         DO 40 J = 1, N
            SUM = ZERO
            DO 30 I = 1, M
               SUM = SUM + ABS( A( I, J ) )
   30       CONTINUE
            VALUE = MAX( VALUE, SUM )
   40    CONTINUE
      ELSE IF( LSAME( NORM, 'I' ) ) THEN
*
*        Find normI(A).
*
         DO 50 I = 1, M
            WORK( I ) = ZERO
   50    CONTINUE
         DO 70 J = 1, N
            DO 60 I = 1, M
               WORK( I ) = WORK( I ) + ABS( A( I, J ) )
   60       CONTINUE
   70    CONTINUE
         VALUE = ZERO
         DO 80 I = 1, M
            VALUE = MAX( VALUE, WORK( I ) )
   80    CONTINUE
      ELSE IF( ( LSAME( NORM, 'F' ) ) .OR. ( LSAME( NORM, 'E' ) ) ) THEN
*
*        Find normF(A).
*
         SCALE = ZERO
         SUM = ONE
         DO 90 J = 1, N
            CALL DLASSQ( M, A( 1, J ), 1, SCALE, SUM )
   90    CONTINUE
         VALUE = SCALE*SQRT( SUM )
      END IF
*
      DLANGE = VALUE
      RETURN
*
*     End of DLANGE
*
      END


subroutine SHAdmitCorr(gilm, tilm, lmax, admit, corr, admit_error, exitstatus)
!------------------------------------------------------------------------------
!
!   This subroutine will compute the admittance and correlation between the two
!   input spherical harmonic coefficients. The optional "error" on the
!   admittance estimates assumes that the two fields are perfectly correlated,
!   which may not necessarily true for certain models (such as Forsyth's
!   loading model).
!
!   Assuming that the two input fields are related by
!
!   gilm = Z tilm + N
!
!   where N is noise, the admittance is calcalated as
!
!       Z = < gilm tilm > / < tilm tilm >,
!
!   the correlation is
!
!       correlation = < gilm tilm > / sqrt(< gilm gilm > < tilm tilm >).
!
!   Assuming that the two fields are perfectly correlated, and that the lack of
!   coherence is a result of noise, the uncertainty on the admittance can be
!   calculaed from
!
!       sigma^2 = ( < gilm gilm > / < tilm tilm > ) ( 1 - gamma^2) / (2 l).
!
!   < > signifies an average of over all m. Since only ratios are being used,
!   < > is implemented only as sums over all m (i.e., by the cross spectra of
!   the fields). Note that the correlation can possess values between -1 and 1,
!   in contrast to the "coherence" (or coherence-squared; the terminology is
!   ambiguous) which only possesses values between 0 and 1.
!
!   Note that in order for the magnitude of the admittance to be correct, the
!   input coefficients must be multiplied by all constants. For instance, in the
!   case of gravity-topography admittances, the potential coefficients must
!   first be multiplied by GM (l+1) (R/r)**(l+2) where R is the refernce radius
!   of the coefficients, and r is the radius at which the field is evaluated.
!
!   Calling Parameters
!
!       IN
!           gilm        First input spherical harmonic coefficients.
!           tilm        Second input spherical harmonic coefficients.
!           lmax        Maximum spherical harmonic degree of the input fields.
!
!       OUT
!           admit       Admittance spectra (length lmax+1).
!           corr        Correlation spectra (length lmax+1).
!
!       OPTIONAL
!           admit_error Addittance error spectra (length lmax+1)
!                       assuming that the two fields perfectly correlated in
!                       the abscence of noise.
!
!       OPTIONAL (OUT)
!           exitstatus  If present, instead of executing a STOP when an error
!                       is encountered, the variable exitstatus will be
!                       returned describing the error.
!                       0 = No errors;
!                       1 = Improper dimensions of input array;
!                       2 = Improper bounds for input variable;
!                       3 = Error allocating memory;
!                       4 = File IO error.
!
!   Copyright (c) 2005-2019, SHTOOLS
!   All rights reserved.
!
!------------------------------------------------------------------------------
    use SHTOOLS, only: SHPowerSpectrum, SHCrossPowerSpectrum
    use ftypes

    implicit none

    real(dp), intent(in) :: gilm(:,:,:), tilm(:,:,:)
    integer(int32), intent(in) :: lmax
    real(dp), intent(out) :: admit(:), corr(:)
    real(dp), intent(out), optional :: admit_error(:)
    integer(int32), intent(out), optional :: exitstatus
    real(dp) :: gt(lmax+1), gg(lmax+1), tt(lmax+1)
    integer(int32) :: l, l1

    if (present(exitstatus)) exitstatus = 0

    if (size(gilm(:,1,1)) < 2 .or. size(gilm(1,:,1)) < lmax+1 .or. &
            size(gilm(1,1,:)) < lmax+1) then
        print*, "Error --- SHAdmitCorr"
        print*, "GILM must be dimensioned as (2, LMAX+1, LMAX+1) where " // &
                "LMAX is ", lmax
        print*, "Input dimension is ", size(gilm(:,1,1)), size(gilm(1,:,1)), &
                size(gilm(1,1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    else if (size(tilm(:,1,1)) < 2 .or. size(tilm(1,:,1)) < lmax+1 .or. &
            size(tilm(1,1,:)) < lmax+1) then
        print*, "Error --- SHAdmitCorr"
        print*, "TILM must be dimensioned as (2, LMAX+1, LMAX+1) " // &
                "where LMAX is ", lmax
        print*, "Input dimension is ", size(tilm(:,1,1)), size(tilm(1,:,1)), &
                size(tilm(1,1,:))
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    else if (size(admit) < lmax+1) then
        print*, "Error --- SHAdmitCorr"
        print*, "ADMIT must be dimensioned as (LMAX+1) where LMAX is ", lmax
        print*, "Input dimension is ", size(admit)
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    else if (size(corr) < lmax+1) then
        print*, "Error --- SHAdmitCorr"
        print*, "CORR must be dimensioned as (LMAX+1) where LMAX is ", lmax
        print*, "Input dimension is ", size(corr)
        if (present(exitstatus)) then
            exitstatus = 1
            return
        else
            stop
        end if

    end if

    if (present(admit_error)) then
        if (size(admit_error) < lmax+1) then
            print*, "Error --- SHAdmitCorr"
            print*, "ADMIT_ERROR must be dimensioned as (LMAX+1) " // &
                    "where LMAX is ", lmax
            print*, "Input dimension is ", size(admit_error)
            if (present(exitstatus)) then
                exitstatus = 1
                return
            else
                stop
            end if

        end if

    end if

    admit = 0.0_dp
    corr = 0.0_dp

    if (present(exitstatus)) then
        call SHCrossPowerSpectrum(gilm, tilm, lmax, gt, exitstatus=exitstatus)
        if (exitstatus /= 0) return
        call SHPowerSpectrum(gilm, lmax, gg, exitstatus=exitstatus)
        if (exitstatus /= 0) return
        call SHPowerSpectrum(tilm, lmax, tt, exitstatus=exitstatus)
        if (exitstatus /= 0) return
    else
        call SHCrossPowerSpectrum(gilm, tilm, lmax, gt) ! < gilm tilm >
        call SHPowerSpectrum(gilm, lmax, gg) ! < gilm gilm >
        call SHPowerSpectrum(tilm, lmax, tt) ! < tilm tilm >
    end if

    admit(1:lmax+1) = gt(1:lmax+1) / tt(1:lmax+1)
    corr(1:lmax+1) = gt(1:lmax+1) / sqrt( tt(1:lmax+1) * gg(1:lmax+1) )

    if (present(admit_error)) then
        admit_error = 0.0_dp
        do l = 1, lmax, 1
            l1 = l + 1
            admit_error(l1) = gg(l1)*(1.0_dp - corr(l1)**2) &
                              / ( tt(l1) * dble(2*l) )
        end do

        admit_error(1:lmax+1) = sqrt(admit_error(1:lmax+1))

    end if

end subroutine SHAdmitCorr


PROGRAM advection_weakscaling

USE Advection, only : GetRHSAdvection, InitializeAdvection, FinalizeAdvection
USE OMP_LIB

IMPLICIT NONE

INCLUDE 'mpif.h'

INTEGER, PARAMETER :: Nthreads = 8, Nx_factor = 10, Nx_base = 20

DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:,:,:) :: Q, RQ
DOUBLE PRECISION                                  :: dx, dy, dz, T0, T1
DOUBLE PRECISION, DIMENSION(Nthreads)             :: runtimes, efficiency
INTEGER                                           :: i, r, nt, ierr, mpi_thread_provided

INTEGER :: Nx, Ny, Nz, i_end, j_end, i_start, j_start, k_start, k_end, order

CALL MPI_INIT_THREAD(MPI_THREAD_FUNNELED, mpi_thread_provided, ierr)
i_start = 1
j_start = 1
k_start = 1

DO order=1,5,4

    DO r=1,Nx_factor

        Nx = r*Nx_base
        Ny = r*Nx_base
        Nz = r*Nx_base
        dx = 1.0/Nx
        dy = 1.0/Ny
        dz = 1.0/Nz

        DO nt=1,Nthreads

            i_end = i_start+Nx-1
            j_end = j_start+Ny-1
            k_end = k_start+Nz-1

            ALLOCATE(Q( j_start-3:j_end+3,   i_start-3:i_end+3,  k_start-3:k_end+3, 0:nt-1))
            ALLOCATE(RQ(j_start-3:j_end+3,   i_start-3:i_end+3,  k_start-3:k_end+3, 0:nt-1))

            CALL InitializeAdvection(i_start-3, i_end+3, j_start-3, j_end+3, k_start-3, k_end+3, nt, .true.)

            CALL OMP_SET_NUM_THREADS(nt)

            !$OMP PARALLEL DO schedule(static)
            DO i=0,nt-1
                Q(:,:,:,i)  = 3.33

                ! Initialize RQ with -1
                RQ(:,:,:,i) = -1.0
            END DO
            !$OMP END PARALLEL DO

            T0 = MPI_WTIME()

            !$OMP PARALLEL DO schedule(static)
            DO i=0,nt-1
                CALL GetRHSAdvection(Q(:,:,:,i), RQ(:,:,:,i), dx, dy, dz, i_start, i_end, j_start, j_end, k_start, k_end, order)
            END DO
            !$OMP END PARALLEL DO
            T1 = MPI_WTIME()

            runtimes(nt) = T1-T0

            DEALLOCATE(Q)
            DEALLOCATE(RQ)
            CALL FinalizeAdvection()

        END DO

        DO nt=1,Nthreads
            efficiency(nt) = (runtimes(1)/runtimes(nt))*100
        END DO

        WRITE(*,*)
        WRITE(*,*) '========================================'
        WRITE(*,'(A, I4, A, I4, A, I4)') 'Problem size : ', Nx, ' x ', Ny, ' x ', Nz
       ! DO nt=1,Nthreads
        !        WRITE(*,'(A, I1, A, I2, A, F5.3)') 'Order ', order, ' -- Runtime for N=', nt , ' threads : ', runtimes(nt)
        !END DO
        WRITE(*,*)
        DO nt=1,Nthreads
            WRITE(*,'(A, I1, A, I2, A, F5.1)') 'Order ', order, ' -- Efficiency for N=', nt , ' threads : ', efficiency(nt)
        END DO
    END DO

END DO

CALL MPI_FINALIZE(ierr)

END PROGRAM advection_weakscaling