!! This module calculates the spacegroup of a crystal, the
!! pointgroup of a bravais lattice, and checks whether or not a given
!! unit cell is primitive
!! Gus Hart
!! UC Davis
!! Started 10/98
!!     Finite precision error fixed 8/99
!! Started using the module again for the UNCLE code
!! GLWH BYU Dec. 2006
!! Added error flag passback on get_transformations
!! Modified SG routine to include fractional shifts for non-primitive
!! lattices (10/26/2009)
MODULE symmetry
  use num_types
  use numerical_utilities, only: equal
  use vector_matrix_utilities, only: matrix_inverse, norm, cross_product, volume

  implicit none
  private
  public get_spaceGroup, get_spaceGroup_atomTypes, rm_3d_operations, make_primitive, &
       get_lattice_pointGroup, does_mapping_exist, get_transformations, check_spaceGroup,&
       bring_into_cell, find_site_equivalencies, put_pointGroup_in_latticeCoords

CONTAINS
  !!This routine takes a crystal structure (basis vectors and
  !! basis atoms and positions) and returns the point operators and
  !! fractional translations of the space group. The routine assumes
  !! that the given crystal structure is already primitive. To reduce
  !! a non-primitive structure to a primitive one, use the function
  !! "make_primitive" in this same module.
  !! No assumptions are made about the orientation of the input
  !! vectors. The positions of the basis atoms may be given in lattice
  !! coordinates or in cartesian coordinates.
  !! The main steps are:
  !! (1) Check inputs and generate matrices for converting vectors
  !! (atom positions an lattice points) from (to) lattice coordinates
  !! to (from) Cartesian coordinates
  !! (2) Convert atom positions from lattice coordinates, if necessary
  !! (3) Translate all atoms into primitive unit cell
  !! (4) Find the point operators of the given lattice
  !!     i) Generate all triplets of lattice points that preserve the length of
  !!        the original lattice vectors.
  !!    ii) Eliminate triplets that aren't primitive (cell volume is changed)
  !!   iii) Compute the transformation that takes the original lattice vectors to
  !!        the new triplet of points.
  !!    iv) Check that the transformation is orthogonal. If so, it is part of the
  !!        point group of the lattice.
  !! (5) Find which of the point operators are part of the space group and compute
  !!     the corresponding fractional translations.
  !!<parameter name="aVecs" regular="true">Real space primitive
  !!lattice vectors.</parameter>
  !!<parameter name="atomType" regular="true">Integers representing
  !!type of each basis atom.  </parameter>
  !!<parameter name="input_pos">Positions of the basis atoms.</parameter>
  !!<parameter name="sg_op">Rotations in the space group.</parameter>
  !!<parameter name="sg_fract">Translations of the space
  !!group.</parameter>
  !!<parameter name="lattcoords" regular="true">If .true., atom
  !!positions are assumed to be in lattice coordinates. Otherwise,
  !!they are treated as cartesian</parameter>
  !!<parameter name="eps_" regular="true">"epsilon" for checking
  !!equivalence in floating point arithmetic ></parameter>
  subroutine get_spaceGroup(aVecs, atomType, input_pos, sg_op, sg_fract, lattcoords, eps_)

    real(dp), intent(in):: aVecs(3,3)
    integer, intent(inout):: atomType(:)
    real(dp), allocatable:: input_pos(:,:)
    real(dp), allocatable:: sg_op(:,:,:)
    real(dp), allocatable:: sg_fract(:,:)
    logical :: lattcoords
    real(dp), intent(in), optional:: eps_

    real(dp) atom_pos(3,size(input_pos,2)) ! Copy of input_pos
    real(dp) latt_to_cart(3,3) ! Transformation for lattice coords to cartesian coords
    real(dp) cart_to_latt(3,3) ! Transformation for cartesian coords to lattice coords
    real(dp), pointer:: lattpg_op(:,:,:) ! The lattice point group operations
    real(dp) temp_sgops (3,3,48*size(atomType)) ! Temporary matrix to store space group
    !operations
    real(dp) temp_sgfracts(3,48*size(atomType)) ! Temporary matrix to store fractional
    ! translations
    ! NB: the previous two allocations originally read 48*4, where *4
    ! was a rough estimate by GH.  We probably should have something
    ! like 48*nD (nD=number of d-vectors in the parent), but right now
    ! I (tk) don't see nD passed into this routine. Because I don't
    ! want to change all interfaces, I resort to size(atomType) which
    ! yields the number of "atoms" or "lattice sites" within a
    ! structure. This number is probably far too high, but certainly
    ! not too low.

    ! GH: I'm not fond of the fact that we have need non-primitive
    ! cells for UNCLE. Could have been designed better and we wouldn't
    ! have had to dirty up this symmetry routine...

    real(dp) fract(3)           ! A fractional translation to check
    real(dp) v(3)               ! A translated vector to check mapping for
    real(dp) v2(3)              ! A second translated vector check
    real(dp) eps                ! "epsilon" for checking equivalence
    ! in floating point arithmetic

    integer i                   ! Generic loop counter
    integer iop                 ! Loop counter for loops over point group operations
    integer jAtom, kAtom        ! Loop counters for loops over atoms
    integer this_type           ! Type of current atom being checked
    integer sgop_count          ! Counter for number of space group operations
    integer nAtoms              ! Number of atoms in the basis

    logical err                 ! Used to check for coplanar vectors in the basis
    logical mapped              ! True when an operation has mapped an atom to another

    if(.not. present(eps_)) then
       eps = 1e-10_dp
    else
       eps =  eps_
    endif

    ! Get number of atoms in the basis
    nAtoms = size(atomType)

    ! Save original original atomic input positions
    atom_pos = input_pos

    ! A vector can be represented as either a set of cartesian coordi-
    ! nates or as a linear combination of primitive lattice vectors
    ! Get transformation matrices to take us back and forth
    !err=.false.
    call get_transformations(aVecs, latt_to_cart, cart_to_latt,err)
    !write(*,*) err
    if(err) stop "ERROR in get_spaceGroup: primitive vectors appear to be coplanar"

    ! Convert the position of the basis atoms from lattice coordinates, if necessary
    if(lattcoords .eqv. .true.) then
       do i = 1, nAtoms
          atom_pos(:,i) = matmul(latt_to_cart, atom_pos(:,i))
       enddo
    endif

    ! Bring all the basis atoms into the unit cell
    do i = 1, nAtoms
       call bring_into_cell(atom_pos(:,i), cart_to_latt, latt_to_cart, eps)
    enddo

    ! Now, find the point group for the given lattice
    call get_lattice_pointGroup(aVecs, lattpg_op, eps)

    ! **** Find the elements of the space group ****
    ! Count the elements
    sgop_count = 0
    ! Apply each of the point operators in the point group to the crystal
    do iop = 1, size(lattpg_op,3)
       ! rotate atom 1 and store its position in the vector v
       v = matmul(lattpg_op(:,:,iop), atom_pos(:,1))
       ! Loop over all possible fractional translations
       do jAtom = 1, nAtoms
          if(atomType(jAtom) /= atomType(1)) cycle
          fract = atom_pos(:,jAtom) - v
          call bring_into_cell(fract, cart_to_latt, latt_to_cart, eps)
          ! Is each atom of every type mapped by this rotation + translation?
          do kAtom = 1, nAtoms
             this_type = atomType(kAtom)
             ! Rotate and translate each atom
             v2 = matmul(lattpg_op(:,:,iop),atom_pos(:,kAtom))
             v2 = v2 + fract
             call bring_into_cell(v2, cart_to_latt, latt_to_cart, eps)
             ! Try to map this rotated atom onto another the same type
             call does_mapping_exist(v2, this_type, atom_pos, atomType, mapped, eps)
             if(.not. mapped) exit ! no mapping for this atom
          enddo
          ! if all atoms mapped successfully then count this element (rotation + translation)
          if(mapped) then
             sgop_count = sgop_count + 1 ! Count the number of elements
             temp_sgfracts(:,sgop_count) = fract ! Save the translational part
             temp_sgops(:,:,sgop_count) = lattpg_op(:,:,iop) ! Store the rotational part
             !exit !exit loop over fractional translations and try next op
             ! By removing the preceding exit, we include fractional translations
             ! for non-primitive lattices. (GLWH 10/26/2009)
          endif
       enddo
    enddo

    deallocate(lattpg_op)

    ! Now that we know how many space group operations there are,
    ! store them in a matrix of the appropriate size as well as the
    ! fractional translations
    allocate(sg_op(3,3,sgop_count),sg_fract(3,sgop_count))
    sg_op = temp_sgops(:,:,1:sgop_count)
    sg_fract = temp_sgfracts(:,1:sgop_count)

  end subroutine get_spaceGroup

  !!Determine the atom types "aTyp" used for get_spaceGroup.
  !! --> IN:
  !!             label  : the possible labels for a site (e.g. 0/1 or 1/2/3)
  !!             digit  : the number of possible labels (e.g.  2,     3)
  !! <-- OUT:
  !!              aTyp  : the atom "types"

  !!<parameter name="label" regular="true">{ digit#, dvector# },
  !!possible labels (occupations) for each dvector.</parameter>
  !!<parameter name="digit" regular="true">{ dvector# }, number of
  !!labels for each dvector.</parameter>
  !!<parameter name="ce" regular="true">{ dvector# }, to which CE does
  !!a dvector belong to?  (refers to CE couplings)</parameter>
  !!<parameter name="aTyp">{ dvector# } intent(out).</parameter>
  subroutine get_spaceGroup_atomTypes(label,digit,ce,aTyp)
    integer, intent(in)  :: label(:,:)
    integer, intent(in)  :: digit(:)
    integer, intent(in)  :: ce(:)
    integer, pointer     :: aTyp(:)

    integer iD1, iD2, nD, maxTyp
    integer digit1, digit2
    integer, pointer :: uqLabel(:,:) ! { digit#, dvector# }: unique
    !labels for each d-vector (see comments below)
    integer :: maxLabel

    ! "label" holds the possible occupations of the dvectors. If there
    !are coupled lattice systems
    !Lattice = Lattice1 (x) Lattice2
    ! the dvectors of Lattice1 and Lattice2 are to be treated
    ! separately.  "label", however, is not unique since the
    ! occupations on both Lattice1 and Lattice2 are denoted by, say,
    ! 0/1. On Lattice1, "0" may represent A, "1" B, while on lattice2,
    ! "0" may represent C, and "1" D.
    ! --> we have to make "label" unique.
    nD = size(digit)
    maxLabel=maxval(label)
    allocate(uqLabel(size(label,1),size(label,2)))
    do iD1=1,nD
       do digit1=1,digit(iD1)
          uqLabel(digit1,iD1) = label(digit1,iD1)  +  (ce(iD1)-1) * maxLabel  ! make label unique
       end do
    end do

    nD=size(digit)
    if (associated(aTyp)) then; aTyp => null(); endif  ! Need strange construction here to make
    if (.not. associated(aTyp)) allocate(aTyp(nD))     ! sure that an associated pointer is
    !handled correctly

    ! now assign each dvector a type called "aTyp". All dvectors that
    !have the same possible occupations are assigned the same aTyp.

    iD1_loop: do iD1=1,nD
       maxTyp=0
       do iD2=1,iD1-1
          digit1=digit(iD1) ! number of occupations on dvector 1
          digit2=digit(iD2) ! number of occupations on dvector 2
          if (digit1==digit2 .and. all(uqLabel(:digit1,iD1)==uqLabel(:digit1,iD2))) then
             ! if dvector 1 and dvector 2 have the same number of
             !possible occupations...
             ! ... and if they have the same possible occupations ...
             aTyp(iD1)=aTyp(iD2)    ! ... then the aTyp is identical.
             cycle iD1_loop ! found a type for iD1, go on with the next
          else
             maxTyp = max(maxTyp,aTyp(iD2)) ! generate a new type for iD1
          endif
       enddo
       ! here: iD2=iD1-1
       maxTyp = maxTyp+1
       aTyp(iD1) = maxTyp
    enddo iD1_loop
  end subroutine get_spaceGroup_atomTypes

  !!This subroutine removes operations.
  !!<parameter name="aVecs" regular="true">Primitive real space
  !!lattice vectors.</parameter>
  !!<parameter name="sgrots"></parameter>
  !!<parameter name="sgshifts"></parameter>
  !!<parameter name="eps" regular="true">"epsilon" for checking
  !!equivalence in floating point arithmetic</parameter>
  subroutine rm_3d_operations(aVecs,sgrots,sgshifts,eps)
    real(dp), intent(in):: aVecs(3,3)
    real(dp), allocatable:: sgrots(:,:,:), sgshifts(:,:)  ! intent(inout)
    real(dp), intent(in) :: eps
    real(dp), allocatable :: tSGrots(:,:,:),tSGshifts(:,:)
    integer :: nRot, iRot, status, i
    real(dp) :: atol  ! An absolute tolerance for the "equal" subroutine

    atol = 5E-4_dp

    if (.not.equal(aVecs(2:3,1),0._dp,eps,atol) .or. &
         .not.equal(aVecs(1,2:3),0._dp,eps,atol) )    &
         stop "Error in rm_3d_operations: only allowed for primitive vectors x00,0xx,0xx"

    nRot = size(sgrots,3)
    allocate(tSGrots(3,3,nRot),tSGshifts(3,nRot),STAT=status)
    if(status/=0) stop "Failed to allocate memory in rm_3d_opertions: tSGrots"

    irot = 0
    do i = 1, nRot
       if (equal(sgrots(2:3,1,i),0._dp,eps,atol) .and. equal(sgrots(1,2:3,i),0._dp,eps,atol) .and.&
            & equal(abs(sgrots(1,1,i)),1._dp,eps,atol)) then ! this operation is "2D"
          irot = irot + 1
          tSGrots(:,:,irot) = sgrots(:,:,i)
          tSGshifts(:,irot) = sgshifts(:,i)
       endif
    enddo
    nRot = irot
    deallocate(sgrots,sgshifts)
    allocate(sgrots(3,3,nRot),sgshifts(3,nRot),STAT=status)
    if(status/=0) stop "Allocation of sgrots failed in rm_3d_operations"

    sgrots=tSGrots(:,:,1:nRot)
    sgshifts=tSGshifts(:,1:nRot)
  end subroutine rm_3d_operations

  !!If the given lattice vectors and basis do not form a
  !!primitive unit cell, reduce the vectors to a set of primitive
  !!vectors and reduce the number of basis atoms, if necessary. Also,
  !!all the atom positions are moved inside the new unit
  !!cell
  !!<parameter name="aVecs" regular="true">Primitive real space
  !!lattice vectors.</parameter>
  !!<parameter name="atomType">intent(inout): Atom types represented
  !!as integers.</parameter>
  !!<parameter name="atom_pos" regular="true">intent(inout): Positions of the basis
  !!atoms.</parameter>
  !!<parameter name="lattCoords" regular="true">True if positions are
  !!in lattice coordinates.</parameter>
  !!<parameter name="eps_" regular="true">"epsilon" for checking
  !!equivalence in floating point arithmetic</parameter>
  !!<parameter name="removed_">the indices of the atoms that have been
  !!removed if the cell is not primitive. </parameter>
  subroutine make_primitive(aVecs, atomType, atom_pos, lattCoords, eps_, removed_)
    real(dp), intent(inout) :: aVecs(3,3)
    integer, pointer :: atomType(:)
    real(dp), allocatable :: atom_pos(:,:)
    logical, intent(in) :: lattCoords
    real(dp), intent(in), optional   :: eps_
    integer, pointer, optional       :: removed_(:)

    integer nFracts         ! Number of possible fractional translations for this structure
    integer iAtom, jAtom    ! Loop counters for looping over atoms
    integer nAtoms          ! Number of atoms in the input basis
    integer this_type       ! Type of atom currently being checked for mapping
    integer i, j, k         ! Loop variables for looping over potentially new lattice vectors
    integer iFract          ! Loop variable for loop over possible fractional translations
    integer removed(size(atomType))     ! Temporary storage for removed_
    integer tempType(size(atomType))    ! Temporary storage for use during reallocation
    real(dp) temp_pos(3,size(atomType)) ! Temporary storage for use during reallocation
    real(dp) fract(3)       ! Fractional translation
    real(dp) v(3)           ! Position of current atom after a fractional translation
    real(dp) cart_to_latt(3,3)  ! Transforms vectors from cartesian coords to lattice coords
    real(dp) latt_to_cart(3,3)  ! Transforms vectors from lattice coords to cartesian coords
    real(dp) eps            ! "epsilon" for checking equivalence in floating point arithmetic
    logical mapped          ! Set to true by "does_mapping_exist" if the atom is mapped
    logical err             ! Used in matrix_inverse to check for coplanar vectors
    real(dp), allocatable:: fracts(:,:) ! Array of possible fractional translations
    real(dp), allocatable:: lattice_point(:,:) ! Stores extra lattice points
    real(dp) :: atol  ! An absolute tolerance for the "equal" subroutine

    nAtoms = size(atomType)

    ! optional arguments
    if(.not. present(eps_)) then; eps = 1e-10_dp
    else; eps = eps_
    endif
    atol = 5E-4_dp

    call get_transformations(aVecs, latt_to_cart, cart_to_latt)

    ! Convert the position of the basis atoms from lattice coordinates, if necessary
    if(lattcoords .eqv. .true.) then
       do i = 1, nAtoms
          atom_pos(:,i) = matmul(latt_to_cart, atom_pos(:,i))
       enddo
    endif

    ! Bring all the basis atoms into the unit cell
    do i = 1, nAtoms
       call bring_into_cell(atom_pos(:,i), cart_to_latt, latt_to_cart, eps)
    enddo

    ! Number of possible fractional translations can be no larger than
    ! the number of translations that exist between atom 1 and all
    ! other atoms of the same type
    nFracts = count(atomType == atomType(1)) - 1
    !write (*,'("nFracts:",i3)') nFracts
    allocate(fracts(3,nFracts),lattice_point(3,3 + nFracts))

    ! Count the number of fractional translations that bring the crystal
    ! back onto itself. In other words, find any lattice points that are
    ! inside the cell
    nFracts = 0 !counter for the number of fractional translations found

    ! If there is a lattice vector inside the unit cell, it will bring
    ! any atom of any type in the basis back onto itself. Such a fract-
    ! ional translation must exist for EACH type of atom, so it is suf-
    ! ficient to check that all of the possible translations for a single
    ! type (might as well be first type) map every atom of each type onto
    ! AT LEAST one other atom of the same type
    do iAtom = 2, nAtoms
       if(atomType(iAtom) /= atomType(1)) cycle
       ! Get fractional translation and bring it into cell (if necessary)
       fract = atom_pos(:,iAtom) - atom_pos(:,1)
       call bring_into_cell(fract, cart_to_latt, latt_to_cart, eps)
       ! Try this fractional translation on ALL atoms
       do jAtom = 1, nAtoms
          this_type = atomType(jAtom)
          ! v contains the coordinates of the atom after the translation
          v = fract + atom_pos(:,jAtom)
          call bring_into_cell(v, cart_to_latt, latt_to_cart, eps)
          ! Check to see that this translation takes each atom to another
          call does_mapping_exist(v, this_type, atom_pos, atomType, mapped, eps)
          ! If any atom is not mapped successfully, then this fractional trans-
          ! lation is not a lattice point. Try the next possible translation
          if(.not. mapped) exit
       enddo
       ! If this loop ends successfully (mapped = .true.) then this translation
       ! takes all atoms to another of the same type. (It's a lattice point.)
       ! Count it and save it in an array
       if(mapped) then
          nFracts = nFracts + 1
          fracts(:,nFracts) = fract
       endif
    enddo
    !write (*,'("nFracts:",i3)') nFracts

    ! If the lattice is not primitive, then extra lattice points were
    ! found inside the given unit cell.
    if (nFracts > 0) then
       !   write(*,*) "Given unit cell is not primitive"
       ! collect all lattice points (i.e. potential new primitive vectors)
       lattice_point(:,1:nFracts) = fracts(:,1:nFracts)
       lattice_point(:,nFracts + 1:nFracts + 3) = aVecs
       ! total number of lattice points (including the original primitive vectors)
       nFracts = 3 + nFracts
       ! Take all the possible triplets of points and check to see if one
       ! of the triplets forms a set of primitive basis vectors. A triplet
       ! of points that constitutes a new set of primitive basis vectors
       ! will have the property that the coefficients of ALL the lattice
       ! points will be integer values when the coefficients are given
       ! in (the new) lattice coordinates
       l1:do i = 1, nFracts - 2
          do j = i + 1, nFracts - 1
             do k = j + 1, nFracts
                ! form the trial lattice vectors
                aVecs(:,1) = lattice_point(:,i)
                aVecs(:,2) = lattice_point(:,j)
                aVecs(:,3) = lattice_point(:,k)
                ! If the points are coplanar, inverse will be singular.
                ! If so, try the next triplet
                call matrix_inverse(aVecs, cart_to_latt, err, eps)
                if(err) cycle
                do iFract = 1, nFracts
                   ! put points in lattice coordinates using the new basis vectors
                   v = matmul(cart_to_latt, lattice_point(:,iFract))
                   ! Are all components integers? If so, new vectors found so exit all
                   ! loops. If not try the next triplet of points
                   mapped = .true.
                   if(.not. equal(v, anint((v),dp), eps,atol)) &
                        then; mapped = .false.; exit; endif
                enddo
                if(mapped) exit l1 ! found new vectors so exit
             enddo
          enddo
       enddo l1
       if(.not. mapped) stop "Error in get_symmetry: New basis vectors not found"
       ! Bring all the atoms inside the new unit cell
       do iAtom = 1, nAtoms
          call bring_into_cell(atom_pos(:,iAtom), cart_to_latt, aVecs, eps)
       enddo
       ! Check to see if there were redundant atoms in the basis and remove them if so
       nAtoms = 0
       do iAtom = 1, size(atomType)
          v = atom_pos(:,iAtom)
          this_type = atomType(iAtom)
          mapped = .false.
          do j = iAtom + 1, size(atomType)
             if(atomType(j) == this_type .and. equal(v, atom_pos(:,j), eps,atol)) then
                mapped = .true.
                removed(iAtom)=iAtom  ! store which atom we remove
             endif
          enddo
          if(.not. mapped) then
             nAtoms = nAtoms + 1
             tempType(nAtoms) = this_type
             temp_pos(:,nAtoms) = v
          endif
       enddo
       deallocate(atomType, atom_pos)
       allocate(atomType(nAtoms), atom_pos(3,nAtoms))
       atomType = tempType(:nAtoms)
       atom_pos = temp_pos(:,:nAtoms)

       if (present(removed_)) then
          allocate(removed_( count(removed/=0) ))
          removed_ = pack(removed,removed/=0)
       endif
    endif

    ! Convert the positions of the basis atoms back to cartesion coordinates, if necessary
    if(lattcoords .eqv. .true.) then
       do i = 1, nAtoms
          atom_pos(:,i) = matmul(cart_to_latt, atom_pos(:,i))
       enddo
    endif
    deallocate(fracts,lattice_point)
  end subroutine make_primitive

  !!This routine returns only the point group of the lattice
  !! rather than the space group of the given crystal
  !! structure
  !!<parameter name="aVecs" regular="true"></parameter>
  !!<parameter name="lattpg_op"></parameter>
  !!<parameter name="eps_" regular="true">"epsilon" for checking
  !!equivalence in floating point arithmetic</parameter>
  subroutine get_lattice_pointGroup(aVecs, lattpg_op, eps_)
    real(dp), intent(in):: aVecs(3,3)
    real(dp), pointer:: lattpg_op(:,:,:)
    real(dp), intent(in), optional:: eps_

    real(dp) temp_op(3,3,48)         ! Temporary storage for the point group operations
    real(dp) new_vectors(3,3)        ! Possibly rotated set of primitive vectors
    real(dp) inverse_aVecs(3,3)      ! Inverse aVecs matrix (used to check rotations)
    real(dp) this_vector(3)          ! Vector used in testing
    real(dp) test_matrix(3,3), rotation_matrix(3,3)
    real(dp) cell_volume             ! Volume of the given unit cell
    real(dp) max_norm                ! Maximum norm of the given lattice vectors
    real(dp), allocatable:: Rvecs(:,:), Rlengths(:)
    real(dp) norm_avecs(3)           ! Norms of the given lattice vectors
    real(dp) length                  ! Length of currenct vector being checked
    real(dp), parameter:: Identity(3,3) = reshape((/1,0,0, 0,1,0, 0,0,1/),(/3,3/))
    real(dp) eps                     ! "epsilon" for checking equivalence in floating point arithmetic
                                     ! This is a "relative" tolerance with the new equal routine
    real(dp) atol                    ! An absolute tolerance for the 'equal' subroutine

    integer n1, n2, n3              ! Upper limit for loops over R vectors
    integer i, j, k      ! Loop variables
    integer num_Rs       ! Number of R vectors in the sphere that is searched
    integer num_ops      ! Used to count the number of point operations found

    if(.not. present(eps_)) then; eps = 1e-10_dp; else; eps = eps_;endif
    atol = 5E-4_dp
    call matrix_inverse(aVecs, inverse_aVecs)
    ! Store the norms of the three lattice vectors
    do i = 1, 3;norm_avecs(i) = norm(aVecs(:,i));enddo

    ! Decide how many lattice points to look in each direction to get all the
    ! points in a sphere that contains all of the longest _primitive_ vectors
    cell_volume = abs(dot_product(aVecs(:,1),cross_product(aVecs(:,2),aVecs(:,3))))
    max_norm = max(norm(aVecs(:,1)),norm(aVecs(:,2)),norm(aVecs(:,3)))
    n1 = ceiling(max_norm*norm(cross_product(aVecs(:,2),aVecs(:,3))/cell_volume)+eps)
    n2 = ceiling(max_norm*norm(cross_product(aVecs(:,3),aVecs(:,1))/cell_volume)+eps)
    n3 = ceiling(max_norm*norm(cross_product(aVecs(:,1),aVecs(:,2))/cell_volume)+eps)

    ! Count the number of R vectors in the sphere before allocating arrays
    num_Rs = 0
    do i = -n1, n1
       do j = -n2, n2
          do k = -n3, n3
             this_vector = i*aVecs(:,1) + j*aVecs(:,2) + k*aVecs(:,3)
             length = norm(this_vector)
             if(length > max_norm + eps) cycle ! Vector is too long
             num_Rs = num_Rs + 1 ! Count number of R vectors in sphere
          enddo
       enddo
    enddo
    allocate(Rvecs(3, num_Rs), Rlengths(num_Rs))

    ! Store the R vectors that lie within the sphere
    num_Rs = 0
    do i = -n1, n1
       do j = -n2, n2
          do k = -n3, n3
             this_vector = i*aVecs(:,1) + j*aVecs(:,2) + k*aVecs(:,3)
             length = norm(this_vector)
             if(length > max_norm + eps) cycle ! This vector is outside sphere
             num_Rs = num_Rs + 1
             Rvecs(:,num_Rs) = this_vector
             Rlengths(num_Rs) = length
          enddo
       enddo
    enddo

    ! Try all R vector triplets in the sphere and see which ones are valid
    ! rotations of the original basis vectors.
    !
    ! The length of all vectors must be preserved under a unitary
    ! transformation so skip any trial vectors that aren't the same
    ! length as the original. We also skip any set of vectors that
    ! have the right lengths but do not form a parallelpiped that has
    ! the same volume as the original set. Also, note that the we skip
    ! sets of vectors that contain the same vector more than once
    ! (i.e. the indices i, j, k must be unique).
    num_ops = 0
    do i = 1, num_Rs
       if(abs(Rlengths(i) - norm_avecs(1)) > eps) cycle
       do j = 1, num_Rs
          if(abs(Rlengths(j) - norm_avecs(2)) > eps) cycle
          if(j == i) cycle
          do k = 1, num_Rs
             if(abs(Rlengths(k) - norm_avecs(3)) > eps) cycle

             if(k == i .or. k == j) cycle
             if(abs(cell_volume - abs(volume(Rvecs(:,i),Rvecs(:,j),Rvecs(:,k)))) > eps) cycle

             ! Form the new set of "rotated" basis vectors
             new_vectors = reshape((/Rvecs(:,i),Rvecs(:,j),Rvecs(:,k)/),(/3,3/))
             ! If the transformation matrix that takes the original set to the new set is
             ! an orthogonal matrix then this rotation is a point symmetry of the lattice.
             rotation_matrix = matmul(new_vectors,inverse_aVecs)
             ! Check orthogonality of rotation matrix by [R][R]^T = [1]
             test_matrix = matmul(rotation_matrix,transpose(rotation_matrix))
             if(equal(test_matrix, Identity, eps, atol)) then ! Found valid rotation
                num_ops = num_ops + 1 ! Count number of rotations
                temp_op(:,:,num_ops) = rotation_matrix
                !write(10,'(i5,3i4)') num_ops, i, j, k
             endif
          enddo
       enddo
    enddo
    allocate(lattpg_op(3,3,num_ops))
    lattpg_op = temp_op(:,:,1:num_ops)

    deallocate(Rvecs, Rlengths)
  end subroutine get_lattice_pointGroup

  !!This routine generates the matrices for converting
  !!vectors from lattice coordinates to cartesion coordinates and vice
  !!versa
  !!<parameter name="aVecs" regular="true"></parameter>
  !!<parameter name="prim_to_cart" regular="true"></parameter>
  !!<parameter name="cart_to_prim" regular="true"></parameter>
  !!<parameter name="errflag" regular="true"></parameter>
  subroutine get_transformations(aVecs, prim_to_cart, cart_to_prim,errflag)
    real(dp) :: aVecs(3,3), cart_to_prim(3,3), prim_to_cart(3,3)
    logical, optional :: errflag

    prim_to_cart = aVecs
    if (present(errflag))then
       call matrix_inverse(prim_to_cart, cart_to_prim,errflag)
    else;call matrix_inverse(prim_to_cart, cart_to_prim)
    endif
  end subroutine get_transformations

  !!This subroutine translates a point back into the unit cell
  !! In lattice coordinates, the coefficients of the point must all be
  !! less than one and at least zero.
  !!<parameter name="v" regular="true"></parameter>
  !!<parameter name="cart_to_latt" regular="true"></parameter>
  !!<parameter name="latt_to_cart" regular="true"></parameter>
  !!<parameter name="eps" regular="true"></parameter>
  subroutine bring_into_cell(v, cart_to_latt, latt_to_cart, eps)
    real(dp), intent(inout) ::  v(3)
    real(dp), intent(in)    ::  cart_to_latt(3,3), latt_to_cart(3,3), eps
    ! Put the representation of the point into lattice coordinates
    v = matmul(cart_to_latt, v)
    v = MOD(v, 1.0_dp)
    ! If a component >= 1, translate by subtracting a lattice vector
    ! If a component < 0, translate by adding a lattice vector
    ! do while(any(v >= 1.0_dp - eps) .or. any(v < 0.0_dp - eps)) ih
    if (any(v >= 1.0_dp - eps) .or. any(v < 0.0_dp - eps)) then
       ! c = c +1
       v = merge(v, v - 1.0_dp, v <  1.0_dp - eps)
       v = merge(v, v + 1.0_dp, v >= 0.0_dp - eps)
    end if

    ! Put the point back into cartesion coordinate representation
    v = matmul(latt_to_cart, v)
  end subroutine bring_into_cell

  !!Checks to see if a mapping exists between the vector v and
  !! the position of any of the atoms of type "this_type".
  !! If a mapping exists, then the logical "mapped" is
  !! returned .true., otherwise .false.
  !!<parameter name="v" regular="true">Position to check mapping
  !!for.</parameter>
  !!<parameter name="this_type" regular="true">Type of atom that is
  !!being checked.</parameter>
  !!<parameter name="atom_pos" regular="true" >Positions of basis
  !!atoms.</parameter>
  !!<parameter name="atomType" regular="true">Types of atoms in the
  !!basis.</parameter>
  !!<parameter name="mapped" regular="true">Set to .true. if a mapping
  !!exists.</parameter>
  !!<parameter name="eps" regular="true">Epsilon for checking
  !!equivalence.</parameter>
  subroutine does_mapping_exist(v, this_type, atom_pos, atomType, mapped, eps)
    real(dp), intent(in) :: v(3)
    integer, intent(in) :: this_type
    real(dp), intent(in) :: atom_pos(:,:)
    integer, intent(in) :: atomType(:)
    logical, intent(out) :: mapped
    real(dp), intent(in) :: eps
    real(dp) :: atol ! An absolute tolerance for the "equal" subroutine
    integer i   ! Loop over atoms
    real(dp) :: this_position(3) ! Position of atom to be checked

    atol = 5E-4_dp
    mapped = .false.
    do i = 1, size(atomType)
       if(atomType(i) == this_type) then
          ! if the coordinates are the same,
          ! their difference will be zero for every component
          this_position = atom_pos(:,i)
          if(equal(v, this_position, eps,atol)) &
          then; mapped = .true.; exit; endif
       endif
    enddo
  end subroutine does_mapping_exist

  !!This subroutine checks the space group.
  !!<parameter name="SGop"></parameter>
  !!<parameter name="eps_" regular="true"></parameter>
  subroutine check_spaceGroup(SGop, eps_)
    real(dp), pointer :: SGop(:,:,:)
    real(dp), optional, intent(in):: eps_

    integer i, j, k, Nops
    real(dp) eps
    real(dp) :: testop(3,3)
    logical exists
    real(dp) :: atol  ! An absolute tolerance for the "equal" subroutine

    if(.not. present(eps_)) then; eps = 1e-10_dp; else; eps = eps_;endif
    atol = 5E-4_dp
    open(11, file="sym_check.out", status="unknown")
    ! Are the operations unique? (Necessary but *insufficient* condition)
    Nops = size(SGop,3)
    do i = 1,Nops
       do j = i+1, Nops
          if (equal(SGop(:,:,i),SGop(:,:,j),eps,atol)) then
             write(11,*) "Error: SG Operations that were found are not unique"
             stop
          endif
       enddo
    enddo
    write(11,*) "The SG operations are unique"

    ! Do the operations form a group? (Again, this is a necessary but not
    ! sufficient condition to double check)
    write(11,'(3x)',advance="no")
    do i = 1, Nops; write(11,'(i3)',advance="no") i; enddo; write(11,*)
    do i = 1, Nops
       write(11,'(i3)',advance="no") i
       do k = 1, i-1; write(11,'(3x)',advance='no');enddo
       do j = i, Nops
          exists = .false.
          ! Is the product of SG_i x SG_j in the set?
          testop = matmul(SGop(:,:,i),SGop(:,:,j))
          do k = 1, Nops
             if (equal(testop,SGop(:,:,k),eps,atol)) then
                exists = .true.
                write(11,'(i3)',advance="no") k
                exit
             endif
          enddo
          if (.not. exists) then
             write(11,*) "The set of SG ops doesn't form a group"
             stop
          endif
       enddo
       write(11,*)
    enddo
    write(11,*) "The SG ops form a group"
    close(11)

  end subroutine check_spaceGroup

  !!This subroutine finds site equivalencies. Only used in UNCLE (2/6/2020)
  !!<parameter name="pBas"></parameter>
  !!<parameter name="siteLabel"></parameter>
  !!<parameter name="pLV" regular="true"></parameter>
  !!<parameter name="BasEq"></parameter>
  !!<parameter name="nSites" regular="true"></parameter>
  !!<parameter name="eps_" regular="true"></parameter>
  SUBROUTINE find_site_equivalencies(pBas, siteLabel, pLV, BasEq, nSites, eps_)
    real(dp), allocatable :: pBas(:,:)

    real(dp), intent(in) :: pLV(3,3)
    integer, pointer :: BasEq(:), siteLabel(:)
    real(dp), intent(in), optional :: eps_
    integer, intent(out) :: nSites

    real(dp), allocatable :: tBas(:,:)
    real(dp) :: invLV(3,3), v(3)
    real(dp), allocatable :: sg_rot(:,:,:), sg_fract(:,:)
    integer :: iOps, iBas, jBas
    integer :: nOps, nBas
    logical :: equivalent
    real(dp) :: eps
    real(dp) :: atol  ! An absolute tolerance for the "equal" subroutine

    if(.not. present(eps_)) then
       eps = 1e-10_dp
    else
       eps =  eps_
    endif
    atol = 1e-3_dp

    allocate(tBas(size(pBas,1), size(pBas,2)))
    tBas = pBas

    call matrix_inverse(pLV, invLV)
    nBas = size(tBas,2)
    allocate(BasEq(nBas))
    BasEq = 0
    BasEq(1) = 1  !independent of basis size, the first always corresponds to itself
    nSites = 1

    call get_spaceGroup(pLV, sitelabel, tBas,  sg_rot, sg_fract, .false.,eps)
    nOps = size(sg_rot,3)

    do iBas=2, nBas
       call bring_into_cell(tBas(:,iBas),invLV,pLV,eps)
       equivalent = .false.
       do jBas=1, iBas-1
          do iOps = 1,nOps
             v = matmul(sg_rot(:,:,iOps),tBas(:,jBas))+sg_fract(:,iOps)
             call bring_into_cell(v,invLV,pLV,eps)
             if (equal(v,tBas(:,iBas),eps,atol)) then
                equivalent = .true.
                exit
             endif
          end do
          if (equivalent) then
             BasEq(iBas) = BasEq(jBas)
             exit
          endif
       end do
       if (.not.equivalent) then
          nSites = nSites + 1
          BasEq(iBas) = nSites
       endif
    end do
    if (any(BasEq==0)) stop "There was a bug in find_site_equivalencies"
  ENDSUBROUTINE find_site_equivalencies

  !!Takes point group operators in Cartesian coordinates and returns them
  !!in lattice coordinates, which are integer matrices instead of floating point matrices.
  !!
  !!<parameter name="pgOps" regular="true">Point group operators of Cartesian basis.</parameter>
  !!<parameter name="A" regular="true">Basis vectors (columns) of the lattice.</parameter>
  !!<parameter name="latticePtGrp_Ops" regular="true">Returned point group operators in direct
  !!coordinates (integer entries).</parameter>
  !!<parameter name="eps_" regular="true">Finite precision parameter</parameter>
  subroutine put_pointGroup_in_latticeCoords(pgOps, A, latticePtGrp_Ops, eps_)
    real(dp), intent(in) :: pgOps(:,:,:), A(3,3)
    integer,  pointer    :: latticePtGrp_Ops(:,:,:)
    real(dp), optional   :: eps_

    real(dp) :: eps ! Local finite precision parameter
    real(dp) :: InvA(3,3) ! Inverse of aBas matrix
    integer  :: iOp ! loop counter over point group operators
    logical  :: err ! Error flag
    integer  :: i ! general loop counter
    real(dp) :: tempMat(3,3) ! Temporary matrix

    if(.not. present(eps_)) then
       eps = 1e-10_dp
    else
       eps =  eps_
    endif

    allocate(latticePtGrp_Ops(3,3,size(pgOps,3)))
    call matrix_inverse(A,InvA,err,eps)
    if (err) then
       write(*,*) "ERROR: (put_pointGroup_in_latticeCoords in symmetry.f90)"
       do i=1,3
          write(*,'("Basis vector #: ",i2,3x,3(f7.3))') i,A(:,i)
       enddo
       stop "Basis vectors of lattice were not linearly independent."
    endif

    do iOp = 1, size(pgOps,1)
       tempMat = matmul(InvA,(matmul(pgOps(:,:,iOp),A)))
       if (any(tempMat - nint(tempMat) > eps)) then
           write(*,*) "ERROR: (put_pointGroup_in_latticeCoords in symmetry.f90)"
           stop "Conversion to lattice coordinates failed to yield integer matrix."
        endif
        latticePtGrp_Ops(:,:,iOp) = nint(tempMat)
    enddo
  endsubroutine put_pointGroup_in_latticeCoords

END MODULE symmetry


      subroutine runY_00lli(k,l,i1,Xtwiddle,Gtwiddle,Shat5,N0)
      implicit none
C---  Expression for Eq. 5.60b
C---  Calculates D00lli, requires D00lll
C---  Small terms of order Xtwiddle(0,k)*Diiii,Xtwiddle(0,0)*Diiiii
C---  Denominator Gtwiddle(k,l)
      include 'pvDnames.f'
      include 'pvDv.f'
      include 'Darraydef.f'
      include 'Darrays.f'
      integer ep,N0,k,l,i1,np
      parameter(np=3)
      double precision Xtwiddle(0:np,0:np),Gtwiddle(np,np)
      double complex Shat5(np,z4max,-2:0)

      if ((i1 .eq. l) .or. (i1 .eq. 0)) then
      return
      endif

      do ep=-2,0

      Dv(dzziii(z3(l,l,i1))+N0,ep)=
     . (-2*Gtwiddle(k,i1)*Dv(dzziii(z3(l,l,l))+N0,ep)
     . +Gtwiddle(k,1)*Shat5(1,z4(l,l,l,i1),ep)
     . +Gtwiddle(k,2)*Shat5(2,z4(l,l,l,i1),ep)
     . +Gtwiddle(k,3)*Shat5(3,z4(l,l,l,i1),ep)
     . +Xtwiddle(0,k)*Dv(diiii(z4(l,l,l,i1))+N0,ep)
     . -Xtwiddle(0,0)*Dv(diiiii(z5(k,l,l,l,i1))+N0,ep))
     . /(6*Gtwiddle(k,l))

      enddo

      return
      end


C FISHPAK9 FROM PORTLIB                                  12/30/83
      SUBROUTINE HSTCYL (A,B,M,MBDCND,BDA,BDB,C,D,N,NBDCND,BDC,BDD,
     1                   ELMBDA,F,IDIMF,PERTRB,IERROR,W)
C
C
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C     *                                                               *
C     *                        F I S H P A K                          *
C     *                                                               *
C     *                                                               *
C     *     A PACKAGE OF FORTRAN SUBPROGRAMS FOR THE SOLUTION OF      *
C     *                                                               *
C     *      SEPARABLE ELLIPTIC PARTIAL DIFFERENTIAL EQUATIONS        *
C     *                                                               *
C     *                  (VERSION 3.1 , OCTOBER 1980)                  *
C     *                                                               *
C     *                             BY                                *
C     *                                                               *
C     *        JOHN ADAMS, PAUL SWARZTRAUBER AND ROLAND SWEET         *
C     *                                                               *
C     *                             OF                                *
C     *                                                               *
C     *         THE NATIONAL CENTER FOR ATMOSPHERIC RESEARCH          *
C     *                                                               *
C     *                BOULDER, COLORADO  (80307)  U.S.A.             *
C     *                                                               *
C     *                   WHICH IS SPONSORED BY                       *
C     *                                                               *
C     *              THE NATIONAL SCIENCE FOUNDATION                  *
C     *                                                               *
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C
C
C     * * * * * * * * *  PURPOSE    * * * * * * * * * * * * * * * * * *
C
C      HSTCYL SOLVES THE STANDARD FIVE-POINT FINITE DIFFERENCE
C      APPROXIMATION ON A STAGGERED GRID TO THE MODIFIED HELMHOLTZ
C      EQUATION IN CYLINDRICAL COORDINATES
C
C          (1/R)(D/DR)(R(DU/DR)) + (D/DZ)(DU/DZ)
C
C                      + LAMBDA*(1/R**2)*U = F(R,Z)
C
C      THIS TWO-DIMENSIONAL MODIFIED HELMHOLTZ EQUATION RESULTS
C      FROM THE FOURIER TRANSFORM OF A THREE-DIMENSIONAL POISSON
C      EQUATION.
C
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C
C     * * * * * * * *    PARAMETER DESCRIPTION     * * * * * * * * * *
C
C             * * * * * *   ON INPUT    * * * * * *
C
C    A,B
C      THE RANGE OF R, I.E. A .LE. R .LE. B.  A MUST BE LESS THAN B AND
C      A MUST BE NON-NEGATIVE.
C
C    M
C      THE NUMBER OF GRID POINTS IN THE INTERVAL (A,B).  THE GRID POINTS
C      IN THE R-DIRECTION ARE GIVEN BY R(I) = A + (I-0.5)DR FOR
C      I=1,2,...,M WHERE DR =(B-A)/M.  M MUST BE GREATER THAN 2.
C
C    MBDCND
C      INDICATES THE TYPE OF BOUNDARY CONDITIONS AT R = A AND R = B.
C
C      = 1  IF THE SOLUTION IS SPECIFIED AT R = A (SEE NOTE BELOW) AND
C           R = B.
C
C      = 2  IF THE SOLUTION IS SPECIFIED AT R = A (SEE NOTE BELOW) AND
C           THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS
C           SPECIFIED AT R = B.
C
C      = 3  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS
C           SPECIFIED AT R = A (SEE NOTE BELOW) AND R = B.
C
C      = 4  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS
C           SPECIFIED AT R = A (SEE NOTE BELOW) AND THE SOLUTION IS
C           SPECIFIED AT R = B.
C
C      = 5  IF THE SOLUTION IS UNSPECIFIED AT R = A = 0 AND THE SOLUTION
C           IS SPECIFIED AT R = B.
C
C      = 6  IF THE SOLUTION IS UNSPECIFIED AT R = A = 0 AND THE
C           DERIVATIVE OF THE SOLUTION WITH RESPECT TO R IS SPECIFIED AT
C           R = B.
C
C      NOTE:  IF A = 0, DO NOT USE MBDCND = 1,2,3, OR 4, BUT INSTEAD
C             USE MBDCND = 5 OR 6.  THE RESULTING APPROXIMATION GIVES
C             THE ONLY MEANINGFUL BOUNDARY CONDITION, I.E. DU/DR = 0.
C             (SEE D. GREENSPAN, 'INTRODUCTORY NUMERICAL ANALYSIS OF
C             ELLIPTIC BOUNDARY VALUE PROBLEMS,' HARPER AND ROW, 1965,
C             CHAPTER 5.)
C
C    BDA
C      A ONE-DIMENSIONAL ARRAY OF LENGTH N THAT SPECIFIES THE BOUNDARY
C      VALUES (IF ANY) OF THE SOLUTION AT R = A.  WHEN MBDCND = 1 OR 2,
C
C               BDA(J) = U(A,Z(J)) ,          J=1,2,...,N.
C
C      WHEN MBDCND = 3 OR 4,
C
C               BDA(J) = (D/DR)U(A,Z(J)) ,    J=1,2,...,N.
C
C      WHEN MBDCND = 5 OR 6, BDA IS A DUMMY VARIABLE.
C
C    BDB
C      A ONE-DIMENSIONAL ARRAY OF LENGTH N THAT SPECIFIES THE BOUNDARY
C      VALUES OF THE SOLUTION AT R = B.  WHEN MBDCND = 1,4, OR 5,
C
C               BDB(J) = U(B,Z(J)) ,          J=1,2,...,N.
C
C      WHEN MBDCND = 2,3, OR 6,
C
C               BDB(J) = (D/DR)U(B,Z(J)) ,    J=1,2,...,N.
C
C    C,D
C      THE RANGE OF Z, I.E. C .LE. Z .LE. D.  C MUST BE LESS
C      THAN D.
C
C    N
C      THE NUMBER OF UNKNOWNS IN THE INTERVAL (C,D).  THE UNKNOWNS IN
C      THE Z-DIRECTION ARE GIVEN BY Z(J) = C + (J-0.5)DZ,
C      J=1,2,...,N, WHERE DZ = (D-C)/N.  N MUST BE GREATER THAN 2.
C
C    NBDCND
C      INDICATES THE TYPE OF BOUNDARY CONDITIONS AT Z = C
C      AND Z = D.
C
C      = 0  IF THE SOLUTION IS PERIODIC IN Z, I.E.
C           U(I,J) = U(I,N+J).
C
C      = 1  IF THE SOLUTION IS SPECIFIED AT Z = C AND Z = D.
C
C      = 2  IF THE SOLUTION IS SPECIFIED AT Z = C AND THE DERIVATIVE
C           OF THE SOLUTION WITH RESPECT TO Z IS SPECIFIED AT
C           Z = D.
C
C      = 3  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO Z IS
C           SPECIFIED AT Z = C AND Z = D.
C
C      = 4  IF THE DERIVATIVE OF THE SOLUTION WITH RESPECT TO Z IS
C           SPECIFIED AT Z = C AND THE SOLUTION IS SPECIFIED AT
C           Z = D.
C
C    BDC
C      A ONE DIMENSIONAL ARRAY OF LENGTH M THAT SPECIFIES THE BOUNDARY
C      VALUES OF THE SOLUTION AT Z = C.   WHEN NBDCND = 1 OR 2,
C
C               BDC(I) = U(R(I),C) ,              I=1,2,...,M.
C
C      WHEN NBDCND = 3 OR 4,
C
C               BDC(I) = (D/DZ)U(R(I),C),         I=1,2,...,M.
C
C      WHEN NBDCND = 0, BDC IS A DUMMY VARIABLE.
C
C    BDD
C      A ONE-DIMENSIONAL ARRAY OF LENGTH M THAT SPECIFIES THE BOUNDARY
C      VALUES OF THE SOLUTION AT Z = D.  WHEN NBDCND = 1 OR 4,
C
C               BDD(I) = U(R(I),D) ,              I=1,2,...,M.
C
C      WHEN NBDCND = 2 OR 3,
C
C               BDD(I) = (D/DZ)U(R(I),D) ,        I=1,2,...,M.
C
C      WHEN NBDCND = 0, BDD IS A DUMMY VARIABLE.
C
C    ELMBDA
C      THE CONSTANT LAMBDA IN THE MODIFIED HELMHOLTZ EQUATION.  IF
C      LAMBDA IS GREATER THAN 0, A SOLUTION MAY NOT EXIST.  HOWEVER,
C      HSTCYL WILL ATTEMPT TO FIND A SOLUTION.  LAMBDA MUST BE ZERO
C      WHEN MBDCND = 5 OR 6.
C
C    F
C      A TWO-DIMENSIONAL ARRAY THAT SPECIFIES THE VALUES OF THE RIGHT
C      SIDE OF THE MODIFIED HELMHOLTZ EQUATION.  FOR I=1,2,...,M
C      AND J=1,2,...,N
C
C               F(I,J) = F(R(I),Z(J)) .
C
C      F MUST BE DIMENSIONED AT LEAST M X N.
C
C    IDIMF
C      THE ROW (OR FIRST) DIMENSION OF THE ARRAY F AS IT APPEARS IN THE
C      PROGRAM CALLING HSTCYL.  THIS PARAMETER IS USED TO SPECIFY THE
C      VARIABLE DIMENSION OF F.  IDIMF MUST BE AT LEAST M.
C
C    W
C      A ONE-DIMENSIONAL ARRAY THAT MUST BE PROVIDED BY THE USER FOR
C      WORK SPACE.  W MAY REQUIRE UP TO 13M + 4N + M*INT(LOG2(N))
C      LOCATIONS.  THE ACTUAL NUMBER OF LOCATIONS USED IS COMPUTED BY
C      HSTCYL AND IS RETURNED IN THE LOCATION W(1).
C
C
C             * * * * * *   ON OUTPUT   * * * * * *
C
C    F
C      CONTAINS THE SOLUTION U(I,J) OF THE FINITE DIFFERENCE
C      APPROXIMATION FOR THE GRID POINT (R(I),Z(J)) FOR
C      I=1,2,...,M, J=1,2,...,N.
C
C    PERTRB
C      IF A COMBINATION OF PERIODIC, DERIVATIVE, OR UNSPECIFIED
C      BOUNDARY CONDITIONS IS SPECIFIED FOR A POISSON EQUATION
C      (LAMBDA = 0), A SOLUTION MAY NOT EXIST.  PERTRB IS A CON-
C      STANT, CALCULATED AND SUBTRACTED FROM F, WHICH ENSURES
C      THAT A SOLUTION EXISTS.  HSTCYL THEN COMPUTES THIS
C      SOLUTION, WHICH IS A LEAST SQUARES SOLUTION TO THE
C      ORIGINAL APPROXIMATION.  THIS SOLUTION PLUS ANY CONSTANT IS ALSO
C      A SOLUTION; HENCE, THE SOLUTION IS NOT UNIQUE.  THE VALUE OF
C      PERTRB SHOULD BE SMALL COMPARED TO THE RIGHT SIDE F.
C      OTHERWISE, A SOLUTION IS OBTAINED TO AN ESSENTIALLY DIFFERENT
C      PROBLEM.  THIS COMPARISON SHOULD ALWAYS BE MADE TO INSURE THAT
C      A MEANINGFUL SOLUTION HAS BEEN OBTAINED.
C
C    IERROR
C      AN ERROR FLAG THAT INDICATES INVALID INPUT PARAMETERS.
C      EXCEPT TO NUMBERS 0 AND 11, A SOLUTION IS NOT ATTEMPTED.
C
C      =  0  NO ERROR
C
C      =  1  A .LT. 0
C
C      =  2  A .GE. B
C
C      =  3  MBDCND .LT. 1 OR MBDCND .GT. 6
C
C      =  4  C .GE. D
C
C      =  5  N .LE. 2
C
C      =  6  NBDCND .LT. 0 OR NBDCND .GT. 4
C
C      =  7  A = 0 AND MBDCND = 1,2,3, OR 4
C
C      =  8  A .GT. 0 AND MBDCND .GE. 5
C
C      =  9  M .LE. 2
C
C      = 10  IDIMF .LT. M
C
C      = 11  LAMBDA .GT. 0
C
C      = 12  A=0, MBDCND .GE. 5, ELMBDA .NE. 0
C
C      SINCE THIS IS THE ONLY MEANS OF INDICATING A POSSIBLY
C      INCORRECT CALL TO HSTCYL, THE USER SHOULD TEST IERROR AFTER
C      THE CALL.
C
C    W
C      W(1) CONTAINS THE REQUIRED LENGTH OF W.
C
C
C     * * * * * * *   PROGRAM SPECIFICATIONS    * * * * * * * * * * * *
C
C     DIMENSION OF   BDA(N),BDB(N),BDC(M),BDD(M),F(IDIMF,N),
C     ARGUMENTS      W(SEE ARGUMENT LIST)
C
C     LATEST         JUNE 1, 1977
C     REVISION
C
C     SUBPROGRAMS    HSTCYL,POISTG,POSTG2,GENBUN,POISD2,POISN2,POISP2,
C     REQUIRED       COSGEN,MERGE,TRIX,TRI3,PIMACH
C
C     SPECIAL        NONE
C     CONDITIONS
C
C     COMMON         NONE
C     BLOCKS
C
C     I/O            NONE
C
C     PRECISION      SINGLE
C
C     SPECIALIST     ROLAND SWEET
C
C     LANGUAGE       FORTRAN
C
C     HISTORY        WRITTEN BY ROLAND SWEET AT NCAR IN MARCH, 1977
C
C     ALGORITHM      THIS SUBROUTINE DEFINES THE FINITE-DIFFERENCE
C                    EQUATIONS, INCORPORATES BOUNDARY DATA, ADJUSTS THE
C                    RIGHT SIDE WHEN THE SYSTEM IS SINGULAR AND CALLS
C                    EITHER POISTG OR GENBUN WHICH SOLVES THE LINEAR
C                    SYSTEM OF EQUATIONS.
C
C     SPACE          8228(DECIMAL) = 20044(OCTAL) LOCATIONS ON THE
C     REQUIRED       NCAR CONTROL DATA 7600
C
C     TIMING AND        THE EXECUTION TIME T ON THE NCAR CONTROL DATA
C     ACCURACY       7600 FOR SUBROUTINE HSTCYL IS ROUGHLY PROPORTIONAL
C                    TO M*N*LOG2(N).  SOME TYPICAL VALUES ARE LISTED IN
C                    THE TABLE BELOW.
C                       THE SOLUTION PROCESS EMPLOYED RESULTS IN A LOSS
C                    OF NO MORE THAN FOUR SIGNIFICANT DIGITS FOR N AND M
C                    AS LARGE AS 64.  MORE DETAILED INFORMATION ABOUT
C                    ACCURACY CAN BE FOUND IN THE DOCUMENTATION FOR
C                    SUBROUTINE POISTG WHICH IS THE ROUTINE THAT
C                    ACTUALLY SOLVES THE FINITE DIFFERENCE EQUATIONS.
C
C
C                       M(=N)    MBDCND    NBDCND    T(MSECS)
C                       -----    ------    ------    --------
C
C                        32       1-6       1-4         56
C                        64       1-6       1-4        230
C
C     PORTABILITY    AMERICAN NATIONAL STANDARDS INSTITUTE FORTRAN.
C                    ALL MACHINE DEPENDENT CONSTANTS ARE LOCATED IN THE
C                    FUNCTION PIMACH.
C
C     REQUIRED       COS
C     RESIDENT
C     ROUTINES
C
C     REFERENCE      SCHUMANN, U. AND R. SWEET,"A DIRECT METHOD FOR
C                    THE SOLUTION OF POISSON"S EQUATION WITH NEUMANN
C                    BOUNDARY CONDITIONS ON A STAGGERED GRID OF
C                    ARBITRARY SIZE," J. COMP. PHYS. 20(1976),
C                    PP. 171-182.
C
C     * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
C
      DIMENSION       F(IDIMF,1) ,BDA(1)     ,BDB(1)     ,BDC(1)     ,
     1                BDD(1)     ,W(1)
      IERROR = 0
      IF (A .LT. 0.) IERROR = 1
      IF (A .GE. B) IERROR = 2
      IF (MBDCND.LE.0 .OR. MBDCND.GE.7) IERROR = 3
      IF (C .GE. D) IERROR = 4
      IF (N .LE. 2) IERROR = 5
      IF (NBDCND.LT.0 .OR. NBDCND.GE.5) IERROR = 6
      IF (A.EQ.0. .AND. MBDCND.NE.5 .AND. MBDCND.NE.6) IERROR = 7
      IF (A.GT.0. .AND. MBDCND.GE.5) IERROR = 8
      IF (IDIMF .LT. M) IERROR = 10
      IF (M .LE. 2) IERROR = 9
      IF (A.EQ.0. .AND. MBDCND.GE.5 .AND. ELMBDA.NE.0.) IERROR = 12
      IF (IERROR .NE. 0) RETURN
      DELTAR = (B-A)/FLOAT(M)
      DLRSQ = DELTAR**2
      DELTHT = (D-C)/FLOAT(N)
      DLTHSQ = DELTHT**2
      NP = NBDCND+1
C
C     DEFINE A,B,C COEFFICIENTS IN W-ARRAY.
C
      IWB = M
      IWC = IWB+M
      IWR = IWC+M
      DO 101 I=1,M
         J = IWR+I
         W(J) = A+(FLOAT(I)-0.5)*DELTAR
         W(I) = (A+FLOAT(I-1)*DELTAR)/(DLRSQ*W(J))
         K = IWC+I
         W(K) = (A+FLOAT(I)*DELTAR)/(DLRSQ*W(J))
         K = IWB+I
         W(K) = ELMBDA/W(J)**2-2./DLRSQ
  101 CONTINUE
C
C     ENTER BOUNDARY DATA FOR R-BOUNDARIES.
C
      GO TO (102,102,104,104,106,106),MBDCND
  102 A1 = 2.*W(1)
      W(IWB+1) = W(IWB+1)-W(1)
      DO 103 J=1,N
         F(1,J) = F(1,J)-A1*BDA(J)
  103 CONTINUE
      GO TO 106
  104 A1 = DELTAR*W(1)
      W(IWB+1) = W(IWB+1)+W(1)
      DO 105 J=1,N
         F(1,J) = F(1,J)+A1*BDA(J)
  105 CONTINUE
  106 CONTINUE
      GO TO (107,109,109,107,107,109),MBDCND
  107 W(IWC) = W(IWC)-W(IWR)
      A1 = 2.*W(IWR)
      DO 108 J=1,N
         F(M,J) = F(M,J)-A1*BDB(J)
  108 CONTINUE
      GO TO 111
  109 W(IWC) = W(IWC)+W(IWR)
      A1 = DELTAR*W(IWR)
      DO 110 J=1,N
         F(M,J) = F(M,J)-A1*BDB(J)
  110 CONTINUE
C
C     ENTER BOUNDARY DATA FOR THETA-BOUNDARIES.
C
  111 A1 = 2./DLTHSQ
      GO TO (121,112,112,114,114),NP
  112 DO 113 I=1,M
         F(I,1) = F(I,1)-A1*BDC(I)
  113 CONTINUE
      GO TO 116
  114 A1 = 1./DELTHT
      DO 115 I=1,M
         F(I,1) = F(I,1)+A1*BDC(I)
  115 CONTINUE
  116 A1 = 2./DLTHSQ
      GO TO (121,117,119,119,117),NP
  117 DO 118 I=1,M
         F(I,N) = F(I,N)-A1*BDD(I)
  118 CONTINUE
      GO TO 121
  119 A1 = 1./DELTHT
      DO 120 I=1,M
         F(I,N) = F(I,N)-A1*BDD(I)
  120 CONTINUE
  121 CONTINUE
C
C     ADJUST RIGHT SIDE OF SINGULAR PROBLEMS TO INSURE EXISTENCE OF A
C     SOLUTION.
C
      PERTRB = 0.
      IF (ELMBDA) 130,123,122
  122 IERROR = 11
      GO TO 130
  123 GO TO (130,130,124,130,130,124),MBDCND
  124 GO TO (125,130,130,125,130),NP
  125 CONTINUE
      DO 127 I=1,M
         A1 = 0.
         DO 126 J=1,N
            A1 = A1+F(I,J)
  126    CONTINUE
         J = IWR+I
         PERTRB = PERTRB+A1*W(J)
  127 CONTINUE
      PERTRB = PERTRB/(FLOAT(M*N)*0.5*(A+B))
      DO 129 I=1,M
         DO 128 J=1,N
            F(I,J) = F(I,J)-PERTRB
  128    CONTINUE
  129 CONTINUE
  130 CONTINUE
C
C     MULTIPLY I-TH EQUATION THROUGH BY  DELTHT**2
C
      DO 132 I=1,M
         W(I) = W(I)*DLTHSQ
         J = IWC+I
         W(J) = W(J)*DLTHSQ
         J = IWB+I
         W(J) = W(J)*DLTHSQ
         DO 131 J=1,N
            F(I,J) = F(I,J)*DLTHSQ
  131    CONTINUE
  132 CONTINUE
      LP = NBDCND
      W(1) = 0.
      W(IWR) = 0.
C
C     CALL GENBUN TO SOLVE THE SYSTEM OF EQUATIONS.
C
      IF (NBDCND .EQ. 0) GO TO 133
      CALL POISTG (LP,N,1,M,W,W(IWB+1),W(IWC+1),IDIMF,F,IERR1,W(IWR+1))
      GO TO 134
  133 CALL GENBUN (LP,N,1,M,W,W(IWB+1),W(IWC+1),IDIMF,F,IERR1,W(IWR+1))
  134 CONTINUE
      W(1) = W(IWR+1)+3.*FLOAT(M)
      RETURN
      END


C
      SUBROUTINE YMODDISP(XX,W,PARAM)
*
*  Module number:
*
*  Module name: YMODDISP
*
*  ------------------------------------------------------------------------
*  Part of "poa_calfos" (STECF/IPMG)
*  ------------------------------------------------------------------------
*
*  Keyphrase:
*  ----------
*       Modify the dispersion solution.
*
*  Description:
*  ------------
*  This is the final FOS dispersion model including S distortion
*
*  Optical path is:   Grating --> Photocathode --> Diodes
*
*  Between Grating and Photocathode: projection lamdba --> z
*  For gratings: lambda = f(z); z = g(lambda);
*               a: gra.const.= m/s; b = grating angle
*               c: half configuration angle
*               d: offset z0; e: focal length
*
*  Inverse relation:
*      z = d + e * [ b+c + arcsin (s/m * lambda - sin(b-c))]
*
*  Between Photocathode and Diodes: projection z --> x
*  For electro-magneto-optic in good approximation
*                      x = o + pz + q*tan(r+sz)
*
*  Solving the double projection by first finding z = h(x)
*  through iteration, then  lambda = f(z) ; need dble precision !!
*
*  From forward relation  x --> z ; pz + q*tan(r+sz) = x-o
*                         z + q/p*tan(r+sz) = (x-o)/p
*     since |q/p*tan(r+sz)| << |z|  --> determine z by iteration
*
*  then:    lambda = m/s [sin(b-c) + sin((z-d)/e - (b+c))]
*
*
*
*  FORTRAN name: ymoddisp.for
*
*  Keywords of accessed files and tables:
*  --------------------------------------
*
*                 I
*
*  Subroutines Called:
*  -------------------
*
*  History:
*  --------
*  Version      Date        Author          Description
*       1.1   Dec 20,1987   M. Rosa      Designed and coded
*-----------------------------------------------------------------------------

      IMPLICIT NONE
      REAL   W
      DOUBLE PRECISION PARAM(10),S,T,U,WW,XX
      DOUBLE PRECISION PI,Z,Z1,Z2,ZL,F,FL,RT,DZ,XP,SWAP
      INTEGER ITER

C      INTEGER ISTAT
C      INTEGER STDOUT
C      PARAMETER (STDOUT = 1)
C      INTEGER STDERR
C      PARAMETER (STDERR = 2)
C      CHARACTER*80 CONTXT

C      COMMON DISPCOEF /NP,PARAM/

      DATA PI/3.1415926535897932384626433D0/
      DATA Z1,Z2/-100.0D0,600.0D0/

C
c For determinataion of dispersion parameters:
C Dependent: observed: y1 (diode position [0:512])
C Independent:        x  (lambda)
C
C Dispersion relation for FOS:
C
C Inverse:
C Full            y-y0 = f(lambda) ; y0 is optical axis on diode array
C Grating only    z-z0 = g(lambda) ; z0 is optical axis on cathode
C Digicon         y-y0 = d(z-z0)
C
C Forward:
C Full            lambda = f'(y-y0) ; y0 is optical axis on diode array
C Grating only    lambda = g'(z-z0) ; z0 is optical axis on cathode
C Digicon         z-z0   = d'(y-y0)
C
C (1)  Reproject from diode array (XX) to photocathode (Z)
C SIN for dispersion relations lambda = f(y); a=gra.const.=m/s
C c = half config angle; b = grating angle; d = y0; e=focal l.
C
C Inverse:  z = d + e * tan( b+c + arcsin (lambda/a - sin(b-c))]
C  + S-Distortion:
C   In extenso:   y-y0 = p(z-z0-r0) + q*abs(tan(s*(z-z0-r0))
C   Simplified:   y = o + p*z + q*abs(tan(s*(z-r0))
C     o = y0; p = linear scale factor; q = amplitude of distortion;
C     s = scale length of distortion; r0 = distance Digicon-optical axis;
C     r = r0+z0.
C
C Forward:
C  y --> z :  y = o + p*z + q*abs(tan(s*(z-r0))
C             z + q*abs(tan(s*(z-r0) = (y-o)/p
C             |q/p*tan(s*(z-r0)| << |z|  --> determine z by iteration
C
C  z --> lambda: z = d + e*tan(b+c + arcsin(lambda/a - sin(b-c)))
C                (z-d)/e = tan(b+c + arcsin(lambda/a - sin(b-c)))
C                arctan((z-d)/e) = b+c + arcsin(lambda/a - sin(b-c))
C                arcsin(lambda/a-sin(b-c))) = arctan((z-d)/e) -(b+c)
C                lambda = a*(sin(b-c) + sin(arctan((z-d)/e) -(b+c)))
C
C Obtain Z for XX using    z + (q*tan(r+sz) -(x-o))/p = 0
C We know that -52 < z < 578 (10% margin on 0-516 diode array)
C We know that |(q*tan(r+sz) -(x-o))/p| << |z|  (distortion small)
C Function is smooth, so we use secant method and
C precalculate a bracket using zg = (x-o)/p
C We want final epsilon to be < 1E-5 * z
C

      XP = XX-PARAM(6)
      Z1 = XP/PARAM(7)+50.
      Z2 = XP/PARAM(7)-50.
 10   FL = Z1+( PARAM(8)*DTAN(PARAM(10)*(Z1-PARAM(9)))
     1         -XP)/PARAM(7)
      F  = Z2+( PARAM(8)*DTAN(PARAM(10)*(Z2-PARAM(9)))
     1         -XP)/PARAM(7)
C Pick bound with smaller function value as most recent guess
      IF(DABS(FL).LT.DABS(F)) THEN
        RT = Z1
        ZL = Z2
        SWAP = FL
        FL = F
        F = SWAP
      ELSE
        ZL = Z1
        RT = Z2
      ENDIF
C Secant loop
      ITER=0
 20   DZ = (ZL-RT)*F/(F-FL)
      ITER=1
      ZL = RT
      FL = F
      RT = RT+DZ
      F = RT+( PARAM(8)*DTAN(PARAM(10)*(RT-PARAM(9)))
     1         -XP)/PARAM(7)
C      IF(DABS(DZ).GE.1.0D-10.OR.F.NE.0.0D0) GOTO 20
      IF(DABS(DZ).GE.1.0D-10) GOTO 20

C We have a good guess of z
      Z = RT
C
      S      = PARAM(2)-PARAM(3)             ! b-c
      U      = (Z-PARAM(4))/PARAM(5)         !
      T      = DATAND(U)-PARAM(2)-PARAM(3)   ! arctand((z-d)/e) -(b+c))

c  lambda = a*(sin(b-c) + sin(arctan((z-d)/e) -(b+c)))
      WW     = PARAM(1)*(DSIND(S)+DSIND(T))
      W      = SNGL(WW)
C Finished, got lambda from diode

C          WRITE(CONTXT, 800), XX,Z,WW
C 800      FORMAT('XX = ', F17.7, ', Z = ', F17.7, ', WW = ', F17.7)
C          CALL YMSPUT(CONTXT,STDOUT+STDERR,0,ISTAT)

      RETURN
      END


!================================================================
!================================================================
!================================================================
!================================================================
subroutine condinit(x,u,dx,nn)
  use amr_parameters
  use hydro_parameters
  use poisson_parameters, ONLY: gravity_params
  implicit none
  integer ::nn                            ! Number of cells
  real(dp)::dx                            ! Cell size
  real(dp),dimension(1:nvector,1:nvar)::u ! Conservative variables
  real(dp),dimension(1:nvector,1:ndim)::x ! Cell center position.
  !================================================================
  ! This routine generates initial conditions for RAMSES.
  ! Positions are in user units:
  ! x(i,1:3) are in [0,boxlen]**ndim.
  ! U is the conservative variable vector. Conventions are here:
  ! U(i,1): d, U(i,2:ndim+1): d.u,d.v,d.w and U(i,ndim+2): E.
  ! Q is the primitive variable vector. Conventions are here:
  ! Q(i,1): d, Q(i,2:ndim+1):u,v,w and Q(i,ndim+2): P.
  ! If nvar >= ndim+3, remaining variables are treated as passive
  ! scalars in the hydro solver.
  ! U(:,:) and Q(:,:) are in user units.
  !================================================================
  integer::ivar,i
  real(dp),dimension(1:nvector,1:nvar),save::q   ! Primitive variables
  real(dp)::rx,ry,rz,rr,xc,yc,zc,eps,rcut,lr,lrmax,lrcut
  real(dp)::pi,v200,spin,c,fgas,dmax,mmin,mmax
  real(dp)::fcut,vcirc,vrot,pnfw,dnfw,mnfw,fc

  ! Add here, if you wish, some user-defined initial conditions
  eps =2.*boxlen*0.5**nlevelmax
  xc  =boxlen/2.0
  yc  =boxlen/2.0
  zc  =boxlen/2.0
  rcut=boxlen/2.0*0.75

  v200=gravity_params(1)
  c   =gravity_params(2)
  spin=gravity_params(3)
  fgas=gravity_params(4)
  dmax=1d0/eps/(1d0+eps)**2
  pi  =acos(-1d0)

  mmin=4d0*pi*dmax/3d0*eps**3
  mmax=4d0*pi*(log(1d0+rcut)-rcut/(1d0+rcut))
  vcirc=sqrt(4.*pi*(log(1d0+c)-c/(1d0+c))/c)
  fc   =c*(0.5-0.5/(1d0+c)**2-log(1d0+c)/(1d0+c))/(log(1d0+c)-c/(1d0+c))**2

  lrmax=log(10*boxlen)
  lrcut=log(rcut)
  do i=1,nn
     rx=x(i,1)-xc
     ry=x(i,2)-yc
     rz=x(i,3)-zc
     rr=sqrt(rx**2+ry**2+rz**2)
     fcut=exp(-(rr/rcut)**50)
     dnfw=1d0/rr/(1d0+rr)**2
     dnfw=MIN(dmax,dnfw)
     dnfw=MAX(dnfw*fcut,1d-6)
     mnfw=(log(1d0+rr)-rr/(1d0+rr))/(log(1d0+c)-c/(1d0+c))
     vrot=2d0*sqrt(2d0/fc)*spin*vcirc*c*mnfw/rr*fcut
     lr=MIN(log(rr),lrcut)
     pnfw=romberg(lr,lrcut,1d-4)+mmax/rr*1d-6

     q(i,1)=fgas*dnfw
     q(i,2)=-vrot/rr*ry
     q(i,3)=+vrot/rr*rx
     q(i,4)=0.0
     q(i,5)=fgas*pnfw
     if(metal)q(i,6)=z_ave*0.02
  enddo

  ! Convert primitive to conservative variables
  ! density -> density
  u(1:nn,1)=q(1:nn,1)
  ! velocity -> momentum
  u(1:nn,2)=q(1:nn,1)*q(1:nn,2)
#if NDIM>1
  u(1:nn,3)=q(1:nn,1)*q(1:nn,3)
#endif
#if NDIM>2
  u(1:nn,4)=q(1:nn,1)*q(1:nn,4)
#endif
  ! kinetic energy
  u(1:nn,ndim+2)=0.0d0
  u(1:nn,ndim+2)=u(1:nn,ndim+2)+0.5*q(1:nn,1)*q(1:nn,2)**2
#if NDIM>1
  u(1:nn,ndim+2)=u(1:nn,ndim+2)+0.5*q(1:nn,1)*q(1:nn,3)**2
#endif
#if NDIM>2
  u(1:nn,ndim+2)=u(1:nn,ndim+2)+0.5*q(1:nn,1)*q(1:nn,4)**2
#endif
  ! pressure -> total fluid energy
  u(1:nn,ndim+2)=u(1:nn,ndim+2)+q(1:nn,ndim+2)/(gamma-1.0d0)
  ! passive scalars
  do ivar=ndim+3,nvar
     u(1:nn,ivar)=q(1:nn,1)*q(1:nn,ivar)
  end do

contains
  !ccccccccccccccccccccccccccccccccccccccccccccccccccccc
  function fffy(rint)
    implicit none
    !      Computes the integrand
    real(dp)::fffy
    real(dp)::rint,rrr,M,rho
    rrr=exp(rint)
    rho=1d0/rrr/(1d0+rrr)**2
    rho=MIN(rho,dmax)
    rho=MAX(rho*exp(-(rrr/rcut)**50),1d-6)
    M=4d0*pi*(log(1d0+rrr)-rrr/(1d0+rrr))
    fffy=M/rrr*rho
    return
  end function fffy
  !cccccccccccccccccccccccccccccccccccccccccccccccccccc
  function romberg(a,b,tol)
    implicit none
    real(dp)::romberg
    !
    !     Romberg returns the integral from a to b of f(x)dx using Romberg
    !     integration. The method converges provided that f(x) is continuous
    !     in (a,b). The function f must be double precision and must be
    !     declared external in the calling routine.
    !     tol indicates the desired relative accuracy in the integral.
    !
    integer::maxiter=16,maxj=5
    real(dp),dimension(100):: g
    real(dp)::a,b,tol,fourj
    real(dp)::h,error,gmax,g0,g1
    integer::nint,i,j,k,jmax

    h=0.5d0*(b-a)
    gmax=h*(fffy(a)+fffy(b))
    g(1)=gmax
    nint=1
    error=1.0d20
    i=0
10  i=i+1
    if(.not.  (i>maxiter.or.(i>5.and.abs(error)<tol)))then
       !	Calculate next trapezoidal rule approximation to integral.

       g0=0.0d0
       do k=1,nint
          g0=g0+fffy(a+(k+k-1)*h)
       end do
       g0=0.5d0*g(1)+h*g0
       h=0.5d0*h
       nint=nint+nint
       jmax=min(i,maxj)
       fourj=1.0d0

       do j=1,jmax
          ! Use Richardson extrapolation.
          fourj=4.0d0*fourj
          g1=g0+(g0-g(j))/(fourj-1.0d0)
          g(j)=g0
          g0=g1
       enddo
       if (abs(g0).gt.tol) then
          error=1.0d0-gmax/g0
       else
          error=gmax
       end if
       gmax=g0
       g(jmax+1)=g0
       go to 10
    end if
    romberg=g0

    if (i>maxiter.and.abs(error)>tol) &
         &    write(*,*) 'Romberg failed to converge; integral, error=', &
         &    romberg,error

    return
  end function romberg

end subroutine condinit


! libRQZ
! author: daan. camps@cs.kuleuven.be
! Description:
!   Swap poles in real block-Hessenberg pairs:
!     1x1 with 1x1, 1x1 with 2x2, and 2x2 with 1x1
! ___________________________________________________________________
module d_swappoles12

use u_parameters
use u_activeparts
use d_ctransformations
use d_memorymgmt
use d_computepoles
use z_ctransformations

implicit none
private
public d_swap11, d_swap12, d_swap21

contains

  subroutine d_swap11(i, j, A, B, compAB, apC, apT, Q, compQ, qcol, &
                    Z, compZ, zcol)
  ! DESCRIPTION
  !___________________________________________________________________________
  ! swaps real poles (bHbH) or eigenvalues (bTbT) i and i+1 in (A,B)
  ! The pencil (A,B) and Q and Z are updated as requested
  ! It is assumed that both i and i+1 are single, real poles/evs
  !
  ! This procedure is based on:
  ! "A generalized eigenvalue approach for solving Ricatti equations"
  ! -- P. Van Dooren
  !
  ! ARGUMENTS
  !___________________________________________________________________________
  ! i       integer [IN]
  !           Index (row in A,B) of the first pole/ev
  ! j       integer [IN]
  !           Index (column in A,B) of the first pole/ev
  !           It is assumed that (i:i+1,j:j+1) is upper triangular
  ! A       double array [INOUT]
  !           Upper Hessenberg matrix A
  ! B       double array [INOUT]
  !           Upper Hessenberg matrix B
  ! compAB  boolean [IN]
  !           If true, the equivalences are applied to (A,B). If not,
  !           then only to the active parts
  ! apC     type(tAp) [IN]
  !           Current active part.
  ! apT     type(tAp) [IN]
  !           Total part of the pencil the method is acting on
  !           (used when compAB is true)
  !           Normally, the following order,
  !             tstrt <= cstrt < i < i+1 < cstp <= tstp,
  !           holds when calling d_swap. c = current, t = total
  ! Q       double array [INOUT]
  !           Unitary equivalence matrix Q
  ! compQ   boolean [IN]
  !           If true, the left Schur vectors are updated
  ! qcol    integer [IN]
  !           Start index of the columns of Q that are updated:
  !              qcol, qcol+1
  ! Z       double array [INOUT]
  !           Unitary equivalence matrix Z
  ! compZ   boolean [IN]
  !           If true, the right Schur vectors are updated
  ! zcol    integer [IN]
  !           Start index of the columns of Z that are updated:
  !              zcol, zcol+1
  !___________________________________________________________________________
  ! last edit: September 5, 2018
    real(kind=dp), intent(inout)    :: A(:,:), B(:,:), Q(:,:), Z(:,:)
    logical, intent(in)             :: compAB, compQ, compZ
    integer, intent(in)             :: i, j, qcol, zcol
    type(tAp), pointer, intent(in)  :: apC, apT

    real(kind=dp)                   :: c, s, r
    real(kind=dp)                   :: an, bn
    integer,pointer                 :: cstrt, cstp, tstrt, tstp

    call tApGet(apC,cstrt,cstp)
    call tApGet(apT,tstrt,tstp)

    an = abs(A(i+1,j+1))
    bn = abs(B(i+1,j+1))

    ! Compute Z
    call d_compute_ct(A(i+1,j+1)*B(i,j+1) - B(i+1,j+1)*A(i,j+1), &
                      A(i+1,j+1)*B(i,j) - B(i+1,j+1)*A(i,j), &
                     c,s,r)

    if (compAB) then
     call d_apply_ct_r(A(tstrt+1:i+1,j),A(tstrt+1:i+1,j+1),c,s)
     call d_apply_ct_r(B(tstrt+1:i+1,j),B(tstrt+1:i+1,j+1),c,s)
    else
     call d_apply_ct_r(A(cstrt+1:i+1,j),A(cstrt+1:i+1,j+1),c,s)
     call d_apply_ct_r(B(cstrt+1:i+1,j),B(cstrt+1:i+1,j+1),c,s)
    endif

    if (compZ) then
      call d_apply_ct_r(Z(:,zcol),Z(:,zcol+1),c,s)
    end if

    ! Compute Q
    !if (abs(B(i+2,i+1)) .ge. abs(A(i+2,i+1))) then
    if (bn .ge. an) then
    !if (abs(B(i+2,i)) .ge. abs(A(i+2,i))) then
      call d_compute_ct(B(i,j),B(i+1,j),c,s,r)
    else
      call d_compute_ct(A(i,j),A(i+1,j),c,s,r)
    end if

    if (compAB) then
     call d_apply_ct_l(A(i,j:tstp),A(i+1,j:tstp),c,s)
     call d_apply_ct_l(B(i,j:tstp),B(i+1,j:tstp),c,s)
    else
     call d_apply_ct_l(A(i,j:cstp),A(i+1,j:cstp),c,s)
     call d_apply_ct_l(B(i,j:cstp),B(i+1,j:cstp),c,s)
    end if
    A(i+1,j) = dzero
    B(i+1,j) = dzero
    if (compQ) then
     call d_apply_ct_r(Q(:,qcol),Q(:,qcol+1),c,-s)
    end if
  end subroutine

  subroutine d_swap21(i, j, A, B, compAB, apC, apT, Q, compQ, qcol, &
                    Z, compZ, zcol)
  !   DESCRIPTION
  !___________________________________________________________________________
  ! swaps complex conjugate poles (bHbH) or eigenvalues (bTbT) i/i+1
  ! with real pole/ev i+2 in (A,B)
  ! The pencil (A,B) and Q and Z are updated as requested
  ! It is assumed that i/i+1 is a complex conjugate pair of poles in
  ! standard form, and i+2 is a real pole.
  !
  ! This procedure is based on:
  ! "A generalized eigenvalue approach for solving Ricatti equations"
  ! -- P. Van Dooren
  !
  ! ARGUMENTS
  !___________________________________________________________________________
  ! i       integer [IN]
  !           Index (row in A,B) of the first pole/ev
  ! j       integer [IN]
  !           Index (column in A,B) of the first pole/ev
  !           It is assumed that (i:i+2,j:j+2) is block upper triangular
  ! A       double array [INOUT]
  !           Upper Hessenberg matrix A
  ! B       double array [INOUT]
  !           Upper Hessenberg matrix B
  ! compAB  boolean [IN]
  !           If true, the equivalences are applied to (A,B). If not,
  !           then only to the active parts
  ! apC     type(tAp) [IN]
  !           Current active part.
  ! apT     type(tAp) [IN]
  !           Total part of the pencil the method is acting on
  !           (used when compAB is true)
  !           Normally, the following order,
  !             tstrt <= cstrt < i < i+1 < cstp <= tstp,
  !           holds when calling d_swap. c = current, t = total
  ! Q       double array [INOUT]
  !           Unitary equivalence matrix Q
  ! compQ   boolean [IN]
  !           If true, the left Schur vectors are updated
  ! qcol    integer [IN]
  !           Start index of the columns of Q that are updated:
  !              qcol, qcol+1
  ! Z       double array [INOUT]
  !           Unitary equivalence matrix Z
  ! compZ   boolean [IN]
  !           If true, the right Schur vectors are updated
  ! zcol    integer [IN]
  !           Start index of the columns of Z that are updated:
  !              zcol, zcol+1
  !___________________________________________________________________________
  ! last edit: September 11, 2018
    real(kind=dp), intent(inout)    :: A(:,:), B(:,:), Q(:,:), Z(:,:)
    logical, intent(in)             :: compAB, compQ, compZ
    integer, intent(in)             :: i, j, qcol, zcol
    type(tAp), pointer, intent(in)  :: apC, apT

    real(kind=dp)                   :: c, c2, s, s2, x1, x2, r, H(2,3)
    integer,pointer                 :: cstrt, cstp, tstrt, tstp
    integer                         :: LDAB, LDC, LDR, LDQ, LDZ

    external                        :: DGEMM

    call tApGet(apC,cstrt,cstp)
    call tApGet(apT,tstrt,tstp)

    if (abs(A(i+1,j)) .gt. dzero) then

    call d_getLeadingDim(LDAB,A)
    call d_getLeadingDim(LDQ,Q)
    call d_getLeadingDim(LDZ,Z)
    call d_getLeadingDim(LDC,Cu)
    call d_getLeadingDim(LDR,Ru)

    ! Initialize Q4 and Z4
    Q4(1:3,1:3) = dzero
    Q4(1,1) = done
    Q4(2,2) = done
    Q4(3,3) = done
    Z4(1:3,1:3) = dzero
    Z4(1,1) = done
    Z4(2,2) = done
    Z4(3,3) = done

    if (abs(B(i+2,j+2)) .ge. abs(A(i+2,j+2))) then
      H = A(i+2,j+2) * B(i:i+1,j:j+2) - B(i+2,j+2) * A(i:i+1,j:j+2)
      call d_compute_ct(H(1,1),H(2,1),c,s,x2)
      call d_apply_ct_l(H(1,2:3),H(2,2:3),c,s)
      ! Compute Z
      call d_compute_ct(H(2,3),H(2,2),c,s,x1)
      x1 = c * H(1,2) -s * H(1,3)
      call d_compute_ct(x1,x2,c2,s2,r)
      ! Form Z
      call d_apply_ct_r(Z4(1:3,2),Z4(1:3,3),c,s)
      call d_apply_ct_r(Z4(1:3,1),Z4(1:3,2),c2,s2)
      ! Apply to (A,B) and compute Q to make B upper triangular
      ! Z1
      call d_apply_ct_r(B(i:i+2,j+1),B(i:i+2,j+2),c,s)
      call d_apply_ct_r(A(i:i+2,j+1),A(i:i+2,j+2),c,s)
      ! Q1
      call d_compute_ct(B(i+1,j+1),B(i+2,j+1),c,s,r)
      call d_apply_ct_l(B(i+1,j:j+2),B(i+2,j:j+2),c,s)
      call d_apply_ct_l(A(i+1,j:j+2),A(i+2,j:j+2),c,s)
      B(i+2,j+1) = dzero
      ! Z2
      call d_apply_ct_r(B(i:i+2,j),B(i:i+2,j+1),c2,s2)
      call d_apply_ct_r(A(i:i+2,j),A(i:i+2,j+1),c2,s2)
      ! Q2
      call d_compute_ct(B(i,j),B(i+1,j),c2,s2,r)
      call d_apply_ct_l(B(i,j:j+2),B(i+1,j:j+2),c2,s2)
      call d_apply_ct_l(A(i,j:j+2),A(i+1,j:j+2),c2,s2)
      B(i+1,j) = dzero
      ! Form Q
      call d_apply_ct_l(Q4(2,1:3),Q4(3,1:3),c,s)
      call d_apply_ct_l(Q4(1,1:3),Q4(2,1:3),c2,s2)
      A(i+1:i+2,j) = dzero
    else
      ! Make A upper triangular and reverse roles of A and B
      call d_compute_ct(A(i+1,j+1),A(i+1,j),c,s,r)
      call d_apply_ct_r(Z4(1:3,1),Z4(1:3,2),c,s)
      call d_apply_ct_r(A(i:i+1,j),A(i:i+1,j+1),c,s)
      call d_apply_ct_r(B(i:i+1,j),B(i:i+1,j+1),c,s)
      A(i+1,j) = dzero
      ! Continue
      H = A(i+2,j+2) * B(i:i+1,j:j+2) - B(i+2,j+2) * A(i:i+1,j:j+2)
      call d_compute_ct(H(1,1),H(2,1),c,s,x2)
      call d_apply_ct_l(H(1,2:3),H(2,2:3),c,s)
      ! Compute Z
      call d_compute_ct(H(2,3),H(2,2),c,s,x1)
      x1 = c * H(1,2) -s * H(1,3)
      call d_compute_ct(x1,x2,c2,s2,r)
      ! Form Z
      call d_apply_ct_r(Z4(1:3,2),Z4(1:3,3),c,s)
      call d_apply_ct_r(Z4(1:3,1),Z4(1:3,2),c2,s2)
      ! Apply to (A,B) and compute Q to make A upper triangular
      ! Z1
      call d_apply_ct_r(B(i:i+2,j+1),B(i:i+2,j+2),c,s)
      call d_apply_ct_r(A(i:i+2,j+1),A(i:i+2,j+2),c,s)
      ! Q1
      call d_compute_ct(A(i+1,j+1),A(i+2,j+1),c,s,r)
      call d_apply_ct_l(B(i+1,j:j+2),B(i+2,j:j+2),c,s)
      call d_apply_ct_l(A(i+1,j:j+2),A(i+2,j:j+2),c,s)
      ! Z2
      call d_apply_ct_r(B(i:i+2,j),B(i:i+2,j+1),c2,s2)
      call d_apply_ct_r(A(i:i+2,j),A(i:i+2,j+1),c2,s2)
      ! Q2
      call d_compute_ct(A(i,j),A(i+1,j),c2,s2,r)
      call d_apply_ct_l(B(i,j:j+2),B(i+1,j:j+2),c2,s2)
      call d_apply_ct_l(A(i,j:j+2),A(i+1,j:j+2),c2,s2)
      ! Form Q
      call d_apply_ct_l(Q4(2,1:3),Q4(3,1:3),c,s)
      call d_apply_ct_l(Q4(1,1:3),Q4(2,1:3),c2,s2)
      ! Finish off by making B triangular
      call d_compute_ct(B(i+2,j+2),B(i+2,j+1),c,s,r)
      call d_apply_ct_r(B(i:i+2,j+1),B(i:i+2,j+2),c,s)
      call d_apply_ct_r(A(i:i+2,j+1),A(i:i+2,j+2),c,s)
      call d_apply_ct_r(Z4(1:3,2),Z4(1:3,3),c,s)
      B(i+1:i+2,j) = dzero
      B(i+2,j+1) = dzero
      A(i+1:i+2,j) = dzero
    end if

    if (compAB) then
      ! Row update
      if (j+3 .le. tstp) then
        call DGEMM('N','N',3,tstp-j-2,3,done,Q4(1,1),4,A(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        A(i:i+2,j+3:tstp) = Ru(1:3,1:tstp-j-2)
        call DGEMM('N','N',3,tstp-j-2,3,done,Q4(1,1),4,B(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        B(i:i+2,j+3:tstp) = Ru(1:3,1:tstp-j-2)
      end if
      ! Column update
      if (i-1 .ge. tstrt+1) then
        call DGEMM('N','N',i-tstrt-1,3,3,done,A(tstrt+1,j),LDAB,&
               Z4(1,1),4,dzero,Cu(1,1),LDC)
        A(tstrt+1:i-1,j:j+2) = Cu(1:i-tstrt-1,1:3)
        call DGEMM('N','N',i-tstrt-1,3,3,done,B(tstrt+1,j),LDAB,&
               Z4(1,1),4,dzero,Cu(1,1),LDC)
        B(tstrt+1:i-1,j:j+2) = Cu(1:i-tstrt-1,1:3)
      end if
    else
      ! Row update
      if (j+3 .le. cstp) then
        call DGEMM('N','N',3,cstp-j-2,3,done,Q4(1,1),4,A(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        A(i:i+2,j+3:cstp) = Ru(1:3,1:cstp-j-2)
        call DGEMM('N','N',3,cstp-j-2,3,done,Q4(1,1),4,B(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        B(i:i+2,j+3:cstp) = Ru(1:3,1:cstp-j-2)
      end if
      ! Column update
      if (i-1 .ge. cstrt+1) then
        call DGEMM('N','N',i-cstrt-1,3,3,done,A(cstrt+1,j),LDAB,&
               Z4(1,1),4,dzero,Cu(1,1),LDC)
        A(cstrt+1:i-1,j:j+2) = Cu(1:i-cstrt-1,1:3)
        call DGEMM('N','N',i-cstrt-1,3,3,done,B(cstrt+1,j),LDAB,&
               Z4(1,1),4,dzero,Cu(1,1),LDC)
        B(cstrt+1:i-1,j:j+2) = Cu(1:i-cstrt-1,1:3)
      end if
    end if

    if (compQ) then
      call DGEMM('N','T',LDQ,3,3,done,Q(1,qcol),LDQ,Q4(1,1),4,dzero,Cu(1,1),LDC)
      Q(1:LDQ,qcol:qcol+2) = Cu(1:LDQ,1:3)
    end if

    if (compZ) then
      call DGEMM('N','N',LDZ,3,3,done,Z(1,zcol),LDZ,Z4(1,1),4,dzero,Cu(1,1),LDC)
      Z(1:LDZ,zcol:zcol+2) = Cu(1:LDZ,1:3)
    end if

    else
      ! We use the single swap twice
      call d_swap11(i+1,j+1,A,B,compAB,apC,apT,Q,compQ,qcol+1,Z,compZ,zcol+1)
      call d_swap11(i,j,A,B,compAB,apC,apT,Q,compQ,qcol,Z,compZ,zcol)
    end if
  end subroutine

  subroutine d_swap12(i, j, A, B, compAB, apC, apT, Q, compQ, qcol, &
                    Z, compZ, zcol)
  !   DESCRIPTION
  !___________________________________________________________________________
  ! swaps real pole (bHbH) or eigenvalue (bTbT) i
  ! with complex conjugate pole/ev i+1/i+2 in (A,B)
  ! The pencil (A,B) and Q and Z are updated as requested
  ! It is assumed that i is a real pole and i+1/i+2 is a complex conjugate
  ! pair of poles in standard form
  !
  ! This procedure is based on:
  ! "A generalized eigenvalue approach for solving Ricatti equations"
  ! -- P. Van Dooren
  !
  ! ARGUMENTS
  !___________________________________________________________________________
  ! i       integer [IN]
  !           Index (row in A,B) of the first pole/ev
  ! j       integer [IN]
  !           Index (column in A,B) of the first pole/ev
  !           It is assumed that (i:i+2,j:j+2) is block upper triangular
  ! A       double array [INOUT]
  !           Upper Hessenberg matrix A
  ! B       double array [INOUT]
  !           Upper Hessenberg matrix B
  ! compAB  boolean [IN]
  !           If true, the equivalences are applied to (A,B). If not,
  !           then only to the active parts
  ! apC     type(tAp) [IN]
  !           Current active part.
  ! apT     type(tAp) [IN]
  !           Total part of the pencil the method is acting on
  !           (used when compAB is true)
  !           Normally, the following order,
  !             tstrt <= cstrt < i < i+1 < cstp <= tstp,
  !           holds when calling d_swap. c = current, t = total
  ! Q       double array [INOUT]
  !           Unitary equivalence matrix Q
  ! compQ   boolean [IN]
  !           If true, the left Schur vectors are updated
  ! qcol    integer [IN]
  !           Start index of the columns of Q that are updated:
  !              qcol, qcol+1
  ! Z       double array [INOUT]
  !           Unitary equivalence matrix Z
  ! compZ   boolean [IN]
  !           If true, the right Schur vectors are updated
  ! zcol    integer [IN]
  !           Start index of the columns of Z that are updated:
  !              zcol, zcol+1
  !___________________________________________________________________________
  ! last edit: September 12, 2018
    real(kind=dp), intent(inout)    :: A(:,:), B(:,:), Q(:,:), Z(:,:)
    logical, intent(in)             :: compAB, compQ, compZ
    integer, intent(in)             :: i, j, qcol, zcol
    type(tAp), pointer, intent(in)  :: apC, apT

    real(kind=dp)                   :: c, c2, s, s2, x1, x2, r, H(3,2)
    integer,pointer                 :: cstrt, cstp, tstrt, tstp
    integer                         :: LDAB, LDC, LDR, LDQ, LDZ

    external                        :: DGEMM

    call tApGet(apC,cstrt,cstp)
    call tApGet(apT,tstrt,tstp)

    call d_getLeadingDim(LDAB,A)
    call d_getLeadingDim(LDQ,Q)
    call d_getLeadingDim(LDZ,Z)
    call d_getLeadingDim(LDC,Cu)
    call d_getLeadingDim(LDR,Ru)

    ! Initialize Q4 and Z4
    Q4(1:3,1:3) = dzero
    Q4(1,1) = done
    Q4(2,2) = done
    Q4(3,3) = done
    Z4(1:3,1:3) = dzero
    Z4(1,1) = done
    Z4(2,2) = done
    Z4(3,3) = done

    if (abs(B(i,j)) .ge. abs(A(i,j))) then
      H = A(i,j) * B(i:i+2,j+1:j+2) - B(i,j) * A(i:i+2,j+1:j+2)
      call d_compute_ct(H(3,2),H(3,1),c,s,x2)
      call d_apply_ct_r(H(1:2,1),H(1:2,2),c,s)
      ! Compute Q
      call d_compute_ct(H(1,1),H(2,1),c,s,x1)
      x1 = -s * H(1,2) +c * H(2,2)
      call d_compute_ct(x1,x2,c2,s2,r)
      ! Form Q
      call d_apply_ct_l(Q4(1,1:3),Q4(2,1:3),c,s)
      call d_apply_ct_l(Q4(2,1:3),Q4(3,1:3),c2,s2)
      ! Apply to (A,B) and compute Z to make B upper triangular
      ! Q1
      call d_apply_ct_l(B(i,j:j+2),B(i+1,j:j+2),c,s)
      call d_apply_ct_l(A(i,j:j+2),A(i+1,j:j+2),c,s)
      ! Z1
      call d_compute_ct(B(i+1,j+1),B(i+1,j),c,s,r)
      call d_apply_ct_r(B(i:i+2,j),B(i:i+2,j+1),c,s)
      call d_apply_ct_r(A(i:i+2,j),A(i:i+2,j+1),c,s)
      B(i+1,j) = dzero
      ! Q2
      call d_apply_ct_l(B(i+1,j:j+2),B(i+2,j:j+2),c2,s2)
      call d_apply_ct_l(A(i+1,j:j+2),A(i+2,j:j+2),c2,s2)
      ! Z2
      call d_compute_ct(B(i+2,j+2),B(i+2,j+1),c2,s2,r)
      call d_apply_ct_r(B(i:i+2,j+1),B(i:i+2,j+2),c2,s2)
      call d_apply_ct_r(A(i:i+2,j+1),A(i:i+2,j+2),c2,s2)
      B(i+2,j+1) = dzero
      ! Form Z
      call d_apply_ct_r(Z4(1:3,1),Z4(1:3,2),c,s)
      call d_apply_ct_r(Z4(1:3,2),Z4(1:3,3),c2,s2)
      A(i+2,j:j+1) = dzero
    else
      ! Make A upper triangular and reverse roles of A and B
      call d_compute_ct(A(i+1,j+1),A(i+2,j+1),c,s,r)
      call d_apply_ct_l(Q4(2,1:3),Q4(3,1:3),c,s)
      call d_apply_ct_l(A(i+1,j:j+2),A(i+2,j:j+2),c,s)
      call d_apply_ct_l(B(i+1,j:j+2),B(i+2,j:j+2),c,s)
      A(i+2,j+1) = dzero
      ! Continue
      H = A(i,j) * B(i:i+2,j+1:j+2) - B(i,j) * A(i:i+2,j+1:j+2)
      call d_compute_ct(H(3,2),H(3,1),c,s,x2)
      call d_apply_ct_r(H(1:2,1),H(1:2,2),c,s)
      ! Compute Q
      call d_compute_ct(H(1,1),H(2,1),c,s,x1)
      x1 = -s * H(1,2) + c * H(2,2)
      call d_compute_ct(x1,x2,c2,s2,r)
      ! Form Q
      call d_apply_ct_l(Q4(1,1:3),Q4(2,1:3),c,s)
      call d_apply_ct_l(Q4(2,1:3),Q4(3,1:3),c2,s2)
      ! Apply to (A,B) and compute Z to make A upper triangular
      ! Q1
      call d_apply_ct_l(B(i,j:j+2),B(i+1,j:j+2),c,s)
      call d_apply_ct_l(A(i,j:j+2),A(i+1,j:j+2),c,s)
      ! Z1
      call d_compute_ct(A(i+1,j+1),A(i+1,j),c,s,r)
      call d_apply_ct_r(B(i:i+2,j),B(i:i+2,j+1),c,s)
      call d_apply_ct_r(A(i:i+2,j),A(i:i+2,j+1),c,s)
      ! Q2
      call d_apply_ct_l(B(i+1,j:j+2),B(i+2,j:j+2),c2,s2)
      call d_apply_ct_l(A(i+1,j:j+2),A(i+2,j:j+2),c2,s2)
      ! Z2
      call d_compute_ct(A(i+2,j+2),A(i+2,j+1),c2,s2,r)
      call d_apply_ct_r(B(i:i+2,j+1),B(i:i+2,j+2),c2,s2)
      call d_apply_ct_r(A(i:i+2,j+1),A(i:i+2,j+2),c2,s2)
      ! Form Z
      call d_apply_ct_r(Z4(1:3,1),Z4(1:3,2),c,s)
      call d_apply_ct_r(Z4(1:3,2),Z4(1:3,3),c2,s2)
      ! Finish off by making B triangular
      call d_compute_ct(B(i,j),B(i+1,j),c,s,r)
      call d_apply_ct_l(Q4(1,1:3),Q4(2,1:3),c,s)
      call d_apply_ct_l(A(i,j:j+2),A(i+1,j:j+2),c,s)
      call d_apply_ct_l(B(i,j:j+2),B(i+1,j:j+2),c,s)
      B(i+1:i+2,j) = dzero
      B(i+2,j+1) = dzero
      A(i+2,j:j+1) = dzero
    end if

    if (compAB) then
      ! Row update
      if (j+3 .le. tstp) then
        call DGEMM('N','N',3,tstp-j-2,3,done,Q4(1,1),4,A(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        A(i:i+2,j+3:tstp) = Ru(1:3,1:tstp-j-2)
        call DGEMM('N','N',3,tstp-j-2,3,done,Q4(1,1),4,B(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        B(i:i+2,j+3:tstp) = Ru(1:3,1:tstp-j-2)
      end if
      ! Column update
      if (i-1 .ge. tstrt+1) then
        call DGEMM('N','N',i-tstrt-1,3,3,done,A(tstrt+1,j),LDAB,&
               Z4(1,1),4,dzero,Cu(1,1),LDC)
        A(tstrt+1:i-1,j:j+2) = Cu(1:i-tstrt-1,1:3)
        call DGEMM('N','N',i-tstrt-1,3,3,done,B(tstrt+1,j),LDAB,&
               Z4(1,1),4,dzero,Cu(1,1),LDC)
        B(tstrt+1:i-1,j:j+2) = Cu(1:i-tstrt-1,1:3)
      end if
    else
      ! Row update
      if (j+3 .le. cstp) then
        call DGEMM('N','N',3,cstp-j-2,3,done,Q4(1,1),4,A(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        A(i:i+2,j+3:cstp) = Ru(1:3,1:cstp-j-2)
        call DGEMM('N','N',3,cstp-j-2,3,done,Q4(1,1),4,B(i,j+3),LDAB,&
               dzero,Ru(1,1),LDR)
        B(i:i+2,j+3:cstp) = Ru(1:3,1:cstp-j-2)
      end if
      ! Column update
      if (i-1 .ge. cstrt+1) then
        call DGEMM('N','N',i-cstrt-1,3,3,done,A(cstrt+1,j),LDAB,&
              Z4(1,1),4,dzero,Cu(1,1),LDC)
        A(cstrt+1:i-1,j:j+2) = Cu(1:i-cstrt-1,1:3)
        call DGEMM('N','N',i-cstrt-1,3,3,done,B(cstrt+1,j),LDAB,&
              Z4(1,1),4,dzero,Cu(1,1),LDC)
        B(cstrt+1:i-1,j:j+2) = Cu(1:i-cstrt-1,1:3)
      end if
    end if

    if (compQ) then
      call DGEMM('N','T',LDQ,3,3,done,Q(1,qcol),LDQ,Q4(1,1),4,dzero,Cu(1,1),LDC)
      Q(:,qcol:qcol+2) = Cu(1:LDQ,1:3)
    end if

    if (compZ) then
      call DGEMM('N','N',LDZ,3,3,done,Z(1,zcol),LDZ,Z4(1,1),4,dzero,Cu(1,1),LDC)
      Z(:,zcol:zcol+2) = Cu(1:LDZ,1:3)
    end if
  end subroutine

end module