!
!   test/nls.f95
!
!   (c) Daniel Bershatsky, 2015
!

program test_nls
    use nls

    call test()

contains

    subroutine test()
        implicit none

        logical, dimension(10) :: success

        success = .true.

        call test_make_banded_matrix(success(1))
        call test_make_laplacian_o3(success(2))
        call test_make_laplacian_o5(success(3))
        call test_make_laplacian_o7(success(4))
        call test_make_laplacian(success(5))
        call test_rgbmv(success(6))
        call test_pumping(success(7))
        call test_hamiltonian(success(8))
        call test_runge_kutta(success(9))
        call test_solve_nls(success(10))

        print *
        print *, 'Success: ', success
    end subroutine test

    subroutine test_make_banded_matrix(success)
        implicit none

        logical, intent(inout) :: success
        integer, parameter :: dp = selected_real_kind(6, 37)
        integer, parameter :: m = 5, n = 7
        real(dp), parameter :: tolerance = 1.0e-6
        real(dp), dimension(m) :: row
        real(dp), dimension(m, n) :: ans, mat
        logical, dimension(m, n) :: equlity
        integer :: i, j

        print *, 'Testing make_banded_matrix():'

        row = (/ 1, 2, 3, 4, 5 /)
        ans = reshape((/ &
            0, 0, 3, 4, 5, &
            0, 2, 3, 4, 5, &
            1, 2, 3, 4, 5, &
            1, 2, 3, 4, 5, &
            1, 2, 3, 4, 5, &
            1, 2, 3, 4, 0, &
            1, 2, 3, 0, 0  &
        /), shape(ans))

        call make_banded_matrix(n, m, row, mat)

        equlity = abs(mat - ans) <= tolerance

        print *, 'Row: ', row

        do i = 1, m
            print *, equlity(i, :)
            do j = 1, n
                success = success .and. equlity(i, j)
            end do
        end do

        print *, 'Success: ', success
        print *
    end subroutine test_make_banded_matrix

    subroutine test_make_laplacian_o3(success)
        implicit none

        logical, intent(inout) :: success

        success = success .and. .true.

        print *, 'Testing make_laplacian_o3():', ' TODO: test body'
        print *, 'Success: ', success
        print *
    end subroutine test_make_laplacian_o3

    subroutine test_make_laplacian_o5(success)
        implicit none

        logical, intent(inout) :: success
        integer, parameter :: dp = selected_real_kind(6, 37), n = 7, m = 5
        integer :: i, j
        real(dp), parameter :: h = 0.01, tolerance = 1.0e-0
        real(dp), dimension(m, n) :: op, ans
        logical, dimension(m, n) :: equlity

        print *, 'Testing make_laplacian_o5():'

        ans = transpose(reshape((/                                                                                &
                 0.00000000,     0.00000000, -1666.66666667, -1250.00000000,  -833.33333333,  -694.44444444,  -625.00000000, &
                 0.00000000, 26666.66666667, 13333.33333333, 10000.00000000,  8888.88888889,  8333.33333333,  8000.00000000, &
            -25000.00000000,-12083.33333333,-12500.00000000,-12500.00000000,-12500.00000000,-12500.00000000,-12500.00000000, &
                0.000000000,  3333.33333333,  4444.44444444,  5000.00000000,  5333.33333333,  5555.55555555,     0.00000000, &
                0.000000000,  -138.88888888,  -208.33333333,  -250.00000000,  -277.77777777,     0.00000000,     0.00000000  &
        /), (/ 7, 5 /)))!shape(ans)))

        call make_laplacian_o5(n, h, op)

        equlity = abs(op - ans) <= tolerance

        do i = 1, m
            print *, equlity(i, :)
            print *, ans(i, :)
            print *, op(i, :)
            do j = 1, n
                success = success .and. equlity(i, j)
            end do
        end do

        print *, 'Success: ', success
        print *
    end subroutine test_make_laplacian_o5

    subroutine test_make_laplacian_o7(success)
        implicit none

        logical, intent(inout) :: success

        success = success .and. .true.

        print *, 'Testing make_laplacian_o7():', ' TODO: test body'
        print *, 'Success: ', success
        print *
    end subroutine test_make_laplacian_o7

    subroutine test_make_laplacian(success)
        implicit none

        logical, intent(inout) :: success

        success = success .and. .true.

        print *, 'Testing test_make_laplacian():', ' TODO: test body'
        print *, 'Success: ', success
        print *
    end subroutine test_make_laplacian

    subroutine test_rgbmv(success)
        implicit none

        logical, intent(inout) :: success
        integer, parameter :: dp = selected_real_kind(6, 37)
        integer, parameter :: m = 5, n = 7, klu = 2
        real(dp), parameter :: sign = 1.0, tolerance = 1.0e-6
        real(dp), dimension(m) :: row
        real(dp), dimension(m, n) :: op
        real(dp), dimension(n, n) :: x, y, z
        logical, dimension(n) :: equlity
        integer :: i, j

        print *, 'Testing test_rgbmv()'

        row = (/ 1, 2, 3, 4, 5 /)
        x = reshape((/            &
            1, 0, 0, 0, 0, 0, 0, &
            0, 1, 0, 0, 0, 0, 0, &
            0, 0, 1, 0, 0, 0, 0, &
            0, 0, 0, 1, 0, 0, 0, &
            0, 0, 0, 0, 1, 0, 0, &
            0, 0, 0, 0, 0, 1, 0, &
            0, 0, 0, 0, 0, 0, 1  &
        /), shape(x))
        z = transpose(reshape((/   &
            3, 4, 5, 0, 0, 0, 0, &
            2, 3, 4, 5, 0, 0, 0, &
            1, 2, 3, 4, 5, 0, 0, &
            0, 1, 2, 3, 4, 5, 0, &
            0, 0, 1, 2, 3, 4, 5, &
            0, 0, 0, 1, 2, 3, 4, &
            0, 0, 0, 0, 1, 2, 3  &
        /), shape(z)))

        call make_banded_matrix(n, m, row, op)

        do i = 1, n
            call rgbmv(x(i, :), y(i, :), sign, op, klu, n)
        end do

        do i = 1, n
            equlity = abs(y(i, :) - z(i, :)) <= tolerance
            print *, 'Row #', i, equlity
            do j = 1, n
                success = success .and. equlity(j)
            end do
        end do

        print *, 'Success: ', success
        print *
    end subroutine test_rgbmv

    subroutine test_pumping(success)
        implicit none

        logical, intent(inout) :: success

        success = success .and. .true.

        print *, 'Testing test_pumping():', ' TODO: test body'
        print *, 'Success: ', success
        print *
    end subroutine test_pumping

    subroutine test_hamiltonian(success)
        implicit none

        logical, intent(inout) :: success

        success = success .and. .true.

        print *, 'Testing test_hamiltonian():', ' TODO: test body'
        print *, 'Success: ', success
        print *
    end subroutine test_hamiltonian

    subroutine test_runge_kutta(success)
        implicit none

        logical, intent(inout) :: success

        success = success .and. .true.

        print *, 'Testing test_runge_kutta():', ' TODO: test body'
        print *, 'Success: ', success
        print *
    end subroutine test_runge_kutta

    subroutine test_solve_nls(success)
        implicit none

        logical, intent(inout) :: success
        integer, parameter :: dp = selected_real_kind(6, 37)
        integer, parameter :: n = 7, iters = 1, order = 5
        complex(dp), dimension(n) :: u
        real(dp), parameter :: dt = 0.00001, dx = 0.01

        print *, 'Testing solve_nls():'

        call solve_nls(dt, dx, n, order, iters, u)

        success = success .and. .true.

        print *, 'Success: ', success
        print *
    end subroutine

end program test_nls


! Smallest multiple
!
! 2520 is the smallest number that can be divided by each of the numbers
! from 1 to 10 without any remainder.
!
! What is the smallest positive number that is evenly divisible by all of
! the numbers from 1 to 20?
module Problem5

    implicit none
    private

    public :: solve
contains
    subroutine solve
        write (*, '(A)') 'Problem 5. Smallest multiple.'

        write (*, '(A, I)') 'Multiple 1: ', multiple1()
        write (*, '(A, I)') 'Multiple 2: ', multiple2()
    end subroutine

    function multiple1()
        integer multiple1
        integer base
        logical flag

        do multiple1 = 1, huge(multiple1)
            flag = .true.

            do base = 1, 20
                if (mod(multiple1, base) /= 0) then
                    flag = .false.
                end if
            end do

            if (flag == .true.) then
                return
            end if
        end do
    end function

    function multiple2()
        integer multiple2
        integer base
        logical flag

        do multiple2 = 1, huge(multiple2)
            flag = .true.

            do base = 2, 20
                if (mod(multiple2, base) /= 0) then
                    flag = .false.
                    exit
                end if
            end do

            if (flag == .true.) then
                return
            end if
        end do
    end function
end module


function i8_bit_hi1 ( n )

!*****************************************************************************80
!
!! I8_BIT_HI1 returns the position of the high 1 bit base 2 in an integer.
!
!  Discussion:
!
!    This routine uses the integer precision corresponding to a KIND of 8.
!
!    The input arguments to this function should NOT be constants; they should
!    be variables of type integer ( kind = 8 )!
!
!  Example:
!
!       N    Binary    Hi 1
!    ----    --------  ----
!       0           0     0
!       1           1     1
!       2          10     2
!       3          11     2
!       4         100     3
!       5         101     3
!       6         110     3
!       7         111     3
!       8        1000     4
!       9        1001     4
!      10        1010     4
!      11        1011     4
!      12        1100     4
!      13        1101     4
!      14        1110     4
!      15        1111     4
!      16       10000     5
!      17       10001     5
!    1023  1111111111    10
!    1024 10000000000    11
!    1025 10000000001    11
!
!  Licensing:
!
!    This code is distributed under the GNU LGPL license.
!
!  Modified:
!
!    28 May 2004
!
!  Author:
!
!    John Burkardt
!
!  Parameters:
!
!    Input, integer ( kind = 8 ) N, the integer to be measured.
!    N should be nonnegative.  If N is nonpositive, I8_BIT_HI1
!    will always be 0.
!
!    Output, integer ( kind = 8 ) I8_BIT_HI1, the number of bits base 2.
!
  implicit none

  integer ( kind = 8 ) :: bit
  integer ( kind = 8 ) :: i8_bit_hi1
  integer ( kind = 8 ) :: i
  integer ( kind = 8 ) :: n

  i = n
  bit = 0

  do

    if ( i <= 0 ) then
      exit
    end if

    bit = bit + 1
    i = i / 2

  end do

  i8_bit_hi1 = bit

  return
end


      module MOD_HMINUSFF
      contains
      subroutine hminusff (sighmff,xlam,T)
C***  source John 1988 A&A 193,189
c     control table: Stilley & Callaway 1970, ApJ 160, 245
!      IMPLICIT REAL*8(A-H,O-Z)
      IMPLICIT NONE
      real*8,intent(out) :: sighmff
      real*8, intent(in) :: xlam,T
      real*8 an,bn,cn,dn,en,fn,sum,sumn,theta,pow,sq,x
      integer i,j
      real*8 :: sqrt
      dimension an(6,2),bn(6,2),cn(6,2),dn(6,2),en(6,2),fn(6,2)
C23456789012345678901234567890123456789012345678901234567890123456789012
      data an/0.,     2483.346, -3449.889,  2200.04,  -696.271,  88.283,
     &      518.1021,  473.2636, -482.2089,  115.5291,   0.,      0./,
     &     bn/0.,      285.827, -1158.382,  2427.719,-1841.4,   444.517,
     &     -734.8666, 1443.4137, -737.1616,  169.6374,   0.,      0./,
     &     cn/0.,    -2054.291,  8746.523,-13651.105, 8624.97,-1863.864,
     &     1021.1775,-1977.3395, 1096.8827, -245.649,    0.,      0./,
     &     dn/0.,     2827.776,-11485.632,16755.524,-10051.53, 2095.288,
     &     -479.0721,  922.3575, -521.1341,  114.243,    0.,      0./,
     &     en/0.,    -1341.537,  5303.609, -7510.494, 4400.067,-901.788,
     &       93.1373, -178.9275,  101.7963,  -21.9972,   0.,      0./,
     &     fn/0.,      208.952,  -812.939,  1132.738, -655.02,  132.985,
     &       -6.4285,   12.36,     -7.057,     1.5097,   0.,      0./
