!==========================================================================
elemental subroutine gsw_specvol_second_derivatives_wrt_enthalpy (sa, ct, &
                                          p, v_sa_sa, v_sa_h, v_h_h, iflag)
! =========================================================================
!
!  Calculates three first-order derivatives of specific volume (v) with
!  respect to enthalpy. Note that this function uses the using the
!  computationally-efficient expression for specific volume
!  (Roquet et al., 2014).
!
!  SA  =  Absolute Salinity                                        [ g/kg ]
!  CT  =  Conservative Temperature (ITS-90)                       [ deg C ]
!  p   =  sea pressure                                             [ dbar ]
!         ( i.e. absolute pressure - 10.1325 dbar )
!
!  v_SA_SA = The second-order derivative of specific volume with respect to
!            Absolute Salinity at constant h & p.       [ J/(kg (g/kg)^2) ]
!  v_SA_h  = The second-order derivative of specific volume with respect to
!            SA and h at constant p.                     [ J/(kg K(g/kg)) ]
!  v_h_h   = The second-order derivative with respect to h at
!            constant SA & p.
!--------------------------------------------------------------------------

use gsw_mod_toolbox, only : gsw_enthalpy_first_derivatives
use gsw_mod_toolbox, only : gsw_enthalpy_second_derivatives
use gsw_mod_toolbox, only : gsw_specvol_first_derivatives
use gsw_mod_toolbox, only : gsw_specvol_second_derivatives

use gsw_mod_kinds

implicit none

real (r8), intent(in) :: sa, ct, p
integer, intent(in), optional :: iflag
real (r8), intent(out), optional :: v_sa_sa, v_sa_h, v_h_h

logical :: flags(3)
real (r8) :: h_ct, h_ct_ct, h_sa, h_sa_ct, h_sa_sa, rec_h_ct, v_h_h_part
real (r8) :: rec_h_ct2, v_ct, vct_ct_ct, vct_sa_ct, vct_sa_sa, v_sa_h_part

if (present(iflag)) then
    flags(1) = present(v_sa_sa) .and. btest(iflag,1)
    flags(2) = present(v_sa_h) .and. btest(iflag,2)
    flags(3) = present(v_h_h) .and. btest(iflag,3)
else
    flags(1) = present(v_sa_sa)
    flags(2) = present(v_sa_h)
    flags(3) = present(v_h_h)
end if

call gsw_specvol_first_derivatives(sa,ct,p,v_ct=v_ct)

if (flags(1) .or. flags(2)) then
   call gsw_enthalpy_first_derivatives(sa,ct,p,h_sa,h_ct)
else
   call gsw_enthalpy_first_derivatives(sa,ct,p,h_ct=h_ct)
end if

if (flags(1)) then
   call gsw_specvol_second_derivatives(sa,ct,p,vct_sa_sa,vct_sa_ct,vct_ct_ct)
else if (flags(2)) then
   call gsw_specvol_second_derivatives(sa,ct,p,v_sa_ct=vct_sa_ct, &
                                                v_ct_ct=vct_ct_ct)
else
   call gsw_specvol_second_derivatives(sa,ct,p,v_ct_ct=vct_ct_ct)
end if

if (flags(1)) then
   call gsw_enthalpy_second_derivatives(sa,ct,p,h_sa_sa,h_sa_ct,h_ct_ct)
else if (flags(2)) then
   call gsw_enthalpy_second_derivatives(sa,ct,p,h_sa_ct=h_sa_ct,h_ct_ct=h_ct_ct)
else
   call gsw_enthalpy_second_derivatives(sa,ct,p,h_ct_ct=h_ct_ct)
end if

rec_h_ct = 1.0_r8/h_ct
rec_h_ct2 = rec_h_ct**2.0_r8

v_h_h_part = (vct_ct_ct*h_ct - h_ct_ct*v_ct)*(rec_h_ct2*rec_h_ct)

if (flags(3)) v_h_h = v_h_h_part

if (flags(1) .or. flags(2)) then

    v_sa_h_part = (vct_sa_ct*h_ct - v_ct*h_sa_ct)*rec_h_ct2 - h_sa*v_h_h_part

    if (flags(2)) v_sa_h = v_sa_h_part

    if (flags(1)) v_sa_sa = vct_sa_sa - (h_ct*(vct_sa_ct*h_sa &
            - v_ct*h_sa_sa) + v_ct*h_sa*h_sa_ct)*rec_h_ct2 - h_sa*v_sa_h_part
end if

return
end subroutine

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


C    Copyright(C) 1999-2020 National Technology & Engineering Solutions
C    of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
C    NTESS, the U.S. Government retains certain rights in this software.
C
C    See packages/seacas/LICENSE for details

      SUBROUTINE EXCORN (MXND, XN, YN, LNODES, ANGLE, N0, N1, N2, XNEW,
     &   YNEW)
C***********************************************************************

C  SUBROUTINE EXCORN = CALCULATES A POSITION AN AVERAGE LENGTH AWAY
C                      FROM A CORNER NODE

C***********************************************************************

      DIMENSION XN (MXND), YN (MXND), LNODES (7, MXND), ANGLE (MXND)

      LOGICAL SIDEP

C      XNEW = XN (N0) + XN (N2) - XN (N1)
C      YNEW = YN (N0) + YN (N2) - YN (N1)

      PID2 = 0.5 * ATAN2(0.0, -1.0)

C      ANG2 = ATAN2 (YN (N1)-YN (N0), XN (N1)-XN (N0))+PID2
      BANG1 = ATAN2 (YN (N1) - YN (N2), XN (N1) - XN (N2))
      BANG2 = ATAN2 (YN (N1) - YN (N0), XN (N1) - XN (N0))
      IF (SIDEP (ANGLE (N2)))THEN
         ANG1 = BANG1 - (ANGLE (N2) * .5)
      ELSE
         ANG1 = BANG1 - PID2
      ENDIF
      IF (SIDEP (ANGLE (N0)))THEN
         ANG2 = BANG2 + (ANGLE (N0) * .5)
      ELSE
         ANG2 = BANG2 + PID2
      ENDIF
      DIST1 = SQRT ((YN (N2)-YN (N1)) **2 +  (XN (N2)-XN (N1)) **2)
      DIST2 = SQRT ((YN (N0)-YN (N1)) **2 +  (XN (N0)-XN (N1)) **2)
      DIST = (DIST1 + DIST2) * .5
      XNEW = ( (DIST * COS (ANG1) + XN (N2)) +
     &   (DIST * COS (ANG2) + XN (N0)) ) * .5
      YNEW = ( (DIST * SIN (ANG1) + YN (N2)) +
     &   (DIST * SIN (ANG2) + YN (N0)) ) * .5
C      XNEW = DIST * COS (ANG1) + XN (N2)
C      YNEW = DIST * SIN (ANG1) + YN (N2)

      RETURN

      END


!     UPDATE
!     ------
!     Jerome Lefevre IRD : Add Three Vertical Stretching Function a la
!     ROMS/RUTGERS in order to get more resolution both in the shallow
!     area but in the deep region too.
!     See Line 185 : option : if true,
!     See Line 316 : option : if true

!    Generate sigma coordinates for ivcor=1 (VQS from Dukhovskoy)
!    Works for mixed tri/quad
!    WARNING: most variables/arrays in this program use '1' as surface, and nvrt*
!    as bottom!!!

