!pqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpqpq
!                             modules                                 !
!bdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbdbd

module compute_twopt
  use settings, only : dp => mcp, feedback
  use numrec
  use quadpack
  use NLRSD_settings
  use NLRSD_power
  use MPIUtils
  USE, INTRINSIC :: IEEE_ARITHMETIC, ONLY: IEEE_IS_NAN
  implicit none
  real(kind=dp)                             :: smin, smax, kmin, kmax
  integer,parameter, private                :: numr = 130, numk = 150  !2016.7
  integer, private                          :: save_ell
  real(kind=dp), private                    :: save_s, save_k
  real(kind=dp), private,dimension(numr)    :: r, xi_0, xi_2, xi_4
  real(kind=dp), private,dimension(numr)    :: xi_0d, xi_2d, xi_4d
  real(kind=dp), private,dimension(numk)    :: kvec, pk_0, pk_2, pk_4
  real(kind=dp), private,dimension(numk)    :: pk_0d, pk_2d, pk_4d
  real(kind=dp), private                    :: save_r
  real(kind=dp), parameter, private         :: linf   = -6.2_dp
  real(kind=dp), parameter, private         :: lsup   = 0._dp
  real(kind=dp), parameter, private         :: inf    = exp(linf)
  real(kind=dp), parameter, private         :: sup    = exp(lsup)
  real(kind=dp), parameter, private         :: kmin_xiell = 1.1e-4_dp
  real(kind=dp), parameter, private         :: kmax_xiell = 1.5_dp
  real(kind=dp), parameter, private         :: epsabs = 1.e-2_dp
  real(kind=dp), parameter, private         :: epsrel = 1.e-2_dp
  real(kind=dp), parameter, private         :: epsabs_2 = 1.e-2_dp
  real(kind=dp), parameter, private         :: epsrel_2 = 1.e-2_dp
  real(kind=dp), parameter, private         :: epsabs_4 = 2.e-1_dp
  real(kind=dp), parameter, private         :: epsrel_4 = 1.e-1_dp
  real(kind=dp), parameter, private         :: kcut_0 = 0.7_dp
  real(kind=dp), parameter, private         :: npow_0 = 2._dp
  real(kind=dp), parameter, private         :: kcut_2 = 0.58_dp
  real(kind=dp), parameter, private         :: npow_2 = 4._dp
  real(kind=dp), parameter, private         :: kcut_4 = 0.6_dp
  real(kind=dp), parameter, private         :: npow_4 = 2._dp

  contains

    function cutoff(kh, kcut, npow)
      real(kind=dp)   :: cutoff, kh, kcut, npow
      cutoff = exp(-(kh/kcut)**npow)
    end function cutoff

    function kernel_xi0_sin(kh)
      implicit none
      real(kind=dp)           :: kh, kernel_xi0_sin, P0
      call splint(kvec, pk_0, pk_0d, numk, kh, P0)
      kernel_xi0_sin = P0*kh*cutoff(kh, kcut_0, npow_0)
    end function kernel_xi0_sin

    function kernel_xi0_jell(kh)
      real(kind=dp)   :: kernel_xi0_jell, kr, lk, kh, ci, si
      real(kind=dp)   :: p0, sj, sy, sjp, syp
      kr = kh*save_r
      call sphbes(0, kr, sj, sy, sjp, syp)
      call splint(kvec, pk_0, pk_0d, numk, kh, P0)
      kernel_xi0_jell = p0*sj*kh**2*cutoff(kh, kcut_0, npow_0)
    end function kernel_xi0_jell

    !configuration-space monopole
    function ximonopole(r)
      implicit none
      integer          :: neval, ierr, integr
      real(kind=dp)    :: ximonopole, r, ans, abserr
      integr = 2
      save_r = r
      call qawf(kernel_xi0_sin,inf,r,integr,epsabs,ans,abserr,neval,ierr)
      if(ierr == 0)then
        ximonopole = ans/r/twopisqr
      else
        !call warning('xi0_sin',ierr)
        !write(*,*)'using alternative integration routine..'
        call qags(kernel_xi0_jell, inf, sup, 1.d-6, 1.d-6, ans, abserr, neval, ierr)
        ximonopole = ans/twopisqr
        !call warning('xi0_jell',ierr)
      end if
    end function ximonopole

    !kernel for the first integral to obtain xi2_nl
    function kernel_xi2_sin(kh)
      implicit none
      real(kind=dp)           :: kh, kr, kernel_xi2_sin, P2
      kr = kh*save_r
      call splint(kvec, pk_2, pk_2d, numk, kh, P2)
      kernel_xi2_sin = P2*(3._dp/kr - kr)*cutoff(kh, kcut_2, npow_2)
    end function kernel_xi2_sin

    !kernel for the second integral to obtain xi2_nl
    function kernel_xi2_cos(kh)
      implicit none
      real(kind=dp)           :: kh, kr, kernel_xi2_cos, P2
      kr = kh*save_r
      call splint(kvec, pk_2, pk_2d, numk, kh, P2)
      kernel_xi2_cos = P2*cutoff(kh, kcut_2, npow_2)
    end function kernel_xi2_cos

    function kernel_xi2_jell(kh)
      real(kind=dp)   :: kernel_xi2_jell, kr, lk, kh, ci, si
      real(kind=dp)   :: p2, sj, sy, sjp, syp
      kr = kh*save_r
      call sphbes(2, kr, sj, sy, sjp, syp)
      call splint(kvec, pk_2, pk_2d, numk, kh, P2)
      kernel_xi2_jell = p2*sj*kh**2*cutoff(kh, kcut_2, npow_2)
    end function kernel_xi2_jell

    !configuration-space quadrupole
    function xiquadrupole(r)
      implicit none
      integer          :: neval, ierr, integr, error
      real(kind=dp)    :: term1, term2, xiquadrupole, r, ans, abserr
      save_r = r
      integr = 2
      error = 0
      call qawf(kernel_xi2_sin,inf,r,integr,epsabs_2,ans,abserr,neval,ierr)
      if(ierr /= 0)  then
        !write(*,*)r
        error = ierr
        !call warning('xi2_sin',ierr)
      end if
      term1 = ans
      integr = 1
      call qawf(kernel_xi2_cos,inf,r,integr,epsabs_2,ans,abserr,neval,ierr)
      if(ierr /= 0)then
        error = ierr
        !call warning('xi2_cos',ierr)
      end if
      term2 = -3._dp*ans
      if(error == 0)then
        xiquadrupole = -(term1 + term2)/twopisqr/r**2
      else
        !write(*,*)'r=',r
        call qags(kernel_xi2_jell, inf, sup, 1.d-7, 1.d-7, ans, abserr, neval, ierr)
        !if(ierr /= 0) then
        !  call warning('xi2_jell',ierr)
        !end if
        xiquadrupole = -ans/twopisqr
      end if
    end function xiquadrupole

    function kernel_xi4_jell(kh)
      real(kind=dp)   :: kernel_xi4_jell, kr, lk, kh, ci, si
      real(kind=dp)   :: p4, sj, sy, sjp, syp
      kr = kh*save_r
      call sphbes(4, kr, sj, sy, sjp, syp)
      call splint(kvec, pk_4, pk_4d, numk, kh, P4)
      kernel_xi4_jell = p4*sj*kh**2*cutoff(kh, kcut_4, npow_4)
    end function kernel_xi4_jell

    !kernel for the first integral to obtain xi4_nl
    function kernel_xi4_sin(kh)
      implicit none
      real(kind=dp)           :: kh, kr, kernel_xi4_sin, P4
      kr = kh*save_r
      call splint(kvec, pk_4, pk_4d, numk, kh, P4)
      kernel_xi4_sin = P4*(105._dp/kr**3 - 45._dp/kr + kr)*cutoff(kh, kcut_4, npow_4)
    end function kernel_xi4_sin

    !kernel for the second integral to obtain xi4_nl
    function kernel_xi4_cos(kh)
      implicit none
      real(kind=dp)           :: kh, kr, kernel_xi4_cos, P4
      kr = kh*save_r
      call splint(kvec, pk_4, pk_4d, numk, kh, P4)
      kernel_xi4_cos = P4*(10._dp - 105._dp/kr**2)*cutoff(kh, kcut_4, npow_4)
    end function kernel_xi4_cos

    !configuration space hexadecapole
    function xihexadecapole(r)
      implicit none
      integer          :: neval, ierr, integr, error
      real(kind=dp)    :: term1, term2, xihexadecapole, r, ans, abserr
      save_r = r
      integr = 2
      error = 0
      call qawf(kernel_xi4_sin,inf,r,integr,epsabs_4,ans,abserr,neval,ierr)
      if(ierr /= 0)then
        error = ierr
        !call warning('xi4_sin',ierr)
      end if
      term1 = ans
      integr = 1
      call qawf(kernel_xi4_cos,inf,r,integr,epsabs_4,ans,abserr,neval,ierr)
      if(ierr /= 0)then
        error = ierr
        !call warning('xi4_cos',ierr)
      end if
      term2 = ans
      if(error == 0)then
        xihexadecapole = (term1 + term2)/twopisqr/r**2
      else
        call qags(kernel_xi4_jell, inf, sup, 1.d-7, 1.d-7, ans, abserr, neval, ierr)
        if(ierr /= 0)  call warning('xi4_jell',ierr)
        xihexadecapole = ans/twopisqr
      end if
    end function xihexadecapole

    subroutine init_rvec
      implicit none
      integer        :: i
      real(kind=dp)  :: ds
      ds = (smax - smin)/real(numr-1,kind=dp)
      do i = 1,numr
        r(i) = smin + ds*real(i-1,kind=dp)
      end do
    end subroutine init_rvec

    subroutine init_xi_multipoles
      implicit none
      integer                          :: i, erro
      logical                          :: found_nan
      !file for debugging
      call init_rvec
      !setup tables with Fourier-space multipoles for interpolation.
      kmin = kmin_xiell
      kmax = kmax_xiell
      call init_pk_multipoles()
      found_nan = .false.
      do i = 1,numr
        xi_0(i) = ximonopole(r(i))
        xi_2(i) = xiquadrupole(r(i))
        xi_4(i) = xihexadecapole(r(i))

        found_nan = found_nan .or. (xi_0(i)/=xi_0(i))
        found_nan = found_nan .or. (xi_2(i)/=xi_2(i))
        found_nan = found_nan .or. (xi_4(i)/=xi_4(i))
      end do
      !close(98)
      if(found_nan)then
        write(*,*)'found NaN in multipoles'
        call test_found_nan
      end if
      call spline(r, xi_0, numr, 3d30, 3d30, xi_0d)
      call spline(r, xi_2, numr, 3d30, 3d30, xi_2d)
      call spline(r, xi_4, numr, 3d30, 3d30, xi_4d)
    end subroutine init_xi_multipoles

    function xi2d_at(s,mu)
      implicit none
      real(kind=dp) :: xi2d_at, s, mu
      real(kind=dp) :: ans0, ans2, ans4
      call splint(r, xi_0, xi_0d, numr, s, ans0)
      call splint(r, xi_2, xi_2d, numr, s, ans2)
      call splint(r, xi_4, xi_4d, numr, s, ans4)
      xi2d_at = ans0 + (1.5_dp*mu**2-0.5_dp)*ans2  &
                + 0.125_dp*(35._dp*mu**4 - 30._dp*mu**2 + 3._dp)*ans4
    end function xi2d_at

    function arg_xiell(muf)
      implicit none
      real(kind=dp) :: arg_xiell, muf
      real(kind=dp) :: mu, s, muf2, fac
      real(kind=dp) :: pl
      muf2 = muf**2
      fac  = sqrt(alpha_lo**2*muf2 + alpha_tr**2*(1._dp - muf2))
      s    = save_s*fac
      mu   = alpha_lo*muf/fac
      pl = plgndr(save_ell, 0, muf)
      arg_xiell = xi2d_at(s,mu)*pl
    end function arg_xiell

    function xi_multipole(s, ell)
      real(kind=dp)  :: xi_multipole, s, ans
      integer        :: ell, neval, ierr
      real(kind=dp)  :: abserr
      save_s   = s
      save_ell = ell
      call qags(arg_xiell, 0._dp, 1._dp, epsabs, epsrel, ans, abserr, neval, ierr)
      if(ierr /= 0)  call warning('xi_multipole',ierr)
      xi_multipole = ans*(2._dp*real(ell,kind=dp) + 1._dp)
    end function xi_multipole

    function arg_wed(muf)
      implicit none
      real(kind=dp) :: arg_wed,muf
      real(kind=dp) :: mu,s,muf2,fac
      muf2 = muf**2
      fac  = sqrt(alpha_lo**2*muf2+ alpha_tr**2*(1._dp-muf2))
      s    = save_s*fac
      mu   = alpha_lo*muf/fac
      arg_wed = xi2d_at(s,mu)
    end function arg_wed

    function xi_wedge(s, iwedge, nwedges)
      real(kind=dp)       :: xi_wedge, s
      integer             :: iwedge, nwedges
      integer             :: ierr, neval
      real(kind=dp)       :: mu_min, mu_max, delta_mu
      real(kind=dp)       :: ans, abserr
      delta_mu = 1._dp/real(nwedges, kind=dp) !divide range from 0 to 1 into nwedges bins
      mu_min = real(iwedge-1,kind=dp)*delta_mu
      mu_max = mu_min + delta_mu
      save_s = s
      call qags(arg_wed, mu_min, mu_max, epsabs, epsrel, ans, abserr, neval, ierr)
      if(ierr /= 0)  call warning('xi_wedges',ierr)
      xi_wedge = ans/delta_mu
    end function xi_wedge

    subroutine init_pk_kvec
      implicit none
      integer       :: i
      real(kind=dp) :: dk, qmin, qmax
      real(kind=dp) :: kcenter, pow
      qmax = log10(kmax)
      qmin = log10(kmin)
      dk = (qmax - qmin)/real(numk-1,kind=dp)
      do i = 1,numk
        kvec(i) = qmin + dk*real(i-1,kind=dp)
        kvec(i) = 10._dp**kvec(i)
      end do

    end subroutine init_pk_kvec

    subroutine init_pk_multipoles
      implicit none
      integer                          :: i
      !file for debugging
      call init_pk_kvec
      do i = 1, numk
        pk_0(i) = gRPTpower_gal_monopole_at(kvec(i))
        pk_2(i) = gRPTpower_gal_quadrupole_at(kvec(i))
        pk_4(i) = gRPTpower_gal_hexadecapole_at(kvec(i))
      end do
      call spline(kvec, pk_0, numk, 3d30, 3d30, pk_0d)
      call spline(kvec, pk_2, numk, 3d30, 3d30, pk_2d)
      call spline(kvec, pk_4, numk, 3d30, 3d30, pk_4d)
    end subroutine init_pk_multipoles

    function pk2d_at(k, mu)
      implicit none
      real(kind=dp) :: pk2d_at, k, mu
      real(kind=dp) :: ans0, ans2, ans4
      call splint(kvec, pk_0, pk_0d, numk, k, ans0)
      call splint(kvec, pk_2, pk_2d, numk, k, ans2)
      call splint(kvec, pk_4, pk_4d, numk, k, ans4)
      pk2d_at = ans0 + (1.5_dp*mu**2 - 0.5_dp)*ans2  &
                + 0.125_dp*(35._dp*mu**4 - 30._dp*mu**2 + 3._dp)*ans4
    end function pk2d_at

    function arg_pkell(muf)
      implicit none
      real(kind=dp) :: arg_pkell, muf
      real(kind=dp) :: mu, k, muf2, fac
      real(kind=dp) :: pl
      muf2 = muf**2
      fac  = sqrt(muf2/alpha_lo**2 + (1._dp - muf2)/alpha_tr**2)
      k    = save_k*fac
      mu   = muf/alpha_lo/fac
      !the function plgndr gives the Legendre polynomial
      pl = plgndr(save_ell, 0, muf)
      arg_pkell = pk2d_at(k,mu)*pl
    end function arg_pkell

    function pk_multipole(k, ell)
      real(kind=dp)  :: pk_multipole, k, ans
      integer        :: ell, neval, ierr
      real(kind=dp)  :: abserr
      save_k   = k
      save_ell = ell
      call qags(arg_pkell, 0._dp, 1._dp, epsabs, epsrel, ans, abserr, neval, ierr)
      if(ierr /= 0)  call warning('pk_multipole',ierr)
      pk_multipole = ans*(2._dp*real(ell,kind=dp) + 1._dp)/alpha**3
    end function pk_multipole

    function arg_pkwed(muf)
      implicit none
      real(kind=dp) :: arg_pkwed, muf
      real(kind=dp) :: mu, k, muf2, fac
      muf2 = muf**2
      fac  = sqrt(muf2/alpha_lo**2 + (1._dp - muf2)/alpha_tr**2)
      k    = save_k*fac
      mu   = muf/alpha_lo/fac
      arg_pkwed = pk2d_at(k,mu)
    end function arg_pkwed

    function pk_wedge(k, iwedge, nwedges)
      real(kind=dp)       :: pk_wedge, k
      integer             :: iwedge, nwedges
      integer             :: ierr, neval
      real(kind=dp)       :: mu_min, mu_max, delta_mu
      real(kind=dp)       :: ans, abserr
      delta_mu = 1._dp/real(nwedges, kind=dp) !divide range from 0 to 1 into nwedges bins
      mu_min = real(iwedge-1,kind=dp)*delta_mu
      mu_max = mu_min + delta_mu
      save_k = k
      call qags(arg_pkwed, mu_min, mu_max, epsabs, epsrel, ans, abserr, neval, ierr)
      if(ierr /= 0)  call warning('pk_wedges',ierr)
      pk_wedge = ans/alpha**3/delta_mu
    end function pk_wedge

    subroutine get_model_pk_multi(kvec, nell, pvec)
      real(kind=dp), dimension(:), intent(in)  :: kvec
      integer, intent(in)                      :: nell
      real(kind=dp), dimension(:), intent(out) :: pvec
      integer                                  :: numk, i, ik, itot, ell
      numk = size(kvec)
      if(size(pvec) /= nell*numk)then
        call MPIStop('wrong size of pvec array in get_model_pk_multi')
      end if
      kmin = min((1._dp/alpha_lo),(1._dp/alpha_tr))*kvec(1)*0.9_dp
      kmax = max((1._dp/alpha_lo),(1._dp/alpha_tr))*kvec(numk)*1.1_dp
      call init_pk_multipoles
      itot = 0
      do i = 1, nell
        do ik = 1, numk
          itot = itot + 1
          ell = 2*(i-1)
          pvec(itot) = pk_multipole(kvec(ik), ell)
          if(ell ==0 )pvec(itot) = pvec(itot) + shot_noise
        end do
      end do
    end subroutine get_model_pk_multi

    subroutine get_model_pk_wedges(kvec, nwedges, pvec)
      real(kind=dp), dimension(:), intent(in)  :: kvec
      integer, intent(in)                      :: nwedges
      real(kind=dp), dimension(:), intent(out) :: pvec
      integer                                  :: numk, iw, ik, itot
      numk = size(kvec)
      if(size(pvec) /= nwedges*numk)then
        call MPIStop('wrong size of pvec array in get_model_pk_wedges')
      end if
      kmin = min((1._dp/alpha_lo),(1._dp/alpha_tr))*kvec(1)*0.9_dp
      kmax = max((1._dp/alpha_lo),(1._dp/alpha_tr))*kvec(numk)*1.1_dp
      call init_pk_multipoles
      itot = 0
      do iw = 1, nwedges
        do ik = 1, numk
          itot = itot + 1
          pvec(itot) = pk_wedge(kvec(ik), iw, nwedges) + shot_noise
        end do
      end do
      !open(94,file='pkwedges_model.dat')
      !do ik = 1,numk
      !  write(94,'(9(E16.8,2x))')kvec(ik),(pvec(ik+(iw-1)*numk),iw=1,nwedges)
      !end do
      !close(94)
      !write(*,*)'model written'
      !read(*,*)

    end subroutine get_model_pk_wedges

    subroutine get_model_xi_multi(svec, nell, xivec)
      real(kind=dp), dimension(:), intent(in)  :: svec
      integer, intent(in)                      :: nell
      real(kind=dp), dimension(:), intent(out) :: xivec
      integer                                  :: nums, i, is, itot, ell
      nums = size(svec)
      if(size(xivec) /= nell*nums)then
        call MPIStop('wrong size of xivec array in get_model_xi_multi')
      end if
      smin = min(alpha_lo, alpha_tr)*svec(1)*0.9_dp
      smax = max(alpha_lo, alpha_tr)*svec(nums)*1.1_dp
      call init_xi_multipoles
      itot = 0
      do i = 1, nell
        do is = 1, nums
          itot = itot + 1
          ell = 2*(i-1)
          xivec(itot) = xi_multipole(svec(is), ell)
        end do
      end do

    end subroutine get_model_xi_multi

    subroutine get_model_xi_wedges(svec, nwedges, xivec)
      real(kind=dp), dimension(:), intent(in)  :: svec
      integer, intent(in)                      :: nwedges
      real(kind=dp), dimension(:), intent(out) :: xivec
      integer                                  :: nums, iw, is, itot
      nums = size(svec)
      if(size(xivec) /= nwedges*nums)then
        call MPIStop('wrong size of xivec array in get_model_xi_multi')
      end if
      smin = min(alpha_lo, alpha_tr)*svec(1)*0.9_dp
      smax = max(alpha_lo, alpha_tr)*svec(nums)*1.1_dp
      call init_xi_multipoles
      itot = 0
      do iw = 1, nwedges
        do is = 1, nums
          itot = itot + 1
          xivec(itot) = xi_wedge(svec(is), iw, nwedges)
        end do
      end do
    end subroutine get_model_xi_wedges

    subroutine test_found_nan
      integer      :: i
      write(*,*)'found a NaN!!'
      open(98,file='multipoles_xi_NaN.dat',status='unknown',form='formatted')
      do i = 1,numr
        write(98,'(4(E16.8,2x))')r(i),xi_0(i),xi_2(i),xi_4(i)
      end do
      close(98)

      open(98,file='multipoles_pk_NaN.dat',status='unknown',form='formatted')
      do i = 1,numk
        write(98,'(4(E16.8,2x))')kvec(i),pk_0(i),pk_2(i),pk_4(i)
      end do
      close(98)
      call write_plin
      !stop
    end subroutine test_found_nan