C23456789012345678901234567890123456789012345678901234567890123456789012
	x=xlam/1.e+4
      if (x.gt.0.3645d0) then
         i=1
      else if (x.ge.0.1823d0) then
         i=2
      else
         sighmff=0.d0
         return
      endif
      theta=5040.d0/T
      sq = sqrt(theta)
      pow=sq
      sum=0.d0
      do j=1,6
         pow=pow*sq
         sumn=(((((x*an(j,i)+bn(j,i))*x+cn(j,i))*x+dn(j,i))*x+en(j,i))
     &        *x+fn(j,i))/x/x/x
         sum=sum+sumn*pow
      enddo
      sighmff=1.d-29*sum
      return
      end subroutine
      end module


      INCLUDE 'VICMAIN_FOR'
      subroutine main44
      implicit integer(a-z)
      real slat,slon,plat,plon,rpol,requ,range,rlat,rlon,tg,cg,ci,ce,
     *   lumlat,lumlon,rad
      DATA rad/57.2957795/

      call XVMESSAGE(
     .    'this pix vgr:1636832 image space line 500,samp 500',' ')
      slat=.55539604/rad
      slon=169.91901/rad
      plat=-.08596189/rad
      plon=155.07442/rad
      rpol=1815.
      requ=1815.
      range=806022.25
      rlat=-13.8540/rad
      rlon=149.8150/rad
      mode=1
      call lumllp(slat,slon,plat,plon,rpol,requ,range,rlat,rlon,
     *  tg,cg,ci,ce,lumlat,lumlon,mode,0)
      tg=rad*(atan(tg))
      cg=rad*(acos(cg))
      ci=rad*(acos(ci))
      ce=rad*(acos(ce))
      call prnt(7,1,tg,    'phase=    .')
      call prnt(7,1,cg,    'phase=    .')
      call prnt(7,1,ci,    'incidence=.')
      call prnt(7,1,ce,    'emission= .')
      call prnt(7,1,lumlat,'lumlat=   .')
      call prnt(7,1,lumlon,'lumlon=   .')
      call XVMESSAGE('answer: phase=14.85,inci=24.57,emis=14.75',' ')
      call XVMESSAGE('lumlat=-12.586,lumlon=55.616',' ')

      call XVMESSAGE
     *('this pix is vgr 1634522 image space line 276.5 samp 429.97 ',
     .     ' ')
      slat=.55803038/rad
      slon=12.827187/rad
      plat=1.1882980/rad
      plon=17.688618/rad
      rpol=1815.
      requ=1815.
      range=2612172.5
      rlat=1./RAD
      rlon=17./rad
      first=1
      call lumllp(slat,slon,plat,plon,rpol,requ,range,rlat,rlon,
     *  tg,cg,ci,ce,lumlat,lumlon,mode,first)
      tg=rad*(atan(tg))
      cg=rad*(acos(cg))
      ci=rad*(acos(ci))
      ce=rad*(acos(ce))
      call prnt(7,1,tg,    'phase=    .')
      call prnt(7,1,cg,    'phase=    .')
      call prnt(7,1,ci,    'incidence=.')
      call prnt(7,1,ce,    'emission= .')
      call prnt(7,1,lumlat,'lumlat=   .')
      call prnt(7,1,lumlon,'lumlon=   .')
      call XVMESSAGE('answer:phase=4.902,inic=4.9156,emiss=0.71436',' ')
      call XVMESSAGE('lumlat=-1.9970,lumlon=-0.95807',' ')
      call XVMESSAGE
     *('this is pix is vgr 1634522 image space line 300 sample 400',' ')
      rlat=19.6076/rad
      rlon=41.2608/RAD
      first=0
      call lumllp(slat,slon,plat,plon,rpol,requ,range,rlat,rlon,
     *  tg,cg,ci,ce,lumlat,lumlon,mode,first)
      tg=rad*(atan(tg))
      cg=rad*(acos(cg))
      ci=rad*(acos(ci))
      ce=rad*(acos(ce))
      call prnt(7,1,tg,    'phase=    .')
      call prnt(7,1,cg,    'phase=    .')
      call prnt(7,1,ci,    'incidence=.')
      call prnt(7,1,ce,    'emission= .')
      call prnt(7,1,lumlat,'lumlat=   .')
      call prnt(7,1,lumlon,'lumlon=   .')
      call XVMESSAGE('answer:phase=4.8850,inci=33.73,emis=29.53',' ')
      call XVMESSAGE('lumlat=-23.42,lumlon=20.972',' ')
      return
      end


program build_particle
   implicit none

   integer :: i, j, k, io, nlines
   character(len=1) :: read_char
   character(len=200) :: current_line
   real(kind=8) :: x, y, z, xh, yh, zh, lc

   open (unit=2, file='particle.in', status='new', action='write')

   !-------------------------------------------------
   ! Generate the coordinates and print a .in file:
   !-------------------------------------------------

   ! Core of 10 angstrom fcc gold atoms
   lc = 4.07 ! lattice constant of gold in angstrom
   do i = -3, 3
      do j = -3, 3
         do k = -3, 3
            x = dble(i)*lc
            y = dble(j)*lc
            z = dble(k)*lc
            xh = (dble(i) + 0.5)*lc ! x and one half
            yh = (dble(j) + 0.5)*lc
            zh = (dble(k) + 0.5)*lc
            ! Face-centered cubic unit cell contains four
            ! unique atoms:
            if (sqrt(x*x + y*y + z*z) < 10.0d0) then
               write (2,*) 'Au    ', x, y, z
            end if
            if (sqrt(xh*xh + yh*yh + z*z) < 10.0d0) then
               write(2,*) 'Au    ', xh, yh, z
            end if
            if (sqrt(x*x + yh*yh + zh*zh) < 10.0d0) then
               write(2,*) 'Au    ', x, yh, zh
            end if
            if (sqrt(xh*xh + y*y + zh*zh) < 10.0d0) then
               write(2,*) 'Au    ', xh, y, zh
            end if
         end do
      end do
   end do

   ! Shell of 5 angstrom fcc platinum atoms
   lc = 3.91 ! lattice constant of platinum in angstrom
   do i = -6, 6
      do j = -6, 6
         do k = -6, 6
            x = dble(i)*lc
            y = dble(j)*lc
            z = dble(k)*lc
            xh = (dble(i) + 0.5)*lc
            yh = (dble(j) + 0.5)*lc
            zh = (dble(k) + 0.5)*lc
            if (sqrt(x*x + y*y + z*z) > 10.0d0) then
               if (sqrt(x*x + y*y + z*z) < 15.0d0) then
                  write (2,*) 'Pt    ', x, y, z
               end if
            end if
            if (sqrt(xh*xh + yh*yh + z*z) > 10.0d0) then
               if (sqrt(xh*xh + yh*yh + z*z) < 15.0d0) then
                  write(2,*) 'Pt    ', xh, yh, z
               end if
            end if
            if (sqrt(x*x + yh*yh + zh*zh) > 10.0d0) then
               if (sqrt(x*x + yh*yh + zh*zh) < 15.0d0) then
                  write(2,*) 'Pt    ', x, yh, zh
               end if
            end if
            if (sqrt(xh*xh + y*y + zh*zh) > 10.0d0) then
               if (sqrt(xh*xh + y*y + zh*zh) < 15.0d0) then
                  write(2,*) 'Pt    ', xh, y, zh
               end if
            end if
         end do
      end do
   end do

   close(2)

   !---------------------------
   ! Write a .xyz file:
   !---------------------------

   open (unit=3, file='particle.in', status='old', action='read')

   ! count the number of lines in particle.in:
	nlines = 0
	do
      read(3,*,iostat=io) read_char
      if (io/=0) exit
      write(*,*) read_char
      nlines = nlines + 1
   end do

   rewind(3)

   open (unit=4, file='particle.xyz', status='new', action='write')

   write(4,*) nlines
   write(4,*) 'kflklv'
   do i = 1, nlines
      read(3,'(A100)') current_line
      write(4,*) trim(current_line)
   end do

   close(3)
   close(4)

end program build_particle


c23456789012345678901234567890123456789012345678901234567890123456789012

      PROGRAM sphere

c  This program determines the surface area and volume of a sphere, given
c  its radius.

c  Variable declarations
      REAL  rad, area, volume, pi

c  Definition of variables
c   rad = radius, area = surface area, volume = volume of the sphere

c  Assign a value to the variable pi.
      pi = 3.141593

c  Input the value of the radius and echo the inputted value.
      PRINT *, 'Enter the radius of the sphere.'
      READ *, rad
      PRINT *, rad, ' is the value of the radius.'

c  Compute the surface area and volume of the sphere.
      area = 4.0 * pi * rad**2
      volume = (4.0/3.0) * pi * rad**3

c  Print the values of the radius (given in cm), the surface area (sq cm),
c  and the volume (cubic cm).
      PRINT *,'In a sphere of radius', rad, ' , the surface area is',
     +          area, ' and its volume is', volume, '.'
      STOP

      END


!> @brief This subroutine reads in coordinates (in degrees) on the surface of Earth and a heading (in degrees) and a distance (in metres) it then calculates the coordinates (in degrees) that are at the end of the vector.
!>
!> @note https://en.wikipedia.org/wiki/Vincenty%27s_formulae
!>
!> @note https://www.movable-type.co.uk/scripts/latlong-vincenty.html
!>

PURE SUBROUTINE sub_calc_loc_from_loc_and_bearing_and_dist(lon1_deg, lat1_deg, alpha1_deg, s_m, lon2_deg, lat2_deg, alpha2_deg, nmax, eps)
    USE ISO_FORTRAN_ENV

    IMPLICIT NONE

    ! Declare input variables/outputs ...
    REAL(kind = REAL64), INTENT(in)                                             :: lon1_deg
    REAL(kind = REAL64), INTENT(in)                                             :: lat1_deg
    REAL(kind = REAL64), INTENT(in)                                             :: alpha1_deg
    REAL(kind = REAL64), INTENT(in)                                             :: s_m
    REAL(kind = REAL64), INTENT(out)                                            :: lon2_deg
    REAL(kind = REAL64), INTENT(out)                                            :: lat2_deg
    REAL(kind = REAL64), INTENT(out)                                            :: alpha2_deg

    ! Declare optional input variables/outputs ...
    INTEGER(kind = INT64), INTENT(in), OPTIONAL                                 :: nmax
    REAL(kind = REAL64), INTENT(in), OPTIONAL                                   :: eps

    ! Declare internal parameters ...
    REAL(kind = REAL64), PARAMETER                                              :: a = 6378137.0e0_REAL64
    REAL(kind = REAL64), PARAMETER                                              :: f = 1.0e0_REAL64 / 298.257223563e0_REAL64

    ! Declare internal derived parameters ...
    REAL(kind = REAL64), PARAMETER                                              :: b = (1.0e0_REAL64 - f) * a

    ! Declare internal variables ...
    INTEGER(kind = INT64)                                                       :: i
    INTEGER(kind = INT64)                                                       :: nmax2
    REAL(kind = REAL64)                                                         :: alpha1
    REAL(kind = REAL64)                                                         :: alpha2
    REAL(kind = REAL64)                                                         :: bigA
    REAL(kind = REAL64)                                                         :: bigB
    REAL(kind = REAL64)                                                         :: c
    REAL(kind = REAL64)                                                         :: cosSq_alpha
    REAL(kind = REAL64)                                                         :: delta_sigma
    REAL(kind = REAL64)                                                         :: eps2
    REAL(kind = REAL64)                                                         :: l
    REAL(kind = REAL64)                                                         :: lambda
    REAL(kind = REAL64)                                                         :: lat1
    REAL(kind = REAL64)                                                         :: lat2
    REAL(kind = REAL64)                                                         :: lon1
    REAL(kind = REAL64)                                                         :: lon2
    REAL(kind = REAL64)                                                         :: sigma
    REAL(kind = REAL64)                                                         :: sigma1
    REAL(kind = REAL64)                                                         :: sigmanew
    REAL(kind = REAL64)                                                         :: sin_alpha
    REAL(kind = REAL64)                                                         :: two_sigma_m
    REAL(kind = REAL64)                                                         :: u1
    REAL(kind = REAL64)                                                         :: uSq

    ! Set value ...
    IF(PRESENT(nmax))THEN
        nmax2 = MAX(3_INT64, MIN(100_INT64, nmax))
    ELSE
        nmax2 = 100_INT64
    END IF

    ! Set value ...
    IF(PRESENT(eps))THEN
        eps2 = eps
    ELSE
        eps2 = 1.0e-12_REAL64
    END IF

    ! Convert to radians ...
    lon1 = func_radians(lon1_deg)                                               ! [rad]
    lat1 = func_radians(lat1_deg)                                               ! [rad]
    alpha1 = func_radians(alpha1_deg)                                           ! [rad]

    ! Set constants ...
    u1 = ATAN((1.0e0_REAL64 - f) * TAN(lat1))
    sigma1 = ATAN2(TAN(u1), COS(alpha1))
    sin_alpha = COS(u1) * SIN(alpha1)
    cosSq_alpha = 1.0e0_REAL64 - sin_alpha ** 2
    c = f * cosSq_alpha * (4.0e0_REAL64 + f * (4.0e0_REAL64 - 3.0e0_REAL64 * cosSq_alpha)) / 16.0e0_REAL64
    uSq = cosSq_alpha * (a ** 2 - b ** 2) / b ** 2
    bigA = 1.0e0_REAL64 + uSq * (4096.0e0_REAL64 + uSq * (-768.0e0_REAL64 + uSq * (320.0e0_REAL64 - 175.0e0_REAL64 * uSq))) / 16384.0e0_REAL64
    bigB = uSq * (256.0e0_REAL64 + uSq * (-128.0e0_REAL64 + uSq * (74.0e0_REAL64 - 47.0e0_REAL64 * uSq))) / 1024.0e0_REAL64

    ! Set initial value of sigma ...
    sigma = s_m / (b * bigA)

    ! Loop over iterations ...
    DO i = 1_INT64, nmax2
        ! Find new value of sigma ...
        two_sigma_m = 2.0e0_REAL64 * sigma1 + sigma
        delta_sigma = bigB * SIN(sigma) * (COS(two_sigma_m) + 0.25e0_REAL64 * bigB * (COS(sigma) * (2.0e0_REAL64 * COS(two_sigma_m) ** 2 - 1.0e0_REAL64) - bigB * COS(two_sigma_m) * (4.0e0_REAL64 * SIN(sigma) ** 2 - 3.0e0_REAL64) * (4.0e0_REAL64 * COS(two_sigma_m) ** 2 - 3.0e0_REAL64) / 6.0e0_REAL64))
        sigmaNew = s_m / (b * bigA) + delta_sigma

        ! Only check the solution after at least 3 iterations ...
        IF(i >= 3_INT64)THEN
            IF(ABS(sigmaNew - sigma) / ABS(sigmaNew) <= eps2)THEN
                ! Replace old sigma with new sigma ...
                sigma = sigmaNew

                EXIT
            END IF
        END IF

        ! Replace old sigma with new sigma ...
        sigma = sigmaNew
    END DO

    ! Calculate end point and forward azimuth ...
    lat2 = ATAN2(SIN(u1) * COS(sigma) + COS(u1) * SIN(sigma) * COS(alpha1), (1.0e0_REAL64 - f) * HYPOT(sin_alpha, SIN(u1) * SIN(sigma) - COS(u1) * COS(sigma) * COS(alpha1)))
    lambda = ATAN2(SIN(sigma) * SIN(alpha1), COS(u1) * COS(sigma) - SIN(u1) * SIN(sigma) * COS(alpha1))
    l = lambda - (1.0e0_REAL64 - c) * f * sin_alpha * (sigma + c * SIN(sigma) * (COS(two_sigma_m) + c * COS(sigma) * (2.0e0_REAL64 * COS(two_sigma_m) ** 2 - 1.0e0_REAL64)))
    lon2 = l + lon1
    alpha2 = ATAN2(sin_alpha, COS(u1) * COS(sigma) * COS(alpha1) - SIN(u1) * SIN(sigma))
    alpha2 = MODULO(alpha2 + 2.0e0_REAL64 * const_pi, 2.0e0_REAL64 * const_pi)              ! NOTE: Normalize to 0 <--> +360 (in radians)

    ! Convert to degrees ...
    lon2_deg = func_degrees(lon2)                                               ! [deg]
    lat2_deg = func_degrees(lat2)                                               ! [deg]
    alpha2_deg = func_degrees(alpha2)                                           ! [deg]