!    Inputs: (1) consts. inside the code (for max. flexibility); (2) hgrid.gr3 (in map projection);
!            (3) screen; (4) transect.bp (optional transect bp file; depths denote seg #)
!    Outputs: vgrid.in; vgrid_master.out;  transect*.out; debug outputs (fort*)
!    Use plot_VQS.m to viz vgrid_master.out; transect*.out
!    ifort -O2 -mcmodel=medium -CB -o gen_vqs_Rutgers.exe ../UtilLib/schism_geometry.f90 gen_vqs_Rutgers.f90

      program gen_vqs_rutgers
      use schism_geometry_mod
!     implicit real*8(a-h,o-z)
      integer,parameter :: rkind = 8      ! Default real datatype

      integer, allocatable :: elnode(:,:),elside(:,:),kbp0(:),kbp(:)
      integer, allocatable :: ic3(:,:),isdel(:,:),isidenode(:,:),m0(:)
      integer, allocatable :: nv_vqs(:)
      integer, allocatable :: i34(:)
      real(rkind), allocatable :: xnd(:),ynd(:),dp(:),xcj(:,:),ycj(:,:),eta2(:)
      real(rkind), allocatable :: znd(:,:),z1tmp(:),z2tmp(:)
      real(rkind), allocatable :: hsm(:),z_mas(:,:),a_vqs(:)
      real(rkind), allocatable :: xybp(:,:),dpbp(:),imap(:),transect_len(:)
      real(rkind), allocatable :: sigma_vqs(:,:)
      real(rkind) :: z1,z2,z3,zrat,cs1,cs2,cs
      integer :: i,k,m,itran,FEXIST,m_vqs
      integer :: NE,NP

      character*200 :: HGRID_FILE
      character*200 :: TRANSECT_FILE
      character*200 :: BUFFER

!-- New Vertical Streching parameters adapted from ROMS/RUTGERS/UCLA
!   Applied both to Shallow area where bottom <= hms and deep ocean too
!
! - See https://www.myroms.org/wiki/Vertical_S-coordinate
!
      REAL(rkind), allocatable :: S_RUTGERS(:,:)
      REAL(rkind), allocatable :: V_RUTGERS(:)
      REAL(rkind), allocatable :: sc_w(:), Cs_w(:)
      INTEGER :: VSTRETCHING  ! Vertical stretching function Id (value:2,3 or 4)
                              ! VSTRETCHING=2: A. Shchepetkin (2005) UCLA-ROMS
                              ! VSTRETCHING=3: R. Geyer function for high
                              ! bottom resolution in relatively shallow applications
                              ! VSTRETCHING=4: A. Shchepetkin (2010) UCLA-ROMS
      REAL(rkind) :: RTHETA_S ! STRETCHING PARAMETER CONTROLLING LEVEL DISTRIBUTION OF SURFACE
      REAL(rkind) :: RTHETA_B ! STRETCHING PARAMETER CONTROLLING LEVEL DISTRIBUTION OF BOTTOM
      REAL(rkind) :: TCLINE   ! or HC : STRETCHING PARAMETER :Critical depth (hc) in meters (positive)
                              ! controlling the stretching. It can be interpreted as the width

! User Choice : please Read Carrefully  https://www.myroms.org/wiki/Vertical_S-coordinate
!               and adapt
       VSTRETCHING = 4
       RTHETA_S = 5.0
       RTHETA_B = 3.0
       TCLINE  = 5.0

!      VSTRETCHING = 2     VSTRETCHING = 2
!      RTHETA_S = 7.0      RTHETA_S = 9.0
!      RTHETA_B = 0.1      RTHETA_B = 0.1
!      TCLINE  = 5.0       TCLINE  = 5.0

!      VSTRETCHING = 3
!      RTHETA_S = 1.0
!      RTHETA_B = 3.0
!      TCLINE  = 5.0

      ! -------------------------------------------------------------------------
      ! Namelist : input parameters :

      print*, 'Please provide the hgrid.gr3 file path'
      !read(*,'(a200)') BUFFER
      read(5,'(a200)') BUFFER
      HGRID_FILE = trim(adjustL(BUFFER))

      INQUIRE(FILE=trim(HGRID_FILE),EXIST=FEXIST)

      IF(.NOT. FEXIST)THEN
          WRITE(*,*)'hgrid.gr3 file is not found'
          STOP
      ELSE
          WRITE(*,*)'Grid File found ',HGRID_FILE
      ENDIF

      print*, 'Want to output along a transect? (0: no; 1:yes)'
      read(5,'(I)') itran
      !itran = 1

      IF(itran) THEN
        TRANSECT_FILE = 'transects.bp'
        INQUIRE(FILE=trim(TRANSECT_FILE),EXIST=FEXIST)
        IF(.NOT. FEXIST)THEN
           itran=0
           WRITE(*,*)'WARNING ',TRANSECT_FILE,' not found ! No transect&
     &                output will be supplied'
        ENDIF
      ENDIF

     ! End Namelist
     ! -------------------------------------------------------------------------

!     Read  vgrid.in
      !m_vqs: # of master grids
      !m_vqs=40

      m_vqs=19
      !dz_bot_min=3.0 !min. bottom layer thickness [m]
      dz_bot_min=3.0

      allocate(hsm(m_vqs),nv_vqs(m_vqs),a_vqs(m_vqs))
      !hsm=(/(2+1*i,i=1,m_vqs)/)
      !nv_vqs(1:m_vqs)=(/(6+2*(i-1),i=1,m_vqs)/) !# of levels for each master grid (increasing with depth)

!      hsm=(/(70+4*i*(i-1),i=1,m_vqs)/)
!      hsm=(/(20+1.115*i*(i-1),i=1,m_vqs)/)

      !nv_vqs(1:m_vqs)=(/(20+1*(i-1),i=1,m_vqs)/) !# of levels for each master grid (increasing with depth)
!      nv_vqs(1:m_vqs)=(/(12+1*(i-1),i=1,m_vqs)/)

      !m_vqs=39
      !m_vqs=23
      !dz_bot_min=1 !min. bottom layer thickness [m]
      !allocate(hsm(m_vqs),nv_vqs(m_vqs),a_vqs(m_vqs))
      !hsm=(/50,60,80,110,150,200,260,330,410,500,600,710,830,960,1100/) !depths for each master grid (ascending order)
      !hsm=(/50,60,80,110,150,200,260,330,410,500,600,710,830,960,1100,1250,1410, & !m_vqs=39
      !    &1580,1760,1950,2150,2360,2580,2810,3050,3300,3560,3830,4110,4400,4700,5010,5330, &
      !    &5660,6000,6350,6710,7080,7460/)
      !hsm=(/5,10,13,16,20,25,30,40,50,60,70,80,90,100,110,120,130,140,150,160,170,180,190,201/) !m_vqs=24
      !hsm=(/50,60,80,110,150,200,260,330,410,500,600,710,830,960,1100,1250,1410,1580,1760,1950,2150,2360,2500/)
      !nv_vqs(1:m_vqs)=(/(21+1*(i-1),i=1,m_vqs)/) !# of levels for each master grid (increasing with depth)

      hsm=(/50,60,80,110,150,200,260,330,410,500,600,710,830,960,1100,1250,1410,1580,1760/)
      nv_vqs(1:m_vqs)=(/(14+1*(i-1),i=1,m_vqs)/)

      if(m_vqs<2) then
        write(*,*)'Check vgrid.in:',m_vqs
        stop
      endif
      if(hsm(1)<0) stop 'hsm(1)<0'
      do m=2,m_vqs
        if(hsm(m)<=hsm(m-1)) then
          write(*,*)'Check hsm:',m,hsm(m),hsm(m-1)
          stop
        endif
      enddo !m_vqs

!     Other consts.
!     Stretching const. for the 1st master grid and also for depth <= hsm(1)
!     |a_vqs0|<=1 (1: skew toward bottom; -1: toward surface; 0: no bias)
      a_vqs0=-1.

!     Generate a master vgrid (z_mas)
      etal=0 !used in master grid only; elev.
      if(etal<=-hsm(1)) then
        write(*,*)'elev<hsm:',etal
        stop
      endif

      nvrt_m=nv_vqs(m_vqs)
      print*, 'nvrt in master vgrid=',nvrt_m
      allocate(z_mas(nvrt_m,m_vqs))
      z_mas=-1.e5
      do m=1,m_vqs
        do k=1,nv_vqs(m)
          sigma=(k-1.0)/(1.0-nv_vqs(m))

!         Alternative transformations below
!         Option 1: quadratic
          if(0)then
           a_vqs(m)=max(-1.d0,a_vqs0-(m-1)*0.03)
           tmp=a_vqs(m)*sigma*sigma+(1+a_vqs(m))*sigma !transformed sigma
           z_mas(k,m)=tmp*(etal+hsm(m))+etal
          endif

!          Option 2: S
          if(1)then
           !theta_b=0
           !theta_f=3+0.0*(m-1)
           !theta_b=0.666666    ! 0 - 1  , toward 0, only the surface is rafined, 1 = both
           theta_b=0.0
           theta_f=4.0 ! 0 - 20 , toward 0 like a traditionalsigma with uniform spacing

           cs1 = (1-theta_b)*sinh(theta_f*sigma)/sinh(theta_f)
           cs2 = theta_b*(tanh(theta_f*(sigma+0.5))-tanh(theta_f*0.5))/2/tanh(theta_f*0.5)
           cs=cs1+cs2
           z_mas(k,m)=etal*(1+sigma)+hsm(1)*sigma+(hsm(m)-hsm(1))*cs
          endif

        enddo !k

!       Option 3: Rutgers Coordinate : z_mas above will be erased
        if(1)then
            !z_mas = 0.0_rkind
            k = nv_vqs(m)
            if(allocated(sc_w)) deallocate(sc_w)
            if(allocated(Cs_w)) deallocate(Cs_w)
            allocate(sc_w(0:k), stat=ierr)
            allocate(Cs_w(0:k), stat=ierr)
            sc_w = 0.0_rkind; Cs_w = 0.0_rkind;
            call SIGMA_RUTGERS( k,sc_w,Cs_w )

           !Compute the sigma coordinate from 0 to hsm(m)
            if(allocated(V_RUTGERS)) deallocate(V_RUTGERS)
            allocate(V_RUTGERS(1:k))
            V_RUTGERS = 0.0_rkind
            call SIGMA_RUTGERS_VEC(hsm(m),k,sc_w,Cs_w,V_RUTGERS)
            !WRITE(*,*) V_RUTGERS
            z_mas(1:k,m) = V_RUTGERS(1:k)*hsm(m)
        endif

      enddo !m_vqs

! DBG
      do m=1,m_vqs
         WRITE(*,*) z_mas(:,m)
      enddo

!     Output master vgrid
      open(13,file='vgrid_master.out',status='replace')
      do m=1,m_vqs
       write(13,'(2(1x,i5),6000(1x,f12.4))')m,nv_vqs(m),hsm(m),z_mas(:,m)
      enddo !m
      close(13)

      do k=1,nvrt_m
        write(12,'(i5,6000(1x,f12.4))')k,z_mas(k,:)
      enddo !k

      nvrt=nvrt_m
!     Read in hgrid
      open(14,file=trim(HGRID_FILE),status='old')
      read(14,*)
      read(14,*)ne,np
      allocate(xnd(np),ynd(np),dp(np),i34(ne),elnode(4,ne))
      allocate(kbp0(np),kbp(np),eta2(np), sigma_vqs(nvrt,np))
      allocate(znd(nvrt,np),m0(np),z1tmp(nvrt),z2tmp(nvrt))
      eta2=etal
      do i=1,np
        read(14,*)j,xnd(i),ynd(i),dp(i)
      enddo !i
      do i=1,ne
        read(14,*)j,i34(i),elnode(1:i34(i),i)
      enddo !i
      close(14)

      !Check max depth
      dpmax=maxval(dp)
      if(dpmax>hsm(m_vqs)) then
        print*, 'Max depth exceeds master depth:',dpmax,hsm(m_vqs)
        stop
      endif

      call compute_nside(np,ne,i34,elnode,ns)
      print*, 'ns=',ns
      allocate(ic3(4,ne),elside(4,ne),isdel(2,ns))
      allocate(isidenode(2,ns),xcj(ns,2),ycj(ns,2))
      call schism_geometry_double(np,ne,ns,xnd,ynd,i34,elnode,ic3,elside,isdel,isidenode,xcj,ycj)
      !deallocate()

! Preparation
! --------------
      ! Shallow area with depth<= hsm
      ! Get Stretching Coeff sc_w and Cs_w
      if(allocated(sc_w)) deallocate(sc_w)
      if(allocated(Cs_w)) deallocate(Cs_w)
      allocate(sc_w(0:nv_vqs(1)), stat=ierr)
      allocate(Cs_w(0:nv_vqs(1)), stat=ierr)
      sc_w = 0.0_rkind; Cs_w = 0.0_rkind;

      call SIGMA_RUTGERS( nv_vqs(1),sc_w,Cs_w )

      ! Compute the sigma coordinate on the whole grid a la RUTGERS,
      ! from 0 to hsm : S_RUTGERS is Updated to be used later line 303
      ! in computing "sigma_vqs" and "znd"

      if(allocated(S_RUTGERS)) deallocate(S_RUTGERS)
      allocate(S_RUTGERS(1:nv_vqs(1),1:NP))
      S_RUTGERS = 0.0_rkind
      call SIGMA_RUTGERS_MAT(nv_vqs(1),sc_w,Cs_w,S_RUTGERS)

!      !Compute the sigma coordinate on the master grid, form 0 to hsm
!      do m=1,m_vqs
!
!          if(dp(i)>hsm(m-1).and.dp(i)<=hsm(m)) then
!            m0(i)=m
!            zrat=(dp(i)-hsm(m-1))/(hsm(m)-hsm(m-1)) !(0,1]
!            exit
!          endif
!      enddo !m

! ---------------------------------------------------
! Compute zcoor
! ---------------------------------------------------
      znd=-1.e6
      do i=1,np

! ----------------------------------------------------------
! Shallow region
! -----------------------------------------------------------

        if(dp(i)<=hsm(1)) then !shallow
          kbp(i)=nv_vqs(1)

         do k=1,nv_vqs(1)

           if(0) then  ! Original Way, The Shism way

             sigma=(k-1.0)/(1.0-nv_vqs(1))
             sigma_vqs(k,i)=a_vqs0*sigma*sigma+(1+a_vqs0)*sigma !transformed sigma
             znd(k,i)=sigma_vqs(k,i)*(eta2(i)+dp(i))+eta2(i)

           else   ! New way to compute sigma in shallow using Rutgers Streching Function

             sigma_vqs(k,i)=S_RUTGERS(k,i)
             znd(k,i) = sigma_vqs(k,i)*(eta2(i)+dp(i))+eta2(i)

           endif

         enddo
!DBG
!           print*,'SHALLOW',i,dp(i),' ',znd(1:nv_vqs(1),i)
          cycle
        endif

! ----------------------------------------------------------
! Deep region : If RUTGERS TRUE Line 188, then z_mas depths are those
!               computed with Rutgers Stretching function
! -----------------------------------------------------------
        !Find a master vgrid
        m0(i)=0
        do m=2,m_vqs
          if(dp(i)>hsm(m-1).and.dp(i)<=hsm(m)) then
            m0(i)=m
            zrat=(dp(i)-hsm(m-1))/(hsm(m)-hsm(m-1)) !(0,1]
            exit
          endif
        enddo !m
        if(m0(i)==0) then
          print*, 'Failed to find a master vgrid:',i,dp(i)
          stop
        endif

        kbp(i)=0
        do k=1,nv_vqs(m0(i))

          z1=z_mas(min(k,nv_vqs(m0(i)-1)),m0(i)-1)
          z2=z_mas(k,m0(i))
          z3=z1+(z2-z1)*zrat
          if(z3>=-dp(i)+dz_bot_min) then
            znd(k,i)=z3
          else
            kbp(i)=k; exit
          endif

        enddo !k
        if(kbp(i)==0) then
          print*, 'Failed to find a bottom:',i,dp(i),z3,z_mas(1:nv_vqs(m0(i)),m0(i))
          stop
        endif
        znd(kbp(i),i)=-dp(i)

!       print*,'DEEP',i,znd(1:nv_vqs(m0(i)),i)

        !Check order
        do k=2,kbp(i)
          if(znd(k-1,i)<=znd(k,i)) then
            print*, 'Inverted z:',i,dp(i),m0(i),k,znd(k-1,i),znd(k,i)
            stop
          endif
        enddo !k
        write(99,*)'Node:',i,real(dp(i)),real(znd(1:kbp(i),i))
      enddo !i=1,np

!      print*,'DEEP',4204,' kbp=',kbp(4204)
!      print*,'DEEP',4204,znd(1:nv_vqs(m0(4204)),4204)
!      print*,'DEEP',4204,znd(1:kbp(4204),4204)

!     Extend beyond bottom for plotting
      do i=1,np
        znd(kbp(i)+1:nvrt,i)=-dp(i)
      enddo !i

!     Optional output along transect
      if(itran/=0) then
        open(9,file=TRANSECT_FILE,status='old')
        read(9,*)
        read(9,*)npbp
        allocate(xybp(2,npbp),dpbp(npbp),imap(npbp),transect_len(npbp))
        transect_len(1)=0 !along transect distance
        do i=1,npbp
          read(9,*)j,xybp(1:2,i),dpbp(i)
          if(i>1) then
          rl=sqrt((xybp(1,i)-xybp(1,i-1))**2+(xybp(2,i)-xybp(2,i-1))**2)
          transect_len(i)=transect_len(i-1)+rl
          endif
        enddo !i
        close(9)
        !Find nearest node
        do i=1,npbp

!          rlmin=huge(1.0d0)
          rlmin=huge(100.0d0)

          do j=1,np
            rl2=((xybp(1,i)-xnd(j))**2+(xybp(2,i)-ynd(j))**2)**.5

            if(rl2<rlmin) then
              imap(i)=j; rlmin=rl2
              print*,'imap(i) rlmin',imap(i),rlmin
            endif
          enddo !j
        enddo !i

        open(13,file='transect1.out',status='replace')
        do i=1,npbp
          nd=imap(i)
          print *,nd
          write(13,'(i6,1x,i4,2(1x,e16.6),20000(1x,f12.3))')i,kbp(nd),xybp(1:2,i),transect_len(i),dp(nd),dpbp(i),znd(:,nd)
        enddo !i
        close(13)
      endif !itran

      if(1==2) then
!----------------------------
!     Check case where a side saddles hsm(1) - try to make the # of level equal
!Error: need to iterate?
      do i=1,ns
        n1=isidenode(1,i)
        n2=isidenode(2,i)
        if(kbp(n1)==kbp(n2).or.(dp(n1)-hsm(1))*(dp(n2)-hsm(1))>0) cycle

        print*, 'Correcting layers near hsm(1)...',n1,n2,real(xnd(n1)),real(xnd(n2))
        if(kbp(n1)>kbp(n2)) then
          in1=n2; in2=n1
        else
          in1=n1; in2=n2
        endif
        k_add=kbp(in2)-kbp(in1)
        if(k_add<=0) stop 'k_add<=0'
        do k=1,k_add
          znd(kbp(in1)-1+k,in1)=znd(kbp(in1)-1,in1)-k*(znd(kbp(in1)-1,in1)+dp(in1))/(k_add+1)
        enddo !k
        znd(kbp(in1)+k_add,in1)=-dp(in1)
        kbp(in1)=kbp(in1)+k_add

        !Check
        do k=2,kbp(in1)
          if(znd(k-1,in1)<=znd(k,in1)) then
            print*, 'Inverted z (2):',n1,n2,dp(n1),dp(n2),k,znd(k-1,in1),znd(k,in1)
            stop
          endif
        enddo !k
      enddo !i=1,ns

!     Adjust staircase size??

!     Optional output along transect
      if(itran/=0) then
        open(13,file='transect2.out',status='replace')
        do i=1,npbp
          nd=imap(i)
          write(13,'(i6,1x,i4,2(1x,e16.6),10000(1x,f12.3))')i,kbp(nd),xybp(1:2,i),dp(nd),znd(:,nd)
        enddo !i
        close(13)
      endif !itran
!----------------------------
      endif !1==2

      nvrt=maxval(kbp)
      print*, 'Final nvrt=',nvrt
!     # of prisms
      nprism=0
      do i=1,ne
        kbpl=maxval(kbp(elnode(1:i34(i),i)))
        nprism=nprism+kbpl
      enddo !i

      print*, '# of prisms=',nprism
      print*, 'Average # of layers=',real(nprism)/ne

!     Output in SCHISM convention
      open(19,file='vgrid.in',status='replace')
      write(19,'(I2)')1 !ivcor
      write(19,'(I2)')nvrt

      do i=1,np

        if(dp(i)<=hsm(1)) then
          !sigma_vqs already assigned
        else
          sigma_vqs(1,i)=0
          sigma_vqs(kbp(i),i)=-1
          do k=2,kbp(i)-1
            sigma_vqs(k,i)=(znd(k,i)-eta2(i))/(eta2(i)+dp(i))
          enddo !k
        endif

        !Check order
        do k=2,kbp(i)
          if(sigma_vqs(k,i)>=sigma_vqs(k-1,i)) then
            print*, 'Inverted sigma:',i,k,dp(i),sigma_vqs(k,i),sigma_vqs(k-1,i)
            stop
          endif
        enddo !k

        write(19,'(2(1x,i10),10000(1x,f14.6))')i,nvrt+1-kbp(i),sigma_vqs(kbp(i):1:-1,i)
      enddo !i
      close(19)

!     Output horizontal map of # of levels for more adjustment
      open(13,file='nlev.gr3',status='replace')
      write(13,*)'# of levels at each node'
      write(13,*)ne,np
      do i=1,np
        write(13,*)i,xnd(i),ynd(i),kbp(i)
      enddo !i
      do i=1,ne
        write(13,*)i,i34(i),elnode(1:i34(i),i)
      enddo !i
      close(13)

      CONTAINS

!--------------------------------------------------------------------
       SUBROUTINE SIGMA_RUTGERS(KB,sc_w,Cs_w)  !S_RUTGERS)