end module compute_twopt


C------------------------------------------------------------------------------
        subroutine ftxypx(xpos,ypos,xref,yref,xrefpix,yrefpix,
     &                    xinc,yinc,rot,type,xpix,ypix,status)

C       Fortran version of worldpos.c -- WCS Algorithms from Classic AIPS
C       Translated by James Kent Blackburn -- HEASARC/GSFC/NASA -- November 1994
C       routine to determine accurate pixel coordinates from an RA and Dec
C       does: -SIN, -TAN, -ARC, -NCP, -GLS, -MER, -AIT projections
C       returns   0 = good,
C               501 = angle too large for projection;
C               502 = bad values
C               503 = ???undocumented error - looks like an underflow???
C Input:
C dbl   xpos   x (RA) coordinate (deg)
C dbl   ypos   y (dec) coordinate (deg)
C dbl   xref    x reference coordinate value (deg)
C dbl   yref    y reference coordinate value (deg)
C dbl   xrefpix x reference pixel
C dbl   yrefpix y reference pixel
C dbl   xinc    x coordinate increment (deg)
C dbl   yinc    y coordinate increment (deg)
C dbl   rot     rotation (deg)  (from N through E)
C chr   type    projection type code e.g. "-SIN"
C Output:
C dbl   xpix    x pixel number  (RA or long without rotation)
C dbl   ypiy    y pixel number  (dec or lat without rotation)
C int   status  error status flag, zero

        integer           status
        double precision  xpos,ypos,xref,yref,xrefpix,yrefpix
        double precision  xinc,yinc,rot,xpix,ypix
        character*(*)     type
        integer           error1,error2,error3,error4
        parameter         (error1=501)
        parameter         (error2=502)
        parameter         (error3=503)
        parameter         (error4=504)
        double precision  dx,dy,dz,r,ra0,dec0,ra,dec
        double precision  coss,sins,dt,da,dd,sint,oldxpos
        double precision  l,m,geo1,geo2,geo3,sinr,cosr
        double precision  cond2r
        parameter         (cond2r=1.745329252d-2)
        double precision  twopi,deps
        parameter         (twopi = 6.28318530717959)
        parameter         (deps = 1.0d-5)
        integer           i,itype
        character*4       ctypes(8)
        data ctypes/ '-SIN', '-TAN', '-ARC', '-NCP',
     &               '-GLS', '-MER', '-AIT', '-STG' /

        if (status .gt. 0) return
C   *** 0 hour wrap around test
        oldxpos = xpos
        dt = (xpos - xref)
        if (dt .gt. +180) xpos = xpos - 360
        if (dt .lt. -180) xpos = xpos + 360
C   *** Default values - Linear
        dx = xpos - xref
        dy = ypos - yref
        dz = 0.0
C   *** Correct for rotation
        r = rot * cond2r
        cosr = dcos(r)
        sinr = dsin(r)
        dz = dx * cosr + dy * sinr
        dy = dy * cosr - dx * sinr
        dx = dz
C   *** Check axis increments - bail out if either 0
        if ((xinc .eq. 0.0) .or. (yinc .eq. 0.0)) then
          xpix = 0.0
          ypix = 0.0
          status = error2
          goto 30
        end if
        xpix = dx / xinc + xrefpix
        ypix = dy / yinc + yrefpix
C   *** Find type of coordinate transformation (0 is linear)
        itype = 0
        do 10 i = 1, 8
          if (ctypes(i) .eq. type) itype = i
  10    continue
C   *** Done if linear
        if (itype .eq. 0) goto 30
C   *** Non-Linear position
        ra0 = xref * cond2r
        dec0 = yref * cond2r
        ra = xpos * cond2r
        dec = ypos * cond2r
C   *** Compute directional cosine
        coss = dcos(dec)
        sins = dsin(dec)
        l = dsin(ra-ra0) * coss
        sint = sins * dsin(dec0) + coss * dcos(dec0) * dcos(ra-ra0)
C   *** Process by case
        if (itype .eq. 1) then
C   *** SINE from '-SIN' type
          if (sint .lt. 0.0) then
            status = error1
            goto 30
          end if
          m = sins * dcos(dec0) - coss * dsin(dec0) * dcos(ra-ra0)
        else if (itype .eq. 2) then
C   *** TANGENT from '-TAN' type
          if (sint .le. 0.0) then
            status = error1
            goto 30
          end if
          m = sins * dsin(dec0) + coss * dcos(dec0) * dcos(ra-ra0)
          l = l / m
          m = (sins*dcos(dec0) - coss*dsin(dec0)*dcos(ra-ra0)) / m
        else if (itype .eq. 3) then
C   *** Arc from '-ARC' type
          m = sins*dsin(dec0) + coss*dcos(dec0)*dcos(ra-ra0)
          if (m .lt. -1.0) m = -1.0
          if (m .gt. 1.0) m = 1.0
          m = dacos(m)
          if (m .ne. 0) then
            m = m / dsin(m)
          else
            m = 1.0
          end if
          l = l * m
          m = (sins*dcos(dec0) - coss*dsin(dec0)*dcos(ra-ra0)) * m
        else if (itype .eq. 4) then
C   *** North Celestial Pole from '-NCP' type
          if (dec0 .eq. 0.0) then
            status = error1
            goto 30
          else
            m = (dcos(dec0) - coss * dcos(ra-ra0)) / dsin(dec0)
          end if
        else if (itype .eq. 5) then
C   *** Global Sinusoid from '-GLS' type
          dt = ra - ra0
          if (dabs(dec) .gt. twopi/4.0) then
            status = error1
            goto 30
          end if
          if (dabs(dec0) .gt. twopi/4.0) then
            status = error1
            goto 30
          end if
          m = dec - dec0
          l = dt * coss
        else if (itype .eq. 6) then
C   *** Mercator from '-MER' type
          dt = yinc * cosr + xinc * sinr
          if (dt .eq. 0.0) dt = 1.0
          dy = (yref/2.0 + 45.0) * cond2r
          dx = dy + dt / 2.0 * cond2r
          dy = dlog(dtan(dy))
          dx = dlog(dtan (dx))
          geo2 = dt * cond2r / (dx - dy)
          geo3 = geo2 * dy
          geo1 = cos (yref * cond2r)
          if (geo1 .le. 0.0) geo1 = 1.0
          dt = ra - ra0
          l = geo1 * dt
          dt = dec / 2.0 + twopi / 8.0
          dt = dtan(dt)
          if (dt .lt. deps) then
            status = error2
            goto 30
          end if
          m = geo2 * dlog(dt) - geo3
        else if (itype .eq. 7) then