END SUBROUTINE sub_calc_loc_from_loc_and_bearing_and_dist


      SUBROUTINE FB01SD( JOBX, MULTAB, MULTRC, N, M, P, SINV, LDSINV,
     $                   AINV, LDAINV, B, LDB, RINV, LDRINV, C, LDC,
     $                   QINV, LDQINV, X, RINVY, Z, E, TOL, IWORK,
     $                   DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To calculate a combined measurement and time update of one
C     iteration of the time-varying Kalman filter. This update is given
C     for the square root information filter, using dense matrices.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOBX    CHARACTER*1
C             Indicates whether X    is to be computed as follows:
C                                i+1
C             = 'X':  X    is computed and stored in array X;
C                      i+1
C             = 'N':  X    is not required.
C                      i+1
C
C     MULTAB  CHARACTER*1             -1
C             Indicates how matrices A   and B  are to be passed to
C                                     i       i
C             the routine as follows:                       -1
C             = 'P':  Array AINV must contain the matrix   A    and the
C                                                       -1  i
C                     array B must contain the product A  B ;
C                                                       i  i
C             = 'N':  Arrays AINV and B must contain the matrices
C                     as described below.
C
C     MULTRC  CHARACTER*1             -1/2
C             Indicates how matrices R     and C    are to be passed to
C                                     i+1       i+1
C             the routine as follows:
C             = 'P':  Array RINV is not used and the array C must
C                                          -1/2
C                     contain the product R    C   ;
C                                          i+1  i+1
C             = 'N':  Arrays RINV and C must contain the matrices
C                     as described below.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The actual state dimension, i.e., the order of the
C                       -1      -1
C             matrices S   and A  .  N >= 0.
C                       i       i
C
C     M       (input) INTEGER
C             The actual input dimension, i.e., the order of the matrix
C              -1/2
C             Q    .  M >= 0.
C              i
C
C     P       (input) INTEGER
C             The actual output dimension, i.e., the order of the matrix
C              -1/2
C             R    .  P >= 0.
C              i+1
C
C     SINV    (input/output) DOUBLE PRECISION array, dimension
C             (LDSINV,N)
C             On entry, the leading N-by-N upper triangular part of this
C                                 -1
C             array must contain S  , the inverse of the square root
C                                 i
C             (right Cholesky factor) of the state covariance matrix
C             P    (hence the information square root) at instant i.
C              i|i
C             On exit, the leading N-by-N upper triangular part of this
C                             -1
C             array contains S   , the inverse of the square root (right
C                             i+1
C             Cholesky factor) of the state covariance matrix P
C                                                              i+1|i+1
C             (hence the information square root) at instant i+1.
C             The strict lower triangular part of this array is not
C             referenced.
C
C     LDSINV  INTEGER
C             The leading dimension of array SINV.  LDSINV >= MAX(1,N).
C
C     AINV    (input) DOUBLE PRECISION array, dimension (LDAINV,N)
C                                                                 -1
C             The leading N-by-N part of this array must contain A  ,
C                                                                 i
C             the inverse of the state transition matrix of the discrete
C             system at instant i.
C
C     LDAINV  INTEGER
C             The leading dimension of array AINV.  LDAINV >= MAX(1,N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB,M)
C             The leading N-by-M part of this array must contain B ,
C                                                      -1         i
C             the input weight matrix (or the product A  B  if
C                                                      i  i
C             MULTAB = 'P') of the discrete system at instant i.
C
C     LDB     INTEGER
C             The leading dimension of array B.  LDB >= MAX(1,N).
C
C     RINV    (input) DOUBLE PRECISION array, dimension (LDRINV,*)
C             If MULTRC = 'N', then the leading P-by-P upper triangular
C                                              -1/2
C             part of this array must contain R    , the inverse of the
C                                              i+1
C             covariance square root (right Cholesky factor) of the
C             output (measurement) noise (hence the information square
C             root) at instant i+1.
C             The strict lower triangular part of this array is not
C             referenced.
C             Otherwise, RINV is not referenced and can be supplied as a
C             dummy array (i.e., set parameter LDRINV = 1 and declare
C             this array to be RINV(1,1) in the calling program).
C
C     LDRINV  INTEGER
C             The leading dimension of array RINV.
C             LDRINV >= MAX(1,P) if MULTRC = 'N';
C             LDRINV >= 1        if MULTRC = 'P'.
C
C     C       (input) DOUBLE PRECISION array, dimension (LDC,N)
C             The leading P-by-N part of this array must contain C   ,
C                                                       -1/2      i+1
C             the output weight matrix (or the product R    C    if
C                                                       i+1  i+1
C             MULTRC = 'P') of the discrete system at instant i+1.
C
C     LDC     INTEGER
C             The leading dimension of array C.  LDC >= MAX(1,P).
C
C     QINV    (input/output) DOUBLE PRECISION array, dimension
C             (LDQINV,M)
C             On entry, the leading M-by-M upper triangular part of this
C                                 -1/2
C             array must contain Q    , the inverse of the covariance
C                                 i
C             square root (right Cholesky factor) of the input (process)
C             noise (hence the information square root) at instant i.
C             On exit, the leading M-by-M upper triangular part of this
C                                    -1/2
C             array contains (QINOV )    , the inverse of the covariance
C                                  i
C             square root (right Cholesky factor) of the process noise
C             innovation (hence the information square root) at
C             instant i.
C             The strict lower triangular part of this array is not
C             referenced.
C
C     LDQINV  INTEGER
C             The leading dimension of array QINV.  LDQINV >= MAX(1,M).
C
C     X       (input/output) DOUBLE PRECISION array, dimension (N)
C             On entry, this array must contain X , the estimated
C                                                i
C             filtered state at instant i.
C             On exit, if JOBX = 'X', and INFO = 0, then this array
C             contains X   , the estimated filtered state at
C                       i+1
C             instant i+1.
C             On exit, if JOBX = 'N', or JOBX = 'X' and INFO = 1, then
C                                  -1
C             this array contains S   X   .
C                                  i+1 i+1
C
C     RINVY   (input) DOUBLE PRECISION array, dimension (P)
C                                      -1/2
C             This array must contain R    Y   , the product of the
C                                      i+1  i+1
C                                      -1/2
C             upper triangular matrix R     and the measured output
C                                      i+1
C             vector Y    at instant i+1.
C                     i+1
C
C     Z       (input) DOUBLE PRECISION array, dimension (M)
C             This array must contain Z , the mean value of the state
C                                      i
C             process noise at instant i.
C
C     E       (output) DOUBLE PRECISION array, dimension (P)
C             This array contains E   , the estimated error at instant
C                                  i+1
C             i+1.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             If JOBX = 'X', then TOL is used to test for near
C                                        -1
C             singularity of the matrix S   . If the user sets
C                                        i+1
C             TOL > 0, then the given value of TOL is used as a
C             lower bound for the reciprocal condition number of that
C             matrix; a matrix whose estimated condition number is less
C             than 1/TOL is considered to be nonsingular. If the user
C             sets TOL <= 0, then an implicitly computed, default
C             tolerance, defined by TOLDEF = N*N*EPS, is used instead,
C             where EPS is the machine precision (see LAPACK Library
C             routine DLAMCH).
C             Otherwise, TOL is not referenced.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (LIWORK)
C             where LIWORK = N if JOBX = 'X',
C             and   LIWORK = 1 otherwise.
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.  If INFO = 0 and JOBX = 'X', DWORK(2) returns
C             an estimate of the reciprocal of the condition number
C                                 -1
C             (in the 1-norm) of S   .
C                                 i+1
C
C     LDWORK  The length of the array DWORK.
C             LDWORK >= MAX(1,N*(N+2*M)+3*M,(N+P)*(N+1)+2*N),
C                           if JOBX = 'N';
C             LDWORK >= MAX(2,N*(N+2*M)+3*M,(N+P)*(N+1)+2*N,3*N),
C                           if JOBX = 'X'.
C             For optimum performance LDWORK should be larger.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;                        -1
C             = 1:  if JOBX = 'X' and the matrix S   is singular,
C                                                 i+1       -1
C                   i.e., the condition number estimate of S    (in the
C                                                           i+1
C                                                         -1    -1/2
C                   1-norm) exceeds 1/TOL.  The matrices S   , Q
C                                                         i+1   i
C                   and E have been computed.
C
C     METHOD
C
C     The routine performs one recursion of the square root information
C     filter algorithm, summarized as follows:
C
C       |    -1/2             -1/2    |     |         -1/2             |
C       |   Q         0      Q    Z   |     | (QINOV )     *     *     |
C       |    i                i    i  |     |       i                  |
C       |                             |     |                          |
C       |  -1 -1     -1 -1    -1      |     |             -1    -1     |
C     T | S  A  B   S  A     S  X     |  =  |    0       S     S   X   |
C       |  i  i  i   i  i     i  i    |     |             i+1   i+1 i+1|
C       |                             |     |                          |
C       |           -1/2      -1/2    |     |                          |
C       |    0     R    C    R    Y   |     |    0         0     E     |
C       |           i+1  i+1  i+1  i+1|     |                     i+1  |
C
C                  (Pre-array)                      (Post-array)
C
C     where T is an orthogonal transformation triangularizing the
C                        -1/2
C     pre-array, (QINOV )     is the inverse of the covariance square
C                      i
C     root (right Cholesky factor) of the process noise innovation
C     (hence the information square root) at instant i, and E    is the
C                                                            i+1
C     estimated error at instant i+1.
C
C     The inverse of the corresponding state covariance matrix P
C                                                               i+1|i+1
C     (hence the information matrix I) is then factorized as
C
C                   -1         -1     -1
C        I       = P       = (S   )' S
C         i+1|i+1   i+1|i+1    i+1    i+1
C
C     and one combined time and measurement update for the state is
C     given by X   .
C               i+1
C
C     The triangularization is done entirely via Householder
C     transformations exploiting the zero pattern of the pre-array.
C
C     REFERENCES
C
C     [1] Anderson, B.D.O. and Moore, J.B.
C         Optimal Filtering.
C         Prentice Hall, Englewood Cliffs, New Jersey, 1979.
C
C     [2] Verhaegen, M.H.G. and Van Dooren, P.
C         Numerical Aspects of Different Kalman Filter Implementations.
C         IEEE Trans. Auto. Contr., AC-31, pp. 907-917, Oct. 1986.
C
C     [3] Vanbegin, M., Van Dooren, P., and Verhaegen, M.H.G.
C         Algorithm 675: FORTRAN Subroutines for Computing the Square
C         Root Covariance Filter and Square Root Information Filter in
C         Dense or Hessenberg Forms.
C         ACM Trans. Math. Software, 15, pp. 243-256, 1989.
C
C     NUMERICAL ASPECTS
C
C     The algorithm requires approximately
C
C           3    2                              2   2
C     (7/6)N  + N x (7/2 x M + P) + N x (1/2 x P + M )
C
C     operations and is backward stable (see [2]).
C
C     CONTRIBUTORS
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1997.
C     Supersedes Release 2.0 routine FB01GD by M. Vanbegin,
C     P. Van Dooren, and M.H.G. Verhaegen.
C
C     REVISIONS
C
C     February 20, 1998, November 20, 2003, February 14, 2004.
C
C     KEYWORDS
C
C     Kalman filtering, optimal filtering, orthogonal transformation,
C     recursive estimation, square-root filtering, square-root
C     information filtering.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE, TWO
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         JOBX, MULTAB, MULTRC
      INTEGER           INFO, LDAINV, LDB, LDC, LDQINV, LDRINV, LDSINV,
     $                  LDWORK, M, N, P
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      INTEGER           IWORK(*)
      DOUBLE PRECISION  AINV(LDAINV,*), B(LDB,*), C(LDC,*), DWORK(*),
     $                  E(*), QINV(LDQINV,*), RINV(LDRINV,*), RINVY(*),
     $                  SINV(LDSINV,*), X(*), Z(*)
C     .. Local Scalars ..
      LOGICAL           LJOBX, LMULTA, LMULTR
      INTEGER           I, I12, I13, I21, I23, IJ, ITAU, JWORK, LDW, M1,
     $                  N1, NP, WRKOPT
      DOUBLE PRECISION  RCOND
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DDOT
      EXTERNAL          DDOT, LSAME
C     .. External Subroutines ..
      EXTERNAL          DAXPY, DCOPY, DGEMM, DGEQRF, DLACPY, DORMQR,
     $                  DTRMM, DTRMV, MB02OD, MB04KD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX
C     .. Executable Statements ..
C
      NP = N + P
      N1 = MAX( 1, N )
      M1 = MAX( 1, M )
      INFO = 0
      LJOBX  = LSAME( JOBX, 'X' )
      LMULTA = LSAME( MULTAB, 'P' )
      LMULTR = LSAME( MULTRC, 'P' )
C
C     Test the input scalar arguments.
C
      IF( .NOT.LJOBX .AND. .NOT.LSAME( JOBX, 'N' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.LMULTA .AND. .NOT.LSAME( MULTAB, 'N' ) ) THEN
         INFO = -2
      ELSE IF( .NOT.LMULTR .AND. .NOT.LSAME( MULTRC, 'N' ) ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -4
      ELSE IF( M.LT.0 ) THEN
         INFO = -5
      ELSE IF( P.LT.0 ) THEN
         INFO = -6
      ELSE IF( LDSINV.LT.N1 ) THEN
         INFO = -8
      ELSE IF( LDAINV.LT.N1 ) THEN
         INFO = -10
      ELSE IF( LDB.LT.N1 ) THEN
         INFO = -12
      ELSE IF( LDRINV.LT.1 .OR. ( .NOT.LMULTR .AND. LDRINV.LT.P ) ) THEN
         INFO = -14
      ELSE IF( LDC.LT.MAX( 1, P ) ) THEN
         INFO = -16
      ELSE IF( LDQINV.LT.M1 ) THEN
         INFO = -18
      ELSE IF( ( LJOBX .AND. LDWORK.LT.MAX( 2, N*(N + 2*M) + 3*M,
     $                                      NP*(N + 1) + 2*N, 3*N ) )
     $                                                             .OR.
     $    ( .NOT.LJOBX .AND. LDWORK.LT.MAX( 1, N*(N + 2*M) + 3*M,
     $                                      NP*(N + 1) + 2*N ) ) ) THEN
         INFO = -26
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'FB01SD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( MAX( N, P ).EQ.0 ) THEN
         IF ( LJOBX ) THEN
            DWORK(1) = TWO
            DWORK(2) = ONE
         ELSE
            DWORK(1) = ONE
         END IF
         RETURN
      END IF
C
C     Construction of the needed part of the pre-array in DWORK.
C     To save workspace, only the blocks (1,3), (2,1)-(2,3), (3,2), and
C     (3,3) will be constructed when needed as shown below.
C
C     Storing SINV x AINV and SINV x AINV x B in the (1,1) and (1,2)
C     blocks of DWORK, respectively.
C     The variables called Ixy define the starting positions where the
C     (x,y) blocks of the pre-array are initially stored in DWORK.
C     Workspace: need N*(N+M).
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      LDW = N1
      I21 = N*N + 1
C
      CALL DLACPY( 'Full', N, N, AINV, LDAINV, DWORK, LDW )
      IF ( LMULTA ) THEN
         CALL DLACPY( 'Full', N, M, B, LDB, DWORK(I21), LDW )
      ELSE
         CALL DGEMM( 'No transpose', 'No transpose', N, M, N, ONE,
     $               DWORK, LDW, B, LDB, ZERO, DWORK(I21), LDW )
      END IF
      CALL DTRMM(  'Left', 'Upper', 'No transpose', 'Non-unit', N, N+M,
     $             ONE, SINV, LDSINV, DWORK, LDW )
C
C     Storing the process noise mean value in (1,3) block of DWORK.
C     Workspace: need N*(N+M) + M.
C
      I13 = N*( N + M ) + 1
C
      CALL DCOPY( M, Z, 1, DWORK(I13), 1 )
      CALL DTRMV( 'Upper', 'No transpose', 'Non-unit', M, QINV, LDQINV,
     $            DWORK(I13), 1 )
C
C     Computing SINV x X in X.
C
      CALL DTRMV( 'Upper', 'No transpose', 'Non-unit', N, SINV, LDSINV,
     $            X, 1 )
C
C     Triangularization (2 steps).
C
C     Step 1: annihilate the matrix SINV x AINV x B.
C     Workspace: need N*(N+2*M) + 3*M.
C
      I12   = I13  + M
      ITAU  = I12  + M*N
      JWORK = ITAU + M
C
      CALL MB04KD( 'Full', M, N, N, QINV, LDQINV, DWORK(I21), LDW,
     $             DWORK, LDW, DWORK(I12), M1, DWORK(ITAU),
     $             DWORK(JWORK) )
      WRKOPT = MAX( 1, N*( N + 2*M ) + 3*M )
C
      IF ( N.EQ.0 ) THEN
         CALL DCOPY( P, RINVY, 1, E, 1 )
         IF ( LJOBX )
     $      DWORK(2) = ONE
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Apply the transformations to the last column of the pre-array.
C     (Only the updated (2,3) block is now needed.)
C
      IJ = I21
C
      DO 10 I = 1, M
         CALL DAXPY( N, -DWORK(ITAU+I-1)*( DWORK(I13+I-1) +
     $               DDOT( N, DWORK(IJ), 1, X, 1 ) ),
     $                        DWORK(IJ), 1, X, 1 )
         IJ = IJ + N
   10 CONTINUE
C
C     Now, the workspace for SINV x AINV x B, as well as for the updated
C     (1,2) block of the pre-array, are no longer needed.
C     Move the computed (2,3) block of the pre-array in the (1,2) block
C     position of DWORK, to save space for the following computations.
C     Then, adjust the implicitly defined leading dimension of DWORK,
C     to make space for storing the (3,2) and (3,3) blocks of the
C     pre-array.
C     Workspace: need (N+P)*(N+1).
C
      CALL DCOPY( N, X, 1, DWORK(I21), 1 )
      LDW = MAX( 1, NP )
C
      DO 30 I = N + 1, 1, -1
         DO 20 IJ = N, 1, -1
            DWORK(NP*(I-1)+IJ) = DWORK(N*(I-1)+IJ)
   20    CONTINUE
   30 CONTINUE
C
C     Copy of RINV x C in the (2,1) block of DWORK.
C
      CALL DLACPY( 'Full', P, N, C, LDC, DWORK(N+1), LDW )
      IF ( .NOT.LMULTR )
     $   CALL DTRMM(  'Left', 'Upper', 'No transpose', 'Non-unit', P, N,
     $                ONE, RINV, LDRINV, DWORK(N+1), LDW )
C
C     Copy the inclusion measurement in the (2,2) block of DWORK.
C
      I21 = NP*N + 1
      I23 = I21  + N
      CALL DCOPY( P, RINVY, 1, DWORK(I23), 1 )
      WRKOPT = MAX( WRKOPT, NP*( N + 1 ) )
C
C     Step 2: QR factorization of the first block column of the matrix
C
C        [ SINV x AINV  SINV x X ]
C        [ RINV x C     RINV x Y ],
C
C     where the first block row was modified at Step 1.
C     Workspace: need   (N+P)*(N+1) + 2*N;
C                prefer (N+P)*(N+1) + N + N*NB.
C
      ITAU  = I21  + NP
      JWORK = ITAU + N
C
      CALL DGEQRF( NP, N, DWORK, LDW, DWORK(ITAU), DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
C
C     Apply the Householder transformations to the last column.
C     Workspace: need (N+P)*(N+1) + 1;  prefer (N+P)*(N+1) + NB.
C
      CALL DORMQR( 'Left', 'Transpose', NP, 1, N, DWORK, LDW,
     $             DWORK(ITAU), DWORK(I21), LDW, DWORK(JWORK),
     $             LDWORK-JWORK+1, INFO )
      WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) )+JWORK-1 )
C
C     Output SINV, X, and E and set the optimal workspace dimension
C     (and the reciprocal of the condition number estimate).
C
      CALL DLACPY( 'Upper', N, N, DWORK, LDW, SINV, LDSINV )
      CALL DCOPY( N, DWORK(I21), 1, X, 1 )
      CALL DCOPY( P, DWORK(I23), 1, E, 1 )
C
      IF ( LJOBX ) THEN
C
C        Compute X.
C        Workspace: need 3*N.
C
         CALL MB02OD( 'Left', 'Upper', 'No transpose', 'Non-unit',
     $                '1-norm', N, 1, ONE, SINV, LDSINV, X, N, RCOND,
     $                TOL, IWORK, DWORK, INFO )
         IF ( INFO.EQ.0 ) THEN
            WRKOPT = MAX( WRKOPT, 3*N )
            DWORK(2) = RCOND
         END IF
      END IF
C
      DWORK(1) = WRKOPT
C
      RETURN
C *** Last line of FB01SD ***
      END


!##############################################################################
! PROGRAM Plotgraphs3D
!
! ## How to plot graphs in 3D using the toolbox
!
! This code is published under the GNU General Public License v3
!                         (https://www.gnu.org/licenses/gpl-3.0.en.html)
!
! Authors: Hans Fehr and Fabian Kindermann
!          contact@ce-fortran.com
!
! #VC# VERSION: 1.0  (23 January 2018)
!
!##############################################################################
program Plotgraphs3D

    use toolbox

    implicit none
    integer, parameter :: nplot1 = 80
    integer, parameter :: nplot2 = 50
    real*8 :: x(0:nplot1), y(0:nplot2), z(0:nplot1, 0:nplot2)
    real*8 :: a(0:nplot1), b(0:nplot1), c(0:nplot1)
    integer :: i1, i2

    ! Initialize x values
    do i1 = 0, nplot1
        x(i1) = -5d0 + 10d0/dble(nplot1)*dble(i1)
    enddo

    ! initialize y values
    do i2 = 0, nplot2
        y(i2) = -3d0 + 6d0/dble(nplot2)*dble(i2)
    enddo

    ! get z values
    do i1 = 0, nplot1
        do i2 = 0, nplot2
            z(i1, i2) = sin(x(i1))*cos(y(i2))
        enddo
    enddo

    ! call 3D plotting routine
    call plot3d(x, y, z)

    ! call 3D plotting routine with complete configuration
    call plot3d(x, y, z, color='black', linewidth=0.5d0, marker=2, &
               markersize=0.3d0, noline=.false., &
               xlim=(/-6d0, 6d0/), xticks=1.0d0, xlabel='x-Axis', &
               ylim=(/-3d0, 3d0/), yticks=0.5d0, ylabel='y-Axis', &
               zlim=(/-1d0, 1d0/), zticks=0.2d0, zlabel='z-Axis', &
               zlevel=0d0, surf=.true., surf_color=2, &
               transparent=.true., view=(/70d0, 50d0/), &
               title='sin(x)*cos(y)', filename='testplot', &
               filetype='eps', output='testdata')

    ! initialize spiral data
    do i1 = 0, nplot1
        c(i1) = 20d0*dble(i1)/dble(nplot1)
        a(i1) = sin(c(i1))
        b(i1) = cos(c(i1))
    enddo

    ! plot spiral
    call plot3d(a, b, c, linewidth=2d0)

end program


IF (i < 0) THEN
   num_digits = two
ELSE
   num_digits = one
END IF
integ = ABS(i)
integ = integ / ten
DO WHILE (integ /= 0)
   num_digits = num_digits + one
   integ = integ / ten
END DO


*DECK D9LGIT
      DOUBLE PRECISION FUNCTION D9LGIT (A, X, ALGAP1)
C***BEGIN PROLOGUE  D9LGIT
C***SUBSIDIARY
C***PURPOSE  Compute the logarithm of Tricomi's incomplete Gamma
C            function with Perron's continued fraction for large X and
C            A .GE. X.
C***LIBRARY   SLATEC (FNLIB)
C***CATEGORY  C7E
C***TYPE      DOUBLE PRECISION (R9LGIT-S, D9LGIT-D)
C***KEYWORDS  FNLIB, INCOMPLETE GAMMA FUNCTION, LOGARITHM,
C             PERRON'S CONTINUED FRACTION, SPECIAL FUNCTIONS, TRICOMI
C***AUTHOR  Fullerton, W., (LANL)
C***DESCRIPTION
C
C Compute the log of Tricomi's incomplete gamma function with Perron's
C continued fraction for large X and for A .GE. X.
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  D1MACH, XERMSG
C***REVISION HISTORY  (YYMMDD)
C   770701  DATE WRITTEN
C   890531  Changed all specific intrinsics to generic.  (WRB)
C   890531  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900315  CALLs to XERROR changed to CALLs to XERMSG.  (THJ)
C   900720  Routine changed from user-callable to subsidiary.  (WRB)
C***END PROLOGUE  D9LGIT
      DOUBLE PRECISION A, X, ALGAP1, AX, A1X, EPS, FK, HSTAR, P, R, S,
     1  SQEPS, T, D1MACH
      LOGICAL FIRST
      SAVE EPS, SQEPS, FIRST
      DATA FIRST /.TRUE./
C***FIRST EXECUTABLE STATEMENT  D9LGIT
      IF (FIRST) THEN
         EPS = 0.5D0*D1MACH(3)
         SQEPS = SQRT(D1MACH(4))
      ENDIF
      FIRST = .FALSE.
C
      IF (X .LE. 0.D0 .OR. A .LT. X) CALL XERMSG ('SLATEC', 'D9LGIT',
     +   'X SHOULD BE GT 0.0 AND LE A', 2, 2)
C
      AX = A + X
      A1X = AX + 1.0D0
      R = 0.D0
      P = 1.D0
      S = P
      DO 20 K=1,200
        FK = K
        T = (A+FK)*X*(1.D0+R)
        R = T/((AX+FK)*(A1X+FK)-T)
        P = R*P
        S = S + P
        IF (ABS(P).LT.EPS*S) GO TO 30
 20   CONTINUE
      CALL XERMSG ('SLATEC', 'D9LGIT',
     +   'NO CONVERGENCE IN 200 TERMS OF CONTINUED FRACTION', 3, 2)
C
 30   HSTAR = 1.0D0 - X*S/A1X
      IF (HSTAR .LT. SQEPS) CALL XERMSG ('SLATEC', 'D9LGIT',
     +   'RESULT LESS THAN HALF PRECISION', 1, 1)
C
      D9LGIT = -X - ALGAP1 - LOG(HSTAR)
      RETURN
C
      END


SUBROUTINE p64(input_file,output_file)
!-------------------------------------------------------------------------
! Program 6.4 Plane strain slope stability analysis of an elastic-plastic
!             (Mohr-Coulomb) material using 8-node rectangular
!             quadrilaterals. Viscoplastic strain method.
!-------------------------------------------------------------------------
 USE main
 USE geom
  IMPLICIT NONE
 CHARACTER(len=60),INTENT(IN) :: input_file
 CHARACTER(len=60),INTENT(OUT) :: output_file

 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 INTEGER::i,iel,iters,iy,limit,ndim=2,ndof=16,nels,neq,nip=4,nlen,nn,     &
   nod=8,nodof=2,nprops=6,np_types,nsrf,nst=4,nx1,nx2,nye,ny1,ny2
 REAL(iwp)::cf,ddt,det,dq1,dq2,dq3,dsbar,dt=1.0e15_iwp,d4=4.0_iwp,        &
   d180=180.0_iwp,e,f,fmax,h1,h2,lode_theta,one=1.0_iwp,phi,phif,pi,psi,  &
   psif,sigm,snph,start_dt=1.e15_iwp,s1,tnph,tnps,tol,two=2.0_iwp,v,w1,w2,&
   zero=0.0_iwp
 CHARACTER(LEN=15)::argv,element='quadrilateral'
 LOGICAL::converged
!-----------------------dynamic arrays------------------------------------
 INTEGER,ALLOCATABLE::etype(:),g(:),g_g(:,:),g_num(:,:),kdiag(:),nf(:,:), &
   num(:)
 REAL(iwp),ALLOCATABLE::bdylds(:),bee(:,:),bload(:),coord(:,:),dee(:,:),  &
   devp(:),elastic(:),eld(:),eload(:),eps(:),erate(:),evp(:),evpt(:,:,:), &
   flow(:,:),fun(:),gravlo(:),g_coord(:,:),km(:,:),kv(:),loads(:),m1(:,:),&
   m2(:,:),m3(:,:),oldis(:),points(:,:),prop(:,:),sigma(:),srf(:),        &
   weights(:)
!-----------------------input and initialisation--------------------------

 OPEN(10,FILE=input_file)
 OPEN(11,FILE=output_file)
 READ(10,*)w1,s1,w2,h1,h2,nx1,nx2,ny1,ny2,np_types
 nye=ny1+ny2
 nels=nx1*nye+ny2*nx2
 nn=(3*nye+2)*nx1+2*nye+1+(3*ny2+2)*nx2
 ALLOCATE(nf(nodof,nn),points(nip,ndim),weights(nip),g_coord(ndim,nn),    &
   num(nod),dee(nst,nst),evpt(nst,nip,nels),coord(nod,ndim),fun(nod),     &
   g_g(ndof,nels),g_num(nod,nels),bee(nst,ndof),km(ndof,ndof),eld(ndof),  &
   eps(nst),sigma(nst),bload(ndof),eload(ndof),erate(nst),evp(nst),       &
   devp(nst),g(ndof),m1(nst,nst),m2(nst,nst),m3(nst,nst),flow(nst,nst),   &
   prop(nprops,np_types),etype(nels))
 READ(10,*)prop
 etype=1
 IF(np_types>1)READ(10,*)etype
 CALL emb_2d_bc(nx1,nx2,ny1,ny2,nf)
 neq=MAXVAL(nf)
 ALLOCATE(kdiag(neq),loads(0:neq),bdylds(0:neq),oldis(0:neq),             &
   gravlo(0:neq),elastic(0:neq))
   kdiag=0
!-----------------------loop the elements to find global arrays sizes-----
 elements_1: DO iel=1,nels
   CALL emb_2d_geom(iel,nx1,nx2,ny1,ny2,w1,s1,w2,h1,h2,coord,num)
   CALL num_to_g(num,nf,g)
   g_num(:,iel)=num
   g_coord(:,num)=TRANSPOSE(coord)
   g_g(:,iel)=g
   CALL fkdiag(kdiag,g)
 END DO elements_1
 CALL mesh(g_coord,g_num,argv,nlen,12)
 DO i=2,neq
   kdiag(i)=kdiag(i)+kdiag(i-1)
 END DO
 ALLOCATE(kv(kdiag(neq)))
 WRITE(11,'(2(A,i7))')                                                    &
   " There are",neq," equations and the skyline storage is",kdiag(neq)
 CALL sample(element,points,weights)
 kv=zero
 gravlo=zero
!-----------------------element stiffness integration and assembly--------
 elements_2: DO iel=1,nels
   CALL deemat(dee,prop(5,etype(iel)),prop(6,etype(iel)))
   num=g_num(:,iel)
   coord=TRANSPOSE(g_coord(:,num))
   g=g_g(:,iel)
   km=zero
   eld=zero
   gauss_pts_1: DO i=1,nip
     CALL shape_fun(fun,points,i)
     CALL bee8(bee,coord,points(i,1),points(i,2),det)
     km=km+MATMUL(MATMUL(TRANSPOSE(bee),dee),bee)*det*weights(i)
     eld(nodof:ndof:nodof)=eld(nodof:ndof:nodof)+fun(:)*det*weights(i)
   END DO gauss_pts_1
   CALL fsparv(kv,km,g,kdiag)
   gravlo(g)=gravlo(g)-eld*prop(4,etype(iel))
 END DO elements_2
!-----------------------factorise equations-------------------------------
 CALL sparin(kv,kdiag)
 pi=ACOS(-one)
!-----------------------trial strength reduction factor loop--------------
 READ(10,*)tol,limit,nsrf
 ALLOCATE(srf(nsrf))
 READ(10,*)srf
 WRITE(11,'(/A)')"    srf    max disp  iters"
 srf_trials: DO iy=1,nsrf
   dt=start_dt
   DO i=1,np_types
     phi=prop(1,i)
     tnph=TAN(phi*pi/d180)
     phif=ATAN(tnph/srf(iy))
     snph=SIN(phif)
     e=prop(5,i)
     v=prop(6,i)
     ddt=d4*(one+v)*(one-two*v)/(e*(one-two*v+snph**2))
     IF(ddt<dt)dt=ddt
   END DO
   iters=0
   bdylds=zero
   evpt=zero
   oldis=zero
!-----------------------plastic iteration loop----------------------------
   its: DO
     fmax=zero
     iters=iters+1
     loads=gravlo+bdylds
     CALL spabac(kv,loads,kdiag)
     loads(0)=zero
     IF(iy==1.AND.iters==1)elastic=loads
!-----------------------check plastic convergence-------------------------
     CALL checon(loads,oldis,tol,converged)
     IF(iters==1)converged=.FALSE.
     IF(converged.OR.iters==limit)bdylds=zero
!-----------------------go round the Gauss Points ------------------------
     elements_3: DO iel=1,nels
       bload=zero
       phi=prop(1,etype(iel))
       tnph=TAN(phi*pi/d180)
       phif=ATAN(tnph/srf(iy))*d180/pi
       psi=prop(3,etype(iel))
       tnps=TAN(psi*pi/d180)
       psif=ATAN(tnps/srf(iy))*d180/pi
       cf=prop(2,etype(iel))/srf(iy)
       e=prop(5,etype(iel))
       v=prop(6,etype(iel))
       CALL deemat(dee,e,v)
       num=g_num(:,iel)
       coord=TRANSPOSE(g_coord(:,num))
       g=g_g(:,iel)
       eld=loads(g)
       gauss_pts_2: DO i=1,nip
         CALL bee8(bee,coord,points(i,1),points(i,2),det)
         eps=MATMUL(bee,eld)
         eps=eps-evpt(:,i,iel)
         sigma=MATMUL(dee,eps)
         CALL invar(sigma,sigm,dsbar,lode_theta)
!-----------------------check whether yield is violated-------------------
         CALL mocouf(phif,cf,sigm,dsbar,lode_theta,f)
         IF(f>fmax)fmax=f
         IF(converged.OR.iters==limit)THEN
           devp=sigma
         ELSE
           IF(f>=zero.OR.(converged.OR.iters==limit))THEN
             CALL mocouq(psif,dsbar,lode_theta,dq1,dq2,dq3)
             CALL formm(sigma,m1,m2,m3)
             flow=f*(m1*dq1+m2*dq2+m3*dq3)
             erate=MATMUL(flow,sigma)
             evp=erate*dt
             evpt(:,i,iel)=evpt(:,i,iel)+evp
             devp=MATMUL(dee,evp)
           END IF
         END IF
         IF(f>=zero)THEN
           eload=MATMUL(devp,bee)
           bload=bload+eload*det*weights(i)
         END IF
       END DO gauss_pts_2
!-----------------------compute the total bodyloads vector----------------
       bdylds(g)=bdylds(g)+bload
       bdylds(0)=zero
     END DO elements_3
     WRITE(*,'(A,F7.2,A,I4,A,F8.3)')                                      &
       "  srf",srf(iy),"  iteration",iters,"  F_max",fmax
     IF(converged.OR.iters==limit)EXIT
   END DO its
   WRITE(11,'(F7.2,E12.4,I5)')srf(iy),MAXVAL(ABS(loads)),iters
   IF(iters==limit)EXIT
 END DO srf_trials
 CALL dismsh(loads-elastic,nf,0.1_iwp,g_coord,g_num,argv,nlen,13)
 CALL vecmsh(loads-elastic,nf,0.1_iwp,0.25_iwp,g_coord,g_num,argv,nlen,14)

END SUBROUTINE p64


      SUBROUTINE SB03OU( DISCR, LTRANS, N, M, A, LDA, B, LDB, TAU, U,
     $                   LDU, SCALE, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To solve for X = op(U)'*op(U) either the stable non-negative
C     definite continuous-time Lyapunov equation
C                                   2
C        op(A)'*X + X*op(A) = -scale *op(B)'*op(B)                   (1)
C
C     or the convergent non-negative definite discrete-time Lyapunov
C     equation
C                                   2
C        op(A)'*X*op(A) - X = -scale *op(B)'*op(B)                   (2)
C
C     where op(K) = K or K' (i.e., the transpose of the matrix K), A is
C     an N-by-N matrix in real Schur form, op(B) is an M-by-N matrix,
C     U is an upper triangular matrix containing the Cholesky factor of
C     the solution matrix X, X = op(U)'*op(U), and scale is an output
C     scale factor, set less than or equal to 1 to avoid overflow in X.
C     If matrix B has full rank then the solution matrix X will be
C     positive-definite and hence the Cholesky factor U will be
C     nonsingular, but if B is rank deficient then X may only be
C     positive semi-definite and U will be singular.
C
C     In the case of equation (1) the matrix A must be stable (that
C     is, all the eigenvalues of A must have negative real parts),
C     and for equation (2) the matrix A must be convergent (that is,
C     all the eigenvalues of A must lie inside the unit circle).
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     DISCR   LOGICAL
C             Specifies the type of Lyapunov equation to be solved as
C             follows:
C             = .TRUE. :  Equation (2), discrete-time case;
C             = .FALSE.:  Equation (1), continuous-time case.
C
C     LTRANS  LOGICAL
C             Specifies the form of op(K) to be used, as follows:
C             = .FALSE.:  op(K) = K    (No transpose);
C             = .TRUE. :  op(K) = K**T (Transpose).
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the matrix A and the number of columns in
C             matrix op(B).  N >= 0.
C
C     M       (input) INTEGER
C             The number of rows in matrix op(B).  M >= 0.
C
C     A       (input) DOUBLE PRECISION array, dimension (LDA,N)
C             The leading N-by-N upper Hessenberg part of this array
C             must contain a real Schur form matrix S. The elements
C             below the upper Hessenberg part of the array A are not
C             referenced. The 2-by-2 blocks must only correspond to
C             complex conjugate pairs of eigenvalues (not to real
C             eigenvalues).
C
C     LDA     INTEGER
C             The leading dimension of array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,N)
C             if LTRANS = .FALSE., and dimension (LDB,M), if
C             LTRANS = .TRUE..
C             On entry, if LTRANS = .FALSE., the leading M-by-N part of
C             this array must contain the coefficient matrix B of the
C             equation.
C             On entry, if LTRANS = .TRUE., the leading N-by-M part of
C             this array must contain the coefficient matrix B of the
C             equation.
C             On exit, if LTRANS = .FALSE., the leading
C             MIN(M,N)-by-MIN(M,N) upper triangular part of this array
C             contains the upper triangular matrix R (as defined in
C             METHOD), and the M-by-MIN(M,N) strictly lower triangular
C             part together with the elements of the array TAU are
C             overwritten by details of the matrix P (also defined in
C             METHOD). When M < N, columns (M+1),...,N of the array B
C             are overwritten by the matrix Z (see METHOD).
C             On exit, if LTRANS = .TRUE., the leading
C             MIN(M,N)-by-MIN(M,N) upper triangular part of
C             B(1:N,M-N+1), if M >= N, or of B(N-M+1:N,1:M), if M < N,
C             contains the upper triangular matrix R (as defined in
C             METHOD), and the remaining elements (below the diagonal
C             of R) together with the elements of the array TAU are
C             overwritten by details of the matrix P (also defined in
C             METHOD). When M < N, rows 1,...,(N-M) of the array B
C             are overwritten by the matrix Z (see METHOD).
C
C     LDB     INTEGER
C             The leading dimension of array B.
C             LDB >= MAX(1,M), if LTRANS = .FALSE.,
C             LDB >= MAX(1,N), if LTRANS = .TRUE..
C
C     TAU     (output) DOUBLE PRECISION array of dimension (MIN(N,M))
C             This array contains the scalar factors of the elementary
C             reflectors defining the matrix P.
C
C     U       (output) DOUBLE PRECISION array of dimension (LDU,N)
C             The leading N-by-N upper triangular part of this array
C             contains the Cholesky factor of the solution matrix X of
C             the problem, X = op(U)'*op(U).
C             The array U may be identified with B in the calling
C             statement, if B is properly dimensioned, and the
C             intermediate results returned in B are not needed.
C
C     LDU     INTEGER
C             The leading dimension of array U.  LDU >= MAX(1,N).
C
C     SCALE   (output) DOUBLE PRECISION
C             The scale factor, scale, set less than or equal to 1 to
C             prevent the solution overflowing.
C
C     Workspace
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, or INFO = 1, DWORK(1) returns the
C             optimal value of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK. LDWORK >= MAX(1,4*N).
C             For optimum performance LDWORK should sometimes be larger.
C
C             If LDWORK = -1, then a workspace query is assumed; the
C             routine only calculates the optimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message related to LDWORK is issued by
C             XERBLA.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value;
C             = 1:  if the Lyapunov equation is (nearly) singular
C                   (warning indicator);
C                   if DISCR = .FALSE., this means that while the matrix
C                   A has computed eigenvalues with negative real parts,
C                   it is only just stable in the sense that small
C                   perturbations in A can make one or more of the
C                   eigenvalues have a non-negative real part;
C                   if DISCR = .TRUE., this means that while the matrix
C                   A has computed eigenvalues inside the unit circle,
C                   it is nevertheless only just convergent, in the
C                   sense that small perturbations in A can make one or
C                   more of the eigenvalues lie outside the unit circle;
C                   perturbed values were used to solve the equation
C                   (but the matrix A is unchanged);
C             = 2:  if matrix A is not stable (that is, one or more of
C                   the eigenvalues of A has a non-negative real part),
C                   if DISCR = .FALSE., or not convergent (that is, one
C                   or more of the eigenvalues of A lies outside the
C                   unit circle), if DISCR = .TRUE.;
C             = 3:  if matrix A has two or more consecutive non-zero
C                   elements on the first sub-diagonal, so that there is
C                   a block larger than 2-by-2 on the diagonal;
C             = 4:  if matrix A has a 2-by-2 diagonal block with real
C                   eigenvalues instead of a complex conjugate pair.
C
C     METHOD
C
C     The method used by the routine is based on the Bartels and
C     Stewart method [1], except that it finds the upper triangular
C     matrix U directly without first finding X and without the need
C     to form the normal matrix op(B)'*op(B) [2].
C
C     If LTRANS = .FALSE., the matrix B is factored as
C
C        B = P ( R ),  M >= N,   B = P ( R  Z ),  M < N,
C              ( 0 )
C
C     (QR factorization), where P is an M-by-M orthogonal matrix and
C     R is a square upper triangular matrix.
C
C     If LTRANS = .TRUE., the matrix B is factored as
C
C        B = ( 0  R ) P,  M >= N,  B = ( Z ) P,  M < N,
C                                      ( R )
C
C     (RQ factorization), where P is an M-by-M orthogonal matrix and
C     R is a square upper triangular matrix.
C
C     These factorizations are used to solve the continuous-time
C     Lyapunov equation in the canonical form
C                                                        2
C       op(A)'*op(U)'*op(U) + op(U)'*op(U)*op(A) = -scale *op(F)'*op(F),
C
C     or the discrete-time Lyapunov equation in the canonical form
C                                                        2
C       op(A)'*op(U)'*op(U)*op(A) - op(U)'*op(U) = -scale *op(F)'*op(F),
C
C     where U and F are N-by-N upper triangular matrices, and
C
C        F = R,                                  if M >= N, or
C
C        F = ( R ),    if LTRANS = .FALSE.,  or
C            ( 0 )
C
C        F = ( 0  Z ), if LTRANS = .TRUE.,       if M < N.
C            ( 0  R )
C
C     The canonical equation is solved for U.
C
C     REFERENCES
C
C     [1] Bartels, R.H. and Stewart, G.W.
C         Solution of the matrix equation  A'X + XB = C.
C         Comm. A.C.M., 15, pp. 820-826, 1972.
C
C     [2] Hammarling, S.J.
C         Numerical solution of the stable, non-negative definite
C         Lyapunov equation.
C         IMA J. Num. Anal., 2, pp. 303-325, 1982.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations and is backward stable.
C
C     FURTHER COMMENTS
C
C     The Lyapunov equation may be very ill-conditioned. In particular,
C     if A is only just stable (or convergent) then the Lyapunov
C     equation will be ill-conditioned. "Large" elements in U relative
C     to those of A and B, or a "small" value for scale, are symptoms
C     of ill-conditioning. A condition estimate can be computed using
C     SLICOT Library routine SB03MD.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, May 1997.
C     Supersedes Release 2.0 routine SB03CZ by Sven Hammarling,
C     NAG Ltd, United Kingdom.
C     Partly based on routine PLYAPS by A. Varga, University of Bochum,
C     May 1992.
C
C     REVISIONS
C
C     Dec. 1997, April 1998, May 1999, July 2011.
C
C     KEYWORDS
C
C     Lyapunov equation, orthogonal transformation, real Schur form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, ONE
      PARAMETER         ( ZERO = 0.0D0, ONE = 1.0D0 )
C     .. Scalar Arguments ..
      LOGICAL           DISCR, LTRANS
      INTEGER           INFO, LDA, LDB, LDU, LDWORK, M, N
      DOUBLE PRECISION  SCALE
C     .. Array Arguments ..
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), DWORK(*), TAU(*), U(LDU,*)
C     .. Local Scalars ..
      LOGICAL           LQUERY
      INTEGER           I, J, K, L, MINWRK, MN, WRKOPT
C     .. External Subroutines ..
      EXTERNAL          DCOPY, DGEQRF, DGERQF, DLACPY, DLASET, SB03OT,
     $                  XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX, MIN
C     .. Executable Statements ..
C
      INFO = 0
C
C     Test the input scalar arguments.
C
      IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( M.LT.0 ) THEN
         INFO = -4
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF( ( LDB.LT.MAX( 1, M ) .AND. .NOT.LTRANS ) .OR.
     $         ( LDB.LT.MAX( 1, N ) .AND.      LTRANS ) ) THEN
         INFO = -8
      ELSE IF( LDU.LT.MAX( 1, N ) ) THEN
         INFO = -11
      ELSE
         MINWRK = MAX( 1, 4*N )
         LQUERY = LDWORK.EQ.-1
         IF( LQUERY ) THEN
            IF ( LTRANS ) THEN
               CALL DGERQF( N, M, B, LDB, TAU, DWORK, -1, INFO )
            ELSE
               CALL DGEQRF( M, N, B, LDB, TAU, DWORK, -1, INFO )
            END IF
            WRKOPT = MAX( MINWRK, INT( DWORK(1) ) )
         ELSE IF( LDWORK.LT.MINWRK ) THEN
            INFO = -14
         END IF
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'SB03OU', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         DWORK(1) = WRKOPT
         RETURN
      END IF
C
C     Quick return if possible.
C
      MN = MIN( N, M )
      IF ( MN.EQ.0 ) THEN
         SCALE = ONE
         DWORK(1) = ONE
         RETURN
      END IF
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the
C     code, as well as the preferred amount for good performance.
C     NB refers to the optimal block size for the immediately
C     following subroutine, as returned by ILAENV.)
C
      IF ( LTRANS ) THEN