!
! JEROME (IRD Noumea, 5-Oct-2017)
!
! Support to use several Vertical Stretching function for vertical
! terrain-following coordinates as implemented in ROMS RUTGERS/UCLA
! ------------------------------------------------------------------
! Various possible vertical stretching are provided. They are tunable
! by using different value for Vstretching (2, 3 or 4), rtheta_s, rtheta_b
! and Hc (Tcline). The original vertical stretching function from Song
! and
! Haidvogel (1994) is not supplied, but can be approached by setting
! Vstretching = 2
!
! See: Shchepetkin, A.F. and J.C. McWilliams, 2005: The regional oceanic
! modeling system (ROMS): a split-explicit, free-surface,
! topography-following-coordinate oceanic model, Ocean
! See details: https://www.myroms.org/wiki/Vertical_S-coordinate
!
! In Subroutine SIGMA_RUTGERS :
! the original ROMS/RUTGERS/UCLA algorithm to compute sigma coordinate S
! The vertical stretching function Cs at ROMS (SHISM) W-points (layer
! interfaces) are applied.
!
! In Subroutine SIGMA_RUTGERS_MAT
! Vertical depths through the whole Vertices and levels are applied
! using the Rutgers Vertical streching function.
! In Section 3, a wrapper is applied to set the value of sigma
! coordinates compatible with FVCOM/SCHISM

! In Subroutine SIGMA_RUTGERS_VEC
! Vertical depths at discrete vertices (in deep region) are applied
! using the Rutgers Vertical streching function.
! In Section 3, a wrapper is applied to set the value of sigma
! coordinates compatible with FVCOM/SCHISM

      IMPLICIT NONE

      integer,parameter :: rkind = 8
      INTEGER :: I,k,kk
      integer status, ierr

!  Local variable declarations.
      real(rkind) :: Aweight, Bweight, Cweight, Cbot, Csur, Hscale
      real(rkind) :: ds, exp_bot, exp_sur
      real(rkind) :: cff_r, cff1_r, cff2_r, cff_w, cff1_w, cff2_w
      real(rkind) :: hinv, hwater, z_r0, z_w0
      real(rkind) :: C2_r, C2_w, hh2, vert_n1, vert_a, vert_h0, vert_s0
      real(rkind) :: hc
      integer,intent(in) :: KB
      real(rkind),allocatable,intent(inout) :: sc_w(:),Cs_w(:)
      real(rkind) :: Zt_avg1
      real(rkind), PARAMETER :: eps = 1.E-8_rkind
      integer :: KBM1

! ------------------------------------------------------------------
! Section 1 : Sigma Vertical coordinate Definition by using various
! stretching function as introduced by the RUTGERS/UCLA team
! --------------------------------------------------------------------
! Set S(sigma(k)), the non dimensionnal stretched vertical coordinate
!  with :  -1 <= S <= 0  ;  S= 0 at the surface   S = -1 at the bottom
! Set  C : nondimensional vertical stretching function, C(sigma(k)),
!  with :  -1 <= C(s) <= 0  : C= 0 at the surface   C = -1 at the bottom
!
! Local naming convention (ROMS Rutgers Like) :
! sc_w at W-point i.e layer interface (dimension KB)
! sc_r at Rho-point i.e mid layer     (dimension KBM1)
! Cs_w at W-point i.e layer interface (dimension KB)
! Cs_r at Rho-point i.e mid layer     (dimension KBM1)
!
! --------------------------------------------------------------------

      WRITE(*,*)"SIGMA_RUTGERS: START"

!     Wrapper :
      KBM1 = KB-1
      hc = TCLINE

!-----------------------------------------------------------------------
! Vstretching = 2 : The New  A. Shchepetkin new vertical stretching
!                   function.
! See :
!    Shchepetkin, A.F. and J.C. McWilliams, 2005: The regional oceanic !
!         modeling system (ROMS): a split-explicit, free-surface,      !
!         topography-following-coordinate oceanic model, Ocean         !
!         Modelling, 9, 347-404.
!-----------------------------------------------------------------------
      IF (VSTRETCHING .eq. 2) THEN

       WRITE(*,*) 'STRECHING TYPE 2'

       Aweight = 1.0_rkind
       Bweight = 1.0_rkind
       ds=1.0_rkind/FLOAT(KBm1)

! W-Point layer Interface
       DO k=KBm1-1,1,-1
        cff_w  = ds*FLOAT(k-KBm1)
        sc_w(k)= cff_w
        IF (rtheta_s.gt.0.0_rkind) THEN
            Csur=(1.0_rkind-COSH(rtheta_s*cff_w))/(COSH(rtheta_s)-1.0_rkind)

          IF (rtheta_b.gt.0.0_rkind) THEN
             Cbot=SINH(rtheta_b*(cff_w+1.0_rkind))/SINH(rtheta_b)-1.0_rkind
             Cweight=(cff_w+1.0_rkind)**Aweight*(1.0_rkind+(Aweight/Bweight)* &
           & (1.0_rkind-(cff_w+1.0_rkind)**Bweight))
             Cs_w(k)=Cweight*Csur+(1.0_rkind-Cweight)*Cbot
          ELSE
             Cs_w(k)=Csur
          END IF

        ELSE
          Cs_w(k)=cff_w
        END IF
       END DO
       sc_w(0)=-1.0_rkind
       Cs_w(0)=-1.0_rkind

!-----------------------------------------------------------------------
! Vstretching = 3 : R. Geyer stretching function for high bottom
!                   boundary layer resolution
!-----------------------------------------------------------------------
       ELSE IF (VSTRETCHING.eq.3) THEN

       WRITE(*,*) 'STRECHING TYPE 3'

       exp_sur=rtheta_s
       exp_bot=rtheta_b
       Hscale=3.0_rkind
       ds=1.0_rkind/FLOAT(KBm1)
! W-Point layer Interface
       sc_w(KBm1)=0.0_rkind
       Cs_w(KBm1)=0.0_rkind
       DO k=KBm1-1,1,-1
         cff_w  = ds*FLOAT(k-KBm1)
         sc_w(k)= cff_w
         Cbot= LOG(COSH(Hscale*(cff_w+1.0_rkind)**exp_bot))/ &
          &    LOG(COSH(Hscale))-1.0_rkind
         Csur=-LOG(COSH(Hscale*ABS(cff_w)**exp_sur))/ &
          &    LOG(COSH(Hscale))
         Cweight=0.5_rkind*(1.0_rkind-TANH(Hscale*(cff_w+0.5_rkind)))
         Cs_w(k)=Cweight*Cbot+(1.0_rkind-Cweight)*Csur
       END DO
         sc_w(0)=-1.0_rkind
         Cs_w(0)=-1.0_rkind
!-----------------------------------------------------------------------
! Vstretching = 4 : A. Shchepetkin (UCLA-ROMS, 2010) double vertical
!                   stretching function
!-----------------------------------------------------------------------
       ELSE IF (VSTRETCHING.eq.4) THEN

       WRITE(*,*) 'STRECHING TYPE 4'

       ds=1.0_rkind/FLOAT(KBm1)
! W-Point layer Interface
       sc_w(KBm1)=0.0_rkind
       Cs_w(KBm1)=0.0_rkind

       DO k=KBm1-1,1,-1
        cff_w = ds*FLOAT(k-KBm1)
        sc_w(k)  = cff_w
        IF (rtheta_s.gt.0.0_rkind) THEN
           Csur=(1.0_rkind-COSH(rtheta_s*cff_w))/(COSH(rtheta_s)-1.0_rkind)
        ELSE
           Csur = (cff_w**2)*(-1.0_rkind)
        END IF
        IF (rtheta_b.gt.0.0_rkind) THEN
           Cbot=(EXP(rtheta_b*Csur)-1.0_rkind)/(1.0_rkind-EXP(-rtheta_b))
           Cs_w(k)=Cbot
        ELSE
           Cs_w(k)=Csur
        END IF
       ENDDO
       sc_w(0)=-1.0_rkind
       Cs_w(0)=-1.0_rkind

! ----------------------
       ELSE
       STOP " SIGMA_RUTGERS: Wrong value for VSTRETCHING, &
            & only 2,3 or 4 allowed"

       END IF

        WRITE(*,*)"SIGMA_RUTGERS: END"

      END SUBROUTINE SIGMA_RUTGERS

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

      SUBROUTINE SIGMA_RUTGERS_MAT(KB,sc_w,Cs_w,S_RUTGERS)
!
! JEROME (IRD Noumea, 5-Oct-2017)
!
! Support to use several schemes for generalized vertical
! terrain-following coordinates as implemented in ROMS RUTGERS/UCLA
! ------------------------------------------------------------------

      IMPLICIT NONE

      integer,parameter :: rkind = 8
      INTEGER :: I,k,kk
      integer status, ierr

!  Local variable declarations.
      real(rkind) :: hinv, hwater, z_r0, z_w0
      real(rkind) :: cff_r, cff1_r, cff2_r, cff_w, cff1_w, cff2_w
      real(rkind) :: C2_w, hh2, vert_n1, vert_a, vert_h0, vert_s0
      real(rkind) :: hc
!      real(rkind),allocatable :: sc_w(:), Cs_w(:), z_w(:,:)
      real(rkind),allocatable :: H(:),z_w(:,:)
      integer,intent(in) :: KB
      real(rkind),intent(in) :: sc_w(:),Cs_w(:)
      real(rkind),intent(inout) :: S_RUTGERS(:,:)
      real(rkind) :: Zt_avg1
      integer :: KBM1

      WRITE(*,*)"SIGMA_RUTGERS_MAT: START"

! ------------------------------------------------------------------
! Section 2 : Compute Vertical Height as in ROMS RUTGERS/UCLA
!-----------------------------------------------------------------------
!  New formulation: Compute vertical depths (meters, negative) at
!                   RHO- and W-points, and vertical grid thicknesses.
!  Various stretching functions are possible, as defined above.
!
!         z_w(x,y,s,t) = zeta(x,y,t) + [zeta(x,y,t)+ h(x,y)] * Zo_w
!
!         Zo_w = [hc * s(k) + C(k) * h(x,y)] / [hc + h(x,y)]
!
!         but with zeta = 0
!-----------------------------------------------------------------------
      KBM1 = KB-1
      hc = TCLINE

      if(allocated(z_w)) deallocate(z_w)
      allocate(z_w(1:NP,0:KB), stat=ierr)
      if(allocated(H)) deallocate(H)
      allocate(H(1:NP))
      z_w = 0.0_rkind
      Zt_avg1 = 0.0_rkind
      H = 0.0_rkind

       DO I=1,NP
         H(I) = min(DP(I),hsm(1)) !*(-1.0_rkind)
!        z_w(I,0) = -H(I)
         z_w(I,0) = -1.0_rkind
       END DO

       DO k=1,KBm1

         cff_w  = hc*sc_w(k)
         cff1_w = Cs_w(k)
         !write(*,*) cff_w,cff1_w

         DO I=1,NP
           hwater=H(I)
           hinv=1.0_rkind/(hc+hwater)
           cff2_w=(cff_w+cff1_w*hwater)*hinv
           z_w(I,k)=cff2_w
         END DO
        END DO
!DBG
!        DO I=1,150
!           write(*,*) 'HWATER=',H(I)
!           write(*,*) (z_w(I,k),k=0,KBm1)
!        ENDDO
! ------------------------------------------------------------------
! Section 3 : WRAPPER : ROMS vert. coord. to SCHISM vert. coord.
!-----------------------------------------------------------------------
        DO I=1,NP
        DO K=1,KB
           KK=KB-K+1
            S_RUTGERS(K,I) = z_w(I,KK)
           ! write(*,*) 'Z_RUTGERS ',Z_RUTGERS(K,I)
        END DO
        END DO

        !if(allocated(sc_w)) deallocate(sc_w)
        !if(allocated(Cs_w)) deallocate(Cs_w)
        if(allocated(z_w)) deallocate(z_w)
        if(allocated(H)) deallocate(H)

        WRITE(*,*)"SIGMA_RUTGERS_MAT: END"

      END SUBROUTINE SIGMA_RUTGERS_MAT

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

      SUBROUTINE SIGMA_RUTGERS_VEC(H,KB,sc_w,Cs_w,V_RUTGERS)