C   *** Aitoff from '-AIT' type
          l = 0.0
          m = 0.0
          da = (ra - ra0) / 2.0
          if (dabs(da) .gt. twopi/4.0) then
            status = error1
            goto 30
          end if
          dt = yinc * cosr + xinc * sinr
          if (dt .eq. 0.0) dt = 1.0
          dt = dt * cond2r
          dy = yref * cond2r
          dx = dsin(dy+dt)/dsqrt((1.0+dcos(dy+dt))/2.0) -
     &         dsin(dy)/dsqrt((1.0+dcos(dy))/2.0)
          if (dx .eq. 0.0) dx = 1.0
          geo2 = dt / dx
          dt = xinc * cosr - yinc * sinr
          if (dt .eq. 0.0) dt = 1.0
          dt = dt * cond2r
          dx = 2.0 * dcos(dy) * dsin(dt/2.0)
          if (dx .eq. 0.0) dx = 1.0
          geo1 = dt*dsqrt((1.0+dcos(dy)*dcos(dt/2.0))/2.0)/dx
          geo3 = geo2 * dsin(dy) / dsqrt((1.0+dcos(dy))/2.0)
          dt = dsqrt ((1.0 + dcos(dec) * dcos(da))/2.0)
          if (dabs(dt) .lt. deps) then
            status = error3
            goto 30
          end if
          l = 2.0 * geo1 * dcos(dec) * dsin(da) / dt
          m = geo2 * dsin(dec) / dt - geo3
        else if (itype .eq. 8) then
C   *** Stereographic from '-STG' type
          da = ra - ra0
          if (dabs(dec) .gt. twopi/4.0) then
            status = error1
            goto 30
          end if
          dd = 1.0 + sins*dsin(dec0) + coss*dcos(dec0)*dcos(da)
          if (dabs(dd) .lt. deps) then
            status = error1
            goto 30
          end if
          dd = 2.0 / dd
          l = l * dd
          m = dd * (sins*dcos(dec0) - coss*dsin(dec0)*dcos(da))
        else
C   *** Unsupported Projection
          status = error4
          goto 30
        end if
C   *** Convert back to degrees
        dx = l / cond2r
        dy = m / cond2r
C   *** Correct for rotation
        dz = dx * cosr + dy * sinr
        dy = dy * cosr - dx * sinr
        dx = dz
C   *** Convert to PIXELS ... yeah!
        xpix = dx / xinc + xrefpix
        ypix = dy / yinc + yrefpix
  30    continue
C   *** reset xpos to correct for in place modification
        xpos = oldxpos
        end


 module binmass
  use numeric_kinds
  use initial_parameters
  implicit none
  private

  ! Main program for calculating the aperture mases
  public :: binmass_Main

  ! helper functions
  private :: integrand,integrate,loopoverbins,aperture_boxed_readfile
  private :: read_binningfile,write_apermass

  real (kind=dp),private, dimension(9) :: binmass_intergrand_global

  ! global for storing the calculated aperture masses before writing
  integer(kind=i4b),private :: global_n_apermass=0
!  real (kind=dp),private, dimension(20000) :: global_apermass=0.0_dp
  real (kind=dp),private, dimension(400_i4b**2*6) :: global_apermass=0.0_dp

contains

function integrand(x) result(res)
  real (kind=dp), intent(in) :: x
  real (kind=dp)             :: res

  real (kind=dp)             :: y0,y1,angle,qi,sigma,surf_km,isotwist
  real (kind=dp)             :: psfwidth,psfweight

  real (kind=dp)             :: f0,f1,k,c,p,psigma,sb,qb,surcor
  real (kind=dp)             :: alpha,dens

   y0       =binmass_intergrand_global(1)
   y1       =binmass_intergrand_global(2)
   angle    =binmass_intergrand_global(3)
   qi       =binmass_intergrand_global(4)
   sigma    =binmass_intergrand_global(5)
   surf_km  =binmass_intergrand_global(6)
   isotwist =binmass_intergrand_global(7)
   psfwidth =binmass_intergrand_global(8)
   psfweight=binmass_intergrand_global(9)

   ! compute new gaussian size after convolving with the psf:
   sb = sqrt( sigma**2.0_dp+psfwidth**2.0_dp)
   qb = sqrt((( sigma*sigma*qi*qi + psfwidth*psfwidth))/(sigma*sigma + psfwidth*psfwidth))
   surcor= surf_km * qi/qb *(sigma**2) /(sb**2)

   ! Angle is the angle form the PA to the X-axis ccw
   ! To rotate the grid to the PA rotate of -angle ccw
   ! Isotwist is the rotation from the PA to the major axis of the gaussian ccw
   ! so this can just be added.
   alpha = -1.0_dp*angle + isotwist

   ! Glenn formula
   !k= sqrt( 1.0_dp+qb*qb+(1.0_dp-qb*qb)*cos(2.0_dp*alpha))
   !f0= 1.0_dp/(2.0_dp*k*qb*sb) * ( k*k*y0 - (1.0_dp-qb*qb)*sin(2.0_dp*alpha)*x)
   !f1= 1.0_dp/(2.0_dp*k*qb*sb) * ( k*k*y1 - (1.0_dp-qb*qb)*sin(2.0_dp*alpha)*x)
   !res = -psfweight * surcor * qb * sb * sqrt(pi_d) &
   !      / k * (derf(f0)-derf(f1)) * exp ( - (x*x)/(k*k*sb*sb))

   ! Cappellari formula from ic1459 paper (2002) Appendix B3, formula (B6 and B7)
   p  = sqrt (1+qb*qb + (1.0_dp-qb*qb) * cos(2.0_dp*alpha))
   f0 = 1.0_dp/(2*p*qb*sb) *  ((1.0_dp-qb*qb)*x*sin(2.0_dp*alpha) - p*p * y0)
   f1 = 1.0_dp/(2*p*qb*sb) *  ((1.0_dp-qb*qb)*x*sin(2.0_dp*alpha) - p*p * y1)
   res  = psfweight * surcor * qb * sb * sqrt(pi_d)/p &
        * (derf(f0)-derf(f1)) * exp(-(x/(p*sb))**2.0_dp)

end function integrand

subroutine integrate(x0,x1,res)
  real   (kind=dp ),intent(in) ::  x0,x1
  real   (kind=dp ),intent(out):: res
!------------------------------------
  integer(kind=i4b),parameter  :: limit=10000,leniw=limit*3,lenw=limit*46
  integer(kind=i4b)            :: ier,last
  integer(kind=i4b),dimension(leniw) ::iwork
  real   (kind=dp )            :: epsabs,epsrel,abserr
  real   (kind=dp ),dimension(limit*46)  :: work

  epsabs = 0.0_dp
  epsrel = 1.0e-5_dp
  call dqxgs(integrand,x0,x1,epsabs,epsrel,res,&
       abserr,ier,limit,leniw,lenw,last,iwork,work)
  if (ier /= 0 .and. ier /=2 ) print*,"dqxgs error in subroutine integrate:",ier

end subroutine integrate

subroutine loopoverbins(mnx,mny,xsize,ysize,xbins,ybins,angle,psfwidth,psfweight)
  use initial_parameters , only : totalmass,ngauss_mge,qobs,psi_view
real (kind=dp),intent(in) :: mnx,mny,xsize,ysize,angle
integer(kind=i4b),intent(in) :: xbins,ybins
real (kind=dp),intent(in) :: psfwidth(:),psfweight(:)
real (kind=dp)            :: bx(xbins+1),by(ybins+1),grid(xbins,ybins)
real (kind=dp)            :: res
integer(kind=i4b)         :: i,j,k,l

! determine the boundaries in the x direction
do i=1,xbins+1
   bx(i)=mnx+ (i-1.0_dp)*xsize / xbins
end do

! determine the boundaries in the y direction
do i=1,ybins+1
   by(i)=mny+ (i-1.0_dp)*ysize / ybins
end do

do i=1,xbins
   print*,i,xbins
   do j=1,ybins

      grid(i,j)=0.0_dp

      do k=1,ngauss_mge
         do l=1,size(psfwidth)
            binmass_intergrand_global(1)=by(j)
            binmass_intergrand_global(2)=by(j+1)
            binmass_intergrand_global(3)=angle
            binmass_intergrand_global(4)=qobs(k)
            binmass_intergrand_global(5)=sigobs_km(k)
            binmass_intergrand_global(6)=surf_km(k)
            binmass_intergrand_global(7)=psi_obs(k)-psi_view ! = isotwist
            binmass_intergrand_global(8)=psfwidth(l)
            binmass_intergrand_global(9)=psfweight(l)

            call integrate(bx(i),bx(i+1),res)
            grid(i,j)=grid(i,j)+res

         end do
      end do
   end do
end do

print*," Percentage of the light in this aperture"
print*,sum(grid(:,:))/totalmass*100.0

! normalising the grid
grid(:,:)=grid(:,:)/totalmass

!Do binning and store in the global variable
call read_binningfile(reshape(grid,(/xbins*ybins/)))

end subroutine loopoverbins

subroutine binmass_main()
  use initial_parameters    , only : conversion_factor
real (kind=dp) :: mnx,mny,xsize,ysize,angle
integer(kind=i4b) :: xbins,ybins,i,j,naper,npsfgauss
real (kind=dp),allocatable,dimension(:) :: psfwidth,psfweight

print*,"  * How many aperture file to integrate?"
read*,naper
print*,naper
do i=1,naper
   print*,"  * aperture :",i
   call aperture_boxed_readfile(mnx,mny,xsize,ysize,xbins,ybins,angle)

   print*,"  * How many Gaussians in the psf for this aperture?"
   read*,npsfgauss
   print*,npsfgauss
   allocate (psfwidth(npsfgauss),psfweight(npsfgauss))
   do j=1,npsfgauss
      read*,psfweight(j),psfwidth(j)
   end do

   psfwidth(:)=psfwidth(:)* conversion_factor

   print*,psfwidth

   call loopoverbins(mnx,mny,xsize,ysize,xbins,ybins,angle,psfwidth,psfweight)
   deallocate(psfwidth,psfweight)
end do

call write_apermass()

end subroutine binmass_main

subroutine aperture_boxed_readfile(mnx,mny,xsize,ysize,xbins,ybins,angle)
  use initial_parameters , only : conversion_factor
  character(len = 80) :: file,string
  integer (kind=i4b) :: handle
  !----------------------------------------------------------------------
  real (kind=dp),intent(out) :: mnx,mny,xsize,ysize,angle
  integer(kind=i4b),intent(out) :: xbins,ybins
  print*, "  * Reading boxed aperture file."
  print*,"  * What's the filename of the aperture file ? :"
  read*, file

  open (unit=handle,file=file,action="read",status="old"&
       &,position="rewind")
  print*,"  * Checking type."
  read(unit=handle,fmt=*) string

  select case (string)
  case ("#counterrotation_polygon_aperturefile_version_1")
     stop "Aperture type not supported"
  case ("#counter_rotation_boxed_aperturefile_version_2")
     ! empty
  case default
     stop " Unkown aperture type"
  end select

  print*,"  *  Reading box info"
  print*,"  *  Order: begin(x,y)"
  read(unit=handle,fmt=*) mnx,mny
  print*,"      size(x,y) "
  read(unit=handle,fmt=*) xsize,ysize
  print*,"      rotation"
  read(unit=handle,fmt=*) angle
  angle=angle*(pi_d/180.0_dp)
  print*,"      bin(x,y)"
  read(unit=handle,fmt=*) xbins,ybins

  ! convert arcsec into km
  mnx=mnx*conversion_factor
  mny=mny*conversion_factor
  xsize=xsize*conversion_factor
  ysize=ysize*conversion_factor

  print*,"   Total bins " , xsize*ysize
  print*,"   begin      " , mnx,mny
  print*,"   size       " , xsize,ysize
  print*,"   rotation   " , angle/(pi_d/180.0_dp)
  print*,"   binx       " , xbins,ybins
  print*," "
  print*,"  * Finished reading aperture"
  close (unit=handle)

end subroutine aperture_boxed_readfile

subroutine read_binningfile(grid)
real    (kind=dp) ,dimension(:),intent(in) :: grid
integer (kind=i4b) :: i,bins,minst,maxst
character (len=256) :: string
integer (kind=i4b),dimension(size(grid)) :: bin_order

! start this array at 0. This is used for unused pixels
real    (kind=dp) ,dimension(0:size(grid)) :: binned

print*,"  * Aperture: ",i
print*,"  * Give the filename of the binning file."
read*,string
print*,"  * Opening: ",string
open (unit=30,file=string,action="read",status="old"&
     &,position="rewind")
read (unit=30,fmt=*) string
if (string /= "#Counterrotaton_binning_version_1") &
     stop " Wrong version of file"
read (unit=30,fmt=*) bins
print*,"  * pixels in this aperture:",bins

if (bins /= size(grid) )&
     stop " bin and aperture file do not agree on the amount of pixels"
read (unit=30,fmt=*) bin_order(:)
close (unit=30)

print*,"  * Bins in this aperture:",maxval(bin_order)

if (maxval(bin_order) > size(grid)) &
     stop " Cannot store the highest bins in the grid"

binned=0.0_dp

do i=1, size(grid)
   binned(bin_order(i))=binned(bin_order(i))+grid(i)
end do

minst=global_n_apermass+1
maxst=global_n_apermass+maxval(bin_order)

if (maxst > size(global_apermass)) &
     stop "  TOo few elements in static array global_apermass"

global_apermass(minst:maxst) = global_apermass(minst:maxst) &
                             + binned(1:maxval(bin_order))

global_n_apermass=maxst

print*,"  * Done binning"
end subroutine read_binningfile

subroutine write_apermass()
integer(kind=i4b) :: i
character (len=256) :: string
print*,"  * Give the file name of the aperture mass output file"
read*,string
print*,string

print*,"  * Writing file"
open (unit=28,file=string,action="write", status="new")
write (unit=28, fmt="(i5)") global_n_apermass
do i=1,global_n_apermass
   write (unit=28, fmt="(i5,es20.12)") i, global_apermass(i)
end do
close (unit=28)
end subroutine write_apermass

end module binmass

program triaxmassbin
  use binmass
  use initial_parameters
  use triaxpotent
  use numeric_kinds
  implicit none

  call iniparam()
  call tp_setup()

  call binmass_main()

end program Triaxmassbin


!	print eap data for latent vector.
!	comment is used for identification.
!	factorname gives names of elements of the latent vector.
!	id is individual id.
!	unit uniteap is the unit for output.
!	eapid indicates if ids are put out.
!	covtheta are conditional covariance matrices.
!	meantheta are eaps.

subroutine eapoutput(comment,factorname,id,uniteap,eapid,covtheta,meantheta)
implicit none
character(len=*),intent(in)::comment
character(len=32),intent(in)::factorname(:)
character(len=32),intent(in),optional::id(:)
integer,intent(in)::uniteap
logical,intent(in)::eapid
real(kind=8),intent(in)::covtheta(:,:,:),meantheta(:,:)