C
C        Case op(K) = K'.
C
C        Perform the RQ factorization of B.
C        Workspace: need   N;
C                   prefer N*NB.
C
         CALL DGERQF( N, M, B, LDB, TAU, DWORK, LDWORK, INFO )
C
C        The triangular matrix F is constructed in the array U so that
C        U can share the same memory as B.
C
         IF ( M.GE.N ) THEN
            CALL DLACPY( 'Upper', MN, N, B(1,M-N+1), LDB, U, LDU )
         ELSE
C
            DO 10 I = M, 1, -1
               CALL DCOPY( N-M+I, B(1,I), 1, U(1,N-M+I), 1 )
   10       CONTINUE
C
            CALL DLASET( 'Full', N, N-M, ZERO, ZERO, U, LDU )
         END IF
      ELSE
C
C        Case op(K) = K.
C
C        Perform the QR factorization of B.
C        Workspace: need   N;
C                   prefer N*NB.
C
         CALL DGEQRF( M, N, B, LDB, TAU, DWORK, LDWORK, INFO )
         CALL DLACPY( 'Upper', MN, N, B, LDB, U, LDU )
         IF ( M.LT.N )
     $      CALL DLASET( 'Upper', N-M, N-M, ZERO, ZERO, U(M+1,M+1),
     $                   LDU )
      END IF
      WRKOPT = DWORK(1)