!
! JEROME (IRD Noumea, 5-Oct-2017)
!
! Support to use several schemes for generalized vertical
! terrain-following coordinates as implemented in ROMS RUTGERS/UCLA
! ----------------------------------------------------------------
      IMPLICIT NONE

      integer,parameter :: rkind = 8
      INTEGER :: I,k,kk
      integer status, ierr

!  Local variable declarations.
      real(rkind) :: hinv, hwater, z_r0, z_w0
      real(rkind) :: C2_w, hh2, vert_n1, vert_a, vert_h0, vert_s0
      real(rkind) :: cff_r, cff1_r, cff2_r, cff_w, cff1_w, cff2_w
      real(rkind) :: hc
!      real(rkind),allocatable :: sc_w(:), Cs_w(:), z_w(:,:)
      real(rkind),allocatable :: z_w(:)
      integer,intent(in) :: KB
      real(rkind),intent(in) :: H
      real(rkind),intent(in) :: sc_w(:),Cs_w(:)
      real(rkind),intent(inout) :: V_RUTGERS(:)
      real(rkind) :: Zt_avg1
      integer :: KBM1

      WRITE(*,*)"SIGMA_RUTGERS_VEC: START"

! ------------------------------------------------------------------
! Section 2 : Compute Vertical Height as in ROMS RUTGERS/UCLA
!-----------------------------------------------------------------------
!  New formulation: Compute vertical depths (meters, negative) at
!                   RHO- and W-points, and vertical grid thicknesses.
!  Various stretching functions are possible, as defined above.
!
!         z_w(x,y,s,t) = zeta(x,y,t) + [zeta(x,y,t)+ h(x,y)] * Zo_w
!
!         Zo_w = [hc * s(k) + C(k) * h(x,y)] / [hc + h(x,y)]
!
!         but with zeta = 0
!-----------------------------------------------------------------------
      KBM1 = KB-1
      hc = TCLINE

      if(allocated(z_w)) deallocate(z_w)
      allocate(z_w(0:KB), stat=ierr)
!      if(allocated(H)) deallocate(H)
!      allocate(H(1:NP))
      z_w = 0.0_rkind
      Zt_avg1 = 0.0_rkind
!      H = 0.0_rkind

!       DO I=1,NP
!         H(I) = min(DP(I),hsm(1)) !*(-1.0_rkind)
!        z_w(I,0) = -H(I)
         z_w(0) = -1.0_rkind
!       END DO

       DO k=1,KBm1

         cff_w  = hc*sc_w(k)
         cff1_w = Cs_w(k)
         !write(*,*) cff_w,cff1_w

         hwater=H
         hinv=1.0_rkind/(hc+hwater)
         cff2_w=(cff_w+cff1_w*hwater)*hinv
         z_w(k)=cff2_w
       END DO
!DBG
!        write(*,*) 'HWATER=',H
!        write(*,*) (z_w(k),k=0,KBm1)
! ------------------------------------------------------------------
! Section 3 : WRAPPER : ROMS vert. coord. to SCHISM vert. coord.
!-----------------------------------------------------------------------
!        DO I=1,NP
        DO K=1,KB
           KK=KB-K+1
            V_RUTGERS(K) = z_w(KK)
            write(*,*) 'V_RUTGERS(K)',V_RUTGERS(K)
           ! write(*,*) 'Z_RUTGERS ',Z_RUTGERS(K,I)
        END DO
!        END DO

        !if(allocated(sc_w)) deallocate(sc_w)
        !if(allocated(Cs_w)) deallocate(Cs_w)
        if(allocated(z_w)) deallocate(z_w)

        !WRITE(*,*)"SIGMA_RUTGERS_VEC: END"

      END SUBROUTINE SIGMA_RUTGERS_VEC

      end


C --- Driver for computing sensitivity coefficients w.r.t. all initial values
      PROGRAM driver

      INCLUDE 'KPP_ROOT_Parameters.h'
      INCLUDE 'KPP_ROOT_Global.h'

C ---  Number of sensitivity coefficients to compute
C ---  Note: this value is set for sensitivities w.r.t. all initial values
C ---       it may have to be changed for other applications
      INTEGER NSENSIT
      PARAMETER (NSENSIT = NVAR)

      KPP_REAL  DVAL(NSPEC)
      KPP_REAL  Y(NVAR*(NSENSIT+1))
C ---  Note: Y contains: (1:NVAR) concentrations, followed by
C ---                   (1:NVAR) sensitivities w.r.t. first parameter, followed by
C ---                   etc.,  followed by
C ---                   (1:NVAR) sensitivities w.r.t. NSENSIT's parameter

      INTEGER i

C --- The type of sensitivity coefficients to compute
C --- DDMTYPE = 0 : sensitivities w.r.t. initial values
C --- DDMTYPE = 1 : sensitivities w.r.t. parameters
      DDMTYPE = 0

C ---- TIME VARIABLES ------------------

      TSTART = 0
      TEND = TSTART + 600
      DT = 60.
      TEMP = 298

      STEPMIN = 0.01
      STEPMAX = 900

      PRINT*,'Please provide: RTOL = , ATOL = '
      READ*,RTOLS, ATOLS
      do i=1,NVAR
        RTOL(i) = RTOLS
        ATOL(i) = ATOLS
      end do

C ********** TIME LOOP *************************

      CALL Initialize()

C -- Initialize Concentrations and Sensitivities
      DO i=1,NVAR
         Y(i) = VAR(i)
      END DO

C ---  Note: the initial values below are for sensitivities w.r.t. initial values;
C ---       they may have to be changed for other applications
      DO j=1,NSENSIT
        DO i=1,NVAR
	   Y(i+NVAR*j) = 0.0D0
	END DO
	Y(j+NVAR*j) = 1.0D0
      END DO

      CALL InitSaveData()

      WRITE(6,990) (SPC_NAMES(MONITOR(i)), i=1,NMONITOR),
     *             (SMASS(i), i=1,NMASS )
990   FORMAT('done[%] Time[h] ',20(4X,A6))

      TIME = TSTART
      DO WHILE (TIME .lt. TEND)

        CALL GetMass( C, DVAL )
        WRITE(6,991) (TIME-TSTART)/(TEND-TSTART)*100, TIME/3600.,
     *               (C(MONITOR(i))/CFACTOR, i=1,NMONITOR),
     *               (DVAL(i)/CFACTOR, i=1,NMASS)
991   FORMAT(F6.1,'% ',F7.2,3X,20(E10.4,2X))

        CALL SaveData()

        CALL Update_SUN()
        CALL Update_RCONST()

        CALL INTEGRATE( NSENSIT, Y, TIME, TIME+DT )

        DO i=1,NVAR
           VAR(i) = Y(i)
        END DO

      END DO

      CALL GetMass( C, DVAL )
      WRITE(6,991) (TIME-TSTART)/(TEND-TSTART)*100, TIME/3600.,
     *               (C(MONITOR(i))/CFACTOR, i=1,NMONITOR),
     *               (DVAL(i)/CFACTOR, i=1,NMASS)

C      DO i=1,NSENSIT
C        WRITE(6,992) i, ( Y(NVAR*i+j), j=1,NVAR )
C      END DO
C 992  FORMAT('SEN(',I3,') = ',1000(E10.4,2X))

      CALL SaveData()

C *********** END TIME LOOP ********
      OPEN(20, FILE='KPP_ROOT_results.m')
      WRITE(6,*) '**************************************************'
      WRITE(6,*) '* Concentrations and Sensitivities at final time *'
      WRITE(6,*) '*  were written in the file KPP_ROOT_results.m  *'
      WRITE(6,*) '**************************************************'
      DO i=0,NSENSIT
        WRITE(20,993) ( Y(NVAR*i+j), j=1,NVAR )
      END DO
 993  FORMAT(1000(E24.16,2X))

      CALL CloseSaveData()

      STOP
      END


program JEDI

use constants
use chem

	! **** Variables declarations *** !
	implicit none
	integer :: i,j,k
	real(dp) :: ab,c,d
		! INPUT
	character(len=100) :: file_eq, file_def, file_red, file_hes
	integer :: natoms, natoms_def
	character(len=2), allocatable :: atom(:), atom_def(:)
	real(dp), allocatable :: cart_eq(:,:), cart_def(:,:)
	integer, dimension(:,:), allocatable :: bonds, angles, dihedrals
	integer :: ncurv(3), ntot, nb, na, nd
	real(dp), dimension(:,:), allocatable :: hessian
		! STRAIN ANALYSIS
	real(dp), allocatable :: strain(:), disp(:), strain_total
	real(dp) :: f1, f2

	! **** Getting command line parameters *** !
	! Checking all parameters were entered correctly
    if(COMMAND_ARGUMENT_COUNT() .LT. 4) then
        write(*,*)
        write(*,*) 'SYNTAX: $ jedi XYZ_eq_geom XYZ_def_geom DAT_redundant DAT_hessian'
        write(*,*) '*** Example of molecule file ***'
        write(*,*) '3'
        write(*,*)
        write(*,*) 'H  0.0000000  0.7948110   -0.4512250'
        write(*,*) 'O  0.0000000  0.0000000   0.1128060'
        write(*,*) 'H  0.0000000  -0.7948110  -0.4512250'
        write(*,*)

        write(*,*) '*** Example of redundant internal coords***'
        write(*,*) 'B p'
        write(*,*) 'p1 p2'
        write(*,*) 'A q'
        write(*,*) 'q1 q2 q3'
        write(*,*) 'D u'
        write(*,*) 'u1 u2 u3 u4'
        write(*,*)
        STOP
    endif
    ! Getting command line parameters
    call GET_COMMAND_ARGUMENT(1,file_eq)
    call GET_COMMAND_ARGUMENT(2,file_def)
    call GET_COMMAND_ARGUMENT(3,file_red)
    call GET_COMMAND_ARGUMENT(4,file_hes)

!-------------------------------------o-----------------------------------!
!                        *** TAKING INPUT DATA ***                        !
!-------------------------------------o-----------------------------------!
	! Reading equilibrium geometry (i)
	call readXYZ(file_eq, natoms, atom, cart_eq)
	! Reading deformed geometry (i+1)
	call readXYZ(file_def, natoms_def, atom_def, cart_def)
	if(natoms .ne. natoms_def) then
		write(*,*) 'Not the same number of atoms in file ' // file_eq // 'and file ' // file_def
	endif
	! Reading redundant internal curvilinear coordinates
	call readINT(file_red, ncurv, bonds, angles, dihedrals)
	! Reading the hessian in curvilinear coordinates
	ntot = ncurv(1) + ncurv(2) + ncurv(3)
	allocate(hessian(ntot,ntot))

	open(20,file=file_hes)
	read(20, *) hessian

!-------------------------------------o-----------------------------------!
!                        *** COMPUTING STRESS ***                        !
!-------------------------------------o-----------------------------------!
	! Converting Hessian to eV/Ang
	f1 = hartree_to_eV/(bohr**2)
	f2 = hartree_to_eV/(bohr)
	do i=1,ntot
		do j=1,ntot
			if( (i .le. ncurv(1)) .and. (j .le. ncurv(1)) ) then
				hessian(i,j) = f1*hessian(i,j)
			elseif( (i .le. ncurv(1)) .or. (j .le. ncurv(1)) ) then
				hessian(i,j) = f2*hessian(i,j)
			else
				hessian(i,j) = hartree_to_eV*hessian(i,j)
			endif
		enddo
	enddo

	! Computing displacements
	nb = ncurv(1)
	na = ncurv(2)
	nd = ncurv(3)
	allocate(disp(ntot))
	do i=1,ntot
		if(i .le. nb) then
			disp(i) = compute_length( cart_def(bonds(i,1),:), cart_def(bonds(i,2),:))- &
				compute_length( cart_eq(bonds(i,1),:), cart_eq(bonds(i,2),:) )
		elseif( i .le. (nb+na) ) then
			disp(i) = compute_angle( cart_def(angles(i-nb,1),:), cart_def(angles(i-nb,2),:), &
				cart_def(angles(i-nb,3),:) ) - compute_angle( cart_eq(angles(i-nb,1),:), &
					cart_eq(angles(i-nb,2),:), cart_eq(angles(i-nb,3),:) )
		else
			disp(i) = ABS( compute_dihedral( cart_def(dihedrals(i-nb-na,1),:), &
				cart_def(dihedrals(i-nb-na,2),:), cart_def(dihedrals(i-nb-na,3),:), &
				cart_def(dihedrals(i-na-nb,4),:) )) - ABS( compute_dihedral( &
				cart_eq(dihedrals(i-nb-na,1),:), cart_eq(dihedrals(i-nb-na,2),:), &
				cart_eq(dihedrals(i-nb-na,3),:), cart_eq(dihedrals(i-nb-na,4),:) ))
		endif
	enddo
	disp = ABS(disp)

	! Computing strain
	allocate(strain(ntot))
	strain = 0
	strain_total = 0
	do i=1,ntot
		do j=1,ntot
			strain(i) = strain(i) + hessian(i,j)*disp(i)*disp(j)
		enddo
		strain(i) = 0.5_dp*strain(i)
		strain_total = strain_total + strain(i)
	enddo

	strain = ABS(strain)
	strain_total = ABS(strain_total)