!	Buffers for writing.
character(len=4)::buffer
character(len=32)::bufferid
character(len=25),allocatable::buffm(:),buffc(:,:)
!	format
character(len=7)::writefmt
!	col is column number.
!	io is error flag.
!	obs is observation number.
!	row is row number.
integer::col,io,obs,row
col=size(factorname)

allocate(buffm(col),buffc(col,col),stat=io)
if(io/=0) stop "Allocation failure for arrays for output of conditional means and covariances."
write(uniteap,'(2a)') "Conditional expectations and covariances ",comment
write(buffer,'(i4)') 1+2*col*(col+1)
writefmt='('//trim(adjustl(buffer))//'a)'
write(unit=uniteap,fmt=writefmt,iostat=io) "ID",(",","Mean_"//trim(adjustl(factorname(row))),row=1,size(factorname)),&
	((",","Cov_"//trim(adjustl(factorname(row)))//"_"//trim(adjustl(factorname(col))),&
	col=1,size(factorname)),row=1,size(factorname))
if(io/=0) stop "Writing of conditional means and covariances failed."

do obs=1,size(meantheta,2)
	do row=1,size(factorname)
		write(buffm(row),'(g25.16e3)')meantheta(row,obs)
		do col=1,size(factorname)
			write(buffc(row,col),'(g25.16e3)')covtheta(row,col,obs)
		end do
	end do
	if(eapid)then
		bufferid=id(obs)
	else
		write(bufferid,'(i12)') obs
	end if
	write(unit=uniteap,fmt=writefmt,iostat=io) trim(adjustl(bufferid)),(",",trim(adjustl(buffm(row))),row=1,size(factorname)),&
			((",",trim(adjustl(buffc(row,col))),col=1,size(factorname)),row=1,size(factorname))

	if(io/=0) stop "Writing of conditional means and covariances failed."
end do
return

end subroutine eapoutput


!
! AtmProfile_Validate_RH
!
! Program to validate relative humidity profiles in AtmProfile files
!
! CREATION HISTORY:
!
!       Created by:     David Groff, EMC/SAIC Nov-2009
!                       david.groff@noaa.gov
!

PROGRAM AtmProfile_Validate_RH

  ! -----------------
  ! Environment setup
  ! -----------------

  ! Module usage
  USE Type_Kinds
  USE Message_Handler
  USE AtmProfile_netCDF_IO
  USE AtmProfile_Define
  USE RH_MR

  IMPLICIT NONE

  ! ----------
  ! Parameters
  ! ----------
  CHARACTER(*), PARAMETER :: PROGRAM_NAME = 'AtmProfile_Validate_RH'
  CHARACTER(*), PARAMETER :: PROGRAM_RCS_ID = &
  INTEGER, PARAMETER :: LUN = 1
  REAL(fp) :: MAX_RELATIVE_HUMIDITY = 100.0_fp

  ! ---------
  ! Variables
  ! ---------
  CHARACTER(256) :: Message
  CHARACTER(15)  :: AtmProfile_Prefix
  CHARACTER(25)  :: AtmProfile_Filename
  CHARACTER(256) :: RH_Output_Filename
  CHARACTER(10)  :: Profile_Number
  TYPE(AtmProfile_Type), ALLOCATABLE :: AtmProfile(:)
  REAL(fp), ALLOCATABLE :: Relative_Humidity(:)
  INTEGER :: k, m
  INTEGER :: n_Profiles
  INTEGER :: Error_Status, Allocate_Status
  LOGICAL :: RH_Test_Failed

  ! Enter The AtmProfile filename
  WRITE( *,FMT='(/5x,"Enter the prefix of an AtmProfile '// &
                 'to get RH profiles for: ")', &
                ADVANCE='NO' )
  READ( *,FMT='(a)' ) AtmProfile_Prefix

  ! From the prefix construct the AtmProfile_Filename
  AtmProfile_Filename = TRIM(AtmProfile_Prefix)//'.AtmProfile.nc'

  ! Allocate AtmProfile
  Error_Status = AtmProfile_netCDF_InquireFile( AtmProfile_Filename, &
                                                n_Profiles = n_Profiles )
  IF ( Error_Status /= SUCCESS ) THEN
    CALL Display_Message( PROGRAM_NAME, &
                          'Error inquiring AtmProfile file '//&
                          TRIM(AtmProfile_Filename), &
                          FAILURE )
    STOP
  END IF

  ALLOCATE( AtmProfile(n_Profiles), STAT = Allocate_Status )
  IF ( Allocate_Status /= 0 ) THEN
    WRITE( Message,'("Error allocating AtmProfile array. STAT = ",i0)' ) &
                      Allocate_Status
    CALL Display_Message( PROGRAM_NAME, &
                          TRIM(Message), &
                          FAILURE )
    STOP
  END IF

  ! Read the AtmProfile file into AtmProfile
  Error_Status = AtmProfile_netCDF_ReadFile( AtmProfile_Filename, AtmProfile )
  IF ( Error_Status /= SUCCESS ) THEN
    CALL Display_Message( PROGRAM_NAME, &
                          'Error reading AtmProfile file '//&
                          TRIM(AtmProfile_Filename), &
                          FAILURE )
    STOP
  END IF

  ! Loop over profiles
  DO m = 1, n_Profiles

    ! Loop over layers
    ALLOCATE ( Relative_Humidity(0:AtmProfile(m)%n_Layers), STAT = Allocate_Status )
    IF ( Allocate_Status /= 0 ) THEN
      WRITE( Message,'("Error allocating AtmProfile array. STAT = ",i0)' ) &
                        Allocate_Status
      CALL Display_Message( PROGRAM_NAME, &
                            TRIM(Message), &
                            FAILURE )
      STOP
    END IF

    WRITE( Profile_Number, FMT='(I4)' ) m

    Profile_Number = ADJUSTL(Profile_Number)

    RH_Output_Filename = TRIM(AtmProfile_Prefix)//'_RH_Profile_'//TRIM(Profile_Number)//'.txt'

    ! Convert mixing ratio to relative humidity
    CALL MR_to_RH( AtmProfile(m)%Level_Pressure,      &
                   AtmProfile(m)%Level_Temperature,   &
                   AtmProfile(m)%Level_Absorber(:,1), &
                   Relative_Humidity                  )

    RH_Test_Failed = ANY(Relative_Humidity > MAX_RELATIVE_HUMIDITY, SIZE(Relative_Humidity))

    ! Write data if there is a RH test failure
    IF ( RH_Test_Failed ) THEN
      OPEN(Unit=LUN, FILE=RH_Output_Filename, Action = "WRITE")
      WRITE(LUN, FMT='(a48)'), AtmProfile(m)%Description
      WRITE(LUN, FMT='(F11.5)'), AtmProfile(m)%Latitude
      WRITE(LUN, FMT='(i1)'), AtmProfile(m)%Climatology_Model
      DO k = 0, AtmProfile(m)%n_Layers
        WRITE(LUN, FMT='(4F11.5)') AtmProfile(m)%Level_Pressure(k), &
                                   AtmProfile(m)%Level_Temperature(k), &
                                   AtmProfile(m)%Level_Absorber(k,1), &
                                   Relative_Humidity(k)
      END DO
      CLOSE(LUN)
    END IF

    DEALLOCATE ( Relative_Humidity, STAT = Allocate_Status )
    IF ( Allocate_Status /= 0 ) THEN
      WRITE( Message,'("Error deallocating Relative Humidity array. STAT = ",i0)' ) &
                        Allocate_Status
      CALL Display_Message( PROGRAM_NAME, &
                            TRIM(Message), &
                            FAILURE )
      STOP
    END IF

  END DO

  DEALLOCATE ( AtmProfile, STAT = Allocate_Status )
  IF ( Allocate_Status /= 0 ) THEN
    WRITE( Message,'("Error deallocating AtmProfile array. STAT = ",i0)' ) &
                      Allocate_Status
    CALL Display_Message( PROGRAM_NAME, &
                          TRIM(Message), &
                          FAILURE )
    STOP
  END IF

END PROGRAM AtmProfile_Validate_RH


!
!     Copyright (c) 2017, NVIDIA CORPORATION.  All rights reserved.
!
! NVIDIA CORPORATION and its licensors retain all intellectual property
! and proprietary rights in and to this software, related documentation
! and any modifications thereto.  Any use, reproduction, disclosure or
! distribution of this software and related documentation without an express
! license agreement from NVIDIA CORPORATION is strictly prohibited.
!

program main
    use openacc
    implicit none
    integer :: n        ! size of the vector
    real,dimension(:),allocatable :: a  ! the vector
    real,dimension(:),allocatable :: r  ! the results
    real,dimension(:),allocatable :: e  ! expected results
    integer :: i, j, nerrors
    integer :: c0, c1, c2, c3, cgpu, chost
    character(10) :: arg1
    if( command_argument_count() .gt. 0 )then
	call get_command_argument( 1, arg1 )
	read(arg1,'(i10)') n
    else
	n = 1000000
    endif
    if( n .le. 0 ) n = 1000000
    allocate(a(n))
    allocate(r(n))
    allocate(e(n))
    do i = 1,n
	a(i) = i*2.0
    enddo
do j = 1, 3
    !call acc_init( acc_device_nvidia )
    call system_clock( count=c1 )
    !$acc kernels loop
	do i = 1,n
	    r(i) = sin(a(i)) ** 2 + cos(a(i)) ** 2
	enddo
    call system_clock( count=c2 )
    cgpu = c2 - c1
	do i = 1,n
	    e(i) = sin(a(i)) ** 2 + cos(a(i)) ** 2
	enddo
    call system_clock( count=c3 )
    chost = c3 - c2
    ! check the results
    nerrors = 0
    do i = 1,n
	if( abs(r(i) - e(i)) .gt. 0.000001 )then
	    print *, i, r(i), e(i)
            nerrors = nerrors + 1
	endif
    enddo
    print *, n, ' iterations completed'
    print *, cgpu, ' microseconds on GPU'
    print *, chost, ' microseconds on host'
    if (nerrors .ne. 0) then
       print *, "Test FAILED"
    else
       print *, "Test PASSED"
    endif
enddo
end program


module test_particles_module

  use bl_types
  use bl_constants_module
  implicit none

  private
  public :: init_umac_2d

  ! width of the gaussian
  real (kind=dp_t), parameter :: W = 0.05

contains

  subroutine init_umac_2d(umac, vmac, ng_um, s, ng_s, lo, hi, dx)

    ! initialize the velocity field.  The s array is initialized to
    ! the magnitude of the velocity.

    use probin_module, only: prob_lo, prob_hi, vel_amp

    integer        , intent(in   ) :: lo(:), hi(:), ng_um, ng_s
    real(kind=dp_t), intent(inout) :: umac(lo(1)-ng_um:,lo(2)-ng_um:)
    real(kind=dp_t), intent(inout) :: vmac(lo(1)-ng_um:,lo(2)-ng_um:)
    real(kind=dp_t), intent(inout) ::    s(lo(1)-ng_s:, lo(2)-ng_s: )
    real(kind=dp_t), intent(in   ) :: dx(:)

    ! Local variables
    integer :: i, j
    real (kind=dp_t) :: x, y
    real (kind=dp_t) :: xc, yc
    real (kind=dp_t) :: dist

    xc = HALF*(prob_hi(1) - prob_lo(1))
    yc = HALF*(prob_hi(2) - prob_lo(2))

    do j = lo(2), hi(2)+1
       ! coordinate of lower edge
       y = (dble(j))*dx(2) + prob_lo(2)

       do i = lo(1), hi(1)+1
          ! coordinate of lower edge
          x = (dble(i))*dx(1) + prob_lo(1)

          dist = sqrt((x - xc)**2 + (y - yc)**2)

          ! u = -A r sin(theta)
          ! v =  A r cos(theta)
          !
          ! sin(theta) = (y-yc)/r
          ! cos(theta) = (x-xc)/r
          umac(i,j) = -vel_amp*(y-yc)
          vmac(i,j) =  vel_amp*(x-xc)

       enddo
    enddo

    do j = lo(2), hi(2)
       do i = lo(1), hi(1)

          s(i,j) = sqrt( (HALF*(umac(i,j) + umac(i+1,j  )))**2 + &
                         (HALF*(vmac(i,j) + vmac(i  ,j+1)))**2 )

       enddo
    enddo

  end subroutine init_umac_2d

end module test_particles_module


*> \brief \b SSYTRD_2STAGE
*
*  @generated from zhetrd_2stage.f, fortran z -> s, Sun Nov  6 19:34:06 2016
*
*  =========== DOCUMENTATION ===========
*
* Online html documentation available at
*            http://www.netlib.org/lapack/explore-html/
*
*> \htmlonly
*> Download SSYTRD_2STAGE + dependencies
*>
*> [TGZ]
*>
*> [ZIP]
*>
*> [TXT]
*> \endhtmlonly
*
*  Definition:
*  ===========
*
*       SUBROUTINE SSYTRD_2STAGE( VECT, UPLO, N, A, LDA, D, E, TAU,
*                                 HOUS2, LHOUS2, WORK, LWORK, INFO )
*
*       IMPLICIT NONE
*
*      .. Scalar Arguments ..
*       CHARACTER          VECT, UPLO
*       INTEGER            N, LDA, LWORK, LHOUS2, INFO
*      ..
*      .. Array Arguments ..
*       REAL               D( * ), E( * )
*       REAL               A( LDA, * ), TAU( * ),
*                          HOUS2( * ), WORK( * )
*       ..
*
*
*> \par Purpose:
*  =============
*>
*> \verbatim
*>
*> SSYTRD_2STAGE reduces a real symmetric matrix A to real symmetric
*> tridiagonal form T by a orthogonal similarity transformation:
*> Q1**T Q2**T* A * Q2 * Q1 = T.
*> \endverbatim
*
*  Arguments:
*  ==========
*
*> \param[in] VECT
*> \verbatim
*>          VECT is CHARACTER*1
*>          = 'N':  No need for the Housholder representation,
*>                  in particular for the second stage (Band to
*>                  tridiagonal) and thus LHOUS2 is of size max(1, 4*N);
*>          = 'V':  the Householder representation is needed to
*>                  either generate Q1 Q2 or to apply Q1 Q2,
*>                  then LHOUS2 is to be queried and computed.
*>                  (NOT AVAILABLE IN THIS RELEASE).
*> \endverbatim
*>
*> \param[in] UPLO
*> \verbatim
*>          UPLO is CHARACTER*1
*>          = 'U':  Upper triangle of A is stored;
*>          = 'L':  Lower triangle of A is stored.
*> \endverbatim
*>
*> \param[in] N
*> \verbatim
*>          N is INTEGER
*>          The order of the matrix A.  N >= 0.
*> \endverbatim
*>
*> \param[in,out] A
*> \verbatim
*>          A is REAL array, dimension (LDA,N)
*>          On entry, the symmetric matrix A.  If UPLO = 'U', the leading
*>          N-by-N upper triangular part of A contains the upper
*>          triangular part of the matrix A, and the strictly lower
*>          triangular part of A is not referenced.  If UPLO = 'L', the
*>          leading N-by-N lower triangular part of A contains the lower
*>          triangular part of the matrix A, and the strictly upper
*>          triangular part of A is not referenced.
*>          On exit, if UPLO = 'U', the band superdiagonal
*>          of A are overwritten by the corresponding elements of the
*>          internal band-diagonal matrix AB, and the elements above
*>          the KD superdiagonal, with the array TAU, represent the orthogonal
*>          matrix Q1 as a product of elementary reflectors; if UPLO
*>          = 'L', the diagonal and band subdiagonal of A are over-
*>          written by the corresponding elements of the internal band-diagonal
*>          matrix AB, and the elements below the KD subdiagonal, with
*>          the array TAU, represent the orthogonal matrix Q1 as a product
*>          of elementary reflectors. See Further Details.
*> \endverbatim
*>
*> \param[in] LDA
*> \verbatim
*>          LDA is INTEGER
*>          The leading dimension of the array A.  LDA >= max(1,N).
*> \endverbatim
*>
*> \param[out] D
*> \verbatim
*>          D is REAL array, dimension (N)
*>          The diagonal elements of the tridiagonal matrix T.
*> \endverbatim
*>
*> \param[out] E
*> \verbatim
*>          E is REAL array, dimension (N-1)
*>          The off-diagonal elements of the tridiagonal matrix T.
*> \endverbatim
*>
*> \param[out] TAU
*> \verbatim
*>          TAU is REAL array, dimension (N-KD)
*>          The scalar factors of the elementary reflectors of
*>          the first stage (see Further Details).
*> \endverbatim
*>
*> \param[out] HOUS2
*> \verbatim
*>          HOUS2 is REAL array, dimension (LHOUS2)
*>          Stores the Householder representation of the stage2
*>          band to tridiagonal.
*> \endverbatim
*>
*> \param[in] LHOUS2
*> \verbatim
*>          LHOUS2 is INTEGER
*>          The dimension of the array HOUS2.
*>          If LWORK = -1, or LHOUS2 = -1,
*>          then a query is assumed; the routine
*>          only calculates the optimal size of the HOUS2 array, returns
*>          this value as the first entry of the HOUS2 array, and no error
*>          message related to LHOUS2 is issued by XERBLA.
*>          If VECT='N', LHOUS2 = max(1, 4*n);
*>          if VECT='V', option not yet available.
*> \endverbatim
*>
*> \param[out] WORK
*> \verbatim
*>          WORK is REAL array, dimension (LWORK)
*> \endverbatim
*>
*> \param[in] LWORK
*> \verbatim
*>          LWORK is INTEGER
*>          The dimension of the array WORK. LWORK = MAX(1, dimension)
*>          If LWORK = -1, or LHOUS2=-1,
*>          then a workspace query is assumed; the routine
*>          only calculates the optimal size of the WORK array, returns
*>          this value as the first entry of the WORK array, and no error
*>          message related to LWORK is issued by XERBLA.
*>          LWORK = MAX(1, dimension) where
*>          dimension   = max(stage1,stage2) + (KD+1)*N
*>                      = N*KD + N*max(KD+1,FACTOPTNB)
*>                        + max(2*KD*KD, KD*NTHREADS)
*>                        + (KD+1)*N
*>          where KD is the blocking size of the reduction,
*>          FACTOPTNB is the blocking used by the QR or LQ
*>          algorithm, usually FACTOPTNB=128 is a good choice
*>          NTHREADS is the number of threads used when
*>          openMP compilation is enabled, otherwise =1.
*> \endverbatim
*>
*> \param[out] INFO
*> \verbatim
*>          INFO is INTEGER
*>          = 0:  successful exit
*>          < 0:  if INFO = -i, the i-th argument had an illegal value
*> \endverbatim
*
*  Authors:
*  ========
*
*> \author Univ. of Tennessee
*> \author Univ. of California Berkeley
*> \author Univ. of Colorado Denver
*> \author NAG Ltd.
*
*> \ingroup realSYcomputational
*
*> \par Further Details:
*  =====================
*>
*> \verbatim
*>
*>  Implemented by Azzam Haidar.
*>
*>  All details are available on technical report, SC11, SC13 papers.
*>
*>  Azzam Haidar, Hatem Ltaief, and Jack Dongarra.
*>  Parallel reduction to condensed forms for symmetric eigenvalue problems
*>  using aggregated fine-grained and memory-aware kernels. In Proceedings
*>  of 2011 International Conference for High Performance Computing,
*>  Networking, Storage and Analysis (SC '11), New York, NY, USA,
*>  Article 8 , 11 pages.
*>  http://doi.acm.org/10.1145/2063384.2063394
*>
*>  A. Haidar, J. Kurzak, P. Luszczek, 2013.
*>  An improved parallel singular value algorithm and its implementation
*>  for multicore hardware, In Proceedings of 2013 International Conference
*>  for High Performance Computing, Networking, Storage and Analysis (SC '13).
*>  Denver, Colorado, USA, 2013.
*>  Article 90, 12 pages.
*>  http://doi.acm.org/10.1145/2503210.2503292
*>
*>  A. Haidar, R. Solca, S. Tomov, T. Schulthess and J. Dongarra.
*>  A novel hybrid CPU-GPU generalized eigensolver for electronic structure
*>  calculations based on fine-grained memory aware tasks.
*>  International Journal of High Performance Computing Applications.
*>  Volume 28 Issue 2, Pages 196-209, May 2014.
*>  http://hpc.sagepub.com/content/28/2/196
*>
*> \endverbatim
*>
*  =====================================================================
      SUBROUTINE SSYTRD_2STAGE( VECT, UPLO, N, A, LDA, D, E, TAU,
     $                          HOUS2, LHOUS2, WORK, LWORK, INFO )
*
      IMPLICIT NONE
*
*  -- LAPACK computational routine --
*  -- LAPACK is a software package provided by Univ. of Tennessee,    --
*  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
*
*     .. Scalar Arguments ..
      CHARACTER          VECT, UPLO
      INTEGER            N, LDA, LWORK, LHOUS2, INFO
*     ..
*     .. Array Arguments ..
      REAL               D( * ), E( * )
      REAL               A( LDA, * ), TAU( * ),
     $                   HOUS2( * ), WORK( * )
*     ..
*
*  =====================================================================
*     ..
*     .. Local Scalars ..
      LOGICAL            LQUERY, UPPER, WANTQ
      INTEGER            KD, IB, LWMIN, LHMIN, LWRK, LDAB, WPOS, ABPOS
*     ..
*     .. External Subroutines ..
      EXTERNAL           XERBLA, SSYTRD_SY2SB, SSYTRD_SB2ST
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      INTEGER            ILAENV2STAGE
      EXTERNAL           LSAME, ILAENV2STAGE
*     ..
*     .. Executable Statements ..
*
*     Test the input parameters
*
      INFO   = 0
      WANTQ  = LSAME( VECT, 'V' )
      UPPER  = LSAME( UPLO, 'U' )
      LQUERY = ( LWORK.EQ.-1 ) .OR. ( LHOUS2.EQ.-1 )
*
*     Determine the block size, the workspace size and the hous size.
*
      KD     = ILAENV2STAGE( 1, 'SSYTRD_2STAGE', VECT, N, -1, -1, -1 )
      IB     = ILAENV2STAGE( 2, 'SSYTRD_2STAGE', VECT, N, KD, -1, -1 )
      LHMIN  = ILAENV2STAGE( 3, 'SSYTRD_2STAGE', VECT, N, KD, IB, -1 )
      LWMIN  = ILAENV2STAGE( 4, 'SSYTRD_2STAGE', VECT, N, KD, IB, -1 )
*      WRITE(*,*),'SSYTRD_2STAGE N KD UPLO LHMIN LWMIN ',N, KD, UPLO,
*     $            LHMIN, LWMIN
*
      IF( .NOT.LSAME( VECT, 'N' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.UPPER .AND. .NOT.LSAME( UPLO, 'L' ) ) THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -5
      ELSE IF( LHOUS2.LT.LHMIN .AND. .NOT.LQUERY ) THEN
         INFO = -10
      ELSE IF( LWORK.LT.LWMIN .AND. .NOT.LQUERY ) THEN
         INFO = -12
      END IF
*
      IF( INFO.EQ.0 ) THEN
         HOUS2( 1 ) = LHMIN
         WORK( 1 )  = LWMIN
      END IF
*
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SSYTRD_2STAGE', -INFO )
         RETURN
      ELSE IF( LQUERY ) THEN
         RETURN
      END IF
*
*     Quick return if possible
*
      IF( N.EQ.0 ) THEN
         WORK( 1 ) = 1
         RETURN
      END IF
*
*     Determine pointer position
*
      LDAB  = KD+1
      LWRK  = LWORK-LDAB*N
      ABPOS = 1
      WPOS  = ABPOS + LDAB*N
      CALL SSYTRD_SY2SB( UPLO, N, KD, A, LDA, WORK( ABPOS ), LDAB,
     $                   TAU, WORK( WPOS ), LWRK, INFO )
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SSYTRD_SY2SB', -INFO )
         RETURN
      END IF
      CALL SSYTRD_SB2ST( 'Y', VECT, UPLO, N, KD,
     $                   WORK( ABPOS ), LDAB, D, E,
     $                   HOUS2, LHOUS2, WORK( WPOS ), LWRK, INFO )
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'SSYTRD_SB2ST', -INFO )
         RETURN
      END IF
*
*
      HOUS2( 1 ) = LHMIN
      WORK( 1 )  = LWMIN
      RETURN
*
*     End of SSYTRD_2STAGE
*
      END


!       ...................................
        subroutine Isospin_t3()
!       ...................................
        USE VAPHFB_PAR
        implicit real*8 (a-h,o-z)

      if(.NOT. ALLOCATED(t3me))  ALLOCATE(t3me(1:HO%NLEV,1:HO%NLEV))

        do ii=1,HO%NLEV
          do jj=1,HO%NLEV
             t3me(jj,ii)=zero
             if((ii.eq.jj).AND.(ii.le.(HO%NLEV/2))) then
                t3me(jj,ii)=-1.d0
             else if((ii.eq.jj).AND.(ii.gt.(HO%NLEV/2))) then
                t3me(jj,ii)=1.d0
             end if
          end do
       end do
       end subroutine

!       ...................................
        subroutine ZNme()
!       ...................................
        USE VAPHFB_PAR
        implicit real*8 (a-h,o-z)

      if(.NOT. ALLOCATED(AZme))  ALLOCATE(AZme(1:HO%NLEV*HO%NLEV))
      if(.NOT. ALLOCATED(ANme))  ALLOCATE(ANme(1:HO%NLEV*HO%NLEV))

       ll=0
        do ii=1,HO%NLEV
          do jj=1,HO%NLEV
             ll=ll+1
             AZme(ll)=zero
             ANme(ll)=zero
             if((ii.eq.jj).AND.(ii.le.(HO%NLEV/2))) then
                AZme(ll)=one
             else if((ii.eq.jj).AND.(ii.gt.(HO%NLEV/2))) then
                ANme(ll)=one
             end if
          end do
       end do
       end subroutine

       subroutine JZ_JZ2_ME(AJX,AJY,AJZ,AJX2,AJY2,AJZ2,NLEV)

        USE VAPHFB_PAR
        implicit real*8 (a-h,o-z)

        DIMENSION AJX(NLEV,NLEV)
        DIMENSION AJX2(NLEV,NLEV)
        DIMENSION AJY(NLEV,NLEV)
        DIMENSION AJY2(NLEV,NLEV)
        DIMENSION AJZ(NLEV,NLEV)
        DIMENSION AJZ2(NLEV,NLEV)
        b2=HO%b_osc**2
!initializing
         do ii=1,HO%NLEV
         do jj=1,HO%NLEV
            AJX(jj,ii)=zero
            AJX2(jj,ii)=zero
            AJY(jj,ii)=zero
            AJY2(jj,ii)=zero
            AJZ(jj,ii)=zero
            AJZ2(jj,ii)=zero
        end do
        end do

        do ia=1,NLEV
         ina=tnljm%n(ia) ! nlindex(ia)
         ila =tnljm%l(ia) ! lang(ia)
         ija =tnljm%twoj(ia) ! jang(ia)
         ima =tnljm%twom(ia) ! mjang(ia)
         ita =tnljm%t(ia)    ! mtisos(ia)

         rja=ija/2.d0
         rma=ima/2.d0

         do ib=1,NLEV
         inb=tnljm%n(ib) ! nlindex(ia)
         ilb =tnljm%l(ib) ! lang(ia)
         ijb =tnljm%twoj(ib) ! jang(ia)
         imb =tnljm%twom(ib) ! mjang(ia)
         itb =tnljm%t(ib)    ! mtisos(ia)

         rjb=ijb/2.d0
         rmb=imb/2.d0

         fp1=rjb*(rjb+1.d0)-rmb*(rmb+1.d0)
         fm1=rjb*(rjb+1.d0)-rmb*(rmb-1.d0)

         fp1p2=rjb*(rjb+1.d0)-(rmb+1.d0)*(rmb+2.d0)
         fm1m2=rjb*(rjb+1.d0)-(rmb-1.d0)*(rmb-2.d0)
         f02=  rjb*(rjb+1.d0)-rmb**2

         if ((ina.eq.inb).AND.(ila.eq.ilb)            &
     &   .AND.(ija.eq.ijb).AND.(ita.eq.itb)) then

         delta_ma_mbp1=0.d0
         delta_ma_mbm1=0.d0
         delta_ma_mb  =0.d0
         delta_ma_mbp2=0.d0
         delta_ma_mbm2=0.d0

         if ((ima.eq.(imb+2))) delta_ma_mbp1=1.d0
         if ((ima.eq.(imb-2))) delta_ma_mbm1=1.d0
         if ((ima.eq.(imb+4))) delta_ma_mbp2=1.d0
         if ((ima.eq.(imb-4))) delta_ma_mbm2=1.d0
         if ((ima.eq.imb)) delta_ma_mb=1.d0

         AJX(ia,ib)=0.5d0*(dsqrt(fp1)*delta_ma_mbp1+               &
     &                    dsqrt(fm1)*delta_ma_mbm1)
         AJY(ia,ib)=0.5d0*(dsqrt(fp1)*delta_ma_mbp1-               &
     &                     dsqrt(fm1)*delta_ma_mbm1)

         AJZ(ia,ib)=rmb*delta_ma_mb

         AJX2(ia,ib)=0.25d0*(dsqrt(fp1*fp1p2)*delta_ma_mbp2+       &
     &                       dsqrt(fm1*fm1m2)*delta_ma_mbm2+         &
     &                                2*f02*delta_ma_mb)

         AJY2(ia,ib)=0.25d0*(dsqrt(fp1*fp1p2)*delta_ma_mbp2+       &
     &                      dsqrt(fm1*fm1m2)*delta_ma_mbm2-       &
     &                                2*f02*delta_ma_mb)

             AJZ2(ia,ib)=(rmb*delta_ma_mb)**2
           end if
          end do
        end do
        end subroutine
!      ..........................................
      subroutine Pairs_TMT_JMJ(IT,MMT,JJ,MMJ,P_TMT_JMJ_me,NLEV)
!     ....................................
!       iso-scalar pairing and iso-vector pairing
!       with the pairing amplitude:
!       P_TMT_JMJ =: 1/sqrt(2) * sum_l (2*l+1)^1/2 [c^+_l c^+l]^LST,
!       where L=0, S=1, T=0 for iso-scalar pairing
!             L=0, S=0, T=1 for iso-vector pairing
!     ....................................
!     In JT-scheme:
!     iso-vector pairing (S=0,T=1) => (J=0,T=1)
!     P_TMT_JMJ = 1/2 * sum_j (2*j+1)^1/2 [c^+_j c^+j]^(JT)_(0mu),
!               = 1/2 * sum_(jm) sum_(t,t') <1/2 t, 1/2 t | T=1, mu>
!                     * C^+_jmt C^+_jmt'
!     ....................................

        USE VAPHFB_PAR
        implicit real*8 (a-h,o-z)
        DIMENSION P_TMT_JMJ_me(NLEV,NLEV)
        REAL(DP) :: CG
        do ia=1,NLEV
          jla=tnljm%l(ia)
          jja=tnljm%twoj(ia) !jang(ia)
          jma=tnljm%twom(ia) !mjang(ia)
          jta=tnljm%t(ia)    !mtisos(ia)

          fact=dsqrt(jja+1.d0)/2.d0   ! 1/2 * (2*j+1)^1/2
         do ib=1,NLEV
!       ............. initialization
           P_TMT_JMJ_me(ia,ib)= 0.d0

          jlb=tnljm%l(ib)
          jjb=tnljm%twoj(ib) !jang(ia)
          jmb=tnljm%twom(ib) !mjang(ia)
          jtb=tnljm%t(ib)    !mtisos(ia): -1 (p) and +1 (n)

          delta_ab=0.d0
          cb1 = 0.d0
          cb3 = 0.d0
!      .......... iso-vector pairing case (T=1, S=0?)
          if(iT.eq.1 .and. JJ.eq.0) then
             if(jja.eq.jjb) delta_ab=1.d0
!             call CJJ(jja,jjb,2*JJ,jma,jmb,2*MMJ,cb1)
!             call CJJ(1,1,2*IT,jta,jtb,2*MMT,cb3)

             cb1 = CG(jja,jma,jjb,jmb,2*JJ,2*MMJ)
             cb3 = CG(1,jta,1,jtb,2*IT,2*MMT)
             P_TMT_JMJ_me(ia,ib)=fact*cb1*cb3*delta_ab

!      .......... iso-scalar pairing case
          elseif(iT.eq.0 .and. JJ.eq.1) then
             if(jla.eq.jlb) delta_ab=1.d0         ! delta_(la,lb)
!             call CJJ(jja,jjb,2*JJ,jma,jmb,2*MMJ,cb1)
!             call CJJ(1,1,2*IT,jta,jtb,2*MMT,cb3)
             cb1 = CG(jja,jma,jjb,jmb,2*JJ,2*MMJ)
             cb3 = CG(1,jta,1,jtb,2*IT,2*MMT)
             P_TMT_JMJ_me(ia,ib) = 2*delta_ab*cb1*cb3  &   ! multiplied by a factor 2 by hand
     &         *Wigner_6j(jjb,jja,2*JJ,1,1,2*jla)    &
     &         *iv(jla+(jja+3)/2)*dsqrt((jja+1.d0)*(jjb+1.d0))/dsqrt(2.d0)
          else
             stop ' Error in the definition of pairing !'
          endif
         end do
        end do

        end subroutine

!     .....................................
        subroutine Pairs_TLS(IT,MMT,IS,MMS,P_TMT_SMS_me,NLEV)
!     ....................................
!       Iso-scalar pairing amplitude:
!       P_ST = 1/sqrt(2) * sum_l (2*l+1)^1/2 [c^+_l c^+l]^LST,
!       where L=0, S=1, T=0 for iso-scalar pairing
!             L=0, S=0, T=1 for iso-vector pairing
!     ....................................
        USE VAPHFB_PAR
        implicit real*8 (a-h,o-z)
        integer IS
        Real(DP) :: CG
        DIMENSION P_TMT_SMS_me(NLEV,NLEV)

        do ia=1,NLEV
          jla=tnljm%l(ia)
          jja=tnljm%twoj(ia) !jang(ia)
          jma=tnljm%twom(ia) !mjang(ia)
          jta=tnljm%t(ia)    !mtisos(ia)

!         fact=dsqrt(jja*1.d0+0.5d0)
!          fact=dsqrt(jja/2.d0+0.5d0)
         do ib=1,NLEV

          jlb=tnljm%l(ib)
          jjb=tnljm%twoj(ib) !jang(ia)
          jmb=tnljm%twom(ib) !mjang(ia)
          jtb=tnljm%t(ib)    !mtisos(ia)

          delta_ab=0.d0
          if(jla.eq.jlb) delta_ab=1.d0

          P_TMT_SMS_me(ia,ib) = zero

          call CJJ(jja,jjb,IS*2,jma,jmb,2*MMS,cbj)       ! isospin part
          call CJJ(1,1,2*IT,jta,jtb,2*MMT,cbt)       ! isospin part

          P_TMT_SMS_me(ib,ia) = P_TMT_SMS_me(ia,ib) &
     &        + delta_ab*cbj*cbt*Wigner_6j(jjb,jja,2*IS,1,1,2*jla) &
     &         *iv(jla+(jja+3)/2)*dsqrt((jja+1.d0)*(jjb+1.d0))/dsqrt(2.d0)
!      ..............
        enddo
        enddo
        return
        end subroutine

        SUBROUTINE Qpair_0(QME,akapa10,akapa01,Q_0,NDIM)

        USE VAPHFB_PAR
        implicit real*8 (a-h,o-z)
!        Parameter (zero=0.d0)
!        Parameter (one=1.d0)

        DIMENSION QME(NDIM,NDIM)
        DIMENSION akapa10(NDIM,NDIM),akapa01(NDIM,NDIM)
        DIMENSION AUX1(NDIM,NDIM)
        DIMENSION AUX2(NDIM,NDIM)

        call DGEMM ('n','t',NDIM,NDIM,NDIM,one,    &
     &  QME,NDIM,akapa10,NDIM,zero,AUX1,NDIM)

        call DGEMM ('n','t',NDIM,NDIM,NDIM,one,    &
     &  QME,NDIM,akapa01,NDIM,zero,AUX2,NDIM)

        Q_0=zero
        do ii=1,NDIM
         Q_0=Q_0+(AUX1(ii,ii)+AUX2(ii,ii))/2.d0
        end do

        END SUBROUTINE


      SUBROUTINE DPOCO(A,LDA,N,RCOND,Z,INFO)
      INTEGER LDA,N,INFO
      DOUBLE PRECISION A(LDA,1),Z(1)
      DOUBLE PRECISION RCOND
C
C     DPOCO FACTORS A DOUBLE PRECISION SYMMETRIC POSITIVE DEFINITE
C     MATRIX AND ESTIMATES THE CONDITION OF THE MATRIX.
C
C     IF  RCOND  IS NOT NEEDED, DPOFA IS SLIGHTLY FASTER.
C     TO SOLVE  A*X = B , FOLLOW DPOCO BY DPOSL.
C     TO COMPUTE  INVERSE(A)*C , FOLLOW DPOCO BY DPOSL.
C     TO COMPUTE  DETERMINANT(A) , FOLLOW DPOCO BY DPODI.
C     TO COMPUTE  INVERSE(A) , FOLLOW DPOCO BY DPODI.
C
C     ON ENTRY
C
C        A       DOUBLE PRECISION(LDA, N)
C                THE SYMMETRIC MATRIX TO BE FACTORED.  ONLY THE
C                DIAGONAL AND UPPER TRIANGLE ARE USED.
C
C        LDA     INTEGER
C                THE LEADING DIMENSION OF THE ARRAY  A .
C
C        N       INTEGER
C                THE ORDER OF THE MATRIX  A .
C
C     ON RETURN
C
C        A       AN UPPER TRIANGULAR MATRIX  R  SO THAT  A = TRANS(R)*R
C                WHERE  TRANS(R)  IS THE TRANSPOSE.
C                THE STRICT LOWER TRIANGLE IS UNALTERED.
C                IF  INFO .NE. 0 , THE FACTORIZATION IS NOT COMPLETE.
C
C        RCOND   DOUBLE PRECISION
C                AN ESTIMATE OF THE RECIPROCAL CONDITION OF  A .
C                FOR THE SYSTEM  A*X = B , RELATIVE PERTURBATIONS
C                IN  A  AND  B  OF SIZE  EPSILON  MAY CAUSE
C                RELATIVE PERTURBATIONS IN  X  OF SIZE  EPSILON/RCOND .
C                IF  RCOND  IS SO SMALL THAT THE LOGICAL EXPRESSION
C                           1.0 + RCOND .EQ. 1.0
C                IS TRUE, THEN  A  MAY BE SINGULAR TO WORKING
C                PRECISION.  IN PARTICULAR,  RCOND  IS ZERO  IF
C                EXACT SINGULARITY IS DETECTED OR THE ESTIMATE
C                UNDERFLOWS.  IF INFO .NE. 0 , RCOND IS UNCHANGED.
C
C        Z       DOUBLE PRECISION(N)
C                A WORK VECTOR WHOSE CONTENTS ARE USUALLY UNIMPORTANT.
C                IF  A  IS CLOSE TO A SINGULAR MATRIX, THEN  Z  IS
C                AN APPROXIMATE NULL VECTOR IN THE SENSE THAT
C                NORM(A*Z) = RCOND*NORM(A)*NORM(Z) .
C                IF  INFO .NE. 0 , Z  IS UNCHANGED.
C
C        INFO    INTEGER
C                = 0  FOR NORMAL RETURN.
C                = K  SIGNALS AN ERROR CONDITION.  THE LEADING MINOR
C                     OF ORDER  K  IS NOT POSITIVE DEFINITE.
C
C     LINPACK.  THIS VERSION DATED 08/14/78 .
C     CLEVE MOLER, UNIVERSITY OF NEW MEXICO, ARGONNE NATIONAL LAB.
C
C     SUBROUTINES AND FUNCTIONS
C
C     LINPACK DPOFA
C     BLAS DAXPY,DDOT,DSCAL,DASUM
C     FORTRAN DABS,DMAX1,DREAL,DSIGN
C
C     INTERNAL VARIABLES
C
      DOUBLE PRECISION DDOT,EK,T,WK,WKM
      DOUBLE PRECISION ANORM,S,DASUM,SM,YNORM
      INTEGER I,J,JM1,K,KB,KP1
C
C
C     FIND NORM OF A USING ONLY UPPER HALF
C
      DO 30 J = 1, N
         Z(J) = DASUM(J,A(1,J),1)
         JM1 = J - 1
         IF (JM1 .LT. 1) GO TO 20
         DO 10 I = 1, JM1
            Z(I) = Z(I) + DABS(A(I,J))
   10    CONTINUE
   20    CONTINUE
   30 CONTINUE
      ANORM = 0.0D0
      DO 40 J = 1, N
         ANORM = DMAX1(ANORM,Z(J))
   40 CONTINUE
C
C     FACTOR
C
      CALL DPOFA(A,LDA,N,INFO)
      IF (INFO .NE. 0) GO TO 180
C
C        RCOND = 1/(NORM(A)*(ESTIMATE OF NORM(INVERSE(A)))) .
C        ESTIMATE = NORM(Z)/NORM(Y) WHERE  A*Z = Y  AND  A*Y = E .
C        THE COMPONENTS OF  E  ARE CHOSEN TO CAUSE MAXIMUM LOCAL
C        GROWTH IN THE ELEMENTS OF W  WHERE  TRANS(R)*W = E .
C        THE VECTORS ARE FREQUENTLY RESCALED TO AVOID OVERFLOW.
C
C        SOLVE TRANS(R)*W = E
C
         EK = 1.0D0
         DO 50 J = 1, N
            Z(J) = 0.0D0
   50    CONTINUE
         DO 110 K = 1, N
            IF (Z(K) .NE. 0.0D0) EK = DSIGN(EK,-Z(K))
            IF (DABS(EK-Z(K)) .LE. A(K,K)) GO TO 60
               S = A(K,K)/DABS(EK-Z(K))
               CALL DSCAL(N,S,Z,1)
               EK = S*EK
   60       CONTINUE
            WK = EK - Z(K)
            WKM = -EK - Z(K)
            S = DABS(WK)
            SM = DABS(WKM)
            WK = WK/A(K,K)
            WKM = WKM/A(K,K)
            KP1 = K + 1
            IF (KP1 .GT. N) GO TO 100
               DO 70 J = KP1, N
                  SM = SM + DABS(Z(J)+WKM*A(K,J))
                  Z(J) = Z(J) + WK*A(K,J)
                  S = S + DABS(Z(J))
   70          CONTINUE
               IF (S .GE. SM) GO TO 90
                  T = WKM - WK
                  WK = WKM
                  DO 80 J = KP1, N
                     Z(J) = Z(J) + T*A(K,J)
   80             CONTINUE
   90          CONTINUE
  100       CONTINUE
            Z(K) = WK
  110    CONTINUE
         S = 1.0D0/DASUM(N,Z,1)
         CALL DSCAL(N,S,Z,1)
C
C        SOLVE R*Y = W
C
         DO 130 KB = 1, N
            K = N + 1 - KB
            IF (DABS(Z(K)) .LE. A(K,K)) GO TO 120
               S = A(K,K)/DABS(Z(K))
               CALL DSCAL(N,S,Z,1)
  120       CONTINUE
            Z(K) = Z(K)/A(K,K)
            T = -Z(K)
            CALL DAXPY(K-1,T,A(1,K),1,Z(1),1)
  130    CONTINUE
         S = 1.0D0/DASUM(N,Z,1)
         CALL DSCAL(N,S,Z,1)
C
         YNORM = 1.0D0
C
C        SOLVE TRANS(R)*V = Y
C
         DO 150 K = 1, N
            Z(K) = Z(K) - DDOT(K-1,A(1,K),1,Z(1),1)
            IF (DABS(Z(K)) .LE. A(K,K)) GO TO 140
               S = A(K,K)/DABS(Z(K))
               CALL DSCAL(N,S,Z,1)
               YNORM = S*YNORM
  140       CONTINUE
            Z(K) = Z(K)/A(K,K)
  150    CONTINUE
         S = 1.0D0/DASUM(N,Z,1)
         CALL DSCAL(N,S,Z,1)
         YNORM = S*YNORM
C
C        SOLVE R*Z = V
C
         DO 170 KB = 1, N
            K = N + 1 - KB
            IF (DABS(Z(K)) .LE. A(K,K)) GO TO 160
               S = A(K,K)/DABS(Z(K))
               CALL DSCAL(N,S,Z,1)
               YNORM = S*YNORM
  160       CONTINUE
            Z(K) = Z(K)/A(K,K)
            T = -Z(K)
            CALL DAXPY(K-1,T,A(1,K),1,Z(1),1)
  170    CONTINUE
C        MAKE ZNORM = 1.0
         S = 1.0D0/DASUM(N,Z,1)
         CALL DSCAL(N,S,Z,1)
         YNORM = S*YNORM
C
         IF (ANORM .NE. 0.0D0) RCOND = YNORM/ANORM
         IF (ANORM .EQ. 0.0D0) RCOND = 0.0D0
  180 CONTINUE
      RETURN
      END


CM
C->>> --------------------------------------------------> ems_it_rfn <<<
c     Perform iterative refinement of the solution given by the INVERT
c     of Ax=b or A^Tx=b, that is -A^{-1}b or -A^{-T}b.
c
c     Use     x+dx = -A^{-1}b        or         x+dx = -A^{-T}b
c      => -A(x+dx) = b                    -A^T(x+dx) = b
c      =>     -Adx = b+Ax                     -A^Tdx = b+A^Tx
c      =>       dx = -A^{-1}(b+Ax)                dx = -A^{-T}(b+A^Tx)
c
      subroutine ems_it_rfn(
     &     ems_rt_cod, is, ds, rp_lvl,
     &     trans,
     &     vr_in_r,
     &     mtx_r_v, mtx_r_ix, mtx_c_sa,
     &     sol_v, sol_ix,
     &     rhs_v, rhs_ix,
     &     rsdu_v,
     &     mx_n_rfn_it, tl_it_rfn_norm_rsdu, it_rfn_tran_ze,
     &     msg)
      implicit none
      include 'EMSV.INC'
      include 'EMSPM.INC'
      include 'EMSMMGR.INC'
      include 'EMSMEM.INC'
      include 'EMSP.INC'
      include 'ICTVR.INC'
      include 'RLCTVR.INC'
      include 'EMSMSG.INC'
      include 'EMSRTCOD.INC'
      integer ems_rt_cod, rp_lvl
      logical trans
      integer vr_in_r(0:n_r)
      integer mtx_r_ix(0:n_a_el)
      integer mtx_c_sa(0:n_c+1)
      integer sol_ix(0:n_r)
      integer rhs_ix(0:n_r)
      integer is(0:is_n_en_m1)
      double precision mtx_r_v(0:n_a_el)
      double precision sol_v(0:n_r)
      double precision rhs_v(0:n_r)
      double precision rsdu_v(0:n_r)
      double precision ds(0:ds_n_en_m1)
      character*(*) msg

      integer mx_n_rfn_it
      double precision tl_it_rfn_norm_rsdu
      double precision it_rfn_tran_ze

      double precision tl_it_rfn_norm_dl

      integer r_n, vr_n, c_n, el_n, og_r_n, nw_r_n
      double precision sol_r_v, dl
      logical perm_inv
      logical alw_f7_wr
      character*6 ch6_sys
      integer sys_l_ch
      character*12 ch12_norm_rsdu
      integer norm_rsdu_f_ch
      double precision sv_tran_ze

      integer rfn_it_n
      double precision it_rfn_iz_norm_rsdu
      double precision it_rfn_norm_rsdu
      double precision it_rfn_norm_dl

c      integer wg_r_n
c      print*, ' Enter component of solution to corrupt '
c      read*, wg_r_n
c      if (wg_r_n .ge. 1 .and. wg_r_n .le. n_r) then
c         print*, ' Enter alternative for ', sol_v(wg_r_n)
c         read*, sol_v(wg_r_n)
c      endif

      ems_rt_cod = ems_rt_cod_ok
      perm_inv = iand(inv_alg_msk, inv_alg_perm) .ne. 0
      alw_f7_wr = ems_msg_no_prt_fm .ge. 1
      tl_it_rfn_norm_dl = zero
      if (trans) then
c
c     The solution to be refined is that of A^Tx=b
c
         ch6_sys = 'A^Tx=b'
         sys_l_ch = 6
         ch12_norm_rsdu = '||A^Tx-b||_2'
         norm_rsdu_f_ch = 1
      else
         ch6_sys = 'Ax=b  '
         sys_l_ch = 4
         ch12_norm_rsdu = '  ||Ax-b||_2'
         norm_rsdu_f_ch = 3
      endif
CM      IF (emsol_dev .EQ. 1) THEN
C?      if (rp_lvl .ge. 1) then
C?         if (alw_f7_wr) write(ems_li, 9100, err=8990)
C?     &        ch6_sys(1:sys_l_ch), msg
C?         call ems_msg_wr_li(info_msg_n)
C?         if (rp_lvl .ge. 2) then
C?            if (alw_f7_wr) write(ems_li, 9200, err=8990)
C?     &           ch12_norm_rsdu
C?            call ems_msg_wr_li(info_msg_n)
C?         endif
C?      endif
CM      ENDIF
c
c     Make sure that the vector used to store the residual is zeroed
c
      do 1, r_n = 1, n_r
         rsdu_v(r_n) = zero
 1    continue
      rfn_it_n = 0
      it_rfn_norm_dl = inf
 10   continue
c
c     Form y = b
c
      if (rhs_ix(0) .lt. 0) then
c
c     It is assumed that, for vr_n = -rhs_ix(0), the RHS is
c     rhs_v(0)*a_{vr_n}, where a_{vr_n} is the corresponding column of
c     [A:-I]
c
         vr_n = -rhs_ix(0)
         if (vr_n .le. n_c) then
            do 20, el_n = mtx_c_sa(vr_n), mtx_c_sa(vr_n+1)-1
               r_n = mtx_r_ix(el_n)
               rsdu_v(r_n) = rhs_v(0)*mtx_r_v(el_n)
 20         continue
         else
            r_n = vr_n - mx_n_c
            rsdu_v(r_n) = -rhs_v(0)
         endif
      else
         do 110, r_n = 1, n_r
            rsdu_v(r_n) = rhs_v(r_n)
 110     continue
      endif
c      do r_n = 1, n_r
c         write(*, 9300)r_n,
c     &        ' sol_v(r_n) = ', sol_v(r_n),
c     &        ' rhs_v(r_n) = ', rsdu_v(r_n)
c      enddo
      if (trans) then
c
c     Form y := y + A^Tx
c
         do 130, r_n = 1, n_r
            vr_n = vr_in_r(r_n)
            if (vr_n .le. n_c) then
               do 120, el_n = mtx_c_sa(vr_n), mtx_c_sa(vr_n+1)-1
                  c_n = mtx_r_ix(el_n)
                  rsdu_v(r_n) = rsdu_v(r_n) + mtx_r_v(el_n)*sol_v(c_n)
 120           continue
            else
               c_n = vr_n - mx_n_c
               rsdu_v(r_n) = rsdu_v(r_n) - sol_v(c_n)
            endif
 130     continue
      else
c
c     Form y := y + Ax
c
         do 150, c_n = 1, n_r
            sol_r_v = sol_v(c_n)
            if (sol_r_v .eq. zero) goto 150
            vr_n = vr_in_r(c_n)
            if (vr_n .le. n_c) then
               do 140, el_n = mtx_c_sa(vr_n), mtx_c_sa(vr_n+1)-1
                  r_n = mtx_r_ix(el_n)
                  rsdu_v(r_n) = rsdu_v(r_n) + mtx_r_v(el_n)*sol_r_v
 140           continue
            else
               r_n = vr_n - mx_n_c
               rsdu_v(r_n) = rsdu_v(r_n) - sol_r_v
            endif
 150     continue
      endif
c      do r_n = 1, n_r
c         write(*, 9300)r_n,
c     &        ' rsdu_v(r_n) = ', rsdu_v(r_n)
c      enddo
      it_rfn_norm_rsdu = zero
      do 160, r_n = 1, n_r
c     write(*, 9300)r_n, ' rsdu_v(r_n) = ', rsdu_v(r_n)
         it_rfn_norm_rsdu =
     &        it_rfn_norm_rsdu + rsdu_v(r_n)*rsdu_v(r_n)
 160  continue
      it_rfn_norm_rsdu = sqrt(it_rfn_norm_rsdu)
      if (rfn_it_n .eq. 0) it_rfn_iz_norm_rsdu = it_rfn_norm_rsdu
      if (rfn_it_n .ge. mx_n_rfn_it .or.
     &     it_rfn_norm_dl .le. tl_it_rfn_norm_dl .or.
     &     it_rfn_norm_rsdu .le. tl_it_rfn_norm_rsdu) then
c
c     One of the termination criteria is satisfied.
c
         do 170, r_n = 1, n_r
            rsdu_v(r_n) = zero
 170     continue
CM      IF (emsol_dev .EQ. 1) THEN
C?         if (rp_lvl .ge. 2) then
C?            if (alw_f7_wr) write(ems_li, 9202, err=8990)
C?     &           rfn_it_n+1, it_rfn_norm_rsdu
C?            call ems_msg_wr_li(info_msg_n)
C?         else if (rp_lvl .ge. 1) then
C?            if (alw_f7_wr) write(ems_li, 9201, err=8990)
C?     &           rfn_it_n, ch12_norm_rsdu(norm_rsdu_f_ch:12),
C?     &           it_rfn_iz_norm_rsdu, it_rfn_norm_rsdu
C?            call ems_msg_wr_li(info_msg_n)
C?         endif
CM      ENDIF
         goto 7000
      endif
c
c     Perform an iteration of refinement.
c
      rfn_it_n = rfn_it_n + 1
      if (trans) then
c
c     Form y := -A^{-T}y
c
         sv_tran_ze = bwd_tran_ze
         bwd_tran_ze = it_rfn_tran_ze
         call ems_btran(rsdu_v, n_r+1, ds, is)
         bwd_tran_ze = sv_tran_ze
      else
c
c     Form y := -A^{-1}y
c
         if (perm_inv) then
c
c     Permute the RHS in copying from rsdu to perm_rsdu.
c
            do 210, og_r_n = 1, n_r
               r_n = is(p_og_t_nw_perm+og_r_n)
               ds(p_perm_tran_vec+r_n) = rsdu_v(og_r_n)
               rsdu_v(og_r_n) = zero
 210        continue
            sv_tran_ze = fwd_tran_ze
            fwd_tran_ze = it_rfn_tran_ze
            call ems_ftran(ds(p_perm_tran_vec), n_r+1, ds, is)
            fwd_tran_ze = sv_tran_ze
         else
c
c     If not permuting INVERT
c
            sv_tran_ze = fwd_tran_ze
            fwd_tran_ze = it_rfn_tran_ze
            call ems_ftran(rsdu_v, n_r+1, ds, is)
            fwd_tran_ze = sv_tran_ze
         endif
      endif
c
c     Form x: = x + y and zero the vector used to compute y.
c
      it_rfn_norm_dl = zero
      if (perm_inv) then
c
c     If permuting INVERT
c
         if (trans) then
c
c     If refining the solution to A^Tx=b, correction is in rsdu_v and
c     have to apply permutation in updating x.
c
            do 310, nw_r_n = 1, n_r
               dl = rsdu_v(nw_r_n)
               it_rfn_norm_dl = it_rfn_norm_dl + dl*dl
               r_n = is(p_nw_t_og_perm+nw_r_n)
               sol_v(r_n) = sol_v(r_n) + dl
               rsdu_v(nw_r_n) = zero
 310        continue
         else
c
c     If refining the solution to Ax=b, correction is in perm_tran_vec.
c
            do 320, r_n = 1, n_r
               dl = ds(p_perm_tran_vec+r_n)
               it_rfn_norm_dl = it_rfn_norm_dl + dl*dl
               sol_v(r_n) = sol_v(r_n) + dl
               ds(p_perm_tran_vec+r_n) = zero
 320        continue
         endif
      else
c
c     If not permuting INVERT then correction is in rsdu_v
c
         do 330, r_n = 1, n_r
            dl = rsdu_v(r_n)
            it_rfn_norm_dl = it_rfn_norm_dl + dl*dl
            sol_v(r_n) = sol_v(r_n) + dl
            rsdu_v(r_n) = zero
 330     continue
      endif
      it_rfn_norm_dl = sqrt(it_rfn_norm_dl)
CM      IF (emsol_dev .EQ. 1) THEN
C?      if (rp_lvl .ge. 2) then
C?         if (alw_f7_wr) write(ems_li, 9202, err=8990)
C?     &        rfn_it_n, it_rfn_norm_rsdu, it_rfn_norm_dl
C?         call ems_msg_wr_li(info_msg_n)
C?      endif
CM      ENDIF
      goto 10
 7000 continue
CM      IF (emsol_dev .EQ. 1) THEN
C? 7100 continue
CM      ENDIF
      return
CM      IF (emsol_dev .EQ. 1) THEN
C? 8990 continue
C?      ems_rt_cod = max(ems_rt_cod_serious_f7_wr_er,
C?     &     ems_rt_cod)
C?      goto 7100
C? 9100 format('Performing iterative refinement on solution of ', a,
C?     &     ': ', a)
C? 9200 format(' It   ', a12, '     ||dx||_2')
C? 9201 format('After ', i3, ' iterations, ', a,
C?     &     ' has changed from ', g11.4, ' to ', g11.4)
C? 9202 format(i3, 2x, 2(2x, g11.4))
CM      ENDIF
c 9300 format(i5, 2(2x, a, g11.4))
      end


subroutine remap(a, res, vals, cols)
	use iso_c_binding
	implicit none
	real(8),    intent(in)    :: a(:), vals(:)
	integer(2), intent(in)    :: cols(:,:)
	integer(2), intent(inout) :: res(:,:)
	real(8) :: v, x(2), y(size(cols,1),2)
	integer :: i, j
	!$omp parallel do private(i,v,x,y)
	do i = 1, size(a)
		v = a(i)
		if(v .ne. v) then
			res(:,i) = 0
			cycle
		end if
		! Find location first greater value in vals
		do j = 1, size(vals)
			if(vals(j) > v) exit
		end do
		! Handle edge cases
		if(j <= 1) then
			res(:,i) = cols(:,j)
		elseif(j > size(vals)) then
			res(:,i) = cols(:,size(vals))
		else
			x = vals(j-1:j)
			y = cols(:,j-1:j)
			res(:,i) = min(max(0,nint(y(:,1) + (v-x(1))*(y(:,2)-y(:,1))/(x(2)-x(1)))),255)
		end if
	end do
end subroutine

subroutine direct(a, res)
	use iso_c_binding
	implicit none
	real(8),    intent(in)    :: a(:,:)
	integer(2), intent(inout) :: res(:,:)
	real(8) :: v
	integer :: i
	!$omp parallel do private(i,v)
	do i = 1, size(a,1)
		v = a(i,1)
		if(v .ne. v) then
			res(:,i) = 0
		else
			res(1:size(a,2),i)  = min(max(0,nint(a(i,:)*256)),255)
			res(size(a,2)+1:,i) = 255
		end if
	end do
end subroutine


    !> compute the anisotropy tensor
    !! @param iFemType =  nint(commonPar(1))
	!! @param iMaterial = nint(commonPar(2))
    !! @author Rocha, Felipe Figueredo
Subroutine computeAnisotropyTensor(AE, BE, MaxLRows, XLL, NDim, iDofT, NodElt, Sol0, Sol1, CommonPar, &
									Param, JParam, DelT, DTm, Time)
!     ------------------------------------------------------------------
	use funcAux
	use globalVariables, only : getF, NdimE, getMaterial,maxMatPar, contributeAsisotropyTensor
	use fibresLib
	use ptsGaussLib, only : setNodG

	implicit none

	!   ===== SUBROUTINE ARGUMENTS  =======
	integer :: MaxLRows,Ndim,iDofT, NodElt! all integers
	Real*8 :: DelT, DTm,Time ! all reals
	Real*8 :: AE(MaxLRows,MaxLRows), BE(MaxLRows), XLL(*), Sol0(*), Sol1(*), Param(*), JParam(*), CommonPar(*)

!~ 	!   =====   END ARGUMENTS  =======

	Real*8 :: MatPar(maxMatPar)
	Real*8 :: afib(NdimE), Lfib, AreaFib, Vfib
	real*8 , allocatable ::  Xel(:) !! all have dimension NodG*NdimE
	integer :: NodG, iMaterial
	integer :: constLaw , iFemType

	iFemType = nint(commonPar(1))
	iMaterial = nint(commonPar(2))

    call setNodG(iFemtype, NodG)

	call getMaterial(constLaw, matPar, iMaterial)
	AreaFib = matpar(3)
 	call getSliceAllocate(Xel,XLL,1,NodG,1 ,NdimE, Ndim)

	call get_afib(afib,Lfib,Xel)

	Vfib = AreaFib * Lfib

	call contributeAsisotropyTensor(afib,Vfib)

end subroutine

!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Subroutine computeAnisotropyTensorS(Coupling,CommonPar,iDofT,Ndim,MaxLRows,iAdd)
!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    IMPLICIT NONE
    Integer :: MaxLRows,Ndim,iDofT, iAdd
    Integer   Coupling(MaxLRows,MaxLRows)
    Real*8 CommonPar(*)

    Coupling = 0

end Subroutine


( for blue pill boards )
VARIABLE posE \ 0
PA0 CONSTANT Xm.Spin
PA1 CONSTANT Xm.Dpin
PA2 CONSTANT Ym.Spin
PA3 CONSTANT Ym.Dpin
PA4 CONSTANT Zm.Spin
PA5 CONSTANT Zm.Dpin
PA6 CONSTANT Em.Spin
PA7 CONSTANT Em.Dpin

\ drv8825 DATA-SHEET SLVSA73F–APRIL2010–REVISEDJULY2014 ti.com page 19
\ 120  ( RPM ) 360 ( degree ) * 32 ( μsteps/step ) * 6 ( sec/10min )  18 ( 10degree/step ) * /
100 VALUE stepFreq
1 VALUE #Xsteps
1 VALUE #Ysteps

( pin step -- )
: step
  0
  DO
    DUP ios! \ on
    stepFreq DELAY_US
    DUP ioc! \ off
    stepFreq DELAY_US
  LOOP
  DROP
;

: moveZ ( steps -1|1)
  1 = IF Zm.Dpin ios!
	  ELSE Zm.Dpin ioc!
	  THEN
  Zm.Spin
  SWAP
  0
  DO
    DUP ios! \ on
    stepFreq 2/ DELAY_US
    DUP ioc! \ off
    stepFreq 2/ DELAY_US
  LOOP
  DROP
;

\ VARIABLE Xsteps
\ VARIABLE Ysteps
\ VARIABLE Zsteps
VARIABLE Esteps
\ 0 Xpos !
\ 0 Ypos !
\ 0 Zpos !
\ 0 Epos !

\ 50 50 TASK Xm
\ 50 50 TASK Ym
\ 50 50 TASK Zm
50 50 TASK Em

\ : Xm& Xm ACTIVATE
\ 	  BEGIN
\ 		Xsteps @ DUP
\ 		0< IF Xm.Dpin ioc!  \ negative
\ 		   ELSE Xm.Dpin ios! \ postive
\ 		   THEN
\ 		ABS
\ 		Xm.Spin SWAP step
\ 		PAUSE

\ 		0
\ 	  UNTIL
\ ;

\ : Ym& Ym ACTIVATE
\ 	  BEGIN
\ 		Ysteps @ DUP
\ 		0< IF Ym.Dpin ioc!  \ negative
\ 		   ELSE Ym.Dpin ios! \ postive
\ 		   THEN
\ 		ABS
\ 		Ym.Spin SWAP step
\ 		PAUSE

\ 		0
\ 	  UNTIL
\ ;

\ : Zm& Zm ACTIVATE
\ 	  BEGIN
\ 		Zsteps @ DUP
\ 		0< IF Zm.Dpin ioc!  \ negative
\ 		   ELSE Zm.Dpin ios! \ postive
\ 		   THEN
\ 		ABS
\ 		Zm.Spin SWAP step
\ 		PAUSE

\ 		0
\ 	  UNTIL
\ ;

: Em& Em ACTIVATE
	  Em.Dpin ios! \ postive
	  BEGIN
		Em.Spin
		Esteps @
		DUP 0<> IF 0
				   DO
					 DUP ios! \ on
					 stepFreq 2/ DELAY_US
					 DUP ioc! \ off
					 stepFreq 2/ DELAY_US
				   LOOP
				ELSE DROP
				THEN
		DROP
		PAUSE

		0
	  UNTIL
;

\ ( -- )

: motors.init OMODE-PP Xm.Spin io-mode!
			  OMODE-PP Xm.Dpin io-mode!
			  OMODE-PP Ym.Spin io-mode!
			  OMODE-PP Ym.Dpin io-mode!
			  OMODE-PP Zm.Spin io-mode!
			  OMODE-PP Zm.Dpin io-mode!
			  OMODE-PP Em.Spin io-mode!
			  OMODE-PP Em.Dpin io-mode!
			  \ Xm&
			  \ Ym&
			  \ Zm&
			  Em&
;
motors.init
VARIABLE dx
VARIABLE dy
\ x0,y1,x0,y1 units are in steps
: lineBresenham ( x0 y0 x1 y1 -- ) \ Bresenham's Line Algorithm
  1 PICK 4 PICK - 2* DUP dx ! ( x0 y0 x1 y1 2dx -- )
  0< IF Xm.Dpin ioc! dx @ NEGATE dx !
	 ELSE Xm.Dpin ios!
	 THEN ( x0 y0 x1 y1  -- )
  DUP 3 PICK - 2* DUP dy ! ( x0 y0 x1 y1 2dy -- )
  0< IF Ym.Dpin ioc! dy @ NEGATE dy !
	 ELSE Ym.Dpin ios!
	 THEN ( x0 y0 x1 y1  -- )

  dx @ dy @ 2DUP > IF SWAP 2/ - ( x0 y0 x1 y1 fraction  )
					  >R DROP SWAP DROP - ABS ( fraction n )
					  R> SWAP
					  0
					  posE @ Esteps !
					  DO ( fraction )
						DUP 0 >= IF Ym.Spin #Ysteps step
									dx @ - (  fraction  )
								 THEN
						Xm.Spin #Xsteps step
						dy @ + ( fraction  )
					  LOOP

				   ELSE SWAP 2/ - ( x0 y0 x1 y1 fraction  )
						>R SWAP DROP ROT DROP - ABS
						R> SWAP ( fraction n )
						0
						posE @ Esteps !
						DO
						  DUP 0 >= IF Xm.Spin #Xsteps step
									  dy @ - (  fraction  )
								   THEN
						  Ym.Spin #Ysteps step
						  dx @ + ( fraction  )
						LOOP
				   THEN
  DROP
;

\ void IC(int xm, int ym, int r,int direction)
: arcBresenham ( xend yend r dir x y )
  \ int binrep = 0;
  \ Fxy,Dx,Dy;
  ( xend yend r dir x y )
  BEGIN
	2DUP DUP * SWAP DUP * + 4 PICK DUP * -
	0 >= IF 4
		 ELSE 0 THEN
	( xend yend r dir x y  binrep )
	3 PICK 0<> IF 8 + THEN
	( xend yend r dir x y  binrep )
   	2 PICK 2*
	0 >= IF 1+ 1+ THEN
	1 PICK 2*
	0 >= IF 1+ THEN
	( xend yend r dir x y  binrep )
	CASE
	  ( xend yend r dir x y)
	  0 OF 1-
		   Ym.Dpin ioc!
		   Ym.Spin #Ysteps step
		ENDOF
	  1 OF >R 1- R>
		   Xm.Dpin ioc!
		   Xm.Spin #Xsteps step
		ENDOF
	  2 OF >R 1+ R>
		   Xm.Dpin ios!
		   Xm.Spin #Xsteps step
		ENDOF
	  3 OF 1+
		   Ym.Dpin ios!
		   Ym.Spin #Ysteps step
		ENDOF
	  4 OF >R 1+ R>
		   Xm.Dpin ios!
		   Xm.Spin #Xsteps step
		ENDOF
	  5 OF 1-
		   Ym.Dpin ioc!
		   Ym.Spin #Ysteps step
		ENDOF
	  6 OF 1+
		   Ym.Dpin ios!
		   Ym.Spin #Ysteps step
		ENDOF
	  7 OF >R 1- R>
		   Xm.Dpin ioc!
		   Xm.Spin #Xsteps step
		ENDOF
	  8 OF >R 1- R>
		   Xm.Dpin ioc!
		   Xm.Spin #Xsteps step
		ENDOF
	  9 OF 1+
		   Ym.Dpin ios!
		   Ym.Spin #Ysteps step
		ENDOF
	  10 OF 1-
			Ym.Dpin ioc!
			Ym.Spin #Ysteps step
		 ENDOF
	  11 OF >R 1+ R>
			Xm.Dpin ios!
			Xm.Spin #Xsteps step
		 ENDOF
	  12 OF 1+
			Ym.Dpin ios!
			Ym.Spin #Ysteps step
		 ENDOF
	  13 OF >R 1+ R>
			Xm.Dpin ios!
			Xm.Spin #Xsteps step
		 ENDOF
	  14 OF >R 1- R>
			Xm.Dpin ioc!
			Xm.Spin #Xsteps step
		 ENDOF
	  15 OF 1-
			Ym.Dpin ioc!
			Ym.Spin #Ysteps step
		 ENDOF
	ENDCASE
	( xend yend r dir x y )
	2DUP 6 PICK = SWAP  7 PICK = AND
  UNTIL
  ( xend yend r dir x y )
  2DROP 2DROP 2DROP
;


! =========================================================
subroutine rp1(maxm,meqn,mwaves,maux,mbc,mx,ql,qr,auxl,auxr,wave,s,amdq,apdq)
! =========================================================
!
! solve Riemann problems for the traffic equation.

! waves: 1
! equations: 1

! Conserved quantities:
!       1 q

! On input, ql contains the state vector at the left edge of each cell
!           qr contains the state vector at the right edge of each cell
! On output, wave contains the waves,
!            s the speeds,
!            amdq the  left-going flux difference  A^- \Delta q
!            apdq the right-going flux difference  A^+ \Delta q
!
! Note that the i'th Riemann problem has left state qr(i-1,:)
!                                    and right state ql(i,:)
! From the basic clawpack routine step1, rp is called with ql = qr = q.

      implicit double precision (a-h,o-z)
      dimension   ql(meqn, 1-mbc:maxm+mbc)
      dimension   qr(meqn, 1-mbc:maxm+mbc)
      dimension    s(mwaves, 1-mbc:maxm+mbc)
      dimension wave(meqn,  mwaves,1-mbc:maxm+mbc)
      dimension amdq(meqn, 1-mbc:maxm+mbc)
      dimension apdq(meqn, 1-mbc:maxm+mbc)
      logical efix
      common /cparam/ umax
!
      efix = .true.   !# Compute correct flux for transonic rarefactions
!
      do 30 i=2-mbc,mx+mbc
!
!        # Compute the wave and speed
!
         wave(1,1,i) = ql(1,i) - qr(1,i-1)
         s(1,i) = umax * (1.d0 - (qr(1,i-1) + ql(1,i)))
!
!
!        # compute left-going and right-going flux differences:
!        ------------------------------------------------------
!
         amdq(1,i) = dmin1(s(1,i), 0.d0) * wave(1,1,i)
         apdq(1,i) = dmax1(s(1,i), 0.d0) * wave(1,1,i)
!
         if (efix) then
!           # entropy fix for transonic rarefactions:
            sim1 = umax*(1.d0 - 2.d0*ql(1,i-1))
            si = umax*(1.d0 - 2.d0*ql(1,i))
            if (sim1.lt.0.d0 .and. si.gt.0.d0) then
               flux0 = 0.25d0*umax
               fluxim1 = qr(1,i-1)*umax*(1.d0 - qr(1,i-1))
               fluxi   = ql(1,i)*umax*(1.d0 - qr(1,i))
               amdq(1,i) = flux0 - fluxim1
               apdq(1,i) = fluxi - flux0
               endif
            endif
   30   continue
!
      return
      end