C
C     Solve the canonical Lyapunov equation
C                                                      2
C     op(A)'*op(U)'*op(U) + op(U)'*op(U)*op(A) = -scale *op(F)'*op(F),
C
C     or
C                                                      2
C     op(A)'*op(U)'*op(U)*op(A) - op(U)'*op(U) = -scale *op(F)'*op(F)
C
C     for U.
C
      CALL SB03OT( DISCR, LTRANS, N, A, LDA, U, LDU, SCALE, DWORK,
     $             INFO )
      IF ( INFO.NE.0 .AND. INFO.NE.1 )
     $   RETURN
C
C     Make the diagonal elements of U non-negative.
C
      IF ( LTRANS ) THEN
C
         DO 30 J = 1, N
            IF ( U(J,J).LT.ZERO ) THEN
C
               DO 20 I = 1, J
                  U(I,J) = -U(I,J)
   20          CONTINUE
C
            END IF
   30    CONTINUE
C
      ELSE
         K = 1
C
         DO 50 J = 1, N
            DWORK(K) = U(J,J)
            L = 1
C
            DO 40 I = 1, J
               IF ( DWORK(L).LT.ZERO ) U(I,J) = -U(I,J)
               L = L + 1
   40       CONTINUE
C
            K = K + 1
   50    CONTINUE