!-------------------------------------o-----------------------------------!
!                        *** PRINTING RESULTS ***                        !
!-------------------------------------o-----------------------------------!
	open(100,file="strain.dat")

	do i=1,ntot
		if(i .le. nb) then
			if(i .eq. 1) then
				write(100, '("B" 2x,i0)') nb
			endif

			write(100,'(i3,i3,f15.7,f15.7,a2)') bonds(i,1), bonds(i,2), strain(i), &
						100*strain(i)/strain_total, ' %'
		elseif(i .le. (nb+na)) then
			if(i .eq. nb+1) then
				write(100, '("A" 2x,i0)') na
			endif

			write(100,'(i3,i3,i3,f15.7,f15.7,a2)') angles(i-nb,1), angles(i-nb,2), &
				angles(i-nb,3), strain(i), 100*strain(i)/strain_total, ' %'
		else
			if(i .eq. nb+na+1) then
				write(100, '("D" 2x,i0)') nd
			endif

			write(100,'(i3,i3,i3,i3,f15.7,f15.7,a2)') dihedrals(i-nb-na,1), &
				dihedrals(i-nb-na,2), dihedrals(i-nb-na,3), dihedrals(i-nb-na,4), &
				strain(i), 100*strain(i)/strain_total, ' %'
		endif
	enddo
	write(100,*)
	write(100,'("Harmonic Strain: "f15.7 " eV")') strain_total

contains

	subroutine readXYZ(filename, natoms, atom, geom)
		implicit none
		real(dp), allocatable :: geom(:,:)
		character(len=2), allocatable :: atom(:)
		character(len=100) :: filename
		integer :: natoms, i

		open(999,file=filename)
		read(999,*) natoms

		allocate(atom(natoms), geom(natoms,3))

		do i=1,natoms
			read(999, *) atom(i), geom(i,:)
		enddo
	end subroutine

	subroutine readINT(filename, ncoord, bonds, angles, dihedrals)
		implicit none
		integer, allocatable :: bonds(:,:), angles(:,:), dihedrals(:,:)
		integer :: ncoord(3), i
		character(len=100) :: filename
		character :: typeCurv

		open(999, file=filename)

		! Reading bonds
		read(999, *) typeCurv, ncoord(1)
		allocate(bonds(ncoord(1), 2))
		do i=1,ncoord(1)
			read(999,*) bonds(i,1), bonds(i,2)
		enddo

		! Reading angles
		if(ncoord(1) .gt. 1) then
			read(999, *) typeCurv, ncoord(2)
			allocate(angles(ncoord(2), 3))
			do i=1,ncoord(2)
				read(999,*) angles(i,1), angles(i,2), angles(i,3)
			enddo
		endif

		! Reading dihedrals
		if(ncoord(2) .gt. 1) then
			read(999, *) typeCurv, ncoord(3)
			allocate(dihedrals(ncoord(3), 4))
			do i=1,ncoord(3)
				read(999,*) dihedrals(i,1), dihedrals(i,2), dihedrals(i,3), dihedrals(i,4)
			enddo
		endif
	end subroutine

end program JEDI


*> \brief \b DTPT01
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*  Definition:
*  ===========
*
*       SUBROUTINE DTPT01( UPLO, DIAG, N, AP, AINVP, RCOND, WORK, RESID )
*
*       .. Scalar Arguments ..
*       CHARACTER          DIAG, UPLO
*       INTEGER            N
*       DOUBLE PRECISION   RCOND, RESID
*       ..
*       .. Array Arguments ..
*       DOUBLE PRECISION   AINVP( * ), AP( * ), WORK( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> DTPT01 computes the residual for a triangular matrix A times its
*> inverse when A is stored in packed format:
*>    RESID = norm(A*AINV - I) / ( N * norm(A) * norm(AINV) * EPS ),
*> where EPS is the machine epsilon.
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] UPLO
*> \verbatim
*>          UPLO is CHARACTER*1
*>          Specifies whether the matrix A is upper or lower triangular.
*>          = 'U':  Upper triangular
*>          = 'L':  Lower triangular
*> \endverbatim
*>
*> \param[in] DIAG
*> \verbatim
*>          DIAG is CHARACTER*1
*>          Specifies whether or not the matrix A is unit triangular.
*>          = 'N':  Non-unit triangular
*>          = 'U':  Unit triangular
*> \endverbatim
*>
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The order of the matrix A.  N >= 0.
*> \endverbatim
*>
*> \param[in] AP
*> \verbatim
*>          AP is DOUBLE PRECISION array, dimension (N*(N+1)/2)
*>          The original upper or lower triangular matrix A, packed
*>          columnwise in a linear array.  The j-th column of A is stored
*>          in the array AP as follows:
*>          if UPLO = 'U', AP((j-1)*j/2 + i) = A(i,j) for 1<=i<=j;
*>          if UPLO = 'L',
*>             AP((j-1)*(n-j) + j*(j+1)/2 + i-j) = A(i,j) for j<=i<=n.
*> \endverbatim
*>
*> \param[in,out] AINVP
*> \verbatim
*>          AINVP is DOUBLE PRECISION array, dimension (N*(N+1)/2)
*>          On entry, the (triangular) inverse of the matrix A, packed
*>          columnwise in a linear array as in AP.
*>          On exit, the contents of AINVP are destroyed.
*> \endverbatim
*>
*> \param[out] RCOND
*> \verbatim
*>          RCOND is DOUBLE PRECISION
*>          The reciprocal condition number of A, computed as
*>          1/(norm(A) * norm(AINV)).
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is DOUBLE PRECISION array, dimension (N)
*> \endverbatim
*>
*> \param[out] RESID
*> \verbatim
*>          RESID is DOUBLE PRECISION
*>          norm(A*AINV - I) / ( N * norm(A) * norm(AINV) * EPS )
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \date December 2016
*
*> \ingroup double_lin
*
*  =====================================================================
      SUBROUTINE DTPT01( UPLO, DIAG, N, AP, AINVP, RCOND, WORK, RESID )
*
*  -- LAPACK test routine (version 3.7.0) --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*     December 2016
*
*     .. Scalar Arguments ..
      CHARACTER          DIAG, UPLO
      INTEGER            N
      DOUBLE PRECISION   RCOND, RESID
*     ..
*     .. Array Arguments ..
      DOUBLE PRECISION   AINVP( * ), AP( * ), WORK( * )
*     ..
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            UNITD
      INTEGER            J, JC
      DOUBLE PRECISION   AINVNM, ANORM, EPS
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      DOUBLE PRECISION   DLAMCH, DLANTP
      EXTERNAL           LSAME, DLAMCH, DLANTP
*     ..
*     .. External Subroutines ..
      EXTERNAL           DTPMV
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          DBLE
*     ..
*     .. Executable Statements ..
*
*     Quick exit if N = 0.
*
      IF( N.LE.0 ) THEN
         RCOND = ONE
         RESID = ZERO
         RETURN
      END IF
*
*     Exit with RESID = 1/EPS if ANORM = 0 or AINVNM = 0.
*
      EPS = DLAMCH( 'Epsilon' )
      ANORM = DLANTP( '1', UPLO, DIAG, N, AP, WORK )
      AINVNM = DLANTP( '1', UPLO, DIAG, N, AINVP, WORK )
      IF( ANORM.LE.ZERO .OR. AINVNM.LE.ZERO ) THEN
         RCOND = ZERO
         RESID = ONE / EPS
         RETURN
      END IF
      RCOND = ( ONE / ANORM ) / AINVNM
*
*     Compute A * AINV, overwriting AINV.
*
      UNITD = LSAME( DIAG, 'U' )
      IF( LSAME( UPLO, 'U' ) ) THEN
         JC = 1
         DO 10 J = 1, N
            IF( UNITD )
     $         AINVP( JC+J-1 ) = ONE
*
*           Form the j-th column of A*AINV
*
            CALL DTPMV( 'Upper', 'No transpose', DIAG, J, AP,
     $                  AINVP( JC ), 1 )
*
*           Subtract 1 from the diagonal
*
            AINVP( JC+J-1 ) = AINVP( JC+J-1 ) - ONE
            JC = JC + J
   10    CONTINUE
      ELSE
         JC = 1
         DO 20 J = 1, N
            IF( UNITD )
     $         AINVP( JC ) = ONE
*
*           Form the j-th column of A*AINV
*
            CALL DTPMV( 'Lower', 'No transpose', DIAG, N-J+1, AP( JC ),
     $                  AINVP( JC ), 1 )
*
*           Subtract 1 from the diagonal
*
            AINVP( JC ) = AINVP( JC ) - ONE
            JC = JC + N - J + 1
   20    CONTINUE
      END IF
*
*     Compute norm(A*AINV - I) / (N * norm(A) * norm(AINV) * EPS)
*
      RESID = DLANTP( '1', UPLO, 'Non-unit', N, AINVP, WORK )
*
      RESID = ( ( RESID*RCOND ) / DBLE( N ) ) / EPS
*
      RETURN
*
*     End of DTPT01
*
      END


c ilut is part of the SPARSKIT library by Y. Saad at the University of Minnesota
c  For the full SPARSKIT library visit:
c                           www.cs.umn.edu/~saad/
c
c-----------------------------------------------------------------------
       subroutine ilut (n,a,ja,ia,lfil,tol,alu,jlu,ju,iwk,
     *                  wu,wl,jr,jwl,jwu,ierr)
c-----------------------------------------------------------------------
       implicit real*8 (a-h,o-z)
       real*8 a(*), alu(*), wu(n), wl(n), tol
       integer ja(*),ia(n+1),jlu(*),ju(n),jr(n), jwu(n),
     *      jwl(n), n, lfil, iwk, ierr
c----------------------------------------------------------------------*
c                      *** ILUT preconditioner ***                     *
c                      ---------------------------                     *
c      incomplete LU factorization with dual truncation mechanism      *
c      VERSION 2 : sorting  done for both L and U.                     *
c                                                                      *
c----------------------------------------------------------------------*
c---- coded by Youcef Saad May, 5, 1990. ------------------------------*
c---- Dual drop-off strategy works as follows.                         *
c                                                                      *
c     1) Theresholding in L and U as set by tol. Any element whose size*
c        is less than some tolerance (relative to the norm of current  *
c        row in u) is dropped.                                         *
c                                                                      *
c     2) Keeping only the largest lfil+il(i) elements in the i-th row  *
c        of L and the largest lfil+iu(i) elements in the i-th row of   *
c        U where il(i), iu(i) are the original number of nonzero       *
c        elements of the L-part and the U-part of the i-th row of A    *
c                                                                      *
c Flexibility: one can use tol=0 to get a strategy based on keeping the*
c largest elements in each row of L and U. Taking tol .ne. 0 but lfil=n*
c will give the usual threshold strategy (however, fill-in is then     *
c impredictible).                                                      *
c                                                                      *
c----------------------------------------------------------------------*
c PARAMETERS
c-----------
c
c on entry:
c==========
c n       = integer. The dimension of the matrix A.
c
c a,ja,ia = matrix stored in Compressed Sparse Row format.
c
c lfil    = integer. The fill-in parameter. Each row of L and
c           each row of U will have a maximum of lfil elements
c           in addition to their original number of nonzero elements.
c           Thus storage can be determined beforehand.
c           lfil must be .ge. 0.
c
c iwk     = integer. The minimum length of arrays alu and jlu
c
c On return:
c===========
c
c alu,jlu = matrix stored in Modified Sparse Row (MSR) format containing
c           the L and U factors together. The diagonal (stored in
c           alu(1:n) ) is inverted. Each i-th row of the alu,jlu matrix
c           contains the i-th row of L (excluding the diagonal entry=1)
c           followed by the i-th row of U.
c
c ju      = integer array of length n containing the pointers to
c           the beginning of each row of U in the matrix alu,jlu.
c
c ierr    = integer. Error message with the following meaning.
c           ierr  = 0    --> successful return.
c           ierr .gt. 0  --> zero pivot encountered at step number ierr.
c           ierr  = -1   --> Error. input matrix may be wrong.
c                            (The elimination process has generated a
c                            row in L or U whose length is .gt.  n.)
c           ierr  = -2   --> The matrix L overflows the array al.
c           ierr  = -3   --> The matrix U overflows the array alu.
c           ierr  = -4   --> Illegal value for lfil.
c           ierr  = -5   --> zero row encountered.
c
c work arrays:
c=============
c jr,jwu,jwl      = integer work arrays of length n.
c wu, wl          = real work arrays of length n+1, and n resp.
c
c Notes:
c ------
c A must have all nonzero diagonal elements.
c-----------------------------------------------------------------------
        if (lfil .lt. 0) goto 998
c-------------------------------
c initialize ju0 (points to next element to be added to alu,jlu)
c and pointer.
c
        ju0 = n+2
        jlu(1) = ju0
c
c  integer double pointer array.
c
        do 1 j=1, n
                jr(j)  = 0
 1           continue
c-----------------------------------------------------------------------
c  beginning of main loop.
c-----------------------------------------------------------------------
        do 500 ii = 1, n
           j1 = ia(ii)
           j2 = ia(ii+1) - 1
           tnorm = 0.0d0
           do 501 k=j1,j2
              tnorm = tnorm+abs(a(k))
 501          continue
              if (tnorm .eq. 0.0) goto 999
              tnorm = tnorm/real(j2-j1+1)
c
c--- unpack L-part and U-part of row of A in arrays wl, wu --
c
       lenu = 1
       lenl = 0
       jwu(1) = ii
       wu(1) = 0.0
       jr(ii) = 1
