      subroutine resid_dgx(ctfmp,n_A,n_Axx,n_alpha,n_beta,n_divbeta,np1_
     &A,np1_Axx,np1_alpha,np1_beta,np1_divbeta,x,Nx,ht,hx,myzero,vee,zep
     &sdis,phys_bdy,res)
      implicit none
      integer i
      integer Nx
      real*8 ht
      real*8 hx
      real*8 myzero
      real*8 vee
      real*8 zepsdis
      real*8 ctfmp(Nx)
      real*8 n_A(Nx)
      real*8 n_Axx(Nx)
      real*8 n_alpha(Nx)
      real*8 n_beta(Nx)
      real*8 n_divbeta(Nx)
      real*8 np1_A(Nx)
      real*8 np1_Axx(Nx)
      real*8 np1_alpha(Nx)
      real*8 np1_beta(Nx)
      real*8 np1_divbeta(Nx)
      real*8 x(Nx)
      integer phys_bdy(2)
      real*8 res
      real*8 qb
      res = 0.0D0
      if (phys_bdy(1) .eq. 1) then
      do i=1, 1, 1
      qb = np1_A(i) - 0.1D1 + myzero * x(i)
      res = res + qb**2
      end do
      endif
      if (phys_bdy(1) .eq. 1) then
      do i=2, 2, 1
      qb = -0.5000000000000000D0 * (0.3D1 * np1_A(i - 1) - 0.4D1 * np1_A
     #(i) + np1_A(i + 1)) / hx + 0.3125000000000000D-1 * zepsdis / ht *
     #(0.7D1 * np1_A(i) + np1_A(i + 2) - 0.4D1 * np1_A(i + 1) - 0.4D1 *
     #np1_A(i - 1)) + 0.3125000000000000D-1 * zepsdis / ht * (0.7D1 * n_
     #A(i) + n_A(i + 2) - 0.4D1 * n_A(i + 1) - 0.4D1 * n_A(i - 1))
      res = res + qb**2
      end do
      endif
      do i=3, Nx-2, 1
      qb = -0.1D1 * (n_A(i) - 0.1D1 * np1_A(i)) / ht + np1_Axx(i) * np1_
     #alpha(i) + 0.3333333333333333D0 * np1_A(i) * vee * np1_divbeta(i)
     #- 0.5000000000000000D0 * ((-0.1D1 * np1_beta(i - 1) + np1_beta(i +
     # 1)) * np1_A(i) + 0.5000000000000000D0 * np1_beta(i) * (-0.1D1 * n
     #p1_A(i - 1) + np1_A(i + 1))) / ctfmp(i) / hx + n_Axx(i) * n_alpha(
     #i) + 0.3333333333333333D0 * n_A(i) * vee * n_divbeta(i) - 0.500000
     #0000000000D0 * ((-0.1D1 * n_beta(i - 1) + n_beta(i + 1)) * n_A(i)
     #+ 0.5000000000000000D0 * n_beta(i) * (-0.1D1 * n_A(i - 1) + n_A(i
     #+ 1))) / ctfmp(i) / hx + 0.3125000000000000D-1 * zepsdis / ht * (0
     #.6D1 * np1_A(i) + np1_A(i + 2) + np1_A(i - 2) - 0.4D1 * np1_A(i +
     #1) - 0.4D1 * np1_A(i - 1)) + 0.3125000000000000D-1 * zepsdis / ht
     #* (0.6D1 * n_A(i) + n_A(i + 2) + n_A(i - 2) - 0.4D1 * n_A(i + 1) -
     # 0.4D1 * n_A(i - 1))
      res = res + qb**2
      end do
      do i=Nx-1, Nx-1, 1
      qb = -0.1D1 * (n_A(i) - 0.1D1 * np1_A(i)) / ht + np1_Axx(i) * np1_
     #alpha(i) + 0.3333333333333333D0 * np1_A(i) * vee * np1_divbeta(i)
     #- 0.5000000000000000D0 * ((-0.1D1 * np1_beta(i - 1) + np1_beta(i +
     # 1)) * np1_A(i) + 0.5000000000000000D0 * np1_beta(i) * (-0.1D1 * n
     #p1_A(i - 1) + np1_A(i + 1))) / ctfmp(i) / hx + n_Axx(i) * n_alpha(
     #i) + 0.3333333333333333D0 * n_A(i) * vee * n_divbeta(i) - 0.500000
     #0000000000D0 * ((-0.1D1 * n_beta(i - 1) + n_beta(i + 1)) * n_A(i)
     #+ 0.5000000000000000D0 * n_beta(i) * (-0.1D1 * n_A(i - 1) + n_A(i
     #+ 1))) / ctfmp(i) / hx
      res = res + qb**2
      end do
      if (phys_bdy(2) .eq. 1) then
      do i=Nx, Nx, 1
      qb = -0.1D1 * (n_A(i) - 0.1D1 * np1_A(i)) / ht - 0.1D1 * myzero *
     #x(i)
      res = res + qb**2
      end do
      endif
      res = sqrt(res/(1*Nx))
      END


c     Subroutine to find maximum distance
c     AJ_Kettle, 28Jun2019
c     16Mar2020: used for uSAF update

      SUBROUTINE find_max_distance_span(l_rgh_datalines,
     +  i_triplediff_cnt,f_triplediff_lat,f_triplediff_lon,
     +  f_max_distance_span_km)

      IMPLICIT NONE
c************************************************************************
c     Variables from outside subroutine
      INTEGER             :: l_rgh_datalines
      INTEGER             :: i_triplediff_cnt
      REAL                :: f_triplediff_lat(l_rgh_datalines)
      REAL                :: f_triplediff_lon(l_rgh_datalines)
      REAL                :: f_max_distance_span_km
c*****
c     Variables used within subroutine

      INTEGER             :: i,j,k,ii,jj,kk
      REAL                :: f_pi

      REAL                :: f_lat_max
      REAL                :: f_lat_min
      REAL                :: f_lon_max
      REAL                :: f_lon_min
c************************************************************************
c      print*,'just inside find_max_distance_span'

c      print*,'i_triplediff_cnt=',i_triplediff_cnt
c      print*,'l_rgh_datalines=',l_rgh_datalines
c      print*,'f_triplediff_lat=',
c     +  (f_triplediff_lat(i),i=1,i_triplediff_cnt)
c      print*,'f_triplediff_lon=',
c     +  (f_triplediff_lon(i),i=1,i_triplediff_cnt)

      f_pi=3.14159

      f_lat_max=-1000.0
      f_lat_min=+1000.0
      f_lon_max=-1000.0
      f_lon_min=+1000.0

      DO i=1,i_triplediff_cnt
       f_lat_max=MAX(f_lat_max,f_triplediff_lat(i))
       f_lat_min=MIN(f_lat_min,f_triplediff_lat(i))

       f_lon_max=MAX(f_lon_max,f_triplediff_lon(i))
       f_lon_min=MIN(f_lon_min,f_triplediff_lon(i))
      ENDDO

      f_max_distance_span_km=SQRT(
     + ((f_lat_max-f_lat_min)*110.0)**2.0+
     + ((f_lon_max-f_lon_min)*110.0*COS(f_lat_min*f_pi/180.0))**2.0)

c      print*,'f_triplediff_lat=',
c     +  (f_triplediff_lat(i),i=1,i_triplediff_cnt)
c      print*,'f_triplediff_lon=',
c     +  (f_triplediff_lon(i),i=1,i_triplediff_cnt)

c      print*,'f_lat_max=',f_lat_max
c      print*,'f_lat_min=',f_lat_min
c      print*,'f_lon_max=',f_lon_max
c      print*,'f_lon_min=',f_lon_min

c      print*,'f_max_distance_span_km=',f_max_distance_span_km

c      print*,'just leaving find_max_distance_span'

c      STOP 'find_max_distance_span'

      RETURN
      END


!---------------------------------------------------------------------!
! OWNER: Ithaca Combustion Enterprise, LLC                            !
! COPYRIGHT: © 2012, Ithaca Combustion Enterprise, LLC                !
! LICENSE: BSD 3-Clause License (The complete text of the license can !
!  be found in the `LICENSE-ICE.txt' file included in the ISAT-CK7    !
!  source directory.)                                                 !
!---------------------------------------------------------------------!

subroutine ell_cov_2d( p, k_ell, L )

!  In 2D, given the point p (|p|<1, |p_1|>0), three ellipses are defined:
!  E_1 is the unit circle, shrunk in the p direction to intersect p
!  E_2 is the unit circle, shrunk in the x_1 direction to intersect p, and
!  E_3 is the minimum-area ellipse covering E_1 and E_2.
!  The 2 x 2 lower trianglular matrix L is returned,
!  where E_{k_ell} = { x | |L^T * x| <1 }.

!  S.B. Pope  12/16/05

implicit none

integer, parameter      :: k_dp = kind(1.d0)
real(k_dp), intent(in)  :: p(2)
integer,    intent(in)  :: k_ell
real(k_dp), intent(out) :: L(2,2)

real(k_dp), parameter   :: tol = 1.d-12

real(k_dp) :: psq, chi, theta, a, b, c, phi, sp, cp, sig1sq, sig2sq, sig1, &
              sig2, alpha, d11, d21, d12, d22, ca, sa

L = 0.d0  ! set L to unit circle
L(1,1) = 1.d0
L(2,2) = 1.d0

!  check that input is valid
if( abs(p(1))==0.d0 ) then
   write(0,*) 'ell_cov_2d: p_1 is zero'
   return
endif

if( k_ell < 1  .or.  k_ell > 3 ) then
   write(0,*) 'ell_cov_2d: invalid k_ell = ', k_ell
   stop
endif

psq = sum(p*p)
if( psq > 1.d0+tol ) then  !  treat boundary and exterior
   write(0,*) 'ell_cov_2d: |p| > 1, ', psq-1.d0
   return
elseif( psq >= 1.d0-tol ) then
   return
endif

chi   = abs(p(1))/sqrt(1-p(2)**2)  ! intersection of E_2 and x_1 axis

if( k_ell == 2 ) then  !  short-cut for E_2
   L(1,1) = 1.d0 / chi
   return
endif

theta = (1-psq)/psq**2             ! E_1={x | x^T*(I+theta*p*p^T)*x <1 }

!  transform so that E_1 is the unit circle: (x,y)->(x/chi,y)
a = (1.d0+theta*p(1)**2)*chi**2     ! B=[a cc b] is the matrix describing E_1
b = 1.d0 + theta*p(2)**2
c = theta*chi*p(1)*p(2)

!  eigendecomposition of B
phi = .5d0*atan2(-2.d0*c,(a-b))
sp  = sin(phi)
cp  = cos(phi)

sig1sq = a*cp*cp-2.d0*c*sp*cp+b*sp*sp
sig2sq = a*sp*sp+2.d0*c*sp*cp+b*cp*cp

!  check that sigs bracket unity
if( min(sig1sq,sig2sq) >1.d0 + tol .or. max(sig1sq,sig2sq) <1.d0 - tol ) then
    write(0,*) 'ell_cov_2d: sig1sq-1, sig2sq-1= ' ,sig1sq-1.d0, sig2sq-1.d0
endif

! set principal axes for E_1, E_2 or E_3
!  no change needed for E_1

if( k_ell == 2 ) then
    sig1sq=1.d0  ! set to E_2
	sig2sq=1.d0

elseif( k_ell ==3 ) then
    sig1sq = min(sig1sq, 1.d0)  ! E_3
    sig2sq = min(sig2sq, 1.d0)
endif

!  the matrix for E_3 is: D*D^T,  D=C*U*Sig = [q r s t]
sig1 = sqrt(sig1sq)
sig2 = sqrt(sig2sq)
d11  = sig1*cp/chi
d12  = sig2*sp/chi
d21  =-sig1*sp
d22  = sig2*cp
!!  D = [d11 d12 d21 d22]
!  transform back
alpha = atan2(-d12, d11)
ca = cos(alpha)
sa = sin(alpha)
!!  QT = [ca sa; -sa ca]
!! L  = D*QT
L(1,1) = ca*d11 - sa*d12
L(2,1) = ca*d21 - sa*d22
L(2,2) = sa*d21 + ca*d22

return

end subroutine ell_cov_2d


!=============================== speed_of_sound ==============================80
!>
!! Calculates and returns the speed of sound given the pressure and density.
!<
!=============================================================================80
pure function speed_of_sound( p, rho )

  use set_precision,   only : dp
  use fluid_constants, only : gamma

  implicit none

  real(dp), intent(in) :: p, rho
  real(dp)             :: speed_of_sound

  continue

  speed_of_sound = gamma*p/rho

end function speed_of_sound


!> Matrix
program main

    use auto_diff
    implicit none
    type(tree_t) :: a(2, 2), b(2, 2)
    type(tree_t) :: y(2, 2)

    a = 1.0_rk
    b = 2.0_rk

    print *, "demo4: y = a + a * b"

    y = a + a*b

    print *, "y     = ", y%get_value()
    call y%backward()

    print *, "dy/da = ", a%get_grad()
    print *, "dy/db = ", b%get_grad()

end program main

! demo4: y = a + a * b
! y     =    3.0000000000000000        3.0000000000000000        3.0000000000000000        3.0000000000000000
! dy/da =    3.0000000000000000        3.0000000000000000        3.0000000000000000        3.0000000000000000
! dy/db =    1.0000000000000000        1.0000000000000000        1.0000000000000000        1.0000000000000000


      Program Ex0205

      Implicit None

*   Pi
      Double Precision  Pi
      Common /CalPar/   Pi

*   Grid Number and Record Number
      Integer           NGrid
      Parameter         (NGrid = 10000)
      Integer           NRec
      Parameter         (NRec = 50)
      Integer           NSep
      Parameter         (NSep = (NGrid - Mod(NGrid,NRec)) / NRec)
*   Dummy
      Integer           I, J
*   Initial Value and Calculation Range
      Double Precision  tMin, tMax, yMin, pMin
      Common /InitV/    tMin, tMax, yMin, pMin
*   Variable Storage
      Double Precision  t(NGrid), y(NGrid), p(NGrid), yExt(NGrid),
     $                  pExt(NGrid)
*   t Axis Grid Separation
      Double Precision  H
*   Internal Variables
      Double Precision  k1, k2, k3, k4
*   For Initial Calculation
      Double Precision  y0, p0, t0
*   Functions
      Double Precision  Fdy, Fdp, FyExt, FpExt
      External          Fdy, Fdp, FyExt, FpExt

      Pi = DACos(-1.0D0)
      tMin = 0.0D0
      tMax = 10.0D0
      yMin = 1.5D0
      pMin = 0.5D0

      Open(11, File = '02-05.txt')
 1000 Format(F6.3, 6F14.7)
 1010 Format(A6,6A14)

*-- Go Forward

      t(1) = (1.0D0 - DBLE(1)/DBLE(NGrid)) * tMin +
     $       DBLE(1)/DBLE(NGrid) * tMax
      t0 = tMin
      H = t(1) - t0
      y0 = yMin
      p0 = pMin

      k1 = H * Fdp(t0, y0, p0)
      k2 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k1)
      k3 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k2)
      k4 = H * Fdp(t0 + H, y0, p0 + k3)
      p(1) = p0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
      p0 = p(1)
      k1 = H * Fdy(t0, y0, p0)
      k2 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k1, p0)
      k3 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k2, p0)
      k4 = H * Fdy(t0 + H, y0 + k3, p0)
      y(1) = y0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
      y0 = y(1)
      t0 = t(1)

      Do 10 I = 2, NGrid
        t(I) = (1.0D0 - DBLE(I)/DBLE(NGrid)) * tMin +
     $         DBLE(I)/DBLE(NGrid) * tMax
        H = t(I) - t0

        k1 = H * Fdp(t0, y0, p0)
        k2 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k1)
        k3 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k2)
        k4 = H * Fdp(t0 + H, y0, p0 + k3)
        p(I) = p0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
        p0 = p(I)
        k1 = H * Fdy(t0, y0, p0)
        k2 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k1, p0)
        k3 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k2, p0)
        k4 = H * Fdy(t0 + H, y0 + k3, p0)
        y(I) = y0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
        y0 = y(I)
        t0 = t(I)
  10  Continue

      Do 50 I = 1, NGrid
        yExt(I) = FyExt(t(I))
        pExt(I) = FpExt(t(I))
  50  Continue

      Write(11,*)
      Write(11,'(38x,A10)') 'Go Forward'
      Write(11,*)
      Write(11,1010) '------',
     $               '--------------','--------------','--------------',
     $               '--------------','--------------','--------------'
      Write(11,1010) 't  ', 'y(t)   ',  'y Exact ', 'y Diff ',
     $               'p(t)   ',  'p Exact ', 'p Diff '
      Write(11,1010) '------',
     $               '--------------','--------------','--------------',
     $               '--------------','--------------','--------------'
      Do 20 J = 1, NRec
        I = J * NSep
        Write(11,1000) t(I), y(I), yExt(I), y(I)-yExt(I),
     $                 p(I), pExt(I), p(I)-pExt(I)
  20  Continue
      Write(11,1010) '------',
     $               '--------------','--------------','--------------',
     $               '--------------','--------------','--------------'

*-- Return Back

      t(1) = (1.0D0 - DBLE(1)/DBLE(NGrid)) * tMax +
     $       DBLE(1)/DBLE(NGrid) * tMin
      H = t(1) - t0

      k1 = H * Fdp(t0, y0, p0)
      k2 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k1)
      k3 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k2)
      k4 = H * Fdp(t0 + H, y0, p0 + k3)
      p(1) = p0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
      p0 = p(1)
      k1 = H * Fdy(t0, y0, p0)
      k2 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k1, p0)
      k3 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k2, p0)
      k4 = H * Fdy(t0 + H, y0 + k3, p0)
      y(1) = y0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
      y0 = y(1)
      t0 = t(1)

      Do 30 I = 2, NGrid
        t(I) = (1.0D0 - DBLE(I)/DBLE(NGrid)) * tMax +
     $         DBLE(I)/DBLE(NGrid) * tMin
        H = t(I) - t0

        k1 = H * Fdp(t0, y0, p0)
        k2 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k1)
        k3 = H * Fdp(t0 + 0.5D0*H, y0, p0 + 0.5D0*k2)
        k4 = H * Fdp(t0 + H, y0, p0 + k3)
        p(I) = p0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
        p0 = p(I)
        k1 = H * Fdy(t0, y0, p0)
        k2 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k1, p0)
        k3 = H * Fdy(t0 + 0.5D0*H, y0 + 0.5D0*k2, p0)
        k4 = H * Fdy(t0 + H, y0 + k3, p0)
        y(I) = y0 + 1.0D0/6.0D0* (k1 + 2.0D0*k2 + 2.0D0*k3 + k4)
        y0 = y(I)
        t0 = t(I)
  30  Continue

      Do 60 I = 1, NGrid
        yExt(I) = FyExt(t(I))
        pExt(I) = FpExt(t(I))
  60  Continue

      Write(11,*)
      Write(11,'(38x,A11)') 'Return Back'
      Write(11,*)
      Write(11,1010) '------',
     $               '--------------','--------------','--------------',
     $               '--------------','--------------','--------------'
      Write(11,1010) 't  ', 'y(t)   ',  'y Exact ', 'y Diff ',
     $               'p(t)   ',  'p Exact ', 'p Diff '
      Write(11,1010) '------',
     $               '--------------','--------------','--------------',
     $               '--------------','--------------','--------------'
      Do 40 J = 1, NRec
        I = J * NSep
        Write(11,1000) t(I), y(I), yExt(I), y(I)-yExt(I),
     $                 p(I), pExt(I), p(I)-pExt(I)
  40  Continue
      Write(11,1010) '------',
     $               '--------------','--------------','--------------',
     $               '--------------','--------------','--------------'

      End Program Ex0205

*-----------------------------------------------------------------------
*
      Function Fdy(t, y, p)

      Implicit None

      Double Precision  Fdy, t, y, p

      Fdy = p

      End Function Fdy

*-----------------------------------------------------------------------
*
      Function Fdp(t, y, p)

      Implicit None

*   Pi
      Double Precision  Pi
      Common /CalPar/   Pi

      Double Precision  Fdp, t, y, p

      Fdp = - 4.0D0 * Pi*Pi * y

      End Function Fdp

*-----------------------------------------------------------------------
*
      Function FyExt(t)

      Implicit None

*   Pi
      Double Precision  Pi
      Common /CalPar/   Pi
*   Initial Value and Calculation Range
      Double Precision  tMin, tMax, yMin, pMin
      Common /InitV/    tMin, tMax, yMin, pMin

      Double Precision  FyExt, t

      FyExt = yMin * DCos(2.0D0*Pi * t) + pMin * DSin(2.0D0*Pi * t) /
     $        (2.0D0*Pi)

      End Function FyExt

*-----------------------------------------------------------------------
*
      Function FpExt(t)

      Implicit None

*   Pi
      Double Precision  Pi
      Common /CalPar/   Pi
*   Initial Value and Calculation Range
      Double Precision  tMin, tMax, yMin, pMin
      Common /InitV/    tMin, tMax, yMin, pMin

      Double Precision  FpExt, t

      FpExt = pMin * DCos(2.0D0*Pi * t) - 2.0D0*Pi * DSin(2.0D0*Pi * t)
     $        * yMin

      End Function FpExt


    !-------------------------------------------------------------------------------------------------------------
    !
    !> \file    Broyden.f90
    !> \brief   Perform a unit iteration using Broyden's method.
    !> \author  M.H.A. Piro
    !> \date    Apr. 26, 2012
    !
    !
    ! Revisions:
    ! ==========
    !
    !    Date          Programmer         Description of change
    !    ----          ----------         ---------------------
    !    11/11/2011    M.H.A. Piro        Original code
    !
    !
    ! Purpose:
    ! ========
    !
    !> \details The purpose of this subroutine is to perform a unit iteration in solving a system of
    !! non-linear equations using the "good" Broyden method.  This subroutine does not perform an iteration
    !! process, but is intended to be used within a main iteration cycle.  An estimate of the inverse of the
    !! Broyden matrix is required as input and an improved estimate is provided as output in addition to a
    !! new direction vector s.
    !!
    !! One of the advantages of using Broyden's method in comparison to the more popular BFGS method is
    !! that Broyden's method permits nonsymmetric updates to the inverse Broyden matrix, whereas the BFGS
    !! method maintains symmetry.  The true Jacobian for some numerical problems may not always be symmetric.
    !!
    !
    ! Pertinent variables:
    ! ====================
    !
    !> \param[in]     nVar          The number of unknown variables (and of course the number of non-linear
    !!                               equations)
    !> \param[in,out] s             The change of the unknown variable vector (nVar)
    !> \param[in,out] y             The change of the function vector (nVar)
    !> \param[in]     f             The function vector (nVar)
    !> \param[in,out] dInvBroyden   The inverse of the Broyden matrix (nVar x nVar)
    !
    !-------------------------------------------------------------------------------------------------------------

subroutine Broyden(nVar,y,s,f,dInvBroyden)

    implicit none

    integer::                       i, j, nVar
    real(8)::                       dTemp
    real(8),dimension(nVar)::       f, y, s, dTempVec
    real(8),dimension(nVar,nVar)::  dInvBroyden

    ! Compute the difference vector multiplied by the current inverse Broyden matrix:
    do i = 1, nVar
        dTempVec(i) = 0D0
        do j = 1, nVar
            dTempVec(i) = dTempVec(i) + dInvBroyden(i,j) * y(j)
        end do
    end do

    ! Calculate the dot products for denominators:
    dTemp = 0D0
    do i = 1, nVar
        dTemp = dTemp + s(i) * dTempVec(i)
    end do

    ! Make the denominator multiplicative:
    dTemp = 1D0 / dTemp

    ! Compute the work vector:
    do i = 1, nVar
        y(i) = 0D0
        do j = 1, nVar
            y(i) = y(i) + s(j) * dInvBroyden(j,i)
        end do
    end do

    ! Update the inverse Broyden matrix:
    do i = 1, nVar
        do j = 1, nVar
            dInvBroyden(i,j) = dInvBroyden(i,j) + dTemp * y(j) * (s(i) - dTempVec(i))
        end do
    end do

    ! Compute the direction vector:
    do i = 1, nVar
        s(i) = 0D0
        do j = 1, nVar
            s(i) = s(i) - dInvBroyden(i,j) * f(j)
        end do
    end do

    return

end subroutine Broyden


program test_math_angle
    use forlab_math, only: angle
    use stdlib_kinds, only: sp
    implicit none
    complex(sp) :: c, cX(2)

    c = (1.0, 2.0)
    cX = (2.0, 3.0)

    print *, angle(c)
    print *, angle(cX)

end program test_math_angle


module matrix
    contains
    function matmul_(matrix_a, matrix_b) result(matrix_r)
        implicit none
        double precision, intent(in), dimension(:, :) :: matrix_a, matrix_b
        double precision, dimension(:, :), allocatable :: matrix_r
        integer :: n, i, j, k
        double precision :: s

        n = size(matrix_a, 1)
        allocate(matrix_r(n, n))

        !$OMP parallel shared(matrix_a, matrix_b, matrix_r, n) private(i, j, s, k)

        !$OMP parallel do
        do i = 1, n
            do j = 1, n
                s = 0.0
                do k = 1, n
                    s = s + matrix_a(i, k) * matrix_b(k, j)
                end do
                matrix_r(i, j) = s
            end do
        end do
        !$OMP end parallel do

        !$OMP end parallel
    end function matmul_
end module matrix

program main
    use matrix
    implicit none
    integer :: i, j, n
    real :: cl1, cl2
    double precision, dimension(:,:), allocatable :: matrix_a, matrix_b, matrix_r
    character(len=32) :: arg

    call get_command_argument(1, arg)

    if(len_trim(arg) == 0) then
        write(0, *) "use: matmul <n>"
        stop 9
    end if

    read(arg, '(i32)') n

    allocate(matrix_a(n, n))
    allocate(matrix_b(n, n))

    call random_number(matrix_a)
    call random_number(matrix_b)

    call cpu_time(cl1)
    !matrix_r = matmul_(matrix_a, matrix_b)
    matrix_r = matmul(matrix_a, matrix_b)
    call cpu_time(cl2)

    do i = 1, size(matrix_a, 1)
        write(*, "(10g10.5)") (matrix_r(j, i), j = 1, size(matrix_a, 1))
    end do

    write(0, "(g16.10' seconds')") (cl2 - cl1)
end program main


program tarefaa
    implicit real(8) (a-h,o-z)
    character(70) filename
    dimension v0(2), e0(3)

    e0 = (/0.01, 0.001, 0.0001/)
    v0 = (/0.0, 10.0/)

    do i = 1, 2
        do j = 1, 3

            ! Parametros iniciais
            e = e0(j)           ! Incremento temporal
            g = 10d0            ! gravidade
            a = -g              ! Aceleração
            r = 100d0           ! Posição
            v = v0(i) +e*a/2    ! Velocidade
            t = 0d0             ! Tempo
            rexato = f(r, v, a, 0d0)

            write(filename,'(A,2(I0,A))') 'saida-a-v',i,'-e',j,'-10407962.dat'

            open(10, file=filename)

            do while (r.ge.0)
                write(10, '(F0.6,2(" ",F0.6)," ",4E0.5)') t, r, v, abs(rexato-r)
                rexato = f(r, v, a, e)
                a = -g
                v = v + e*a
                r = r + e*v
                t = t + e
            end do

            close(10)
        end do
    end do

contains

    function f(r0, v0, a0, t)
        implicit real(8) (a-h,o-z)
        f = r0 + v0*t + 0.5*a0*t**2
        return
    end function

end program tarefaa


!********************************************************************************!
!                                                                                !
! The MIT License (MIT)                                                          !
!                                                                                !
! Copyright (c) 2020 Sarit Dutta                                                 !
!                                                                                !
! Permission is hereby granted, free of charge, to any person obtaining a copy   !
! of this software and associated documentation files (the "Software"), to deal  !
! in the Software without restriction, including without limitation the rights   !
! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      !
! copies of the Software, and to permit persons to whom the Software is          !
! furnished to do so, subject to the following conditions:                       !
!                                                                                !
! The above copyright notice and this permission notice shall be included in all !
! copies or substantial portions of the Software.                                !
!                                                                                !
! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     !
! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       !
! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    !
! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         !
! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  !
! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE  !
! SOFTWARE.                                                                      !
!                                                                                !
!********************************************************************************!

module utils_math_m
    !!Various (mostly linear algebra) functions, particularly for use with small
    !!matrices.

use, intrinsic :: ieee_arithmetic, only: ieee_is_nan, ieee_is_finite
use constants_m

implicit none

interface allclose
    !! Checks if two arrays are elementwise close within tolerance
    module procedure allclose_rank1
    module procedure allclose_rank2
    module procedure allclose_rank3
end interface

interface swap
    !! Swaps two arrays
    module procedure swap_integer
    module procedure swap_real
    module procedure swap_complex
end interface swap

contains

!************************************************************************

elemental subroutine rad2deg(rad, deg)

    real(rp), intent(in) :: rad
    real(rp), intent(out) :: deg

    deg = (180.0_rp/math_pi)*rad

    end subroutine

!************************************************************************

elemental subroutine deg2rad(deg, rad)

    real(rp), intent(in) :: deg
    real(rp), intent(out) :: rad

    rad = (math_pi/180.0_rp)*deg

    end subroutine

!************************************************************************

subroutine cross(a, b, c)
    !!  Calculates the cross product between two 3-element vectors
    real(rp), dimension(3), intent(in)  :: a
    real(rp), dimension(3), intent(in)  :: b
    real(rp), dimension(3), intent(out) :: c
        !! Cross product of `a` and `b`; **c** = **a** x **b**

    c(1) = a(2)*b(3) - a(3)*b(2)
    c(2) = a(3)*b(1) - a(1)*b(3)
    c(3) = a(1)*b(2) - a(2)*b(1)

    end subroutine

!******************************************************************************

subroutine cross_mat(a, mat)
    !! Calculates the cross product matrix of a 3-element vector. The cross
    !! product matrix **A** of **a** is defined as **a** x **b** = **A** . **b**,
    !! where **b** is another 3-element vector.
    real(rp), dimension(3), intent(in)    :: a
    real(rp), dimension(3,3), intent(out) :: mat
        !! Cross product matrix of `a`

    mat = 0.0_rp
    mat(2,1) =  a(3)
    mat(3,1) = -a(2)
    mat(1,2) = -a(3)
    mat(3,2) =  a(1)
    mat(1,3) =  a(2)
    mat(2,3) = -a(1)

    end subroutine

!******************************************************************************

subroutine outer(a, b, c)
    !! Calculates the outer product of two vectors, \(c_{ij} = a_i  b_j\).
    real(rp), dimension(:), intent(in)    :: a
        !! (m,) array
    real(rp), dimension(:), intent(in)    :: b
        !! (n,) array
    real(rp), dimension(:,:), intent(out) :: c
        !! (m,n) array; Outer product
    integer :: m, n
    integer :: i, j

    m = size(a)
    n = size(b)

    do j = 1, n
        do i = 1, m
            c(i,j) = a(i)*b(j)
        end do
    end do

    end subroutine

!******************************************************************************

function scalar_triple_product (a, b, c) result(res)
    !! Returns the scalar triple product **a**.(**b** x **c**)
    real(rp), dimension(3), intent(in)  :: a
    real(rp), dimension(3), intent(in)  :: b
    real(rp), dimension(3), intent(in)  :: c
    real(rp)  :: res

    res = a(1)*( b(2)*c(3)-c(2)*b(3) ) - a(2)*( b(1)*c(3)-c(1)*b(3) ) &
            + a(3)*( b(1)*c(2)-c(1)*b(2) )

    end function

!******************************************************************************

subroutine vector_triple_product (a, b, c, d)
    !! Returns the vector triple product **d** = **a** x (**b** x **c**)
    real(rp), dimension(3), intent(in)  :: a
    real(rp), dimension(3), intent(in)  :: b
    real(rp), dimension(3), intent(in)  :: c
    real(rp), dimension(3), intent(out) :: d
     !! Vector triple product

    d = b*( a(1)*c(1) + a(2)*c(2) + a(3)*c(3) )  &
        - c*( a(1)*b(1) + a(2)*b(2) + a(3)*b(3) )

    end subroutine

!******************************************************************************

function det(A) result(res)
    !!  Returns the determinant of an (N x N) matrix, where N = 2, 3, or 4.
    !!
    !!  Original routine by [David Simpson](http://www.davidgsimpson.com/software.html)
    !!  @note For a general NxN matrix do an LU decomp
    !""
    real(rp), dimension(:,:), intent(in) :: A
        !! (N,N) array, where N = 2, 3, or 4.
    real(rp) :: res
    integer :: nrows

    nrows = size(A, 1)

    if (nrows==2) then
        res = A(1,1)*A(2,2) - A(1,2)*A(2,1)

    else if (nrows==3) then
        res = A(1,1)*A(2,2)*A(3,3) - A(1,1)*A(2,3)*A(3,2)  &
            - A(1,2)*A(2,1)*A(3,3) + A(1,2)*A(2,3)*A(3,1)  &
            + A(1,3)*A(2,1)*A(3,2) - A(1,3)*A(2,2)*A(3,1)

    else if (nrows==4) then
        res = A(1,1) * ( A(2,2) * (A(3,3)*A(4,4) - A(3,4)*A(4,3))    &
                       + A(2,3) * (A(3,4)*A(4,2) - A(3,2)*A(4,4))    &
                       + A(2,4) * (A(3,2)*A(4,3) - A(3,3)*A(4,2)) )  &
            - A(1,2) * ( A(2,1) * (A(3,3)*A(4,4) - A(3,4)*A(4,3))    &
                       + A(2,3) * (A(3,4)*A(4,1) - A(3,1)*A(4,4))    &
                       + A(2,4) * (A(3,1)*A(4,3) - A(3,3)*A(4,1)) )  &
            + A(1,3) * ( A(2,1) * (A(3,2)*A(4,4) - A(3,4)*A(4,2))    &
                       + A(2,2) * (A(3,4)*A(4,1) - A(3,1)*A(4,4))    &
                       + A(2,4) * (A(3,1)*A(4,2) - A(3,2)*A(4,1)) )  &
            - A(1,4) * ( A(2,1) * (A(3,2)*A(4,3) - A(3,3)*A(4,2))    &
                       + A(2,2) * (A(3,3)*A(4,1) - A(3,1)*A(4,3))    &
                       + A(2,3) * (A(3,1)*A(4,2) - A(3,2)*A(4,1)) )

     else
         stop 'matrix size must be 2, 3, or 4'
     end if

    end function

!******************************************************************************

function trace(mat) result (res)
    !! Returns the trace of a square matrix

    real(rp), intent(in) :: mat(:,:)
        !! (N,N) array
    real(rp) :: res
    integer :: nrows
    integer :: i

    nrows = size(mat, 1)
    res = 0.0_rp

    do i = 1, nrows
        res = res + mat(i,i)
    end do

    end function

!******************************************************************************

elemental logical function isclose(a, b, rel_tol, abs_tol)
    !!  Checks if two floating point numbers of type double are close within
    !!  tolerance.
    !!
    !!  Based on python implementation at
    !!  <https://github.com/PythonCHB/close_pep/blob/master/is_close.py>.
    !!  The *method='weak'* option is used here.
    !""

    real(rp), intent(in)    :: a
    real(rp), intent(in)    :: b
    real(rp), intent(in), optional    :: rel_tol
        !! Relative tolerance, `rel_tol` >= 0, default 1e-10
    real(rp), intent(in), optional    :: abs_tol
        !! Absolute tolerance, `abs_tol` >= 0, default 0.0
    real(rp) :: rel_tol_
    real(rp) :: abs_tol_
    real(rp) :: diff

    rel_tol_ = 1e-10_rp
    abs_tol_ = 0.0_rp

    if (present(rel_tol)) rel_tol_ = rel_tol
    if (present(abs_tol)) abs_tol_ = abs_tol

    if (a == b) then  ! short-circuit exact equality
        isclose = .true.
    end if

    if ((.not. ieee_is_finite(a)) .or. (.not. ieee_is_finite(b))) then
         ! Includes the case of two infinities of opposite sign, or
         ! one infinity and one finite number. Two infinities of opposite sign
         ! would otherwise have an infinite relative tolerance.
         isclose = .false.
    end if

    diff = abs(b - a)
    isclose = ( ((diff <= abs(rel_tol_*b)) .or. (diff <= abs(rel_tol_*a))) &
            .or. (diff <= abs_tol_) )

    end function

!******************************************************************************

logical function allclose_rank1(a, b, rel_tol, abs_tol)
    !!  Checks if two rank-1 floating point arrays of type double are close within
    !!  tolerance.
    !""
    real(rp), dimension(:), intent(in) :: a
        !! (m,) array
    real(rp), dimension(:), intent(in) :: b
        !! (m,) array
    real (rp), intent(in), optional :: rel_tol
        !! Relative tolerance; default 1e-10
    real (rp), intent(in), optional :: abs_tol
        !! Absolute tolerance; default 0.0
    real (rp) :: rel_tol_ = 1e-10_rp
    real (rp) :: abs_tol_ = 0.0_rp

    if (present(rel_tol)) rel_tol_ = rel_tol
    if (present(abs_tol)) abs_tol_ = abs_tol

    allclose_rank1 = all(isclose(a, b, rel_tol_, abs_tol_))

    end function

logical function allclose_rank2(a, b, rel_tol, abs_tol)
    !!  Checks if two rank-2 floating point arrays of type double are close within
    !!  tolerance.
    !""
    real(rp), dimension(:,:), intent(in) :: a
        !! (m,n) array
    real(rp), dimension(:,:), intent(in) :: b
        !! (m,n) array
    real (rp), intent(in), optional :: rel_tol
        !! Relative tolerance; default 1e-10
    real (rp), intent(in), optional :: abs_tol
        !! Absolute tolerance; default 0.0
    real (rp) :: rel_tol_ = 1e-10_rp
    real (rp) :: abs_tol_ = 0.0_rp

    if (present(rel_tol)) rel_tol_ = rel_tol
    if (present(abs_tol)) abs_tol_ = abs_tol

    allclose_rank2 = all(isclose(a, b, rel_tol_, abs_tol_))

    end function

logical function allclose_rank3(a, b, rel_tol, abs_tol)
    !!  Checks if two rank-3 floating point arrays of type double are close within
    !!  tolerance.
    !""
    real(rp), dimension(:,:,:), intent(in) :: a
        !! (m,n,p) array
    real(rp), dimension(:,:,:), intent(in) :: b
        !! (m,n,p) array
    real (rp), intent(in), optional :: rel_tol
        !! Relative tolerance; default 1e-10
    real (rp), intent(in), optional :: abs_tol
        !! Absolute tolerance; default 0.0
    real (rp) :: rel_tol_ = 1e-10_rp
    real (rp) :: abs_tol_ = 0.0_rp

    if (present(rel_tol)) rel_tol_ = rel_tol
    if (present(abs_tol)) abs_tol_ = abs_tol

    allclose_rank3 = all(isclose(a, b, rel_tol_, abs_tol_))

    end function

!******************************************************************************

elemental subroutine swap_integer(a, b)
    integer, intent(in out) :: a
    integer, intent(in out) :: b
    integer :: temp

    temp = a
    a = b
    b = temp
    end subroutine

elemental subroutine swap_real(a, b)
    real(rp), intent(in out) :: a
    real(rp), intent(in out) :: b
    real(rp) :: temp

    temp = a
    a = b
    b = temp
    end subroutine

elemental subroutine swap_complex(a, b)
    complex(rp), intent(in out) :: a
    complex(rp), intent(in out) :: b
    complex(rp) :: temp

    temp = a
    a = b
    b = temp
    end subroutine

!******************************************************************************

subroutine unitize(a)
    !! Normalizes a vector in-place. If the magnitude of the vector is nearly
    !! zero, no normalization takes place and the vector is returned as is with
    !! a warning message.
    real(rp), dimension(:), intent(in out)  :: a
        !! (m,) array
    real(rp) :: norm

    norm = norm2(a)

    if (isclose(norm, 0.0_rp, rel_tol=1.e-15_rp, abs_tol=0.0_rp)) then
        write(*,*) '[unitize] norm close to zero'
    else
        a = a/norm2(a)
    end if

    end subroutine

!******************************************************************************

subroutine linspace(start, finish, num, val, step)
    !! Generates evenly spaced numbers over a specified interval. Both end
    !! points are included. If `start` < `finish`, the returned step size (if
    !! `step` is present) will be negative.
    !""
    real(rp), intent(in) :: start
        !! Starting point
    real(rp), intent(in) :: finish
        !! Ending point, `finish` /= `start`
    integer, intent(in) :: num
        !! Number of values to generate, `num >= 2`
    real(rp), dimension(:), intent(out) :: val
        !! (`num`,) array; Generated values
    real(rp), intent(out), optional :: step
        !! Step size
    real(rp) :: step_
    integer :: i

    if (num < 2) then
        write(*,*) '[linspace] `num` must be >= 2'
        stop
    end if

    if (num < size(val)) then
        write(*,*) '[linspace] `size(val)` must be >= `num`'
        stop
    end if

    if (start == finish) then
        write(*,*) '[linspace] `start` must not be equal to `finish`'
        stop
    end if

    val = 0.0_rp

    step_ = (finish-start)/(num-1)

    if (present(step)) step = step_

    val(1) = start
    val(num) = finish

    do i = 2, (num-1)
        val(i) = start + (i-1)*step_
    end do

    end subroutine

!******************************************************************************

subroutine logspace(start, finish, num, val, base)
    !!  Generates numbers spaced evenly on a log scale.
    !!  In linear space, the sequence starts at `base ** start`
    !!  (`base` to the power of `start`) and ends with `base ** stop`
    !""
    real(rp), intent(in) :: start
        !! Starting point, `base ** start` is the starting value
    real(rp), intent(in) :: finish
        !! Ending point, `finish` /= `start`,  `base ** start`
        !! is the ending value
    integer, intent(in) :: num
        !! Number of values to generate, `num >= 2`
    real(rp), dimension(:), intent(out) :: val
        !! (`num`,) array; Generated values
    real(rp), intent(in), optional :: base
        !! Base of the logspace, default 10
    real(rp) :: base_
    integer :: i

    if (num < 2) then
        write(*,*) 'logspace `num` must be >= 2'
        stop
    end if

    if (num < size(val)) then
        write(*,*) 'logspace `size(val)` must be >= `num`'
        stop
    end if

    if (start == finish) then
        write(*,*) 'logspace `start` must not be equal to `finish`'
        stop
    end if

    val = 0.0_rp
    base_ = 10.0_rp
    if (present(base)) base_ = base

    call linspace(start, finish, num, val)

    do i = 1, num
        val(i) = base_**val(i)
    end do

    end subroutine

!******************************************************************************

subroutine identity(mat_eye)
    !! Creates an identity matrix of size n x n.
    real(rp), dimension(:,:), intent(out) :: mat_eye
        !! (n,n) array
    integer :: nrows
    integer :: i

    nrows = size(mat_eye, 1)

    mat_eye = 0.0_rp

    do i = 1, nrows
        mat_eye(i,i) = 1.0_rp
    end do

    end subroutine

!******************************************************************************

subroutine get_diagonal(mat, d)
    !! Returns the diagonal elements of a square matrix.
    real(rp), dimension(:,:), intent(in) :: mat
        !! (n,n) array
    real(rp), dimension(:), intent(out) :: d
        !! (n,) array; contains the entries of the main diagonal
    integer :: n
    integer :: i

    n = size(mat, 1)

    do i = 1, n
        d(i) = mat(i,i)
    end do

    end subroutine

!******************************************************************************

subroutine add_transpose(mat)
    !! Adds a square matrix and its transpose in place: \(A_{ij} = A_{ij } + A_{ji}\)
    real(rp), dimension(:,:), intent(in out) :: mat
        !! (n,n) array
    integer :: n
    integer :: i
    integer :: j

    n = size(mat,1)

    !Dealing with the upper triangular part (including diagonal)
    do j = 1, n
        do i = 1, j
            mat(i,j) = mat(i,j) + mat(j,i)
        end do
    end do

    !Dealing with the strictly lower triangular part (i.e. excluding diagonal)
    do j = 1, (n-1)
        do i = (j+1), n
            mat(i,j) = mat(j,i)
        end do
    end do

    end subroutine

!******************************************************************************

subroutine subtract_transpose(mat)
    !! Calculates the difference of a square matrix and its transpose in place:
    !! \(A_{ij} = A_{ij } - A_{ji}\)
    real(rp), dimension(:,:), intent(in out) :: mat
    integer :: n
    integer :: i
    integer :: j

    n = size(mat,1)

    !Dealing with the strictly upper triangular part (i.e. excluding diagonal)
    do j = 2, n
        do i = 1, (j-1)
            mat(i,j) = mat(i,j) - mat(j,i)
        end do
    end do

    !Dealing with the strictly lower triangular part (i.e. excluding diagonal)
    do j = 1, (n-1)
        do i = (j+1), n
            mat(i,j) = -mat(j,i)
        end do
    end do

    !Putting the diagonal to zero
    do i = 1, n
        mat(i,j) = 0.0_rp
    end do

    end subroutine

!******************************************************************************

subroutine multiply_transpose(A, B)
    !! Multiplies a matrix with its transpose:
    !! \(\mathbf{\mathrm{B}} = \mathbf{\mathrm{A}} \cdot \mathbf{\mathrm{A}}^T\)
    !""
    real(rp), dimension(:,:), intent(in) :: A
        !! (m,n) array
    real(rp), dimension(:,:), intent(out) :: B
        !! (m,m) array
        !A is m x n, A^T is n x m, so B is m x m

    B = matmul(A, transpose(A))

    end subroutine

!******************************************************************************

function get_quad_form(A, x) result(res)
    !!  Calculates the quadratic form *x^T A x*, where A is an *n x n* matrix and *x* is a
    !!  vector of length *n*
    !""
    real(rp), dimension(:,:), intent(in) :: A
        !! (n,n) array
    real(rp), dimension(:), intent(in) :: x
        !! (n,) array
    real(rp) :: res
    integer :: n
    integer :: i, j

    n = size(A,2)
    res = 0.0_rp

    do j = 1, n
        do i = 1, n
            res = res + A(i,j)*x(j)*x(i)
        end do
    end do

    end function

!******************************************************************************

subroutine orth(a)
    !! Orthogonalizes a set of vectors in-place using Gram-Schmidt orthonormalization
    !!
    !! *Reference:* Golub and Van Loan, Matrix Computations, 3rd edition, Section 5.2.8,
    !! Algorithm 5.2.5, p. 231.

    real(rp), dimension(:,:), intent(in out) :: a
        !! (m,n) array, where m <= n. The first m columns of the matrix are
        !!  overwritten with the orthogonal basis vectors.
    integer :: m
    integer :: i, j

    m = size(a, 1)

    do i = 1, m
        a(i,:) = a(i,:)/norm2(a(i,:))
        do j = (i+1), m
            a(j,:) = a(j,:) - dot_product(a(j,:),a(i,:))*a(i,:)
        end do
    end do

    end subroutine

!******************************************************************************

subroutine invert_mat33(a, inv_a)
    !! Inverts a 3x3 matrix.
    !!
    !!*Reference:* https://www.geometrictools.com/Documentation/LaplaceExpansionTheorem.pdf

    real(rp), dimension(3,3),  intent(in) :: a
    real(rp), dimension(3,3), intent(out) :: inv_a
    real(rp) :: det_a

    inv_a(1,1) =   a(2,2)*a(3,3) - a(2,3)*a(3,2)
    inv_a(2,1) = -(a(2,1)*a(3,3) - a(2,3)*a(3,1))
    inv_a(3,1) =   a(2,1)*a(3,2) - a(2,2)*a(3,1)

    inv_a(1,2) = -(a(1,2)*a(3,3) - a(1,3)*a(3,2))
    inv_a(2,2) =   a(1,1)*a(3,3) - a(1,3)*a(3,1)
    inv_a(3,2) = -(a(1,1)*a(3,2) - a(1,2)*a(3,1))

    inv_a(1,3) =   a(1,2)*a(2,3) - a(1,3)*a(2,2)
    inv_a(2,3) = -(a(1,1)*a(2,3) - a(1,3)*a(2,1))
    inv_a(3,3) =   a(1,1)*a(2,2) - a(1,2)*a(2,1)

    !Determinant
    det_a = a(1,1)*inv_a(1,1) + a(1,2)*inv_a(2,1) + a(1,3)*inv_a(3,1)
    inv_a = inv_a/det_a

    end subroutine

!******************************************************************************

subroutine eigval_33rsym(a, ev)
    !! Calculates the eigenvalues of a 3 x 3 real symmetric matrix. The
    !! eigenvalues calculated are in decreasing order. Only the diagonal and
    !! lower triangular part of the matrix is accessed.
    !!
    !! *Reference:* https://en.wikipedia.org/wiki/Eigenvalue_algorithm#cite_note-Smith-12
    !!
    !! See also David Eberly's notes and implementation at
    !! https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf

    real(rp), dimension(3,3), intent(in) :: a
    real(rp), dimension(3), intent(out) :: ev
    real(rp), dimension(3,3) :: eye
    real(rp), dimension(3,3) :: b
    real(rp) :: p, q, r
    real(rp) :: p1, p2
    real(rp) :: phi

    !Sum of the elements in the lower triangular part
    p1 = a(2,1)**2 + a(3,1)**2 + a(3,2)**2

    if ( isclose(p1, 0.0_rp) ) then
        !Matrix a is diagonal
        ev(1) = a(1,1); ev(2) = a(2,2); ev(3) = a(3,3)
        return
    else
        q = (a(1,1) + a(2,2) + a(3,3))/3.0_rp
        p2 = (a(1,1) - q)**2 + (a(2,2) - q)**2 + (a(3,3) - q)**2 + 2*p1
        p = sqrt(p2/6.0_rp)

        call identity(eye)

        b = (a - q*eye)/p
        r = 0.5_rp*det(b)

        !r must be within [-1, 1] in exact computation; but need to handle
        !slightly out of range in computation.
        if (r <= -1.0_rp) then
            phi = math_pi/3.0_rp
        else if ( r >= 1.0_rp) then
            phi = 0.0_rp
        else
            phi = acos(r)/3.0_rp
        end if

        !Eigen values are ordered as ev(3) <= ev(2) <= ev(1)
        ev(1) = q + 2*p*cos(phi)
        ev(3) = q + 2*p*cos(phi + (2*math_pi/3.0_rp))
        ev(2) = 3*q - ev(1) - ev(3)
    end if

    end subroutine

!******************************************************************************

subroutine dsyevc3(a, w)
    !!author: Joachim Kopp
    !!date: 2006
    !!
    !! Calculates the eigenvalues of a symmetric 3x3 matrix A using Cardano's
    !! analytical algorithm.
    !! Only the diagonal and upper triangular parts of A are accessed. The access
    !! is read-only.
    !!
    !! Copyright (C) 2006  Joachim Kopp
    !  https://www.mpi-hd.mpg.de/personalhomes/globes/3x3/index.html
    ! ----------------------------------------------------------------------------
    ! Parameters:
    !   A: The symmetric input matrix
    !   W: Storage buffer for eigenvalues
    ! ----------------------------------------------------------------------------
    ! .. Arguments ..
      REAL(RP), DIMENSION(3,3), INTENT(IN) :: A
      REAL(RP), DIMENSION(3), INTENT(OUT) ::  W(3)

     !.. Local Variables ..
      REAL(RP) ::  M, C1, C0
      REAL(RP) ::  DE, DD, EE, FF
      REAL(RP) ::  P, SQRTP, Q, C, S, PHI

     !Determine coefficients of characteristic poynomial. We write
     !      | A   D   F  |
     ! A =  | D*  B   E  |
     !      | F*  E*  C  |

      DE    = A(1,2) * A(2,3)
      DD    = A(1,2)**2
      EE    = A(2,3)**2
      FF    = A(1,3)**2
      M     = A(1,1) + A(2,2) + A(3,3)
      C1    = ( A(1,1)*A(2,2) + A(1,1)*A(3,3) + A(2,2)*A(3,3) ) &
               - (DD + EE + FF)
      C0    = A(3,3)*DD + A(1,1)*EE + A(2,2)*FF - A(1,1)*A(2,2)*A(3,3) &
               - 2.0_RP * A(1,3)*DE

      P     = M**2 - 3.0_RP * C1
      Q     = M*(P - (3.0_RP/2.0_RP)*C1) - (27.0_RP/2.0_RP)*C0
      SQRTP = SQRT(ABS(P))

      PHI   = 27.0_RP * ( 0.25_RP * C1**2 * (P - C1) &
                + C0 * (Q + (27.0_RP/4.0_RP)*C0) )
      PHI   = (1.0_RP/3.0_RP) * ATAN2(SQRT(ABS(PHI)), Q)

      C     = SQRTP * COS(PHI)
      S     = (1.0_RP/MATH_SQRT3) * SQRTP * SIN(PHI)

      W(2) = (1.0_RP/3.0_RP) * (M - C)
      W(3) = W(2) + S
      W(1) = W(2) + C
      W(2) = W(2) - S

      END SUBROUTINE

!******************************************************************************

end module utils_math_m


FUNCTION fexp(x)
!-----------------------------------------------------------------------
!
!    File:         fexp
!    Module:       fexp
!    Type:         Function
!    Author:       S. W. Bruenn, Dept of Physics, FAU,
!                  Boca Raton, FL 33431-0991
!
!    Date:         7/28/02
!
!    Purpose:
!      To exponentiate a given value without over or underflow
!
!----------------------------------------------------------------------c

USE wlKindModule, ONLY: dp

IMPLICIT NONE

!-----------------------------------------------------------------------
!        Input variables.
!-----------------------------------------------------------------------

REAL(dp), INTENT(in)  :: x               ! function argument

!-----------------------------------------------------------------------
!        Output variables.
!-----------------------------------------------------------------------

REAL(dp)              :: fexp            ! declare function

!-----------------------------------------------------------------------
!        Local variables
!-----------------------------------------------------------------------

REAL(dp), PARAMETER   :: expmax = 300.d0 ! maximum absolute value of function argument
REAL(dp), PARAMETER   :: expmin = -300.d0 ! maximum absolute value of function argument

!-----------------------------------------------------------------------
!-----------------------------------------------------------------------

fexp            = DEXP( DMIN1( expmax, DMAX1( expmin, x ) ) )

END FUNCTION fexp


program main
   use  m_unique_eigvec
   implicit none
   integer, parameter :: N=70, lwork = 3*N
   real(kind=8), allocatable :: H(:,:), vecs1(:,:), vecs2(:,:), eig1(:), eig2(:), work(:)
   integer :: info, i, ierr

   allocate(H(N,N), vecs1(N,N), vecs2(N,N), eig1(N), eig2(N), work(lwork))

   ! Hermitian matrix with a few degenerate values
   H = 0.0
   do i = 1,N
      H(i,i) = floor(sqrt(1.0*(i)))
   enddo

   ! hermitianize
   H = 0.5*H +  0.5*transpose(H)
   call shuffle_matrix(H)

   vecs1 = H
   vecs2 = H

   call dsyev("V", "U", N, vecs1, N, eig1, work, lwork, info)
   if(info /= 0) write (*,*) "diag U failed"

   call dsyev("V", "L", N, vecs2, N, eig2, work, lwork, info)
   if(info /= 0) write (*,*) "diag L failed"

   write (*,*) "Before:"
   write (*,*) "Eigenvalue diff norm:", norm2(eig1  - eig2)
   write (*,*) "Eigenvector diff norm:", norm2(vecs1 - vecs2)
   write (*,*) "Eigenvector maxdiff:", maxval(abs(vecs1 - vecs2))

   call unique_eigvec(eig1, vecs1, ierr)
   if(ierr /= 0) stop
   call unique_eigvec(eig2, vecs2, ierr)
   if(ierr /= 0) stop

   write (*,*) new_line("a") // " After:"
   !write (*,*) "Eigenvalues:", eig1
   write (*,*) "Eigenvector diff norm:", norm2(vecs1 - vecs2)
   write (*,*) "Eigenvector maxdiff:", maxval(abs(vecs1 - vecs2))

   call investigate(eig1, vecs1, vecs2)
   !call check_orthogonality(vecs1)
   !call check_eigenvectors(H, vecs1, eig1)
contains
   subroutine shuffle_matrix(H)
      implicit none
      real(kind=8), intent(inout) :: H(:,:)

      real(kind=8), allocatable  :: P(:,:), tmp(:,:)
      real(kind=8), parameter    :: one = 1.0, zero = 0.0
      real(kind=8)               :: theta
      integer :: i,j, dim

      dim = size(H,1)

      allocate(P, tmp, mold=H)

      do i = 1,floor(dim/2.0)
         j = N-i+1
         theta = 0.01*i
         call givens(i,j, theta, P)

         ! tmp = H * P
         call dgemm("N", "N", dim, dim, dim, one, H, dim, P, dim, zero, tmp, dim)

         ! H = P^-1 * tmp
         call dgemm("T", "N", dim, dim, dim, one, P, dim, tmp, dim, zero, H, dim)
      enddo
   end subroutine shuffle_matrix

   subroutine givens(i,j,theta, mat)
      implicit none
      integer, intent(in)         :: i,j
      real(kind=8), intent(in)    :: theta
      real(kind=8), intent(inout) :: mat(:,:)
      integer :: k

      mat = 0.0

      do k = 1,N
         mat(k,k) = 1
      enddo

      mat(i,i) = cos(theta)
      mat(j,j) = cos(theta)
      mat(j,i) = sin(theta)
      mat(i,j) = - sin(theta)

   end subroutine givens

   subroutine investigate(eigval, eigvec1, eigvec2)
      implicit none
      real(kind=8), intent(in) :: eigval(:), eigvec1(:,:), eigvec2(:,:)
      integer :: beg_group, end_group, n_g
      integer, allocatable :: groups(:)
      integer :: deg

      groups = make_groups(eigval)

      write (*,*) "degeneracy    eigenvalue             norm diff"
      do n_g = 1, size(groups)
         beg_group = sum(groups(1:n_g-1)) + 1
         end_group = sum(groups(1:n_g))

         deg = end_group - beg_group + 1
         write (*,*) deg, eigval(beg_group), norm2(eigvec1(:,beg_group:end_group) - eigvec2(:,beg_group:end_group))
      enddo
   end subroutine investigate

   subroutine check_orthogonality(mat)
      implicit none
      real(kind=8), intent(in) :: mat(:,:)
      integer :: i,j

      do j = 1, size(mat,2)
         do i = 1, j-1
            if(abs(dot_product(mat(:,i), mat(:,j))) > 1e-8 ) then
               write (*,*) i, j, "not orthogonal", dot_product(mat(:,i), mat(:,j))
            endif
         enddo
      enddo

      do i = 1,size(mat,2)
         if(abs(dot_product(mat(:,i), mat(:,i)) - 1) > 1e-8 ) then
            write (*,*) i, "not normalized"
         endif
      enddo
   end subroutine check_orthogonality

   subroutine check_eigenvectors(A, u, eigval)
      implicit none
      real(kind=8), intent(in)  :: A(:,:), u(:,:), eigval(:)
      real(kind=8), allocatable :: diff(:)

      real(kind=8), parameter :: one=1.0, zero=0.0

      do i = 1,size(eigval)
         diff = matmul(A,u(:,i)) - eigval(i)* u(:,i)
         if(norm2(diff) > 1e-10) then
            write (*,*) i, "diff"!, diff
         endif
      enddo
   end subroutine check_eigenvectors
end program main