C
      END IF
C
      DWORK(1) = MAX( WRKOPT, 4*N )
      RETURN
C *** Last line of SB03OU ***
      END


      SUBROUTINE sphbes(n,x,sj,sy,sjp,syp)
      INTEGER n
      REAL sj,sjp,sy,syp,x
CU    USES bessjy
      REAL factor,order,rj,rjp,ry,ryp,RTPIO2
      PARAMETER (RTPIO2=1.2533141)
      if(n.lt.0.or.x.le.0.)pause 'bad arguments in sphbes'
      order=n+0.5
      call bessjy(x,order,rj,ry,rjp,ryp)
      factor=RTPIO2/sqrt(x)
      sj=factor*rj
      sy=factor*ry
      sjp=factor*rjp-sj/(2.*x)
      syp=factor*ryp-sy/(2.*x)
      return
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/sphbes.for


      SUBROUTINE MB03MD( N, L, THETA, Q, E, Q2, E2, PIVMIN, TOL, RELTOL,
     $                   IWARN, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute an upper bound THETA using a bisection method such that
C     the bidiagonal matrix
C
C              |q(1) e(1)  0    ...   0   |
C              | 0   q(2) e(2)        .   |
C          J = | .                    .   |
C              | .                  e(N-1)|
C              | 0   ...        ...  q(N) |
C
C     has precisely L singular values less than or equal to THETA plus
C     a given tolerance TOL.
C
C     This routine is mainly intended to be called only by other SLICOT
C     routines.
C
C     ARGUMENTS
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the bidiagonal matrix J.  N >= 0.
C
C     L       (input/output) INTEGER
C             On entry, L must contain the number of singular values
C             of J which must be less than or equal to the upper bound
C             computed by the routine.  0 <= L <= N.
C             On exit, L may be increased if the L-th smallest singular
C             value of J has multiplicity greater than 1. In this case,
C             L is increased by the number of singular values of J which
C             are larger than its L-th smallest one and approach the
C             L-th smallest singular value of J within a distance less
C             than TOL.
C             If L has been increased, then the routine returns with
C             IWARN set to 1.
C
C     THETA   (input/output) DOUBLE PRECISION
C             On entry, THETA must contain an initial estimate for the
C             upper bound to be computed. If THETA < 0.0 on entry, then
C             one of the following default values is used.
C             If L = 0, THETA is set to 0.0 irrespective of the input
C             value of THETA; if L = 1, then THETA is taken as
C             MIN(ABS(Q(i))), for i = 1,2,...,N; otherwise, THETA is
C             taken as ABS(Q(N-L+1)).
C             On exit, THETA contains the computed upper bound such that
C             the bidiagonal matrix J has precisely L singular values
C             less than or equal to THETA + TOL.
C
C     Q       (input) DOUBLE PRECISION array, dimension (N)
C             This array must contain the diagonal elements q(1),
C             q(2),...,q(N) of the bidiagonal matrix J. That is,
C             Q(i) = J(i,i) for i = 1,2,...,N.
C
C     E       (input) DOUBLE PRECISION array, dimension (N-1)
C             This array must contain the superdiagonal elements
C             e(1),e(2),...,e(N-1) of the bidiagonal matrix J. That is,
C             E(k) = J(k,k+1) for k = 1,2,...,N-1.
C
C     Q2      (input) DOUBLE PRECISION array, dimension (N)
C             This array must contain the squares of the diagonal
C             elements q(1),q(2),...,q(N) of the bidiagonal matrix J.
C             That is, Q2(i) = J(i,i)**2 for i = 1,2,...,N.
C
C     E2      (input) DOUBLE PRECISION array, dimension (N-1)
C             This array must contain the squares of the superdiagonal
C             elements e(1),e(2),...,e(N-1) of the bidiagonal matrix J.
C             That is, E2(k) = J(k,k+1)**2 for k = 1,2,...,N-1.
C
C     PIVMIN  (input) DOUBLE PRECISION
C             The minimum absolute value of a "pivot" in the Sturm
C             sequence loop.
C             PIVMIN >= max( max( |q(i)|, |e(k)| )**2*sf_min, sf_min ),
C             where i = 1,2,...,N, k = 1,2,...,N-1, and sf_min is at
C             least the smallest number that can divide one without
C             overflow (see LAPACK Library routine DLAMCH).
C             Note that this condition is not checked by the routine.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             This parameter defines the multiplicity of singular values
C             by considering all singular values within an interval of
C             length TOL as coinciding. TOL is used in checking how many
C             singular values are less than or equal to THETA. Also in
C             computing an appropriate upper bound THETA by a bisection
C             method, TOL is used as a stopping criterion defining the
C             minimum (absolute) subinterval width.  TOL >= 0.
C
C     RELTOL  DOUBLE PRECISION
C             This parameter specifies the minimum relative width of an
C             interval. When an interval is narrower than TOL, or than
C             RELTOL times the larger (in magnitude) endpoint, then it
C             is considered to be sufficiently small and bisection has
C             converged.
C             RELTOL >= BASE * EPS, where BASE is machine radix and EPS
C             is machine precision (see LAPACK Library routine DLAMCH).
C
C     Warning Indicator
C
C     IWARN   INTEGER
C             = 0:  no warnings;
C             = 1:  if the value of L has been increased as the L-th
C                   smallest singular value of J coincides with the
C                   (L+1)-th smallest one.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     Let s(i), i = 1,2,...,N, be the N non-negative singular values of
C     the bidiagonal matrix J arranged so that s(1) >= ... >= s(N) >= 0.
C     The routine then computes an upper bound T such that s(N-L) > T >=
C     s(N-L+1) as follows (see [2]).
C     First, if the initial estimate of THETA is not specified by the
C     user then the routine initialises THETA to be an estimate which
C     is close to the requested value of THETA if s(N-L) >> s(N-L+1).
C     Second, a bisection method (see [1, 8.5]) is used which generates
C     a sequence of shrinking intervals [Y,Z] such that either THETA in
C     [Y,Z] was found (so that J has L singular values less than or
C     equal to THETA), or
C
C        (number of s(i) <= Y) < L < (number of s(i) <= Z).
C
C     This bisection method is applied to an associated 2N-by-2N
C     symmetric tridiagonal matrix T" whose eigenvalues (see [1]) are
C     given by s(1),s(2),...,s(N),-s(1),-s(2),...,-s(N). One of the
C     starting values for the bisection method is the initial value of
C     THETA. If this value is an upper bound, then the initial lower
C     bound is set to zero, else the initial upper bound is computed
C     from the Gershgorin Circle Theorem [1, Theorem 7.2-1], applied to
C     T". The computation of the "number of s(i) <= Y (or Z)" is
C     achieved by calling SLICOT Library routine MB03ND, which applies
C     Sylvester's Law of Inertia or equivalently Sturm sequences
C     [1, 8.5] to the associated matrix T". If
C
C        Z - Y <= MAX( TOL, PIVMIN, RELTOL*MAX( ABS( Y ), ABS( Z ) ) )
C
C     at some stage of the bisection method, then at least two singular
C     values of J lie in the interval [Y,Z] within a distance less than
C     TOL from each other. In this case, s(N-L) and s(N-L+1) are assumed
C     to coincide, the upper bound T is set to the value of Z, the value
C     of L is increased and IWARN is set to 1.
C
C     REFERENCES
C
C     [1] Golub, G.H. and Van Loan, C.F.
C         Matrix Computations.
C         The Johns Hopkins University Press, Baltimore, Maryland, 1983.
C
C     [2] Van Huffel, S. and Vandewalle, J.
C         The Partial Total Least Squares Algorithm.
C         J. Comput. and Appl. Math., 21, pp. 333-341, 1988.
C
C     NUMERICAL ASPECTS
C
C     None.
C
C     CONTRIBUTOR
C
C     Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Apr. 1997.
C     Supersedes Release 2.0 routine MB03AD by S. Van Huffel, Katholieke
C     University, Leuven, Belgium.
C
C     REVISIONS
C
C     June 16, 1997, Oct. 26, 2003.
C
C     KEYWORDS
C
C     Bidiagonal matrix, singular values.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ZERO, TWO
      PARAMETER         ( ZERO = 0.0D0, TWO = 2.0D0 )
      DOUBLE PRECISION  FUDGE
      PARAMETER         ( FUDGE = TWO )
C     .. Scalar Arguments ..
      INTEGER           INFO, IWARN, L, N
      DOUBLE PRECISION  PIVMIN, RELTOL, THETA, TOL
C     .. Array Arguments ..
      DOUBLE PRECISION  E(*), E2(*), Q(*), Q2(*)
C     .. Local Scalars ..
      INTEGER           I, NUM, NUMZ
      DOUBLE PRECISION  H, TH, Y, Z
C     .. External Functions ..
      INTEGER           MB03ND
      DOUBLE PRECISION  DLAMCH, MB03MY
      EXTERNAL          DLAMCH, MB03MY, MB03ND
C     .. External Subroutines ..
      EXTERNAL          XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         ABS, DBLE, MAX
C     .. Executable Statements ..
C
C     Test some input scalar arguments.
C
      IWARN = 0
      INFO = 0
      IF( N.LT.0 ) THEN
         INFO = -1
      ELSE IF( L.LT.0 .OR. L.GT.N ) THEN
         INFO = -2
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'MB03MD', -INFO )
         RETURN
      END IF