c
       do 170  j = j1, j2
           k = ja(j)
           t = a(j)
           if (abs(t) .lt. tol*tnorm .and. k .ne. ii) goto 170
           if (k .lt. ii) then
              lenl = lenl+1
              jwl(lenl) = k
              wl(lenl) = t
              jr(k) = lenl
           else if (k .eq. ii) then
              wu(1) = t
           else
              lenu = lenu+1
              jwu(lenu) = k
              wu(lenu) = t
              jr(k) = lenu
           endif
 170    continue
        tnorm = tnorm/real(j2-j1+1)
        lenl0 = lenl
        lenu0 = lenu
        jj = 0
        nl = 0
c-------------------------------------------------------------------
c---------------------- eliminate previous rows --------------------
c-------------------------------------------------------------------
 150    jj = jj+1
        if (jj .gt. lenl) goto 160
c-------------------------------------------------------------------
c in order to do the elimination in the correct order we need to
c exchange the current row number with the one that has
c smallest column number, among jj,jj+1,...,lenl.
c-------------------------------------------------------------------
        jrow = jwl(jj)
        k = jj
c
c determine smallest column index
c
        do 151 j=jj+1,lenl
           if (jwl(j) .lt. jrow) then
              jrow = jwl(j)
              k = j
           endif
 151    continue
c
c exchange in jwl
c
       if (k .ne. jj) then
           j = jwl(jj)
           jwl(jj) = jwl(k)
           jwl(k) = j
c
c exchange in jr
c
           jr(jrow) = jj
           jr(j) = k
c
c exchange in wl
c
           s = wl(jj)
           wl(jj) = wl(k)
           wl(k) = s
        endif
c
        if (jrow .ge. ii) goto 160
c---------get the multiplier for row to be eliminated: jrow
        fact = wl(jj)*alu(jrow)
c zero out element in row by setting jr(jrow) = 0
        jr(jrow) = 0
        if (abs(fact)*wu(n+2-jrow) .le. tol*tnorm) goto 150
c-------------------------------------------------------------------
c------------ combine current row and row jrow ---------------------
c-------------------------------------------------------------------
        do 203 k = ju(jrow), jlu(jrow+1)-1
           s = fact*alu(k)
           j = jlu(k)
           jpos = jr(j)
c
c if fill-in element is small then disregard:
c
           if (abs(s) .lt. tol*tnorm .and. jpos .eq. 0) goto 203
           if (j .ge. ii) then
c
c     dealing with upper part.
c
              if (jpos .eq. 0) then
c     this is a fill-in element
                 lenu = lenu+1
                 if (lenu .gt. n) goto 995
                 jwu(lenu) = j
                 jr(j) = lenu
                 wu(lenu) = - s
              else
c     no fill-in element --
                 wu(jpos) = wu(jpos) - s
              endif
           else
c
c     dealing with lower part.
c
              if (jpos .eq. 0) then
c     this is a fill-in element
                 lenl = lenl+1
                 if (lenl .gt. n) goto 995
                 jwl(lenl) = j
                 jr(j) = lenl
                 wl(lenl) = - s
              else
c     no fill-in element --
                 wl(jpos) = wl(jpos) - s
              endif
           endif
 203    continue
        nl = nl+1
        wl(nl) = fact
        jwl(nl)  = jrow
        goto 150
c----------------------------------------------------------
c------------ update l-matrix -----------------------------
c----------------------------------------------------------
 160    len = min0(nl,lenl0+lfil)
c 160    len = min0(nl,lfil)

        call qsplit (wl,jwl,nl,len)
c
        do 204 k=1, len
           if (ju0 .gt. iwk) goto 996
           alu(ju0) =  wl(k)
           jlu(ju0) =  jwl(k)
           ju0 = ju0+1
 204    continue
c
c  save pointer to beginning of row ii of U
c
        ju(ii) = ju0
c
c  reset double-pointer jr to zero (L-part - except first
c  jj-1 elements which have already been reset)
        do 306 k= jj, lenl
              jr(jwl(k)) = 0
 306    continue
        len = min0(lenu,lenu0+lfil)
c        len = min0(lenu,lfil)
        call qsplit (wu(2), jwu(2), lenu-1,len)
c----------------------------------------------------------
c------------ update u-matrix -----------------------------
c----------------------------------------------------------
        t = 0.0d0
        if (len + ju0 .gt. iwk) goto 997
        do 302 k=2, len
           jlu(ju0) = jwu(k)
           alu(ju0) = wu(k)
           t = t + abs(wu(k) )
           ju0 = ju0+1
 302    continue
c
c     save norm in wu (backwards). Norm is in fact average abs value
c
        wu(n+2-ii) = t / real(len+1)
c
c     store inverse of diagonal element of u
c
        if (wu(1) .eq. 0.0) wu(1) = (0.0001 + tol)*tnorm
c
        alu(ii) = 1.0d0/ wu(1)
c
c     update pointer to beginning of next row of U.
c
        jlu(ii+1) = ju0
c
c     reset double-pointer jr to zero (U-part)
c
        do 308 k=1, lenu
           jr(jwu(k)) = 0
 308    continue
c-----------------------------------------------------------------------
c     end main loop
c-----------------------------------------------------------------------
 500  continue
        ierr = 0
        return
c
c     zero pivot :
c
c 900    ierr = ii
c        return
c
c     incomprehensible error. Matrix must be wrong.
c
 995    ierr = -1
        return
c
c     insufficient storage in L.
c
 996    ierr = -2
        return
c
c     insufficient storage in U.
c
 997    ierr = -3
        return
c
c     illegal lfil entered.
c
 998    ierr = -4
        return
c
c     zero row encountered
c
 999    ierr = -5
        return
c---------------- end of ilut  -----------------------------------------
        end
c---------------- routine lusol0----------------------------------------
        subroutine lusol0 (n, y, x, alu, jlu, ju)
        real*8 x(n), y(n), alu(*)
        integer n, jlu(*), ju(*)
c-----------------------------------------------------------------------
c
c performs a forward followed by a backward solve
c for LU matrix as produced by  ILUT
c
c-----------------------------------------------------------------------
c local variables
c
        integer i,k
c
c forward solve
c
        do 40 i = 1, n
           x(i) = y(i)
           do 41 k=jlu(i),ju(i)-1
              x(i) = x(i) - alu(k)* x(jlu(k))
 41        continue
 40     continue
c
c     backward solve.
c
        do 90 i = n, 1, -1
           do 91 k=ju(i),jlu(i+1)-1
              x(i) = x(i) - alu(k)*x(jlu(k))
 91        continue
           x(i) = alu(i)*x(i)
 90     continue
c
        return
c----------------end of lusol0 -----------------------------------------
c-----------------------------------------------------------------------
        end
c-----------------------------------------------------------------------
        subroutine qsplit  (a, ind, n, ncut)
        real*8 a(n)
        integer ind(n), n, ncut
c-----------------------------------------------------------------------
c     does a quick-sort split of a real array.
c     on input a(1:n). is a real array
c     on output a(1:n) is permuted such that its elements satisfy:
c     a(i) .le. a(ncut) for i .le. ncut and
c     a(i) .ge. a(ncut) for i .ge. ncut
c     ind(1:n) is an integer array which permuted in the same way as a(*).
c-----------------------------------------------------------------------
        real*8 tmp, abskey
        integer itmp, first, last
c-----
        first = 1
        last = n
        if (ncut .lt. first .or. ncut .gt. last) return
c
c     outer loop -- while mid .ne. ncut do
c
 1      mid = first
        abskey = abs(a(mid))
        do 2 j=first+1, last
           if (abs(a(j)) .gt. abskey) then
              mid = mid+1
c     interchange
              tmp = a(mid)
              itmp = ind(mid)
              a(mid) = a(j)
              ind(mid) = ind(j)
              a(j)  = tmp
              ind(j) = itmp
           endif
 2      continue
c
c     interchange
c
        tmp = a(mid)
        a(mid) = a(first)
        a(first)  = tmp
c
        itmp = ind(mid)
        ind(mid) = ind(first)
        ind(first) = itmp
c
c     test for while loop
c
        if (mid .eq. ncut) return
        if (mid .gt. ncut) then
           last = mid-1
        else
           first = mid+1
        endif
        goto 1
c----------------end-of-qsplit------------------------------------------
c-----------------------------------------------------------------------
        end


module advance_diffusion_module

  use ml_layout_module
  use define_bc_module
  use bc_module
  use stochastic_n_fluxdiv_module
  use diffusive_n_fluxdiv_module
  use multifab_physbc_module
  use implicit_diffusion_module
  use multinomial_diffusion_module
  use probin_common_module, only: variance_coef_mass
  use probin_reactdiff_module, only: nspecies, D_Fick, diffusion_type, midpoint_stoch_flux_type

  implicit none

  private

  public :: advance_diffusion