C
C     Quick return if possible.
C
      IF ( N.EQ.0 )
     $    RETURN
C
C     Step 1: initialisation of THETA.
C             -----------------------
      IF ( L.EQ.0 ) THETA = ZERO
      IF ( THETA.LT.ZERO ) THEN
         IF ( L.EQ.1 ) THEN
C
C           An upper bound which is close if S(N-1) >> S(N):
C
            THETA = MB03MY( N, Q, 1 )
            IF ( N.EQ.1 )
     $         RETURN
         ELSE
C
C           An experimentally established estimate which is good if
C           S(N-L) >> S(N-L+1):
C
            THETA = ABS( Q(N-L+1) )
         END IF
      END IF
C
C     Step 2: Check quality of initial estimate THETA.
C             ---------------------------------------
      NUM = MB03ND( N, THETA, Q2, E2, PIVMIN, INFO )
      IF ( NUM.EQ.L )
     $   RETURN
C
C     Step 3: initialisation starting values for bisection method.
C             ---------------------------------------------------
C     Let S(i), i=1,...,N, be the singular values of J in decreasing
C     order. Then, the computed Y and Z will be such that
C     (number of S(i) <= Y) < L < (number of S(i) <= Z).
C
      IF ( NUM.LT.L ) THEN
         TH = ABS( Q(1) )
         Z = ZERO
         Y = THETA
         NUMZ = N
C
         DO 20 I = 1, N - 1
            H = ABS( Q(I+1) )
            Z  = MAX( MAX( TH, H ) + ABS( E(I) ), Z )
            TH = H
   20    CONTINUE
C
C        Widen the Gershgorin interval a bit for machines with sloppy
C        arithmetic.
C
         Z = Z + FUDGE*ABS( Z )*DLAMCH( 'Epsilon' )*DBLE( N )
     $         + FUDGE*PIVMIN
      ELSE
         Z = THETA
         Y = ZERO
         NUMZ = NUM
      END IF
C
C     Step 4: Bisection method for finding the upper bound on the L
C             smallest singular values of the bidiagonal.
C             ------------------------------------------
C     A sequence of subintervals [Y,Z] is produced such that
C         (number of S(i) <= Y) < L < (number of S(i) <= Z).
C     NUM : number of S(i) <= TH,
C     NUMZ: number of S(i) <= Z.
C
C     WHILE ( ( NUM .NE. L ) .AND.
C             ( ( Z-Y ) .GT. MAX( TOL, PIVMIN, RELTOL*ABS( Z ) ) ) ) DO
   40 IF ( ( NUM.NE.L ) .AND.
     $     ( ABS( Z-Y ).GT.MAX( TOL, PIVMIN,
     $                          RELTOL*MAX( ABS( Y ), ABS( Z ) ) ) ) )
     $      THEN
         TH = ( Y + Z )/TWO
         NUM = MB03ND( N, TH, Q2, E2, PIVMIN, INFO )
         IF ( NUM.LT.L ) THEN
            Y = TH
         ELSE
            Z = TH
            NUMZ = NUM
         END IF
         GO TO 40
      END IF
C     END WHILE 40
C
C     If NUM <> L and ( Z - Y ) <= TOL, then at least two singular
C     values of J lie in the interval [Y,Z] within a distance less than
C     TOL from each other. S(N-L) and S(N-L+1) are then assumed to
C     coincide. L is increased, and a warning is given.
C
      IF ( NUM.NE.L ) THEN
         L = NUMZ
         THETA = Z
         IWARN = 1
      ELSE
         THETA = TH
      END IF
C
      RETURN
C *** Last line of MB03MD ***
      END