contains

  ! Solves n_t = div ( D grad (n)) + div (sqrt(2*variance*D*n)*W) + g
  !  where g is a constant in time external source

  subroutine advance_diffusion(mla,n_old,n_new,dx,dt,the_bc_tower,ext_src)

    type(ml_layout), intent(in   ) :: mla
    type(multifab) , intent(in   ) :: n_old(:)
    type(multifab) , intent(inout) :: n_new(:)
    real(kind=dp_t), intent(in   ) :: dx(:,:),dt
    type(bc_tower) , intent(in   ) :: the_bc_tower
    type(multifab) , intent(in   ) :: ext_src(:) ! The value of g

    ! local
    type(multifab) :: diff_fluxdiv(mla%nlevel)
    type(multifab) :: stoch_fluxdiv(mla%nlevel)
    type(multifab) :: diff_coef_face(mla%nlevel,mla%dim)

    integer :: n,nlevs,i,dm,spec

    type(bl_prof_timer),save :: bpt

    nlevs = mla%nlevel
    dm = mla%dim

    ! do not do diffusion if only one cell (well-mixed system)
    ! there is no restriction on the number of cells
    ! but we can shortcut the single cell case anyway for simplicity
    if((multifab_volume(n_old(1))/nspecies)<=1) then
       do n=1,nlevs
          ! make sure n_new contains the new state
          call multifab_copy_c(n_new(n),1,n_old(n),1,nspecies,n_new(n)%ng)
          call multifab_saxpy_3(n_new(n),dt,ext_src(n))
       end do
    end if

    call build(bpt,"advance_diffusion")

    do n=1,nlevs
       do i=1,dm
          call multifab_build_edge(diff_coef_face(n,i),mla%la(n),nspecies,0,i)
       end do
    end do

    ! compute the diffusion coefficients (for now just setting each to a different constant)
    ! If one wants a space-dependent D or state-dependent D see multispecies code as example
    ! We have a routine average_cc_to_face there that is meant to compute face-averaged values
    do n=1,nlevs
       do i=1,dm
          do spec=1,nspecies
             call multifab_setval_c(diff_coef_face(n,i), D_Fick(spec),spec,1,all=.true.)
          end do
       end do
    end do

    ! multinomial diffusion
    if (diffusion_type .eq. 3) then

       call multinomial_diffusion(mla,n_old,n_new,diff_coef_face,dx,dt,the_bc_tower)

       do n=1,nlevs
          do i=1,dm
             call multifab_destroy(diff_coef_face(n,i))
          end do
       end do
       call destroy(bpt)
       return

    end if

    do n=1,nlevs
       call multifab_build(diff_fluxdiv(n) ,mla%la(n),nspecies,0)
       call multifab_build(stoch_fluxdiv(n),mla%la(n),nspecies,0)
    end do

    ! compute diffusive flux divergence
    call diffusive_n_fluxdiv(mla,n_old,diff_coef_face,diff_fluxdiv,dx,the_bc_tower)

    ! compute stochastic flux divergence
    if (variance_coef_mass .gt. 0.d0) then
       call fill_mass_stochastic(mla,the_bc_tower%bc_tower_array)
       call stochastic_n_fluxdiv(mla,n_old,diff_coef_face,stoch_fluxdiv,dx,dt, &
                                 the_bc_tower,increment_in=.false.)
    else
       do n=1,nlevs
          call multifab_setval(stoch_fluxdiv(n),0.d0,all=.true.)
       end do
    end if

    if (diffusion_type .eq. 0 .or. diffusion_type .eq. 4) then
       ! explicit trapezoidal predictor-corrector OR forward Euler

       ! forward Euler predictor
       ! n_k^{n+1,*} = n_k^n + dt div (D_k grad n_k)^n
       !                     + dt div (sqrt(2 D_k n_k / dt) Z)^n
       !                     + dt ext_src
       do n=1,nlevs
          call multifab_copy_c(n_new(n),1,n_old(n),1,nspecies,0)
          ! By default, saxpy updates zero ghost cells
          call multifab_saxpy_3(n_new(n),dt,diff_fluxdiv(n))
          call multifab_saxpy_3(n_new(n),dt,stoch_fluxdiv(n))
          call multifab_saxpy_3(n_new(n),dt,ext_src(n))
          call multifab_fill_boundary(n_new(n))
          call multifab_physbc(n_new(n),1,scal_bc_comp,nspecies, &
                               the_bc_tower%bc_tower_array(n),dx_in=dx(n,:))
       end do

       if (diffusion_type .eq. 0) then

          ! Trapezoidal corrector:
          ! n_k^{n+1} = n_k^n + (dt/2) div (D_k grad n_k)^n
          !                   + (dt/2) div (D_k grad n_k)^{n+1,*}
          !                   +  dt    div (sqrt(2 D_k n_k / dt) Z)^n
          !                   +  dt    ext_src
          ! This is the same as stepping to time t+2*dt and then averaging with the state at time t:
          !  n_new = 1/2 * (n_old + n_new + dt*div (D grad n_new) + div (sqrt(2 D_k n_k dt) Z)^n)
          !  which is what we use below

          ! compute diffusive flux divergence
          call diffusive_n_fluxdiv(mla,n_new,diff_coef_face,diff_fluxdiv,dx,the_bc_tower)

          do n=1,nlevs
             call multifab_plus_plus_c(n_new(n),1,n_old(n),1,nspecies,0)
             call multifab_saxpy_3(n_new(n),dt,diff_fluxdiv(n))
             call multifab_saxpy_3(n_new(n),dt,stoch_fluxdiv(n))
             call multifab_saxpy_3(n_new(n),dt,ext_src(n))
             call multifab_mult_mult_s_c(n_new(n),1,0.5d0,nspecies,0)
             call multifab_fill_boundary(n_new(n))
             call multifab_physbc(n_new(n),1,scal_bc_comp,nspecies, &
                                  the_bc_tower%bc_tower_array(n),dx_in=dx(n,:))
          end do

       end if

    else if (diffusion_type .eq. 1) then
       ! Crank-Nicolson
       ! n_k^{n+1} = n_k^n + (dt/2)(div D_k grad n_k)^n
       !                   + (dt/2)(div D_k grad n_k)^n+1
       !                   +  dt    div (sqrt(2 D_k n_k / dt) Z)^n
       !                   +  dt    ext_src
       !
       ! in delta formulation:
       !
       ! (I - div (dt/2) D_k grad) delta n_k =   dt div (D_k grad n_k^n)
       !                                       + dt div (sqrt(2 D_k n_k / dt) Z)^n
       !                                       + dt ext_src
       !
       ! we combine the entire rhs into stoch_fluxdiv
       do n=1,nlevs
          call multifab_plus_plus(stoch_fluxdiv(n),ext_src(n),0)
          call multifab_plus_plus(stoch_fluxdiv(n),diff_fluxdiv(n),0)
          call multifab_mult_mult_s(stoch_fluxdiv(n),dt)
       end do
       call implicit_diffusion(mla,n_old,n_new,stoch_fluxdiv,diff_coef_face,dx,dt,the_bc_tower)

    else if (diffusion_type .eq. 2) then
       ! explicit midpoint scheme

       ! n_k^{n+1/2} = n_k^n + (dt/2) div (D_k grad n_k)^n
       !                     + (dt/2) div (sqrt(2 D_k n_k / (dt/2) ) Z_1)^n
       !                     + (dt/2) ext_src
       do n=1,nlevs
          call multifab_copy_c(n_new(n),1,n_old(n),1,nspecies,0)
          call multifab_saxpy_3(n_new(n),dt/2.d0      ,diff_fluxdiv(n))
          call multifab_saxpy_3(n_new(n),dt/sqrt(2.d0),stoch_fluxdiv(n))
          call multifab_saxpy_3(n_new(n),dt/2.d0      ,ext_src(n))
          call multifab_fill_boundary(n_new(n))
          call multifab_physbc(n_new(n),1,scal_bc_comp,nspecies, &
                               the_bc_tower%bc_tower_array(n),dx_in=dx(n,:))
       end do

       ! compute diffusive flux divergence at t^{n+1/2}
       call diffusive_n_fluxdiv(mla,n_new,diff_coef_face,diff_fluxdiv,dx,the_bc_tower)

       if (variance_coef_mass .gt. 0.d0) then
          ! fill random flux multifabs with new random numbers
          call fill_mass_stochastic(mla,the_bc_tower%bc_tower_array)

          ! compute second-stage stochastic flux divergence and
          ! add to first-stage stochastic flux divergence
          select case (midpoint_stoch_flux_type)
          case (1)
             ! use n_old
             call stochastic_n_fluxdiv(mla,n_old,diff_coef_face,stoch_fluxdiv,dx,dt, &
                                       the_bc_tower,increment_in=.true.)
          case (2)
             ! use n_pred
             call stochastic_n_fluxdiv(mla,n_new,diff_coef_face,stoch_fluxdiv,dx,dt, &
                                       the_bc_tower,increment_in=.true.)
          case (3)
             ! We use n_new=2*n_pred-n_old here as temporary storage since we will overwrite it shortly
             do n=1,nlevs
                call multifab_mult_mult_s_c(n_new(n),1,2.d0,nspecies,n_new(n)%ng)
                call multifab_sub_sub_c(n_new(n),1,n_old(n),1,nspecies,n_new(n)%ng)
             end do
             ! use n_new=2*n_pred-n_old
             call stochastic_n_fluxdiv(mla,n_new,diff_coef_face,stoch_fluxdiv,dx,dt, &
                                       the_bc_tower,increment_in=.true.)
          case default
             call bl_error("advance_diffusion: invalid midpoint_stoch_flux_type")
          end select
       end if

       ! n_k^{n+1} = n_k^n + dt div (D_k grad n_k)^{n+1/2}
       !                   + dt div (sqrt(2 D_k n_k^n dt) Z_1 / sqrt(2) )
       !                   + dt div (sqrt(2 D_k n_k^? dt) Z_2 / sqrt(2) )
       !                   + dt ext_src
       ! where
       ! n_k^? = n_k^n               (midpoint_stoch_flux_type=1)
       !       = n_k^pred            (midpoint_stoch_flux_type=2)
       !       = 2*n_k^pred - n_k^n  (midpoint_stoch_flux_type=3)
       do n=1,nlevs
          call multifab_copy_c(n_new(n),1,n_old(n),1,nspecies,0)
          call multifab_saxpy_3(n_new(n),dt           ,diff_fluxdiv(n))
          call multifab_saxpy_3(n_new(n),dt/sqrt(2.d0),stoch_fluxdiv(n))
          call multifab_saxpy_3(n_new(n),dt           ,ext_src(n))
          call multifab_fill_boundary(n_new(n))
          call multifab_physbc(n_new(n),1,scal_bc_comp,nspecies, &
                               the_bc_tower%bc_tower_array(n),dx_in=dx(n,:))
       end do

    else

       call bl_error("invalid diffusion_type")
    end if

    do n=1,nlevs
       call multifab_destroy(diff_fluxdiv(n))
       call multifab_destroy(stoch_fluxdiv(n))
       do i=1,dm
          call multifab_destroy(diff_coef_face(n,i))
       end do
    end do

    call destroy(bpt)

  end subroutine advance_diffusion

end module advance_diffusion_module