!===============================================================================
program test
!===============================================================================

   use anisotropy_ajn

   implicit none

   integer, parameter :: rs = 8
   real(rs) :: C(6,6), Cave(6,6), rho

   ! Constants of San Carlos olivine, Abramson et al, 1997
   C(1,1) = 320.50e9
   C(1,2) = 68.10e9
   C(1,3) = 71.60e9
   C(1,4) = 0.00e9
   C(1,5) = 0.00e9
   C(1,6) = 0.00e9
   C(2,1) = 68.10e9
   C(2,2) = 196.50e9
   C(2,3) = 76.80e9
   C(2,4) = 0.00e9
   C(2,5) = 0.00e9
   C(2,6) = 0.00e9
   C(3,1) = 71.60e9
   C(3,2) = 76.80e9
   C(3,3) = 233.50e9
   C(3,4) = 0.00e9
   C(3,5) = 0.00e9
   C(3,6) = 0.00e9
   C(4,1) = 0.00e9
   C(4,2) = 0.00e9
   C(4,3) = 0.00e9
   C(4,4) = 64.00e9
   C(4,5) = 0.00e9
   C(4,6) = 0.00e9
   C(5,1) = 0.00e9
   C(5,2) = 0.00e9
   C(5,3) = 0.00e9
   C(5,4) = 0.00e9
   C(5,5) = 77.00e9
   C(5,6) = 0.00e9
   C(6,1) = 0.00e9
   C(6,2) = 0.00e9
   C(6,3) = 0.00e9
   C(6,4) = 0.00e9
   C(6,5) = 0.00e9
   C(6,6) = 78.70e9
   rho = 3355.e0

   ! Rotate a little bit to remove the perfect match when using few rotations
   call CIJ_rot3(C,13._rs,194._rs,38._rs,Cave)
   C = Cave

   ! Rotate about the 3-axis
   call CIJ_axial_average(C,3,Cave,nrot=10,ave_type='VRH')

   ! Write out constants density normalised
   write(*,*) Cave/rho

end program test
!-------------------------------------------------------------------------------


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
cc                                                                    cc
cc              mnevpo : evapol test program                          cc
cc                                                                    cc
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      real fac,r1,r2
      integer i,ir,j,m,mx,my,m0,m1,m2,nc,nu4,nv4,nu,nv
      real tu(11),tv(10),c(42),x(6),y(6),f(36)
      external r1,r2
c  we set up the grid points for evaluating the polar spline.
      mx = 6
      my = 6
      do 10 i=1,6
      x(i) = (2*i-7)*0.1
      y(i) = x(i)
  10  continue
c  the interior knots with respect to the u-variable.
      tu(5) = 0.4
      tu(6) = 0.7
      tu(7) = 0.9
      nu = 11
c  the interior knots with respect to the v-variable.
      tv(5) = 0.3
      tv(6) = 0.8
      nv = 10
c  the boundary knots
      do 20 i=1,4
        tu(i) = 0.
        tv(i) = 0.
        tu(i+7) = 1.
        tv(i+6) = 1.
  20  continue
c  the number of b-spline coefficients
      nu4 = nu-4
      nv4 = nv-4
      nc = nu4*nv4
c  we generate the b-spline coefficients for the function s(u,v)=u**2
      m0 = 1
      m1 = m0+nv4
      c(m0) = 0.
      c(m1) = 0.
      do 70 i=3,nu4
        m2 = m1+nv4
        c(m2) = c(m1)+(tu(i+3)-tu(i))*((c(m1)-c(m0))/(tu(i+2)-tu(i-1))
     *    +(tu(i+2)-tu(i))/3.)
        m0 = m1
        m1 = m2
  70  continue
      do 80 i=1,nu4
        m0 = (i-1)*nv4+1
        fac = c(m0)
        do 80 j=2,nv4
          m0 = m0+1
          c(m0) = fac
  80  continue
      write(6,900)
      write(6,910)
      write(6,920) (tu(i),i=1,nu)
      write(6,930)
      write(6,920) (tv(i),i=1,nv)
      write(6,940)
      write(6,950) (c(j),j=1,nc)
c the spline s(u,v) defines a function f(x,y) through the transformation
c    x = r(v)*u*cos(v)   y = r(v)*u*sin(v)
c we consider two different functions r(v)
      do 300 ir=1,2
        go to (110,130),ir
c if r(v) =1 and s(u,v) = u**2 then f(x,y) = x**2+y**2
c evaluation of f(x,y)
 110    m = 0
        do 120 i=1,my
          do 120 j=1,mx
            m = m+1
            f(m) = evapol(tu,nu,tv,nv,c,r1,x(j),y(i))
 120    continue
        write(6,960)
        go to 200
c if r(v) = (1+cos(v)**2)/2 and s(u,v) = u**2 then f(x,y) =
c    4*(x**2+y**2)**3/(4*x**4 +y**4 +4*x**2*y**2)
c evaluation of f(x,y)
 130    m = 0
        do 140 i=1,my
          do 140 j=1,mx
            m = m+1
            f(m) = evapol(tu,nu,tv,nv,c,r2,x(j),y(i))
 140    continue
        write(6,965)
 200    write(6,970) (x(i),i=1,mx)
        write(6,975)
        m1 = 0
        do 210 j=1,my
          m0 = m1+1
          m1 = m1+mx
          write(6,980) y(j),(f(m),m=m0,m1)
 210    continue
 300  continue
      stop
c  format statements.
 900  format(24h0polar spline evaluation)
 910  format(1x,40hposition of the knots in the u-direction)
 920  format(1x,15f5.1)
 930  format(1x,40hposition of the knots in the v-direction)
 940  format(23h b-spline coefficients )
 950  format(5x,8f9.4)
 960  format(1h0,30hf(x,y) corresponding to r(v)=1)
 965  format(1h0,44hf(x,y) corresponding to r(v)=(1+cos(v)**2)/2)
 970  format(1h0,1hx,2x,6f7.1)
 975  format(3x,1hy)
 980  format(1x,f4.1,6f7.3)
      end
      real function r1(v)
      real v
      r1 = 1.
      return
      end
      real function r2(v)
      real v,cos
      r2 = (1.+cos(v)**2)*0.5
      return
      end


SUBROUTINE p73(input_file,output_file)
!-------------------------------------------------------------------------
! SUBROUTINE 7.3 Analysis of plane free-surface flow using 4-node
!             quadrilaterals. "Analytical" form of element conductivity
!             matrix.
!-------------------------------------------------------------------------
 USE main;
 USE geom;
 IMPLICIT NONE
 CHARACTER(len=60),INTENT(IN) :: input_file
 CHARACTER(len=60),INTENT(OUT) :: output_file
 INTEGER,PARAMETER::iwp=SELECTED_REAL_KIND(15)
 INTEGER::fixed_down,fixed_seep,fixed_up,i,iel,iters,k,limit,nci,ndim=2,  &
   nels,neq,nlen,nod=4,nn,nxe,nye,np_types;
   CHARACTER(LEN=15)::argv
 REAL(iwp)::d180=180.0_iwp,hdown,hup,initial_height,one=1.0_iwp,          &
   penalty=1.e20_iwp,tol,zero=0.0_iwp;
 LOGICAL::converged
!-----------------------dynamic arrays------------------------------------
 INTEGER,ALLOCATABLE::etype(:),g_num(:,:),kdiag(:),node_down(:),          &
   node_seep(:),node_up(:),num(:)
 REAL(iwp),ALLOCATABLE::angs(:),bottom_width(:),coord(:,:),disps(:),      &
   g_coord(:,:),kay(:,:),kp(:,:),kv(:),kvh(:),loads(:),oldpot(:),         &
   prop(:,:),surf(:),top_width(:)
!-----------------------input and initialisation--------------------------

 OPEN(10,FILE=input_file);
 OPEN(11,FILE=output_file)
 READ(10,*)nxe,nye,tol,limit,np_types;
 nels=nxe*nye
 nn=(nxe+1)*(nels/nxe+1);
 neq=nn
 ALLOCATE(g_coord(ndim,nn),coord(nod,ndim),bottom_width(nxe+1),           &
   top_width(nxe+1),surf(nxe+1),angs(nxe+1),kp(nod,nod),num(nod),         &
   g_num(nod,nels),prop(ndim,np_types),kdiag(neq),kay(ndim,ndim),         &
   etype(nels),loads(0:neq),disps(0:neq),oldpot(0:neq))
 READ(10,*)prop;
 etype=1;
 IF(np_types>1)READ(10,*)etype
 READ(10,*)bottom_width;
 READ(10,*)top_width;
 READ(10,*)initial_height
 surf=initial_height
 angs=ATAN(surf/(top_width-bottom_width))*d180/acos(-one)
 READ(10,*)hup,fixed_up;
 ALLOCATE(node_up(fixed_up));
 READ(10,*)node_up
 READ(10,*)hdown,fixed_down;
 ALLOCATE(node_down(fixed_down))
 READ(10,*)node_down;
 fixed_seep=nels/nxe-fixed_down
 ALLOCATE(node_seep(fixed_seep))
 DO i=1,fixed_seep;
   node_seep(i)=i*(nxe+1)+1;
 END DO;
 kdiag=0
!-----------------------loop the elements to find global arrays sizes-----
 elements_1: DO iel=1,nels
   CALL geom_freesurf(iel,nxe,fixed_seep,fixed_down,                      &
     hdown,bottom_width,angs,surf,coord,num);
   CALL fkdiag(kdiag,num)
 END DO elements_1
 DO i=2,neq;
   kdiag(i)=kdiag(i)+kdiag(i-1);
 END DO
 ALLOCATE(kv(kdiag(neq)),kvh(kdiag(neq)))
 WRITE(11,'(2(A,I5))')                                                    &
   "There are ",neq,"  equations and the skyline storage is ",kdiag(neq)
!-----------------------global conductivity matrix assembly---------------
 oldpot=zero;
 iters=0
 its: DO
   iters=iters+1;
   kv=zero
   elements_2: DO iel=1,nels
     kay=zero;
     DO i=1,ndim;
       kay(i,i)=prop(i,etype(iel));
     END DO
     CALL geom_freesurf(iel,nxe,fixed_seep,fixed_down,                    &
       hdown,bottom_width,angs,surf,coord,num)
     g_num(:,iel)=num;
     g_coord(:,num)=TRANSPOSE(coord)
     CALL seep4(kp,coord,kay);
     CALL fsparv(kv,kp,num,kdiag)
   END DO elements_2;
   kvh=kv
!-----------------------specify boundary values---------------------------
   loads=zero;
   kv(kdiag(node_up))=kv(kdiag(node_up))+penalty
   loads(node_up)=kv(kdiag(node_up))*hup
   kv(kdiag(node_down))=kv(kdiag(node_down))+penalty
   loads(node_down)=kv(kdiag(node_down))*hdown
   kv(kdiag(node_seep))=kv(kdiag(node_seep))+penalty
   DO i=1,fixed_seep
     loads(node_seep(i))=kv(kdiag(node_seep(i)))*                         &
       (hdown+(surf(1)-hdown)*(fixed_seep+1-i)/(fixed_seep+1))
   END DO
!-----------------------equation solution---------------------------------
   CALL sparin(kv,kdiag);
   CALL spabac(kv,loads,kdiag)
   surf(1:nxe)=loads(1:nxe)
!-----------------------check convergence---------------------------------
   CALL checon(loads,oldpot,tol,converged)
   IF(converged.OR.iters==limit)EXIT
 END DO its;
 CALL linmul_sky(kvh,loads,disps,kdiag)
 WRITE(11,'(/A)')"  Node Total Head  Flow rate"
 DO k=1,nn;
   WRITE(11,'(I5,2E12.4)')k,loads(k),disps(k);
 END DO
 disps(0)=zero;
 WRITE(11,'(/A)')"       Inflow      Outflow"
 WRITE(11,'(5X,2E12.4)')                                                  &
    SUM(disps,MASK=disps<zero),SUM(disps,MASK=disps>zero)
 WRITE(11,'(/A,I3,A)')" Converged in",iters," iterations"
 CALL mesh(g_coord,g_num,argv,nlen,12)
 READ(10,*)nci;
 CALL contour(loads,g_coord,g_num,nci,argv,nlen,13)

END SUBROUTINE p73


      subroutine forif (fun,n,m,a,b,ier)
C...subroutine     forif     01/09/73
C...subroutine     forif     01/09/78
      dimension a(1),b(1)
	external fun
C        check for parametr errors
      ier=0
  20  if(m.ge.0) goto 40
      ier=2
      return
  40  if (m.le.n) goto 60
      ier=1
      return
C        compute and preset constants
  60  an=n
      coef=2.0/(2.0*an+1.0)
      const=3.141593*coef
      s1=sin(const)
      c1=cos(const)
      c=1.0
      s=0.0
      j=1
      funz=fun(0.0)
  70  u2=0.0
      u1=0.0
      ai=2*n
C        form fourier coefficients recursively
  75  x=ai*const
      u0=fun(x)+2.0*c*u1-u2
      u2=u1
      u1=u0
      ai=ai-1.0
      if (ai.gt.0.) goto 75
      a(j)=coef*(funz+c*u1-u2)
      b(j)=coef*s*u1
      if (j.ge.(m+1)) goto 100
      q=c1*c-s1*s
      s=c1*s+s1*c
      c=q
      j=j+1
      go to 70
 100  a(1)=a(1)*0.5
      return
      end