!*******************************************************************************
!>
!  Halo orbit routines.
!
!### References
!  * D.L. Richardson, "Analytic Construction of Periodic
!    Orbits About the Collinear Points", Celestial Mechanics 22 (1980)
!
!@todo Add differentially-corrected option using the STM derivatives.

    module halo_orbit_module

    use iso_fortran_env, only: wp => real64, error_unit
    use numbers_module
    use crtbp_module

    implicit none

    private

    public :: halo_to_rv
    public :: halo_to_rv_diffcorr
    public :: compute_halo_monodromy_matrix
    public :: compute_monodromy_matrix_eigenvalues

    public :: halo_orbit_test ! test routine

    contains
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the state vector for a halo orbit.
!  This uses the approximation, which is retargeted in the
!  real CR3BP system to produce a periodic orbit.
!
!@todo use a variable-step size integrator

    subroutine halo_to_rv_diffcorr(libpoint,mu1,mu2,dist,A_z,n,t1,rv,info,period)

    use rk_module
    use minpack_module
    use math_module,     only: wrap_angle

    implicit none

    integer,intent(in)  :: libpoint         !! Libration point number: [1,2,3]
    real(wp),intent(in) :: mu1              !! grav param for primary body [km3/s2]
    real(wp),intent(in) :: mu2              !! grav param for secondary body [km3/s2]
    real(wp),intent(in) :: dist             !! distance between bodies [km]
    real(wp),intent(in) :: A_z              !! halo z amplitude [km]
    integer,intent(in)  :: n                !! halo family: 1, 3
    real(wp),intent(in) :: t1               !! tau1 [rad]
    real(wp),dimension(6),intent(out) :: rv !! cr3bp normalized state vector
                                            !! [wrt barycenter]
    integer,intent(out) :: info             !! status code (1=no errors)
    real(wp),intent(out),optional :: period !! period of halo (normalized time units)

    integer,parameter  :: n_state_vars    = 6         !! number of state variables in the equations of motion
    integer,parameter  :: n_opt_vars      = 2         !! number of variables in the targeting problem
    real(wp),parameter :: t0              = 0.0_wp    !! initial time (normalized) (epoch doesn't matter for cr3bp)
    real(wp),parameter :: tol             = 1.0e-8_wp !! tolerance for event finding
    real(wp),parameter :: xtol            = 1.0e-6_wp !! tolerance for [[hybrd]]
    integer,parameter  :: maxfev          = 1000      !! max number of function evaluations for [[hybrd]]
    integer,parameter  :: n_steps_per_rev = 100       !! number of integration steps per orbit rev

    type(rk8_10_class)               :: prop   !! integrator
    real(wp),dimension(n_opt_vars)   :: x_vy0  !! variables in the targeting problem (x0 and vy0)
    real(wp),dimension(n_opt_vars)   :: vx_vzf !! constraints in the targeting problem (vxf and vzf)
    real(wp),dimension(n_state_vars) :: x0     !! halo initial guess from richardson approximation
    real(wp),dimension(n_state_vars) :: xf     !! state after 1/2 rev (to get the period)
    real(wp) :: tf_actual     !! 1/2 period for retargeted orbit (normalized time)
    real(wp) :: actual_period !! actual halo orbit period for retargeted orbit (normalized time)
    real(wp) :: approx_period !! period approximation (normalized time)
    real(wp) :: dt_to_t1      !! time from `t1=0` to input `t1`
    real(wp) :: gf            !! function value after 1/2 rev (y-coordinate)
    real(wp) :: tau           !! `t1` wrapped from \(- \pi \) to \( \pi \)
    real(wp) :: dt            !! time step (normalized)
    real(wp) :: tmax          !! max final time for event finding integration
    real(wp) :: mu            !! CRTBP parameter

    ! compute the CRTBP mu parameter:
    mu = compute_crtpb_parameter(mu1,mu2)

    ! first we get the halo state approximation at tau1=0:
    call halo_to_rv(libpoint,mu1,mu2,dist,A_z,n,zero,x0,approx_period)

    ! for now, fixed number of integration steps per period:
    dt = approx_period / real(n_steps_per_rev,wp)
    tmax = two * approx_period ! should be enough to find the x-z crossing

    ! initialize the integrator:
    call prop%initialize(n_state_vars,func,g=xz_plane_crossing)

    ! now, solve for a halo:
    x_vy0 = [x0(1),x0(5)]  ! x0 and vy0
    call hybrd1(halo_fcn,2,x_vy0,vx_vzf,tol=xtol,info=info)
    if (info==1) then ! solution converged

        ! now have the solution at t1=0:
        x0(1) = x_vy0(1)
        x0(5) = x_vy0(2)

        ! this is the t1 we want:
        tau = wrap_angle(t1)

        ! if we need the period:
        if (present(period) .or. tau/=zero) then
            ! integrate to the first x-axis crossings (one half rev):
            ! [need to check output...]
            call prop%integrate_to_event(t0,x0,dt,tmax,tol,tf_actual,xf,gf)
            actual_period = two * tf_actual ! normalized period
        end if

        ! now we want to propagate to the input tau1
        if (tau==zero) then
            ! already have the solution
            rv = x0
        else
            ! now, integrate from t1=0 to input t1 to get rv:
            dt_to_t1 = actual_period * (tau / twopi)
            call prop%integrate(t0,x0,dt,dt_to_t1,rv)
        end if

        if (present(period)) period = actual_period

    else ! there was an error
        write(error_unit,'(A)') 'Error: the halo targeting problem did not converge.'
        rv = x0
    end if

    !call prop%destroy()

    contains
!*******************************************************************************

    !***************************************************************************
        subroutine halo_fcn(n,xvec,fvec,iflag)
        !! Halo function for [[hybrd1]]

        implicit none

        integer,intent(in)                :: n      !! `n=2` in this case
        real(wp),dimension(n),intent(in)  :: xvec   !! x_vy0
        real(wp),dimension(n),intent(out) :: fvec   !! [vxf,vzf]
        integer,intent(inout)             :: iflag  !! status flag (set negative
                                                    !! to terminate solver)

        real(wp) :: gf
        real(wp),dimension(6) :: x,x1,xf

        x    = x0      ! initial guess state (z is held fixed)
        x(1) = xvec(1) ! x0
        x(5) = xvec(2) ! vy0

        !integrate to the next x-z-plane crossing:
        call prop%integrate_to_event(t0,x,dt,tmax,tol,tf_actual,xf,gf)

        !want x and z-velocity at the x-z-plane crossing to be zero:
        fvec = [xf(4),xf(6)]

        end subroutine halo_fcn
    !***************************************************************************

    !***************************************************************************
        subroutine func(me,t,x,xdot)
        !! CRTBP derivative function
        implicit none
        class(rk_class),intent(inout)        :: me
        real(wp),intent(in)                  :: t
        real(wp),dimension(me%n),intent(in)  :: x
        real(wp),dimension(me%n),intent(out) :: xdot

        call crtbp_derivs(mu,x,xdot)

        end subroutine func
    !**************************************************************************

    !***************************************************************************
        subroutine xz_plane_crossing(me,t,x,g)
        !! x-z-plane crossing event function
        implicit none
        class(rk_class),intent(inout)        :: me
        real(wp),intent(in)                  :: t
        real(wp),dimension(me%n),intent(in)  :: x
        real(wp),intent(out)                 :: g

        g = x(2)  ! y = 0 at x-z-plane crossing

        end subroutine xz_plane_crossing
    !***************************************************************************

    end subroutine halo_to_rv_diffcorr
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the state vector from the halo orbit approximation.
!  This will be an approximation of a halo orbit in the CR3BP system,
!  and will need to be corrected to produce a real halo orbit.

    subroutine halo_to_rv(libpoint,mu1,mu2,dist,A_z,n,t1,rv,period)

    implicit none

    integer,intent(in)  :: libpoint         !! Libration point number: [1,2,3]
    real(wp),intent(in) :: mu1              !! grav param for primary body [km3/s2]
    real(wp),intent(in) :: mu2              !! grav param for secondary body [km3/s2]
    real(wp),intent(in) :: dist             !! distance between bodies [km]
    real(wp),intent(in) :: A_z              !! halo z amplitude [km]
    integer,intent(in)  :: n                !! halo family: 1, 3
    real(wp),intent(in) :: t1               !! tau1 [rad]
    real(wp),dimension(6),intent(out) :: rv !! cr3bp normalized state vector
                                            !! [wrt barycenter]
    real(wp),intent(out),optional :: period !! normalized halo orbit period

    real(wp) :: mu          !! CRTBP parameter
    integer  :: delta_n     !! 2 - n
    real(wp) :: gamma_l     !! dimensionless quantity from reference
    real(wp) :: lambda      !! linearized frequency
    real(wp) :: Ax,Ay,Az,Ax2,Az2
    real(wp) :: x,y,z,vx,vy,vz
    real(wp) :: a1,a2,a21,a22,a23,a24,a31,a32,b21,&
                b22,b31,b32,c2,c3,c4,delta,w,&
                d1,d2,d21,d3,d31,d32,k,l1,l2,s1,s2,term
    real(wp),dimension(3) :: x_libpoint  !! x-coordinates of the libration
                                         !! point (wrt barycenter, normalized)
    logical :: ok

    ! error check:
    if (n/=1 .and. n/=3) then
        error stop 'invalid n input to halo_to_rv'
    end if

    ! compute all the intermediate parameters:
    mu = compute_crtpb_parameter(mu1,mu2)

    ! lib point x-coordinate: wrt to barycenter - normalized
    select case (libpoint)
    case(1)
        call compute_libration_points(mu,r1=x_libpoint(1))
        gamma_l = (1.0_wp - mu) - x_libpoint(1)
    case(2)
        call compute_libration_points(mu,r2=x_libpoint(2))
        gamma_l = x_libpoint(2) - ( 1.0_wp - mu )
    case(3)
        call compute_libration_points(mu,r3=x_libpoint(3))
        gamma_l = - (x_libpoint(3) + mu)
    case default
        error stop 'invalid libration point input to halo_to_rv'
    end select

    Az      = A_z / (dist*gamma_l) ! normalized z-Amplitude
    c2      = c_n(libpoint, 2, mu, gamma_l)
    c3      = c_n(libpoint, 3, mu, gamma_l)
    c4      = c_n(libpoint, 4, mu, gamma_l)
    lambda  = sqrt((-c2+two+sqrt(nine*c2**2-eight*c2))/two)  ! root of quartic eqn
    k       = two*lambda/(lambda**2+one-c2)
    delta   = lambda**2-c2
    d1      = 16.0_wp*lambda**4+four*lambda**2*(c2-two)-two*c2**2+c2+one
    d2      = 81.0_wp*lambda**4+nine*lambda**2*(c2-two)-two*c2**2+c2+one
    d3      = two*lambda*(lambda*(one+k**2)-two*k)
    a21     =  three*c3*(k**2-two)/four/(one+two*c2)
    a23     = -three*lambda*c3*(three*k**3*lambda-six*k*(k-lambda)+four)/four/k/d1
    b21     = -three*c3*lambda*(three*lambda*k-four)/two/d1
    s1      = ((three/two)*c3*(two*a21*(k**2-two)-a23*(k**2+two)-&
              two*k*b21)-(three/eight)*c4*(three*k**4-eight*k**2+eight))/d3
    a22     = three*c3/four/(one+two*c2)
    a24     = -three*c3*lambda*(two+three*lambda*k)/four/k/d1
    b22     = three*lambda*c3/d1
    d21     = -c3/two/lambda**2
    s2      = ((three/two)*c3*(two*a22*(k**2-two)+&
              a24*(k**2+two)+two*k*b22+five*d21) + &
              (three/eight)*c4*(12.0_wp-k**2))/d3
    a1      = -(three/two)*c3*(two*a21+a23+five*d21) - &
              (three/eight)*c4*(12.0_wp-k**2)
    a2      = (three/two)*c3*(a24-two*a22)+(nine/eight)*c4
    l1      = two*s1*lambda**2+a1
    l2      = two*s2*lambda**2+a2
    Az2     = Az**2

    ! check if this Az is feasible
    ok = (l1/=zero)
    if (ok) then
        term = (-delta-l2*Az2)/l1
        ok = (term>=zero)
    end if
    if (.not. ok) then
        rv = zero
        if (present(period)) period = zero
        write(error_unit,'(A)') 'Error: infeasible input.'
        return
    end if

    Ax      = sqrt(term) ! equation 18
    Ax2     = Ax**2
    Ay      = k*Ax
    w       = one+s1*Ax2+s2*Az2  ! frequency correction
    a31     = -nine*lambda*(c3*(k*a23-b21)+k*c4*(one+(one/four)*k**2))/d2 + &
              (nine*lambda**2+one-c2)*(three*c3*(two*a23-k*b21)+ &
              c4*(two+three*k**2))/two/d2
    a32     = -nine*lambda*(four*c3*(k*a24-b22)+k*c4)/four/d2 - &
              three*(nine*lambda**2+one-c2)*(c3*(k*b22+d21-two*a24)-c4)/two/d2
    b31     = (three*lambda*(three*c3*(k*b21-two*a23)-c4*(two+three*k**2)) + &
              (nine*lambda**2+1+two*c2)*(12.0_wp*c3*(k*a23-b21)+&
              three*k*c4*(four+k**2))/eight)/d2
    b32     = (three*lambda*(three*c3*(k*b22+d21-two*a24)-three*c4) + &
              (nine*lambda**2+one+two*c2)*(12.0_wp*c3*(k*a24-b22)+three*c4*k)/eight)/d2
    d31     = three*(four*c3*a24+c4)/64.0_wp/lambda**2
    d32     = three*(four*c3*(a23-d21)+c4*(four+k**2))/64.0_wp/lambda**2
    delta_n = 2 - n  ! equation 21

    if (present(period)) period = twopi/(lambda*w)

    ! Equations 20a, 20b, 20c (and their derivatives):
    x  = a21*Ax2+a22*Az2-Ax*cos(t1)+&
         (a23*Ax2-a24*Az2)*cos(two*t1)+&
         (a31*Ax**3-a32*Ax*Az2)*cos(three*t1)
    y  = k*Ax*sin(t1)+(b21*Ax2-b22*Az2)*sin(two*t1)+&
         (b31*Ax**3-b32*Ax*Az2)*sin(three*t1)
    z  = delta_n*(Az*cos(t1)+d21*Ax*Az*(cos(two*t1)-three)+&
         (d32*Az*Ax2-d31*Az**3)*cos(three*t1))
    vx = Ax*sin(t1)-(a23*Ax2-a24*Az2)*sin(two*t1)*two-&
         (a31*Ax**3-a32*Ax*Az2)*sin(three*t1)*three
    vy = k*Ax*cos(t1)+(b21*Ax2-b22*Az2)*cos(two*t1)*two+&
         (b31*Ax**3-b32*Ax*Az2)*cos(three*t1)*three
    vz = delta_n*(-Az*sin(t1)+d21*Ax*Az*(-sin(two*t1)*two)-&
         (d32*Az*Ax2-d31*Az**3)*sin(three*t1)*three)

    rv = [x,y,z,vx,vy,vz]

    ! convert from richardson scale, libration point centered to
    ! standard normalized coordinates wrt barycenter:
    rv(1:3) = rv(1:3) * gamma_l
    rv(4:6) = rv(4:6) * gamma_l * (lambda*w)
    rv(1)   = rv(1) + x_libpoint(libpoint)

    contains
!*******************************************************************************

    !***************************************************************************
        pure function c_n(lib,n,mu,gl) result(cn)

        !! Equations 8a, 8b in the reference.

        implicit none

        integer,intent(in)   :: lib  !! libration point (1,2,3)
        integer,intent(in)   :: n    !! the n in cn
        real(wp),intent(in)  :: mu   !! cr3bp normalized grav parameter
        real(wp),intent(in)  :: gl   !! \( \gamma_l \)
        real(wp)             :: cn   !! result

        ! Equation 8a and 8b:
        select case(lib)
        case(1); cn = (mu+(-1)**n*(one-mu)*(gl/(one-gl))**(n+1)  )/gl**3
        case(2); cn = ((-1)**n*(mu+(one-mu)*(gl/(one+gl))**(n+1)))/gl**3
        case(3); cn = (one-mu+mu*(gl/(one+gl))**(n+1)            )/gl**3
        end select

        end function c_n
    !***************************************************************************

    end subroutine halo_to_rv
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the halo orbit monodromy matrix
!  (which is the state transition matrix propagated for one period)
!  The input should be the result from the [[halo_to_rv_diffcorr]] routine.

    subroutine compute_halo_monodromy_matrix(mu,rv,period,phi)

    use rk_module

    implicit none

    real(wp),intent(in)                 :: mu     !! CRTBP parameter
    real(wp),dimension(6),intent(in)    :: rv     !! halo orbit state vector
                                                  !! (normalized)
    real(wp),intent(in)                 :: period !! halo orbit period
                                                  !! (normalized)
    real(wp),dimension(6,6),intent(out) :: phi    !! monodromy matrix

    real(wp),parameter :: t0 = zero  !! initial time (normalized)
                                     !! (epoch doesn't matter for cr3bp)
    integer,parameter  :: n_steps_per_rev = 100   !! number of integration steps
                                                  !! per orbit rev

    real(wp),dimension(42)  :: x0     !! initial normalized state and STM
    real(wp),dimension(42)  :: xf     !! final normalized state and STM
    real(wp),dimension(6,6) :: phi0   !! initial STM
    integer                 :: i      !! counter
    type(rk8_10_class)      :: prop   !! integrator
    real(wp)                :: dt     !! integration time step (normalized)

    ! initial state:
    x0(1:6) = rv

    ! initial stm is the identity matrix:
    phi0 = zero
    do i = 1, 6
       phi0(i,i) = one
    end do
    x0(7:42) = pack (phi0, mask=.true.)

    ! for now, use a fixed time step:
    ! (same as was used in [[halo_to_rv_diffcorr]])
    dt = period / real(n_steps_per_rev,wp)

    ! initialize the integrator:
    call prop%initialize(42,func)

    ! propagate for one period:
    call prop%integrate(t0,x0,dt,period,xf)

    ! extract the STM:
    phi = reshape(xf(7:42), shape=[6,6])

    contains
!*******************************************************************************

    !***************************************************************************
        subroutine func(me,t,x,xdot)
        !! CRTBP derivative function (with STM)
        implicit none
        class(rk_class),intent(inout)        :: me
        real(wp),intent(in)                  :: t
        real(wp),dimension(me%n),intent(in)  :: x
        real(wp),dimension(me%n),intent(out) :: xdot

        call crtbp_derivs_with_stm(mu,x,xdot)

        end subroutine func
    !**************************************************************************

    end subroutine compute_halo_monodromy_matrix
!*******************************************************************************

!*******************************************************************************
!>
!  Compute the eigenvalues of the monodromy matrix.
!
!### Reference
!  * J.S. Parker, R.L. Anderson, "Low-Energy Lunar Trajectory Design",
!    2014. (p 79)

    subroutine compute_monodromy_matrix_eigenvalues(phi,lambda)

    use matrix_module, only: matrix_trace

    implicit none

    real(wp),dimension(6,6),intent(in)   :: phi    !! monodromy matrix
    complex(wp),dimension(6),intent(out) :: lambda !! eigenvalues of `phi`

    real(wp) :: alpha, beta, alpha2
    complex(wp) :: p, q, a, b, c

    alpha  = two - matrix_trace(6,phi)
    alpha2 = alpha*alpha
    beta   = (alpha2 - matrix_trace(6,matmul(phi,phi)))/two + one
    a      = sqrt(alpha2 - four*beta + eight)
    p      = (alpha + a) / two
    q      = (alpha - a) / two
    b      = sqrt(p*p - four)
    c      = sqrt(q*q - four)

    ! eigenvalues:
    lambda(1) = (-p + b) / two
    lambda(2) = (-p - b) / two
    lambda(3) = (-q + c) / two
    lambda(4) = (-q - c) / two
    lambda(5) = (one, zero)
    lambda(6) = (one, zero)

    end subroutine compute_monodromy_matrix_eigenvalues
!*******************************************************************************

!*******************************************************************************
!>
!  Unit test for the halo orbit module.

    subroutine halo_orbit_test()

    use celestial_body_module

    implicit none

    integer               :: libpoint  !! Libration point number: [1,2,3]
    real(wp)              :: mu1       !! grav param for primary body [km3/s2]
    real(wp)              :: mu2       !! grav param for secondary body [km3/s2]
    real(wp)              :: dist      !! distance between bodies [km]
    real(wp)              :: Az        !! halo z amplitude [km]
    integer               :: n         !! halo family: 1, 3
    real(wp)              :: t1        !! tau1 [rad]
    real(wp),dimension(6) :: rv        !! normalized state [wrt barycenter]

    write(*,*) ''
    write(*,*) '----------------'
    write(*,*) ' halo_orbit_test'
    write(*,*) '----------------'
    write(*,*) ''

    libpoint = 2
    mu1  = body_earth%mu
    mu2  = body_moon%mu
    dist = 384400.0_wp
    Az   = 10000.0_wp
    n    = 1
    t1   = 0.0_wp
    call halo_to_rv(libpoint,mu1,mu2,dist,Az,n,t1,rv)

    write(*,*) ''
    write(*,*) 'halo orbit state:'
    write(*,*) rv
    write(*,*) ''

    end subroutine halo_orbit_test
!*******************************************************************************

!*******************************************************************************
    end module halo_orbit_module
!*******************************************************************************