!##############   STANDARD  ##############
subroutine make_standard(nside,map,mskv,npixr,mean,var)

implicit none

integer, intent (in) :: nside
Real*8, intent (in) :: mskv
Real*8, intent (inout) :: map(nside,nside)
integer, intent (out) :: npixr
Real*8, intent (out) :: mean,var

integer  i,j

npixr=0
mean=0
do i=1,nside
	do j=1,nside
	  if (map(i,j)/=mskv) then
	    mean=mean+map(i,j)
	    npixr=npixr+1
	  end if
	end do
end do

mean=mean/npixr
map=map-mean

var=0
do i=1,nside
	do j=1,nside
	  if (map(i,j)/=mskv) then
	    var=var+map(i,j)**2
	  end if
	end do
end do
var=sqrt(var/npixr)
map=map/var

end

!##############   Threshold finder  ##############
subroutine thsh_f(rows,columns,d,lt,ut,nt,thsh)
implicit none

integer, intent (in) :: rows,columns
Real*8, intent (in) :: lt,ut
Real*8, intent (in) :: d(rows,columns)
integer, intent (out) :: nt
integer, intent (out) :: thsh(2,rows*columns)

integer  i,j

nt=0

do i=2,rows-1
  do j=2,columns-1
    if(d(i,j)>lt .and. d(i,j)<ut) then
      nt=nt+1
      thsh(1,nt)=i
      thsh(2,nt)=j
    end if
  end do
end do

end

!##############   Peak finder  ##############
subroutine peak_f(rows,columns,d,lt,ut,np,peaks)
implicit none

integer, intent (in) :: rows,columns
Real*8, intent (in) :: lt,ut
Real*8, intent (in) :: d(rows,columns)
integer, intent (out) :: np
integer, intent (out) :: peaks(2,rows*columns)

integer  i,j
Real*8 temp

np=0

do i=2,rows-1
  do j=2,columns-1
    if(d(i,j)>lt .and. d(i,j)<ut) then

      temp=max(d(i+1,j),d(i,j+1),d(i-1,j),d(i,j-1), &
& d(i+1,j+1),d(i+1,j-1),d(i-1,j+1),d(i-1,j-1))
      if (d(i,j)>temp)  then
        np=np+1
        peaks(1,np)=i
        peaks(2,np)=j
      end if
    end if
  end do
end do

end

!##############   Cross finder  ##############
subroutine cross_f(rows,columns,d,lt,ut,nc1,nc2,cr1,cr2)
implicit none

integer, intent (in) :: rows,columns
Real*8, intent (in) :: lt,ut
Real*8, intent (in) :: d(rows,columns)
integer, intent (out) :: nc1,nc2
integer, intent (out) :: cr1(2,rows*columns),cr2(2,rows*columns)

integer  i,j
Real*8 temp

nc1=0; nc2=0

do i=2,rows-1
  do j=2,columns-1

! First Direction
    if(d(i,j)>lt .and. d(i-1,j)<lt) then
	     nc1=nc1+1
	     cr1(1,nc1)=i
	     cr1(2,nc1)=j
		end if
! Second Direction
    if(d(i,j)>lt .and. d(i,j-1)<lt) then
		   nc2=nc2+1
		   cr2(1,nc2)=i
		   cr2(2,nc2)=j
		end if

  end do
end do
end

!##############   Ridge finder  ##############
subroutine ridge_f(rows,columns,d,lt,ut,nr1,nr2,rdg1,rdg2)
implicit none

integer, intent (in) :: rows,columns
Real*8, intent (in) :: lt,ut
Real*8, intent (in) :: d(rows,columns)
integer, intent (out) :: nr1,nr2
integer, intent (out) :: rdg1(2,rows*columns),rdg2(2,rows*columns)

integer  i,j
Real*8 temp

nr1=0; nr2=0

do i=2,rows-1
  do j=2,columns-1

! First Direction
    if(d(i,j)>lt .and. d(i-1,j)<lt) then
			if (d(i,j+1)<d(i,j) .and. d(i,j-1)<d(i,j)) then
		    nr1=nr1+1
		    rdg1(1,nr1)=i
		    rdg1(2,nr1)=j
			end if
    end if

! Second Direction
    if(d(i,j)>lt .and. d(i,j-1)<lt) then
			if (d(i+1,j)<d(i,j) .and. d(i-1,j)<d(i,j)) then
		    nr2=nr2+1
		    rdg2(1,nr2)=i
		    rdg2(2,nr2)=j
			end if
    end if

  end do
end do
end

!##############   P-P Correlation  ##############
subroutine   ppcf(ds,sze,x,y,n_pks,ksi,vksi,n_sh,mth,lng)
implicit none

integer, intent (in) :: sze,n_pks,n_sh,mth,lng
Real*8, intent (in) :: ds
integer, intent (in) :: x(n_pks),y(n_pks)
Real*8, intent (out) :: ksi(0:lng),vksi(0:lng)

Integer  i,j,k
Real*8  gam,ran1,ran2
integer DD(0:lng),DR(0:lng),RR(0:lng)
integer x_sh(n_sh),y_sh(n_sh)

 call init_random_seed()

ksi=0; vksi=0
dd=0; dr=0; rr=0

do i=1,n_sh
  call random_number(ran1)
  call random_number(ran2)
  x_sh(i)=int(ran1*dble(sze))
  y_sh(i)=int(ran2*dble(sze))
end do

do i=1,n_pks-1
  do j=i+1,n_pks
!    if (max(abs(x(i)-x(j)),abs(y(i)-y(j)))>lng )cycle
    gam=sqrt( real( (x(i)-x(j))**2 + (y(i)-y(j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) DD(k)=DD(k)+1
  enddo
enddo

do i=1,n_pks
  do j=1,n_sh
!    if (max(abs(x(i)-x_sh(j)),abs(y(i)-y_sh(j)))>lng )cycle
    gam=sqrt( real( (x(i)-x_sh(j))**2 + (y(i)-y_sh(j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) DR(k)=DR(k)+1
  end do
end do

if (mth==2 .or. mth==3) then
do i=1,n_sh
  do j=i+1,n_sh
    gam=sqrt( real( (x_sh(i)-x_sh(j))**2 + (y_sh(i)-y_sh(j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) RR(k)=RR(k)+1
  end do
end do
end if

do k=1, lng
  if(DR(k).ne.0) then
if (mth==1) ksi(k)=2.*(n_sh*1./n_pks)*DD(k)*1./DR(k)-1.
if (mth==2) ksi(k)=4.*(DD(k)*RR(k)*1./DR(k)**2)-1.
if (mth==3) ksi(k)=(DD(k)*1./RR(k))*(n_sh*1./n_pks)**2-2*(DR(k)*1./RR(k))*(n_sh*1./n_pks) + 3.

    vksi(k)=2.*(n_sh*1./n_pks)*(  ((1.-2*DD(k)/(n_pks*(n_pks-1)))/DR(k))**2 + &
	& (DD(k)*(1.-2*DR(k)/n_sh*(n_sh-1))/DR(k)**2)**2  )
  end if
end do

end

!##############   P-P Correlation  ##############
subroutine   ppcf_getrand(ds,sze,x,y,n_pks,ksi,vksi,n_sh,x_sh,y_sh,mth,lng)
implicit none

integer, intent (in) :: sze,n_pks,n_sh,mth,lng
Real*8, intent (in) :: ds
integer, intent (in) :: x(n_pks),y(n_pks)
integer, intent (in) :: x_sh(n_sh),y_sh(n_sh)
Real*8, intent (out) :: ksi(0:lng),vksi(0:lng)

Integer  i,j,k
Real*8  gam,ran1,ran2
integer DD(0:lng),DR(0:lng),RR(0:lng)
integer x_sh(n_sh),y_sh(n_sh)

 call init_random_seed()

ksi=0; vksi=0
dd=0; dr=0; rr=0

do i=1,n_pks-1
  do j=i+1,n_pks
!    if (max(abs(x(i)-x(j)),abs(y(i)-y(j)))>lng )cycle
    gam=sqrt( real( (x(i)-x(j))**2 + (y(i)-y(j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) DD(k)=DD(k)+1
  enddo
enddo

do i=1,n_pks
  do j=1,n_sh
!    if (max(abs(x(i)-x_sh(j)),abs(y(i)-y_sh(j)))>lng )cycle
    gam=sqrt( real( (x(i)-x_sh(j))**2 + (y(i)-y_sh(j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) DR(k)=DR(k)+1
  end do
end do

if (mth==2 .or. mth==3) then
do i=1,n_sh
  do j=i+1,n_sh
    gam=sqrt( real( (x_sh(i)-x_sh(j))**2 + (y_sh(i)-y_sh(j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) RR(k)=RR(k)+1
  end do
end do
end if

do k=1, lng
  if(DR(k).ne.0) then
if (mth==1) ksi(k)=2.*(n_sh*1./n_pks)*DD(k)*1./DR(k)-1.
if (mth==2) ksi(k)=4.*(DD(k)*RR(k)*1./DR(k)**2)-1.
if (mth==3) ksi(k)=(DD(k)*1./RR(k))*(n_sh*1./n_pks)**2-2*(DR(k)*1./RR(k))*(n_sh*1./n_pks) + 3.

    vksi(k)=2.*(n_sh*1./n_pks)*(  ((1.-2*DD(k)/(n_pks*(n_pks-1)))/DR(k))**2 + &
	& (DD(k)*(1.-2*DR(k)/n_sh*(n_sh-1))/DR(k)**2)**2  )
  end if
end do

end

!##############   RANDOM SEED GERERATOR  ##############
subroutine init_random_seed()
  use iso_fortran_env, only: int64
implicit none
  integer, allocatable :: seed(:)
  integer :: i, n, un, istat, dt(8), pid
  integer(int64) :: t

  call random_seed(size = n)
  allocate(seed(n))
  ! First try if the OS provides a random number generator
  open(newunit=un, file="/dev/urandom", access="stream", &
       form="unformatted", action="read", status="old", iostat=istat)
  if (istat == 0) then
     read(un) seed
     close(un)
  else
     ! Fallback to XOR:ing the current time and pid. The PID is
     ! useful in case one launches multiple instances of the same
     ! program in parallel.
     call system_clock(t)
     if (t == 0) then
        call date_and_time(values=dt)
        t = (dt(1) - 1970) * 365_int64 * 24 * 60 * 60 * 1000 &
   + dt(2) * 31_int64 * 24 * 60 * 60 * 1000 &
   + dt(3) * 24_int64 * 60 * 60 * 1000 &
   + dt(5) * 60 * 60 * 1000 &
   + dt(6) * 60 * 1000 + dt(7) * 1000 &
   + dt(8)
     end if
     pid = getpid()
     t = ieor(t, int(pid, kind(t)))
     do i = 1, n
        seed(i) = lcg(t)
     end do
  end if
  call random_seed(put=seed)
contains
  ! This simple PRNG might not be good enough for Real*8 work, but is
  ! sufficient for seeding a better PRNG.
  function lcg(s)
    integer :: lcg
    integer(int64) :: s
    if (s == 0) then
       s = 104729
    else
       s = mod(s, 4294967296_int64)
    end if
    s = mod(s * 279470273_int64, 4294967291_int64)
    lcg = int(mod(s, int(huge(0), int64)), kind(0))
  end function lcg
end subroutine init_random_seed

!##############   TIME STAMP  ##############
subroutine timestamp ( )

!   call timestamp ( )

implicit none

  character ( len = 8 ) ampm
  integer ( kind = 4 ) d
  integer ( kind = 4 ) h
  integer ( kind = 4 ) m
  integer ( kind = 4 ) mm
  character ( len = 9 ), parameter, dimension(12) :: month = (/ &
    'January  ', 'February ', 'March    ', 'April    ', &
    'May      ', 'June     ', 'July     ', 'August   ', &
    'September', 'October  ', 'November ', 'December ' /)
  integer ( kind = 4 ) n
  integer ( kind = 4 ) s
  integer ( kind = 4 ) values(8)
  integer ( kind = 4 ) y

  call date_and_time ( values = values )

  y = values(1)
  m = values(2)
  d = values(3)
  h = values(5)
  n = values(6)
  s = values(7)
  mm = values(8)

  if ( h < 12 ) then
    ampm = 'AM'
  else if ( h == 12 ) then
    if ( n == 0 .and. s == 0 ) then
      ampm = 'Noon'
    else
      ampm = 'PM'
    end if
  else
    h = h - 12
    if ( h < 12 ) then
      ampm = 'PM'
    else if ( h == 12 ) then
      if ( n == 0 .and. s == 0 ) then
        ampm = 'Midnight'
      else
        ampm = 'AM'
      end if
    end if
  end if

  write ( *, '(i2,1x,a,1x,i4,2x,i2,a1,i2.2,a1,i2.2,a1,i3.3,1x,a)' ) &
    d, trim ( month(m) ), y, h, ':', n, ':', s, '.', mm, trim ( ampm )

  return
end

!##############   CROSS  ##############
subroutine cross_old(p00,p0m,pm0,thsh,mskv,msrx,msry)

implicit none

real*8, intent(in) :: p00,p0m,pm0,thsh,mskv
 logical, intent(out) :: msrx,msry

msrx=(pm0<thsh) .and. (p00>thsh) .and. (p00/=mskv)
msry=(p0m<thsh) .and. (p00>thsh) .and. (p00/=mskv)

end

!##############   F-F Correlation  ##############
subroutine   ffcf(ds,sze,fl1,n_pks1,fl2,n_pks2,ksi,vksi,n_sh,mth,lng)

implicit none

integer, intent (in) :: sze,n_pks1,n_pks2,n_sh,mth,lng
Real*8, intent (in) :: ds
integer, intent (in) :: fl1(2,n_pks1)
integer, intent (in) :: fl2(2,n_pks2)
Real*8, intent (out) :: ksi(0:lng),vksi(0:lng)

Integer  i,j,k
Real*8  gam,ran1,ran2
Real*8 DD(0:lng),DR1(0:lng),DR2(0:lng),RR(0:lng)
Integer x_sh(n_sh),y_sh(n_sh)

 call init_random_seed()

ksi=0; vksi=0
dd=0; dr1=0; dr2=0; rr=0

do i=1,n_sh
  call random_number(ran1)
  call random_number(ran2)
  x_sh(i)=int(ran1*dble(sze))
  y_sh(i)=int(ran2*dble(sze))
end do

do i=1,n_pks1
  do j=1,n_pks2
!    if (max(abs(x(i)-x(j)),abs(y(i)-y(j)))>lng )cycle
    gam=sqrt( real( (fl1(1,i)-fl2(1,j))**2 + (fl1(2,i)-fl2(2,j))**2 ) )
    k=floor(gam/ds)
    if (k<=lng) DD(k)=DD(k)+1
  enddo
enddo
DD=1.*DD/(n_pks1*n_pks2)

do i=1,n_pks1
  do j=1,n_sh
!    if (max(abs(x(i)-x_sh(j)),abs(y(i)-y_sh(j)))>lng )cycle
    gam=sqrt( real((fl1(1,i)-x_sh(j))**2 + (fl1(2,i)-y_sh(j))**2) )
    k=floor(gam/ds)
    if (k<=lng) DR1(k)=DR1(k)+1
  end do
end do
DR1=1.*DR1/(n_pks1*n_sh)

do i=1,n_pks2
  do j=1,n_sh
!    if (max(abs(x(i)-x_sh(j)),abs(y(i)-y_sh(j)))>lng )cycle
    gam=sqrt( real((fl2(1,i)-x_sh(j))**2 + (fl2(2,i)-y_sh(j))**2) )
    k=floor(gam/ds)
    if (k<=lng) DR2(k)=DR2(k)+1
  end do
end do
DR2=1.*DR2/(n_pks2*n_sh)

if (mth==2 .or. mth==3) then
print*, "This part is out of service for now!"
!do i=1,n_sh
!  do j=i+1,n_sh
!    gam=sqrt( real( (x_sh(i)-x_sh(j))**2 + (y_sh(i)-y_sh(j))**2 ) )
!    k=floor(gam/ds)
!    if (k<=lng) RR(k)=RR(k)+1
!  end do
!end do
end if

do k=1, lng
  if(DR1(k)+DR2(k).ne.0) then
if (mth==1) ksi(k)=2*DD(k)/(DR1(k)+DR2(k))-1
!if (mth==2) ksi(k)=4.*(DD(k)*RR(k)*1./DR(k)**2)-1.
!if (mth==3) ksi(k)=(DD(k)*1./RR(k))*(n_sh*1./n_pks)**2-2*(DR(k)*1./RR(k))*(n_sh*1./n_pks) + 3.
vksi(k)=0
!    vksi(k)=2.*(n_sh*1./n_pks)*(  ((1.-2*DD(k)/(n_pks*(n_pks-1)))/DR(k))**2 + &
!	& (DD(k)*(1.-2*DR(k)/n_sh*(n_sh-1))/DR(k)**2)**2  )
  end if
end do

end

!##############  Correlation  ##############
subroutine cf(map,l,n_p,mskv,cor,vcor)
implicit none

integer, intent(in) :: l
real*8, intent(in) :: map(l,l)
real*8, intent(in) :: n_p
real*8, intent(in) :: mskv
real*8, intent(out) :: cor(0:l),vcor(0:l)

integer :: i,i1,j1,i2,j2,k,kp,limit
integer :: nu(0:l)
real*8 :: r1,r2,r3,gam
real*8 :: c(0:l,int(3*n_p/l))

limit=int(3*n_p/l)
cor=0
vcor=0
nu=0

i=0
Do while (i<n_p/100)
i=i+1

  call random_number(r1)
  call random_number(r2)
  call random_number(r3)
	r1=floor((l*l-1)*r1)
	r2=floor((l-1)/10.*r2)
	r3=floor((l-1)/10.*r3)
  i1=mod(int(r1),l)+1
  j1=floor(r1/l)+1
  i2=mod(int(i1+r2+1),l)+1
  j2=mod(int(j1+r3+1),l)+1

  if(map(i1,j1)/=mskv.and.map(i2,j2)/=mskv) then
    gam=sqrt( real( (i1-i2)**2 + (j1-j2)**2 ) )
    k=floor(gam)
		if (k<=l .and. nu(k)<limit) then
			nu(k)=nu(k)+1
			kp=nu(k)
			c(k,kp)=map(i1,j1)*map(i2,j2)
		end if
  end if
end do !i

Do while (i<n_p)
i=i+1

  call random_number(r1)
  call random_number(r2)
	r1=floor((l*l-1)*r1)
	r2=floor((l*l-1)*r2)
  i1=mod(int(r1),l)+1
  j1=floor(r1/l)+1
  i2=mod(int(r2),l)+1
  j2=floor(r2/l)+1

  if(map(i1,j1)/=mskv.and.map(i2,j2)/=mskv) then

    gam=sqrt( real( (i1-i2)**2 + (j1-j2)**2 ) )
    k=floor(gam)
		if (k<=l .and. nu(k)<limit) then
			nu(k)=nu(k)+1
			kp=nu(k)
			c(k,kp)=map(i1,j1)*map(i2,j2)
		end if
  end if
end do !i

! mean
  do k=0,l
    kp=nu(k)
    do i=1,kp
      cor(k)=cor(k)+c(k,i)
    end do
    if(kp.ne.0.0) cor(k)=cor(k)/real(kp)
  end do

! error
  do k=0,l
    kp=nu(k)
    do i=1,kp
      vcor(k)=vcor(k)+(c(k,i)-cor(k))**2
    end do
    if(kp.ne.0.0) vcor(k)=sqrt(vcor(k)/real(kp))
  end do

end

!##############  Cross Correlation  ##############
subroutine cross_cf(map1,map2,l,n_p,mskv,cor,vcor)
implicit none

integer, intent(in) :: l
real*8, intent(in) :: map1(l,l),map2(l,l)
real*8, intent(in) :: n_p
real*8, intent(in) :: mskv
real*8, intent(out) :: cor(0:l),vcor(0:l)

integer :: i,i1,j1,i2,j2,k,kp,limit
integer :: nu(0:l)
real*8 :: r1,r2,r3,gam
real*8 :: c(0:l,int(3*n_p/l))

limit=int(3*n_p/l)
cor=0
vcor=0
nu=0

i=0
Do while (i<n_p/100)
i=i+1

11 call random_number(r1)
  call random_number(r2)
  call random_number(r3)
	r1=floor((l*l-1)*r1)
	r2=floor((l-1)/10.*r2)
	r3=floor((l-1)/10.*r3)
  i1=mod(int(r1),l)+1
  j1=floor(r1/l)+1
  i2=i1+r2+1
  j2=j1+r3+1

	if (i2>l .or. j2>l) goto 11

  if(map1(i1,j1)/=mskv.and.map2(i2,j2)/=mskv) then
    gam=sqrt( real( (i1-i2)**2 + (j1-j2)**2 ) )
    k=floor(gam)
		if (k<=l .and. nu(k)<limit) then
			nu(k)=nu(k)+1
			kp=nu(k)
			c(k,kp)=map1(i1,j1)*map2(i2,j2)
		end if
  end if
end do !i

Do while (i<n_p)
i=i+1

  call random_number(r1)
  call random_number(r2)
	r1=floor((l*l-1)*r1)
	r2=floor((l*l-1)*r2)
  i1=mod(int(r1),l)+1
  j1=floor(r1/l)+1
  i2=mod(int(r2),l)+1
  j2=floor(r2/l)+1

  if(map1(i1,j1)/=mskv.and.map2(i2,j2)/=mskv) then

    gam=sqrt( real( (i1-i2)**2 + (j1-j2)**2 ) )
    k=floor(gam)
		if (k<=l .and. nu(k)<limit) then

			nu(k)=nu(k)+1
			kp=nu(k)
			c(k,kp)=map1(i1,j1)*map2(i2,j2)
		end if

  end if
end do !i

! mean
  do k=0,l
    kp=nu(k)
    do i=1,kp
      cor(k)=cor(k)+c(k,i)
    end do
    if(kp.ne.0.0) cor(k)=cor(k)/real(kp)
  end do

! error
  do k=0,l
    kp=nu(k)
    do i=1,kp
      vcor(k)=vcor(k)+(c(k,i)-cor(k))**2
    end do
    if(kp.ne.0.0) vcor(k)=sqrt(vcor(k)/real(kp))
  end do

end

!##############  VDD  ##############
subroutine vdd(map,l,w,dx,dy,vx,vy)

integer, intent(in) :: l,w
Real*8, intent(in) :: map(l,w)
Real*8, intent(out) :: dx(l,w),dy(l,w),vx,vy

integer i,j,npix,nind

do i=1,l
  nind=mod(i,l)+1
  dx(i,:)=map(i,:)-map(nind,:)
end do
do j=1,w
  nind=mod(j,w)+1
  dy(:,j)=map(:,j)-map(:,nind)
end do

mx=0
vx=0
my=0
vy=0

do i=1,l
  do j=1,w
    mx=dx(i,j)+mx
  end do
end do
mx=mx/((l)*w)

do i=1,l
  do j=1,w
    vx=(dx(i,j)-mx)**2+vx
  end do
end do
vx=sqrt(vx/((l)*w))

do i=1,l
  do j=1,w
    my=dy(i,j)+my
  end do
end do
my=my/((w)*l)

do i=1,l
  do j=1,w
    vy=(dy(i,j)-my)**2+vy
  end do
end do
vy=sqrt(vy/((w)*l))

end subroutine

!##############  Beamer  ##############
subroutine beamer(num,map,siz,fwhm,beamfun)

integer, intent (in) :: num
real*8, intent (inout) :: map(num,num)
real*8, intent (in) :: siz,fwhm
 character(256), intent (in) :: beamfun

integer lensav,ier,ldim,lenwrk,m_fft,l_fft
real*8 nu_band,diam,optele,FWHM_rad,pi,sigma_beam
real*8 delta_wavelength_x,l_k
real*8 ,allocatable ::WSAVE(:),WORK(:)
COMPLEX(8) ,allocatable :: cmap(:,:)

pi=4*atan(1.0)

nu_band=fwhm         !147e9    !Hz
diam=6    !m
optele=70     !Degree
ldim=num
m_fft=num
l_fft=num

lensav=2*(L_fft+M_fft) + INT(LOG(REAL(L_fft))/log(2.0))+ INT(LOG(REAL(M_fft))/log(2.0)) + 8
lenwrk=2*L_fft*M_fft+10

allocate(cmap(ldim,m_fft),WSAVE(lensav),WORK(lenwrk))

delta_wavelength_x=2*pi/(siz*pi/180)

if (beamfun=="gaussian") then
  FWHM_rad=pi*FWHM/(60.0*180.0)
  sigma_beam=FWHM_rad/sqrt(8*log(2.0))
else if (beamfun=="airy") then
  sigma_beam=360.*nu_band*diam/(optele*299792458.)
end if

 cmap = map
 call cfft2i ( l_fft,m_fft, wsave, lensav, ier )
 call cfft2f ( ldim, l_fft, m_fft, cmap, wsave, lensav, work, lenwrk, ier )

!********************** for me ********************
do i=1,num
do j=1,num

! this true when we use L as radian
l_k=sqrt((i*1.0)**2+(j*1.0)**2)*delta_wavelength_x

if (beamfun=="gaussian") then
  beam= exp(-(sigma_beam**2)*l_k*(l_k+1)/2.)
else if (beamfun=="airy") then
  if (l_k<sigma_beam) then
    beam=(2./pi)*(acos(l_k/sigma_beam)-(l_k/sigma_beam) &
& *sqrt(1-(l_k/sigma_beam)**2))
  else
    beam=0
  end if
end if
 cmap(i,j)=cmap(i,j)*beam    !*(size/2*pi)**2)
enddo
enddo

 call cfft2b ( ldim, l_fft, m_fft, cmap, wsave, lensav, work, lenwrk, ier )

map = real(cmap)

end


! **************************************************************************
! ... nudging.f90
! ... Quim Ballabrera, February 2013
! ...
! ... System Dynamics
! ...
! **************************************************************************

PROGRAM nudging

implicit none

use sysdyn

!INCLUDE 'lorenz.h90'
!INCLUDE 'obs.h90'
!INCLUDE 'nudging.h90'

integer, parameter                          :: NVE=3

logical default,Zexist,continuous

integer i,ii,step,ensemble,nensembles,iu,idum
real(dp) Tf,T,Jc,a,b,mumax, mumin,Tmin
real(dp), DIMENSION(NVE)                :: Xo,X,Xn,Xm,Xs
real(dp), DIMENSION(:,:), ALLOCATABLE   :: Yint
character(LEN=180) ofile,dfile,opath,efile,nfile

REAL(KIND=8) gasdev,xran1

NAMELIST/namnudging/continuous,default,idum,to,dt,tf,sigma,rho,beta,ofile,Iobs,dfile,opath

EXTERNAL rhnud,rhwnud
EXTERNAL gasdev,xran1

idum = -1
nensembles = 20
Tx = 0.25
Ty = 0.25
Tz = 0.25

call getarg(1,nfile)

inquire(file=nfile,exist=Zexist)

if (Zexist) then
  write(*,*) 'Reading namelist file: ', trim(nfile)
  open(10,file=nfile,status='old')
  rewind(10)
  read(10,namnudging)
  close(10)
else
  default = .true.
endif

IF (default) THEN
  opath = './'
  ofile = './obs_noise_02_sampl_10.dat'
  dfile = 'nudging_noise_02_sampl_010.dat'

  ! ... Model parameters:
  ! ...
  sigma = 10.0D0
  rho   = 28.0D0
  beta  = 8.0D0/3.0D0

  ! ... Initial conditions
  ! ...
  to =  0.0D0
  tf = 20.0D0
  dt = 0.01D0

  ! ... What do we assimilate
  ! ...
  Iobs(:) = 1   ! All obs are assimilated

  ! ... How do assimilate:
  ! ...
  continuous = .false.
ENDIF

! ... Outputh directory:
! ...
! ... Open output path
! ...
IF ((TRIM(opath).EQ.'.').OR.         &
    (TRIM(opath).EQ.'./').OR.        &
    (LEN_TRIM(opath).EQ.0)) THEN
   ! ... Do nothing
   opath = '.'
ELSE
  i = LEN_TRIM(opath)
  IF (opath(i:i).NE.'/') opath(i+1:i+1) = '/'
  WRITE(*,*) 'mkdir -p '//TRIM(opath)
  CALL SYSTEM ('mkdir -p '//TRIM(opath))
ENDIF

! ... Model mean and standard deviation
! ...
OPEN(10,file='../makeobs/true_simulation_ave.dat',status='old')
REWIND(10)
READ(10,*) T, Xm
CLOSE(10)

OPEN(10,file='../makeobs/true_simulation_std.dat',status='old')
REWIND(10)
READ(10,*) T, Xs
CLOSE(10)

WRITE(*,*)
WRITE(*,*) 'Long term average'
WRITE(*,'(3F9.3)') Xm
WRITE(*,*) 'Standard deviation'
WRITE(*,'(3F9.3)') Xs

WRITE(*,*)
WRITE(*,*) 'Output diagnostic file : ', TRIM(opath)//TRIM(dfile)

OPEN(72,file=TRIM(opath)//TRIM(dfile),status='unknown')
REWIND(72)

! ... Read the observations
! ...
CALL read_obs(ofile)

WRITE(*,*)
WRITE(*,*) 'obs file          = ', TRIM(ofile)
WRITE(*,*) 'Nobs              = ', Nobs
WRITE(*,*) 'First time record = ', Tobs(1)
WRITE(*,*) 'Last time record  = ', Tobs(Nobs)
WRITE(*,*) 'Iobs              = ', Iobs(:)
WRITE(*,*) 'continuous        = ', continuous

WRITE(72,*)
WRITE(72,*) 'obs file          = ', TRIM(ofile)
WRITE(72,*) 'Nobs              = ', Nobs
WRITE(72,*) 'First time record = ', Tobs(1)
WRITE(72,*) 'Last time record  = ', Tobs(Nobs)
WRITE(72,*) 'Iobs              = ', Iobs(:)
WRITE(72,*) 'continuous        = ', continuous

To = Tobs(1)
Tf = Tobs(Nobs)

nstep = NINT((tf-to)/dt)

WRITE(*,*)
WRITE(*,*) 'Model initial time = ', to
WRITE(*,*) 'Model final time   = ', tf
WRITE(*,*) 'Model time step    = ', dt
WRITE(*,*) 'Number time steps  = ', nstep + 1

WRITE(72,*)
WRITE(72,*) 'Model initial time = ', to
WRITE(72,*) 'Model final time   = ', tf
WRITE(72,*) 'Model time step    = ', dt
WRITE(72,*) 'Number time steps  = ', nstep + 1

! ... Make the mapping between observations and the model time steps and the
! ...
ii = 1
t  = to
DO i=1,nstep+1
  IF (ii.LE.Nobs) THEN
  IF (ABS(t-Tobs(ii)).LT.0.49*dt) THEN
    Pobs(ii) = i
    ii = ii + 1
  ENDIF
  ENDIF
  t = t + dt
ENDDO

! ... Interpolate the obs to each time step of the model
! ...
ALLOCATE (Yint(NVE,nstep+1))

t = to
DO step=1,nstep+1
  IF (step.EQ.1) THEN
    Yint(:,1) = Yobs(:,1)
  ELSE IF (step.EQ.nstep+1) THEN
    Yint(:,nstep+1) = Yobs(:,Nobs)
  ELSE
    DO ii=1,Nobs-1
      IF (Tobs(ii).LE.t.AND.Tobs(ii+1).GT.t) EXIT
    ENDDO
! ... Y = a*t + b
! ... Yo(ii) = a*Tobs(ii) + b
! ... Yo(ii+1) = a*Tobs(ii+1) + b
! ... Yo(ii+1)-Yo(ii) = a*(Tobs(ii+1)-Tobs(ii))
    DO i=1,NVE
      a = (Yobs(i,ii+1)-Yobs(i,ii))/(Tobs(ii+1)-Tobs(ii))
      b = Yobs(i,ii)-a*Tobs(ii)
      Yint(i,step) = a*t + b
    ENDDO
  ENDIF
  t = t + dt
ENDDO

WRITE(*,*)
WRITE(*,*) 'Idum         = ', idum
WRITE(*,*) 'nensembles   = ', nensembles

WRITE(72,*)
WRITE(72,*) 'Idum         = ', idum
WRITE(72,*) 'nensembles   = ', nensembles

mumax = 1.0D0/dt
mumin = 1.0D0/(3.0d0*dt)

WRITE(*,*)
WRITE(*,*) 'min mu = ', mumin, mumin*dt
WRITE(*,*) 'max mu = ', mumax, mumax*dt

WRITE(72,*)
WRITE(72,*) 'min mu = ', mumin, mumin*dt
WRITE(72,*) 'max mu = ', mumax, mumax*dt

OPEN(45,file=TRIM(opath)//'initial.condition',status='unknown')
REWIND(45)
WRITE(45,*) NVE, nensembles

OPEN(46,file=TRIM(opath)//'nudging.coefs',status='unknown')
REWIND(46)

OPEN(47,file=TRIM(opath)//'scale.coefs',status='unknown')
REWIND(47)

Tmin = 1*dt

DO ensemble=1,nensembles

  DO i=1,NVE
    Xo(i) = Xm(i) + Xs(i)*gasdev(idum)
  ENDDO
  WRITE(45,*) ensemble*1.0
  WRITE(45,*) Xo(:)

  mux = (mumax-mumin)*xran1(idum) + mumin
  muy = (mumax-mumin)*xran1(idum) + mumin
  muz = (mumax-mumin)*xran1(idum) + mumin

  Tx = (0.25-Tmin)*xran1(idum) + Tmin
  Ty = (0.25-Tmin)*xran1(idum) + Tmin
  Tz = (0.25-Tmin)*xran1(idum) + Tmin

  Tx = 0.10D0; Ty = 0.10D0; Tz = 0.10D0

  WRITE(72,*)
  WRITE(72,*) 'ensemble, unit         = ', ensemble, 100+ensemble-1
  WRITE(72,*) 'Initial conditions     = ', Xo
  WRITE(72,*) 'X nudging coefficient  = ', mux
  WRITE(72,*) 'Y nudging coefficient  = ', muy
  WRITE(72,*) 'Z nudging coefficient  = ', muz
  WRITE(72,*) 'X decorrelation scale  = ', Tx
  WRITE(72,*) 'Y decorrelation scale  = ', Ty
  WRITE(72,*) 'Z decorrelation scale  = ', Tz
  WRITE(46,'(3F9.3)') mux,muy,muz
  WRITE(47,'(3F7.3)') Tx,Ty,Tz

! ... Nudging data assimilation:
! ...
  iu = 100 + ensemble - 1
  WRITE(efile,'(T1,"fort.",i3.3)') iu
  OPEN(iu,file=TRIM(opath)//TRIM(efile),STATUS='unknown')
  REWIND(iu)

  t = To
  X = Xo
  WRITE(iu,'(4F10.4)') t, X

  ii = 2
  DO step=1,nstep

    applyx = 0
    applyy = 0
    applyz = 0
    Yo(:) = Yint(:,step+1)

    IF (continuous) THEN
      applyx = 1
      applyy = 1
      applyz = 1
    ELSE
      IF (step.EQ.Pobs(ii)) THEN
        applyx = 1
        applyy = 1
        applyz = 1
        ii = ii + 1
      ENDIF
    ENDIF

    CALL rk4 (X,NVE,T,dt,XN,rhwnud)
    X = XN
    t = t + dt
    WRITE(iu,'(4F10.4)') t, X

  ENDDO
  CLOSE(iu)

ENDDO

CLOSE(45)

STOP 'Ok'
END PROGRAM nudging
! ...
! ==========================================================================
! ...
SUBROUTINE rhnud (t,x,dxdt)

IMPLICIT NONE

INCLUDE 'lorenz.h90'
INCLUDE 'nudging.h90'
INCLUDE 'obs.h90'

REAL(KIND=8), INTENT(in)    :: t
REAL(KIND=8), INTENT(in)    :: x(NVE)
REAL(KIND=8), INTENT(out)   :: dxdt(NVE)

dxdt(1) = sigma*(X(2)-X(1))
dxdt(2) = (rho-X(3))*X(1) - X(2)
dxdt(3) = X(1)*X(2) - beta*X(3)

IF (Iobs(1).GE.1) dxdt(1) = dxdt(1) + applyx*mux*(Yo(1)-X(1))
IF (Iobs(2).GE.1) dxdt(2) = dxdt(2) + applyy*muy*(Yo(2)-X(2))
IF (Iobs(3).GE.1) dxdt(3) = dxdt(3) + applyz*muz*(Yo(3)-X(3))

RETURN
END SUBROUTINE rhnud
! ...
! ==========================================================================
! ...
SUBROUTINE rhwnud (t,x,dxdt)

IMPLICIT NONE

INCLUDE 'lorenz.h90'
INCLUDE 'nudging.h90'
INCLUDE 'obs.h90'

REAL(KIND=8), INTENT(in)    :: t
REAL(KIND=8), INTENT(in)    :: x(NVE)
REAL(KIND=8), INTENT(out)   :: dxdt(NVE)

REAL(KIND=8) xsum,ysum,zsum

CALL weight_obs (t,x,xsum,ysum,zsum)

dxdt(1) = sigma*(X(2)-X(1))
dxdt(2) = (rho-X(3))*X(1) - X(2)
dxdt(3) = X(1)*X(2) - beta*X(3)

IF (Iobs(1).GE.1) dxdt(1) = dxdt(1) + mux*xsum
IF (Iobs(2).GE.1) dxdt(2) = dxdt(2) + muy*ysum
IF (Iobs(3).GE.1) dxdt(3) = dxdt(3) + muz*zsum

RETURN
END SUBROUTINE rhwnud
! ...
! ==========================================================================
! ...
SUBROUTINE weight_obs (Time,X,xsum,ysum,zsum)

IMPLICIT NONE

INCLUDE 'lorenz.h90'
INCLUDE 'nudging.h90'
INCLUDE 'obs.h90'

REAL(KIND=8), INTENT(in)                   :: Time
REAL(KIND=8), DIMENSION(NVE), INTENT(in)   :: X
REAL(KIND=8), INTENT(out)                  :: xsum,ysum,zsum

INTEGER i
REAL(KIND=8) TT,argx,argy,argz,wx,wy,wz

xsum = 0.0D0
ysum = 0.0D0
zsum = 0.0D0
DO i=1,Nobs
  TT   = Time - Tobs(i)
  argx = TT/Tx
  argy = TT/Ty
  argz = TT/Tz
  argx = argx*argx
  argy = argy*argy
  argz = argz*argz
  wx   = exp(-argx)
  wy   = exp(-argy)
  wz   = exp(-argz)
  xsum = xsum + wx*(Yobs(1,i)-X(1))
  ysum = ysum + wy*(Yobs(2,i)-X(2))
  zsum = zsum + wz*(Yobs(3,i)-X(3))
ENDDO

RETURN
END SUBROUTINE weight_obs


module assert

use, intrinsic:: iso_fortran_env, only: sp=>real32, dp=>real64, qp=>real128, stderr=>error_unit
use, intrinsic:: ieee_arithmetic
implicit none (type, external)
private

integer,parameter :: wp = dp

public :: wp,isclose, assert_isclose

contains

elemental logical function isclose(actual, desired, rtol, atol, equal_nan)
! inputs
! ------
! actual: value "measured"
! desired: value "wanted"
! rtol: relative tolerance
! atol: absolute tolerance
! equal_nan: consider NaN to be equal?
!
!  rtol overrides atol when both are specified
!
! https://www.python.org/dev/peps/pep-0485/#proposed-implementation
! https://github.com/PythonCHB/close_pep/blob/master/is_close.py

real(wp), intent(in) :: actual, desired
real(wp), intent(in), optional :: rtol, atol
logical, intent(in), optional :: equal_nan

real(wp) :: r,a
logical :: n

r = merge(rtol, 1e-5_wp, present(rtol))
a = merge(atol, 0._wp, present(atol))
n = merge(equal_nan, .false., present(equal_nan))

!print*,r,a,n,actual,desired

!--- sanity check
if ((r < 0._wp).or.(a < 0._wp)) error stop 'tolerances must be non-negative'
!--- simplest case
isclose = (actual == desired)
if (isclose) return
!--- equal nan
isclose = n.and.(ieee_is_nan(actual).and.ieee_is_nan(desired))
if (isclose) return
!--- Inf /= -Inf, unequal NaN
if (.not.ieee_is_finite(actual) .or. .not.ieee_is_finite(desired)) return
!--- floating point closeness check
isclose = abs(actual-desired) <= max(r * max(abs(actual), abs(desired)), a)

end function isclose

impure elemental subroutine assert_isclose(actual, desired, rtol, atol, equal_nan, err_msg)
! inputs
! ------
! actual: value "measured"
! desired: value "wanted"
! rtol: relative tolerance
! atol: absolute tolerance
! equal_nan: consider NaN to be equal?
! err_msg: message to print on mismatch
!
! rtol overrides atol when both are specified

real(wp), intent(in) :: actual, desired
real(wp), intent(in), optional :: rtol, atol
logical, intent(in), optional :: equal_nan
character(*), intent(in), optional :: err_msg

if (.not.isclose(actual,desired,rtol,atol,equal_nan)) then
  write(stderr,*) merge(err_msg,'',present(err_msg)),': actual',actual,'desired',desired
  error stop
endif

end subroutine assert_isclose

end module assert


      SUBROUTINE TVELVC	(nplot, IERROR)
C
C LATEST REVISION   JULY, 1984
C
C PURPOSE		 TO PROVIDE A SIMPLE DEMONSTRATION OF
C			 SUBROUTINES VELVCT AND	EZVEC.
C
C USAGE			 CALL TVELVC (IERROR)
C
C ARGUMENTS
C
C ON OUTPUT		 IERROR
C			   AN INTEGER VARIABLE
C			   =0 IF THERE IS A NORMAL EXIT	FROM THE
C			      ROUTINES VELVCT AND EZVEC
C			   =1 OTHERWISE
C
C I/O			 IF THERE IS A NORMAL EXIT FROM	THE ROUTINES
C			 VELVCT	AND EZVEC THE MESSAGE
C			   VELVCT TEST SUCCESSFUL . . .	SEE PLOTS TO
C			   VERIFY PERFORMANCE
C			 IS PRINTED.
C
C PRECISION		 SINGLE
C
C LANGUAGE		 FORTRAN
C
C HISTORY		 ORIGINALLY WRITTEN NOVEMBER 1976
C
C ALGORITHM		 ROUTINE TVELVC	CALLS ROUTINES EZVEC AND
C			 VELVCT	ONCE.  EACH CALL PRODUCES A PLOT
C			 REPRESENTING A	VECTOR FIELD.  THE VECTOR
C			 FIELD IS OBTAINED FROM	THE FUNCTION
C			   Z(X,Y) = X +	Y + 1./((X-.1)**2+Y**2+.09)
C				    -1./((X+.1)**2+Y**2+.09),
C			 BY USING THE DIRECTION	OF THE Z GRADIENT
C			 VECTORS AND THE LOGARITHM OF THE ABSOLUTE
C			 VALUE OF THE COMPONENTS.
C
C
C
C
      DIMENSION	      U(21,25)	 ,V(21,25)
C
C SPECIFY COORDS FOR PLOT TITLES
C
	DATA IX/94/,IY/1000/
C
C SPECIFY SOME OF THE ARGUMENTS	IN VELVCT CALLING SEQUENCE
C
      DATA FLO/0./,HI/0./,NSET/0/,LENGTH/0/,ISPV/0/,SPV/0./
C
C INITIALIZE ERROR PARAMETER
C
      IERROR = 1
C
C SPECIFY VELOCITY FIELD FUNCTIONS U AND V
C
      M	= 21
      N	= 25
      DO  20 I=1,M
	 X = .1*FLOAT(I-11)
	 DO  10	J=1,N
	    Y =	.1*FLOAT(J-13)
	    DZDX = 1.-2.*(X-.10)/((X-.10)**2+Y**2+.09)**2+
     1		   2.*(X+.10)/((X+.10)**2+Y**2+.09)**2
	    DZDY = 1.-2.*Y/((X-.10)**2+Y**2+.09)**2+
     1		   2.*Y/((X+.10)**2+Y**2+.09)**2
	    UVMAG = ALOG(SQRT(DZDX*DZDX+DZDY*DZDY))
	    UVDIR = ATAN2(DZDY,DZDX)
	    U(I,J) = UVMAG*COS(UVDIR)
	    V(I,J) = UVMAG*SIN(UVDIR)
   10	 CONTINUE
   20 CONTINUE
C
C CALL WTSTR FOR EZVEC PLOT TITLE
C
c +noao: flag used to plot either velvct or ezvec
	if (nplot .eq. 1) then
	CALL GQCNTN(IERR,ICN)
	CALL GSELNT(0)
c       X = PAU2FX(IX)
	x = cpux (ix)
c       Y = PAU2FY(IY)
        y = cpuy (iy)
      CALL WTSTR (X,Y,'DEMONSTRATION PLOT FOR ENTRY EZVEC OF VELVCT',
     1		 2,0,-1)
	CALL GSELNT(ICN)
C
C CALL EZVEC FOR VELOCITY FIELD	PLOT
C
      CALL EZVEC (U,V,M,N)
      endif
c -noao
C
C CALL VELVCT FOR VELOCITY FIELD PLOT
C
c +noao: flag used to plot either velvct or ezvec
      if (nplot .eq. 2) then
      CALL VELVCT (U,M,V,M,M,N,FLO,HI,NSET,LENGTH,ISPV,SPV)
C
C CALL WTSTR FOR VELVCT	PLOT TITLE
C
	CALL GQCNTN(IERR,ICN)
	CALL GSELNT(0)
c       X = PAU2FX(IX)
        x = cpux (ix)
c       Y = PAU2FY(IY)
        y = cpuy (iy)
      CALL WTSTR (X,Y,
     1		 'DEMONSTRATION PLOT FOR ENTRY VELVCT OF VELVCT',2,
     2		 0,-1)
	CALL GSELNT(ICN)
	endif
c -noao
c
c     CALL NEWFM
C
      IERROR = 0
c     WRITE (6,1001)
      RETURN
C
c1001 FORMAT ('	    VELVCT TEST	SUCCESSFUL',24X,
c    1	      'SEE PLOTS TO VERIFY PERFORMANCE')
C
      END


MODULE FUN_xyLINESEGMENT
!!#### PURPOSE
!! Determine a line segment in R^2 <xyLINESEGMENT_> using:
!!  @ two points <PP>.

!!#### EXTERNAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_Rsp,KIND_Rdp           !!((01-A-KND_IntrinsicTypes.f90))

!!#### EXTERNAL PARAMETERS
USE PAR_ComputationalGeometry,ONLY: xySHAPE_Ls,xySHAPE_P !!((02-A-PAR_ComputationalGeometry.f90))

!!#### EXTERNAL PROCEDURES
USE FUN_xyDIRECTION,ONLY: xyDIRECTION_V                  !!((04-B-FUN_xyDIRECTION.f90))

!!#### DEFAULT IMPLICIT
IMPLICIT NONE

!!#### DEFAULT ACCESS
PRIVATE

!!#### PROCEDURE OVERLOADING
INTERFACE xyLINESEGMENT_PP
 MODULE PROCEDURE xyLINESEGMENT_PP_Rsp
 MODULE PROCEDURE xyLINESEGMENT_PP_Rdp
END INTERFACE

INTERFACE xyLINESEGMENT_
 MODULE PROCEDURE xyLINESEGMENT__Rsp
 MODULE PROCEDURE xyLINESEGMENT__Rdp
END INTERFACE

!!#### PUBLIC ACCESS LIST
PUBLIC :: xyLINESEGMENT_PP
PUBLIC :: xyLINESEGMENT_

CONTAINS

PURE FUNCTION xyLINESEGMENT_PP_Rsp( P_a , P_b ) RESULT( xyLs )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp            !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyLINESEGMENT_PP.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyLINESEGMENT_PP.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyLINESEGMENT_PP_Rdp( P_a , P_b ) RESULT( xyLs )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp            !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyLINESEGMENT_PP.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyLINESEGMENT_PP.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyLINESEGMENT__Rsp( ARRAY ) RESULT( xyLs )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp            !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyLINESEGMENT_.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyLINESEGMENT_.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyLINESEGMENT__Rdp( ARRAY ) RESULT( xyLs )
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp            !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyLINESEGMENT_.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyLINESEGMENT_.f90.bdy"
!!--end--
END FUNCTION

END MODULE


! RUN: bbc -emit-fir %s -o - | FileCheck %s

! CHECK-LABEL: func @_QPmaxval_test(
! CHECK-SAME: %[[arg0:.*]]: !fir.box<!fir.array<?xi32>>{{.*}}) -> i32
integer function maxval_test(a)
integer :: a(:)
! CHECK-DAG:  %[[c0:.*]] = arith.constant 0 : index
! CHECK-DAG:  %[[a2:.*]] = fir.absent !fir.box<i1>
! CHECK-DAG:  %[[a4:.*]] = fir.convert %[[arg0]] : (!fir.box<!fir.array<?xi32>>) -> !fir.box<none>
! CHECK:  %[[a6:.*]] = fir.convert %[[c0]] : (index) -> i32
! CHECK:  %[[a7:.*]] = fir.convert %[[a2]] : (!fir.box<i1>) -> !fir.box<none>
maxval_test = maxval(a)
! CHECK:  %{{.*}} = fir.call @_FortranAMaxvalInteger4(%[[a4]], %{{.*}}, %{{.*}}, %[[a6]], %[[a7]]) : (!fir.box<none>, !fir.ref<i8>, i32, i32, !fir.box<none>) -> i32
end function

! CHECK-LABEL: func @_QPmaxval_test2(
! CHECK-SAME: %[[arg0:.*]]: !fir.ref<!fir.char<1>>, %[[arg1:.*]]: index, %[[arg2:.*]]: !fir.box<!fir.array<?x!fir.char<1>>> {fir.bindc_name = "a"}) -> !fir.boxchar<1> {
character function maxval_test2(a)
character :: a(:)
! CHECK-DAG:  %[[a0:.*]] = fir.alloca !fir.box<!fir.heap<!fir.char<1,?>>>
! CHECK:  %[[a1:.*]] = fir.absent !fir.box<i1>
! CHECK-DAG:  %[[a5:.*]] = fir.convert %[[a0]] : (!fir.ref<!fir.box<!fir.heap<!fir.char<1,?>>>>) -> !fir.ref<!fir.box<none>>
! CHECK:  %[[a6:.*]] = fir.convert %[[arg2]] : (!fir.box<!fir.array<?x!fir.char<1>>>) -> !fir.box<none>
! CHECK-DAG:  %[[a8:.*]] = fir.convert %[[a1]] : (!fir.box<i1>) -> !fir.box<none>
maxval_test2 = maxval(a)
! CHECK:  %{{.*}} = fir.call @_FortranAMaxvalCharacter(%[[a5]], %[[a6]], %{{.*}}, %{{.*}}, %[[a8]]) : (!fir.ref<!fir.box<none>>, !fir.box<none>, !fir.ref<i8>, i32, !fir.box<none>) -> none
end function

! CHECK-LABEL: func @_QPmaxval_test3(
! CHECK-SAME: %[[arg0:.*]]: !fir.box<!fir.array<?x?xi32>>{{.*}}, %[[arg1:.*]]: !fir.box<!fir.array<?xi32>>
subroutine maxval_test3(a,r)
integer :: a(:,:)
integer :: r(:)
! CHECK-DAG:  %[[c2_i32:.*]] = arith.constant 2 : i32
! CHECK-DAG:  %[[a0:.*]] = fir.alloca !fir.box<!fir.heap<!fir.array<?xi32>>>
! CHECK:  %[[a1:.*]] = fir.absent !fir.box<i1>
! CHECK-DAG:  %[[a6:.*]] = fir.convert %[[a0]] : (!fir.ref<!fir.box<!fir.heap<!fir.array<?xi32>>>>) -> !fir.ref<!fir.box<none>>
! CHECK:  %[[a7:.*]] = fir.convert %[[arg0]] : (!fir.box<!fir.array<?x?xi32>>) -> !fir.box<none>
! CHECK-DAG:  %[[a9:.*]] = fir.convert %[[a1]] : (!fir.box<i1>) -> !fir.box<none>
r = maxval(a,dim=2)
! CHECK:  %{{.*}} = fir.call @_FortranAMaxvalDim(%[[a6]], %[[a7]], %[[c2_i32]], %{{.*}}, %{{.*}}, %[[a9]]) : (!fir.ref<!fir.box<none>>, !fir.box<none>, i32, !fir.ref<i8>, i32, !fir.box<none>) -> none
! CHECK:  %[[a11:.*]] = fir.load %[[a0]] : !fir.ref<!fir.box<!fir.heap<!fir.array<?xi32>>>>
! CHECK-DAG:  %[[a13:.*]] = fir.box_addr %[[a11]] : (!fir.box<!fir.heap<!fir.array<?xi32>>>) -> !fir.heap<!fir.array<?xi32>>
! CHECK-DAG:  fir.freemem %[[a13]]
end subroutine

! CHECK-LABEL: func @_QPtest_maxval_optional_scalar_mask(
! CHECK-SAME:  %[[VAL_1:.*]]: !fir.ref<!fir.logical<4>>
subroutine test_maxval_optional_scalar_mask(mask, array)
integer :: array(:)
logical, optional :: mask
print *, maxval(array, mask)
! CHECK:  %[[VAL_7:.*]] = fir.is_present %[[VAL_1]] : (!fir.ref<!fir.logical<4>>) -> i1
! CHECK:  %[[VAL_8:.*]] = fir.embox %[[VAL_1]] : (!fir.ref<!fir.logical<4>>) -> !fir.box<!fir.logical<4>>
! CHECK:  %[[VAL_9:.*]] = fir.absent !fir.box<!fir.logical<4>>
! CHECK:  %[[VAL_10:.*]] = arith.select %[[VAL_7]], %[[VAL_8]], %[[VAL_9]] : !fir.box<!fir.logical<4>>
! CHECK:  %[[VAL_17:.*]] = fir.convert %[[VAL_10]] : (!fir.box<!fir.logical<4>>) -> !fir.box<none>
! CHECK: fir.call @_FortranAMaxvalInteger4(%{{.*}}, %{{.*}}, %{{.*}}, %{{.*}}, %[[VAL_17]]) : (!fir.box<none>, !fir.ref<i8>, i32, i32, !fir.box<none>) -> i32
end subroutine

! CHECK-LABEL: func @_QPtest_maxval_optional_array_mask(
! CHECK-SAME:  %[[VAL_0:.*]]: !fir.box<!fir.array<?x!fir.logical<4>>>
subroutine test_maxval_optional_array_mask(mask, array)
integer :: array(:)
logical, optional :: mask(:)
print *, maxval(array, mask)
! CHECK:  %[[VAL_13:.*]] = fir.convert %[[VAL_0]] : (!fir.box<!fir.array<?x!fir.logical<4>>>) -> !fir.box<none>
! CHECK: fir.call @_FortranAMaxvalInteger4(%{{.*}}, %{{.*}}, %{{.*}}, %{{.*}}, %[[VAL_13]]) : (!fir.box<none>, !fir.ref<i8>, i32, i32, !fir.box<none>) -> i32
end subroutine


c ****************************************************
c  setgrid.f
c  25 jan. 2011    written by D.KAWATA
c ****************************************************

      subroutine setgrid(npg,ngr,ngz,ri,ro,zi,zo)
      include 'define.f'
      integer npg,ngr,ngz
      double precision ri,ro,zi,zo
      integer i,j
      double precision zz,rr

      npg=0
      lri_rzg = dlog10(ri)
      lro_rzg = dlog10(ro)
      dlr_rzg = (lro_rzg-lri_rzg)/dble(ngr-1)
      lzi_rzg = dlog10(zi)
      lzo_rzg = dlog10(zo)
      dlz_rzg = (lzo_rzg-lzi_rzg)/dble(ngz-1)
c *** generate particles ***
      zz=0.0d0
      npg=0
      do j=0,ngz
        if(j.ne.0) then
          zz=10.0d0**(lzi_rzg+dble(j-1)*dlz_rzg)
        else
          zz=0.0d0
        endif
        rr=0.0d0
        do i=0,ngr
          if(i.ne.0) then
            rr=10.0d0**(lri_rzg+dble(i-1)*dlr_rzg)
          else
            rr=0.0d0
          endif
          x_rzg(npg)=rr
          y_rzg(npg)=0.0d0
          z_rzg(npg)=zz
          id_rzg(i,j)=npg
          npg=npg+1
        enddo
      enddo

      return
      end


    NTEMP=0
    NDER=1
    XOLD=0.0D0
    TOLF=1.0D-3    ! for residuals  1.0D-3
    TOL= 1.0D-4    ! for variables  1.0D-4
    N=3    ! DLSARG CONSTANTS
    INCX=1
    LDA=3
    ldb=3

    TCmod=TC

    if(TCGC(1).ne.0.0D0)TCmod=TCGC
    inf=2

    if(icomp.eq.2)inf=1

    rn (inf) =0.0
    rn(icomp)=1.0

c    Starting from critical point    c    c
    WRITE(nout,15) TCmod(icomp),Pc(icomp),Dc(icomp),Dc(icomp)

    T = 0.9999 * TCmod(icomp)
    Vc = 1/DC(icomp)
    Vv = 1.03*Vc
    Zc = Pc(icomp)*Vc/RGAS/T
    aaa = min(0.89 + (Zc - 0.2) / 2, 0.95)
    Vl = aaa*Vc
    NS = 3
    delXS = 0.10
    go to 2
c    c    c    c    c    c    c    c    c    c    c

 2  XVAR = log([T, Vl, Vv])
    DFDS=0.0D0
    DFDS(3)=1.0D0
    RJAC=0.0D0
    RJAC(3,NS)=1.0D0
 1  NITER=0
    T=exp(XVAR(1))
    Vl=exp(XVAR(2))
    Vv=exp(XVAR(3))
    FMAXOLD=8.0D0
    FMAX=7.0D0
    DMAXOLD=8.0D0
    DMAX=7.0D0
    F(3)=0.0D0
    delX=0.0
    NV=0

c    Newton procedure for solving the present point
    DO WHILE (DMAX.GT.TOL.or.FMAX.GT.TOLF)

        if(NV.GE.5.and.T.LT.0.4*Tcmod(icomp))return

        IF ((FMAX.GT.FMAXOLD.and.DMAX.GT.DMAXOLD).OR.NITER.eq.10) THEN
            delXS=0.8*delXS
            if(abs(delXS).lt.0.001)return
            if(XOLD(1).NE.0.0D0)go to 7
        END IF

        NITER=NITER+1
 21     CALL XTVTERMO(2,T,Vl,Pl,rn,FUGx,FUGTx,FUGVx,DFGN)

        if(Pl.eq.0.0d0)Pl=1.0D-17

        DPDTx=DPDT
        DPDVx=DPDV

        CALL XTVTERMO(2,T,Vv,Pv,rn,FUGy,FUGTy,FUGVy,DFGN)
        DPDTy=DPDT
        DPDVy=DPDV

        if((Pl.lt.Pv/2.and.Pv-Pl.gt.1.d-8).or.
    1     (Pl.gt.1.5*Pv.and.Pl-Pv.gt.1.d-8))then    ! Pv.lt.1.0D-8.or.
            NV=NV+1
            V1=Vl+(Pv-Pl)/DPDVx    ! very important for convergence and accuracy of P at low T
            V2=(Vl+B)/2
            Vl=max(V1,V2)
            XVAR(2)=log(Vl)
            go to 21
        end if

        if(Pl.lt.0)then
            F(1)=-TOLF
        else
            F(1)=log(Pl/Pv)
        end if

        F(2)=FUGx(icomp)-FUGy(icomp)
        RJAC(1,1)=DPDTx/Pl-DPDTy/Pv
        RJAC(1,1)=T*RJAC(1,1)
        RJAC(1,2)=Vl*DPDVx/Pl
        RJAC(1,3)=-Vv*DPDVy/Pv
C
        RJAC(2,1)=FUGTx(icomp)-FUGTy(icomp)
        RJAC(2,1)=T*RJAC(2,1)
        RJAC(2,2)=Vl*FUGVx(icomp)
        RJAC(2,3)=-Vv*FUGVy(icomp)
C
        dold=delx

        db = -F
        AJ=RJAC
        call dgesv( N, 1, AJ, lda, ipiv, db, ldb, info )
        if (info.ne.0) write(6,*)"error with dgesv in parameter ",info
        delX = db
c
        dot(1:3)=delx(1:3)*dold(1:3)
        dotp=sum(dot(1:3))
        dxmax=maxval(abs(delx))

c    oscillations are recognized by negative dotp
        oscil=.false.

        if(maxval(dot).le.0.0.or.dotp.lt.-1.d-5)oscil=.true.

        if (oscil.and.dxmax.ge.1.0d-4) then
            delx=delx/2
        end if

      if(dxmax.ge.1.0d-3.and.dotp.eq.0.0) delx=delx/2  !half step for first iteration (precaution)

    if(delXS.eq.0.015.and.maxval(abs(delX)).gt.7)delX=6*delX/abs(delX(3))

 17     XVAR=XVAR+delX
 18     T=exp(XVAR(1))
        Vl=exp(XVAR(2))

        IF(T.GT.TCMOD(icomp).or.Vl.ge.Vc)THEN
            delX=delX/2
            XVAR=XVAR-delX
            go to 18
        END IF

        Vv=exp(XVAR(3))
        FMAXOLD=FMAX
        FMAX=MAXVAL(ABS(F))
        DMAXOLD=DMAX
        DMAX=MAXVAL(ABS(DELX))

        if(FMAX.lt.1.0D-1.and.FMAX*DMAX.lt.1.0D-11)exit
    END DO

    RHOL=1/Vl
    RHOV=1/Vv

    db = dFdS
    AJ=RJAC
    call dgesv( N, 1, AJ, lda, ipiv, db, ldb, info )
    if (info.ne.0) write(6,*)"error with dgesv in parameter ",info
    dXdS = db

c
    NSOLD=NS

    IF(VV.GT.1.1*VC)THEN  ! .and.T.GT.0.3*Tcmod(icomp)
        NS=1
    ELSE
        NS=3
    END IF

    if(NS.ne.NSOLD)then
        dSdSold=dXdS(NS)
        dXdS=dXdS/dSdSold
        RJAC(3,1:3)=0.0D0
        RJAC(3,NS)=1.0D0
    end if

    NITER=min(niter,10)
    delXS=delXS*5/NITER/dXdS(NSOLD)

    IF(delXS.LT.0)then
        if(NS.EQ.1)
            delXS=max(delXS,-0.008) ! Max lnT decrease allowed
        if(NS.eq.2)
            delXS=max(delXS,-0.01) ! Max lnVl decrease allowed: 0.01  ! DO NOT CHANGE !!!
        if(NS.eq.3)
            delXS=max(delXS,-0.10) ! Max lnVv decrease allowed: 0.20
    ELSE
        if(NS.EQ.1)
            delXS=min(delXS,0.01) ! Max lnT increase allowed: 0.5 K
        if(NS.eq.2)
            delXS=min(delXS,0.05) ! Max lnVl increase allowed: 0.01
        if(NS.eq.3)
            delXS=min(delXS,0.20) ! Max lnVv increase allowed: 0.20
    END IF

    XOLD=XVAR
    TOLD=T
    POLD=Pv
    DVOLD=RHOV
    DLOLD=RHOL
    NV=0

 7  S = XOLD(NS) + delXS
    XVAR = XOLD+dXdS*delXS/dXdS(NS)    ! Initial estimates for the 3 variables in the next point

 8  T=exp(XVAR(1))
    Vl=exp(XVAR(2))
    Vv=exp(XVAR(3))

    if(Pv.gt.1.D-20.and.(T.gt.0.25*TCmod(icomp).or.T>250)     ! modified April 6, 2016
    1                              .and.T.lt.TCmod(icomp))go to 1

#
      SUBROUTINE TERMO(MTYP,INDIC,IC,T,P,rn,V,PHILOG,DLPHIT,DLPHIP,FUGN)
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      PARAMETER (MAXC=2,nco=2,RGAS=0.08314472d0)
      DIMENSION FUGN(MAXC,MAXC)
      DIMENSION PHILOG(MAXC),DLPHIT(MAXC),DLPHIP(MAXC),DPDN(MAXC)
    dimension rn(nco),Arn(nco),ArVn(nco),ArTn(nco),Arn2(nco,nco)
c   The output PHILOG is actually the vector ln(phi(i)*P)
    NC=2
    NTEMP=0
      IGZ=0
      NDER=1
      IF (INDIC.GT.2) NDER=2
      IF (INDIC.EQ.2 .OR. INDIC.EQ.4) NTEMP=1
    TOTN = sum(rn)
    if(P.le.0.0d0)MTYP=1
    CALL VCALC(MTYP,NC,rn,T,P,V)
      RT = RGAS*T
      Z = V/(TOTN*RT)   ! this is Z/P
    call ArVnder(NDER,NTEMP,rn,V,T,Ar,ArV,ArTV,ArV2,Arn,ArVn,ArTn,Arn2)
      DPV = -ArV2-RT*TOTN/V**2
    DPDT = -ArTV+TOTN*RGAS/V
      DO 60 I=1,NC
      PHILOG(I)=-LOG(Z)+Arn(I)/RT
      DPDN(I) = RT/V-ArVn(I)
      DLPHIP(I)=-DPDN(I)/DPV/RT-1.D0/P
    IF(NTEMP.EQ.0) GOTO 60
    DLPHIT(I)=(ArTn(I)-Arn(I)/T)/RT+DPDN(I)*DPDT/DPV/RT+1.D0/T
   60 CONTINUE
   62 IF(NDER.LT.2) GOTO 64
      DO 63 I=1,NC
      DO 61 K=I,NC
      FUGN(I,K)=1.D0/TOTN+(Arn2(I,K)+DPDN(I)*DPDN(K)/DPV)/RT
   61 FUGN(K,I)=FUGN(I,K)
   63 CONTINUE
   64 RETURN
      END


c*************************************************************************
c                            SPIN_SUN.F
c*************************************************************************
c This subroutine evolves spin axis due to torque from Sun for dt
c
c             Input:
c                 nbod          ==>  number of massive bodies (int scalar)
c                 mass          ==>  mass of bodies (real array)
c                 xh,yh,zh      ==>  initial position in helio coord
c                                    (real arrays)
c                 preconst,aref ==>  Precession constant and its reference
c                                    semimajor axis (real arrays)
c                 sx,sy,sz      ==>  initial direction of spin (real arrays)
c                 dt            ==>  time step
c             Output:
c                 sx,sy,sz      ==>  final direction of spin (real arrays)
c Remarks:
c
c Authors:  Man Hoi Lee
c Date:    7/18/06
c Last revision:

      subroutine spin_sun(nbod,nbodm,mass,xh,yh,zh,preconst,aref,
     &                    sx,sy,sz,dt)

      include '../swift.inc'

c...  Inputs Only:
      integer nbod,nbodm
      real*8 mass(nbod),dt
      real*8 xh(nbod),yh(nbod),zh(nbod)
      real*8 preconst(NTPMAX),aref(NTPMAX)

c...  Inputs and Outputs:
      real*8 sx(NTPMAX),sy(NTPMAX),sz(NTPMAX)

c...  Internals:
      integer n
      real*8 rsq,r,rdots,rxsx,rxsy,rxsz,phi,cosphi,sinphi,fac1,fac2

c----
c...  Executable code

      do n=2,nbod
         rsq = xh(n)**2 + yh(n)**2 + zh(n)**2
         r = dsqrt(rsq)
         rdots = xh(n)*sx(n) + yh(n)*sy(n) + zh(n)*sz(n)
         rxsx = yh(n)*sz(n) - zh(n)*sy(n)
         rxsy = zh(n)*sx(n) - xh(n)*sz(n)
         rxsz = xh(n)*sy(n) - yh(n)*sx(n)
         phi = 2.d0*preconst(n)*(aref(n)/r)**3*(rdots/r)*dt
         cosphi = dcos(phi)
         sinphi = dsin(phi)
         fac1 = (1.d0 - cosphi)*rdots/rsq
         fac2 = sinphi/r
         sx(n) = cosphi*sx(n) + fac1*xh(n) + fac2*rxsx
         sy(n) = cosphi*sy(n) + fac1*yh(n) + fac2*rxsy
         sz(n) = cosphi*sz(n) + fac1*zh(n) + fac2*rxsz
      enddo

      return
      end   ! spin_sun
c---------------------------------------------------------------------


      subroutine bndacc (g,mdg,nb,ip,ir,mt,jt)
c     c.l.lawson and r.j.hanson, jet propulsion laboratory, 1973 jun 12
c     to appear in 'solving least squares problems', prentice-hall, 1974
c	   sequential algorithm for banded least squares problem..
c	   accumulation phase.	    for solution phase use bndsol.
c
c     the calling program must set ir=1 and ip=1 before the first call
c     to bndacc for a new case.
c
c     the second subscript of g( ) must be dimensioned at least
c     nb+1 in the calling program.
      dimension g(mdg,1)
      zero=0.
c
c	       alg. steps 1-4 are performed external to this subroutine.
c
      nbp1=nb+1
      if (mt.le.0) return
c					      alg. step 5
      if (jt.eq.ip) go to 70
c					      alg. steps 6-7
      if (jt.le.ir) go to 30
c					      alg. steps 8-9
      do 10 i=1,mt
	ig1=jt+mt-i
	ig2=ir+mt-i
	do 10 j=1,nbp1
   10	g(ig1,j)=g(ig2,j)
c					      alg. step 10
      ie=jt-ir
      do 20 i=1,ie
	ig=ir+i-1
	do 20 j=1,nbp1
   20	g(ig,j)=zero
c					      alg. step 11
      ir=jt
c					      alg. step 12
   30 mu=min0(nb-1,ir-ip-1)
      if (mu.eq.0) go to 60
c					      alg. step 13
      do 50 l=1,mu
c					      alg. step 14
	k=min0(l,jt-ip)
c					      alg. step 15
	lp1=l+1
	ig=ip+l
	do 40 i=lp1,nb
	  jg=i-k
   40	  g(ig,jg)=g(ig,i)
c					      alg. step 16
	do 50 i=1,k
	jg=nbp1-i
   50	g(ig,jg)=zero
c					      alg. step 17
   60 ip=jt
c					      alg. steps 18-19
   70 mh=ir+mt-ip
      kh=min0(nbp1,mh)
c					      alg. step 20
      do 80 i=1,kh
   80	call h12 (1,i,max0(i+1,ir-ip+1),mh,g(ip,i),1,rho,
     1		  g(ip,i+1),1,mdg,nbp1-i)
c					      alg. step 21
      ir=ip+kh
c					      alg. step 22
      if (kh.lt.nbp1) go to 100
c					      alg. step 23
      do 90 i=1,nb
   90	g(ir-1,i)=zero
c					      alg. step 24
  100 continue
c					      alg. step 25
      return
      end


PROGRAM DISPDEMO
! Example driver for M_display.
USE M_display
! USE DISP_R16MOD
IMPLICIT NONE
INTEGER, PARAMETER :: RK = SELECTED_REAL_KIND(6), N = 3
REAL(RK) :: A(N,N), B(N,N), X
INTEGER I, J, K(5)
  CALL DISP_SET(ADVANCE = 'DOUBLE')
  FORALL(I=1:N, J=1:N)
    A(I,J) = EXP(REAL(I+J-1, RK))
    B(I,J) = EXP(REAL(I**J, RK))
  END FORALL
  CALL DISP('A = ', A)
  CALL DISP(B)
  CALL DISP(A(1:2,:),'F0.5')
  CALL DISP('MATRIX', A, STYLE='UNDERLINE & NUMBER', UNIT=-3, DIGMAX=4)
  K = (/-3,0,12,14,0/)
  CALL DISP('K', K, STYLE='PAD', ORIENT='ROW', SEP=' ', ZEROAS='.')
  X = 1.5
  CALL DISP('The square of '//TOSTRING(X)//' is '//TOSTRING(X*X))
  CALL DISP_SET(MATSEP = ' | ')
  CALL DISP((/11,12,13/), ADVANCE='NO')
  CALL DISP((/.TRUE., .FALSE., .TRUE./), ADVANCE='NO')
  CALL DISP((/'A','B','C'/))
END PROGRAM DISPDEMO
!
! A =  2.718   7.389   20.086
!      7.389  20.086   54.598
!     20.086  54.598  148.413
!
! 2.71828E+00  2.71828E+00  2.71828E+00
! 7.38906E+00  5.45982E+01  2.98096E+03
! 2.00855E+01  8.10308E+03  5.32048E+11
!
! 2.71828   7.38906  20.08554
! 7.38906  20.08554  54.59815
!
!        MATRIX
! --------------------
!      1     2      3
! 1   2.7   7.4   20.1
! 2   7.4  20.1   54.6
! 3  20.1  54.6  148.4
!
! ------K-----
! -3 . 12 14 .
!
! The square of 1.5 is 2.25
!
! 11 | T | A
! 12 | F | B
! 13 | T | C
!


!!# MODULE <<FUN_NormEll2>>
MODULE FUN_NormEll2

!!## PURPOSE
!! Defines the $\ell^2$ norm of <A>, with optional weight <W>.

!!## DETAILS
!! The $\ell^2$ norm is a discrete norm of the form
!!
!! $$ \ell^2(A) = \sqrt{ \sum_i [ A_i ]^2 W_i } $$
!!
!! Where for example $A_i$ could be average values
!! $A_i = \frac{1}{\Delta x_i} \int_{a_i}^{b_i} A(x) dx$
!! and the weights $W_i = \Delta x_i$ returned from a finite
!! volume solution of a PDE.

!!## USAGE
!
!       s = NormEll2( A [,W] )
!

!!## REFERENCES
!
![1] Weisstein, Eric W. "L2-Norm." From MathWorld--A Wolfram Web Resource.
!    http://mathworld.wolfram.com/L2-Norm.html

!!## OWNER
![1] Wieselquist, William W. (william.wieselquist AT gmail.com)
!    July 8, 2006 - present

!!## EXTERNAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_Rsp,KIND_Rdp !!((01-A-KND_IntrinsicTypes.f90))

!!## DEFAULT IMPLICIT
IMPLICIT NONE

!!## DEFAULT ACCESS
PRIVATE

!!## NormEll2 PROCEDURE OVERLOADING
INTERFACE NormEll2
 MODULE PROCEDURE NormEll2_A1_Rsp
 MODULE PROCEDURE NormEll2_A2_Rsp
 MODULE PROCEDURE NormEll2_A3_Rsp
 MODULE PROCEDURE NormEll2_A4_Rsp
 MODULE PROCEDURE NormEll2_A5_Rsp
 MODULE PROCEDURE NormEll2_A6_Rsp
 MODULE PROCEDURE NormEll2_A7_Rsp

 MODULE PROCEDURE NormEll2_A1_Rdp
 MODULE PROCEDURE NormEll2_A2_Rdp
 MODULE PROCEDURE NormEll2_A3_Rdp
 MODULE PROCEDURE NormEll2_A4_Rdp
 MODULE PROCEDURE NormEll2_A5_Rdp
 MODULE PROCEDURE NormEll2_A6_Rdp
 MODULE PROCEDURE NormEll2_A7_Rdp
END INTERFACE
INTERFACE NormEll2
 MODULE PROCEDURE NormEll2_A1W1_Rsp
 MODULE PROCEDURE NormEll2_A2W2_Rsp
 MODULE PROCEDURE NormEll2_A3W3_Rsp
 MODULE PROCEDURE NormEll2_A4W4_Rsp
 MODULE PROCEDURE NormEll2_A5W5_Rsp
 MODULE PROCEDURE NormEll2_A6W6_Rsp
 MODULE PROCEDURE NormEll2_A7W7_Rsp

 MODULE PROCEDURE NormEll2_A1W1_Rdp
 MODULE PROCEDURE NormEll2_A2W2_Rdp
 MODULE PROCEDURE NormEll2_A3W3_Rdp
 MODULE PROCEDURE NormEll2_A4W4_Rdp
 MODULE PROCEDURE NormEll2_A5W5_Rdp
 MODULE PROCEDURE NormEll2_A6W6_Rdp
 MODULE PROCEDURE NormEll2_A7W7_Rdp
END INTERFACE

!!## PUBLIC ACCESS LIST
PUBLIC :: NormEll2

!## MODULE PROCEDURES
CONTAINS

!!### L-2 Norm for single precision
PURE FUNCTION NormEll2_A1_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A2_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A3_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A4_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A5_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A6_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A7_Rsp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

!!### L-2 Norm for double precision
PURE FUNCTION NormEll2_A1_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A2_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A3_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A4_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A5_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A6_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A7_Rdp(A) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 ))
!!--end--
END FUNCTION

!!### L-2 Norm for single precision
PURE FUNCTION NormEll2_A1W1_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:),W(:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A2W2_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:),W(:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A3W3_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:),W(:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A4W4_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:),W(:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A5W5_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:),W(:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A6W6_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:),W(:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A7W7_Rsp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:,:),W(:,:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

!!### L-2 Norm for double precision
PURE FUNCTION NormEll2_A1W1_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:),W(:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A2W2_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:),W(:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A3W3_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:),W(:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A4W4_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:),W(:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A5W5_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:),W(:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A6W6_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:),W(:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

PURE FUNCTION NormEll2_A7W7_Rdp(A,W) RESULT(s)
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
REAL(KIND_R),INTENT(IN) :: A(:,:,:,:,:,:,:),W(:,:,:,:,:,:,:)
REAL(KIND_R) :: s
!!--begin--
s = SQRT( SUM( ( A )**2 * ABS(W) ))
!!--end--
END FUNCTION

END MODULE


program test

    use,intrinsic :: iso_fortran_env, only: wp => real64
    use pyplot_module

    implicit none

    real(wp),dimension(100) :: x,sx
    type(pyplot) :: plt
    integer :: i

    !generate some data:
    x = [(real(i,wp), i=0,size(x)-1)]/5.0_wp
    sx = sin(x)

    !plot it:
    call plt%initialize(grid=.true.,xlabel='angle (rad)',title='Plot of $\sin(x)$',legend=.true.)
    call plt%add_plot(x,sx,label='$\sin(x)$',linestyle='b-o',markersize=5,linewidth=2)
    call plt%showfig()

end program test


program test

use COESA_module
use iso_fortran_env, wp => real64

implicit none

real(wp) :: d
integer :: i
real(wp),dimension(*),parameter :: alts = [ 0.0_wp,  &
                                            5.0_wp, &
                                            10.0_wp, &
                                            80000.0_wp, &
                                            80000.1_wp, &
                                            80000.5_wp, &
                                            80000.6_wp, &
                                            81000.0_wp, &
                                            81000.1_wp, &
                                            81000.5_wp, &
                                            81000.6_wp, &
                                            82000.0_wp, &
                                            82000.1_wp, &
                                            82000.5_wp, &
                                            82000.6_wp, &
                                            83000.0_wp, &
                                            83000.1_wp, &
                                            83000.5_wp, &
                                            83000.6_wp, &
                                            84000.0_wp, &
                                            84000.1_wp, &
                                            84000.5_wp, &
                                            84000.6_wp, &
                                            84852.0_wp, &
                                            84852.1_wp, &
                                            85000.0_wp, &
                                            85000.1_wp, &
                                            85000.5_wp, &
                                            85000.6_wp, &
                                            86000.0_wp, &
                                            86000.1_wp, &
                                            91000.0_wp, &
                                            91000.1_wp, &
                                            110000.0_wp, &
                                            110000.1_wp, &
                                            120000.0_wp, &
                                            120000.1_wp, &
                                            200000.0_wp, &
                                            300000.0_wp, &
                                            400000.0_wp, &
                                            500000.0_wp, &
                                            1000000.0_wp ]

do i = 1, size(alts)
    d = COESA_density(alts(i))
    write(*,*) alts(i),d
end do

end program test


MODULE grid_utils

! This module contains utilities related to grid manipulation
! (e.g., creating data for the B and C grids on an Arakawa-C
!  type stagger when only the A grid is available)

CONTAINS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  SUBROUTINE arakawa_c_n2t(datain, nx, ny, nz, dataout)

    ! Staggers a 3D array of data from the non-staggered points
    ! to the mass grid of an Arakawa C stagger.

    IMPLICIT NONE
    INTEGER, INTENT(IN)                :: nx
    INTEGER, INTENT(IN)                :: ny
    INTEGER, INTENT(IN)                :: nz
    REAL, INTENT(IN)                   :: datain(nx,ny,nz)
    REAL, INTENT(OUT)                  :: dataout(nx,ny,nz)

    INTEGER                            :: i,j,k
    PRINT *, 'Staggering to T grid (Arakawa C)'
    DO k = 1, nz
      DO j = 1, ny-1
        DO i = 1, nx-1
          dataout(i,j,k) = 0.25 * ( datain(i,j,k)    + &
                                    datain(i+1,j,k)  + &
                                    datain(i+1,j+1,k)+ &
                                    datain(i,j+1,k) )
        ENDDO
        ! Fill unused rightmost column
        dataout(nx,j,k) = dataout(nx-1,j,k)
      ENDDO
      ! Fill unused uppermost row
      dataout(:,ny,k) = dataout(:,ny-1,k)
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_n2t
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  SUBROUTINE arakawa_c_n2u(datain, nx, ny, nz, dataout)

    ! Staggers a 3D array of data from the non-staggered points
    ! to the U grid of an Arakawa C stagger.

    IMPLICIT NONE
    INTEGER, INTENT(IN)                :: nx
    INTEGER, INTENT(IN)                :: ny
    INTEGER, INTENT(IN)                :: nz
    REAL, INTENT(IN)                   :: datain(nx,ny,nz)
    REAL, INTENT(OUT)                  :: dataout(nx,ny,nz)

    INTEGER                            :: i,j,k
    PRINT *, 'Staggering to U grid (Arakawa C)'
    DO k = 1, nz
      DO j = 1, ny-1
        DO i = 1, nx
          dataout(i,j,k) = 0.50 * ( datain(i,j,k)    + &
                                    datain(i,j+1,k) )
        ENDDO
      ENDDO
      ! Fill unused uppermost row
      dataout(:,ny,k) = dataout(:,ny-1,k)
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_n2u
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  SUBROUTINE arakawa_c_n2v(datain, nx, ny, nz, dataout)

    ! Staggers a 3D array of data from the non-staggered points
    ! to the V grid of an Arakawa C stagger.

    IMPLICIT NONE
    INTEGER, INTENT(IN)                :: nx
    INTEGER, INTENT(IN)                :: ny
    INTEGER, INTENT(IN)                :: nz
    REAL, INTENT(IN)                   :: datain(nx,ny,nz)
    REAL, INTENT(OUT)                  :: dataout(nx,ny,nz)

    INTEGER                            :: i,j,k
    PRINT *, 'Staggering to V Grid (Arakawa C)'
    DO k = 1, nz
      DO j = 1, ny
        DO i = 1, nx-1
          dataout(i,j,k) = 0.50 * ( datain(i,j,k)    + &
                                    datain(i+1,j,k) )
        ENDDO
        ! Fill unused right column
        dataout(nx,j,k) = dataout(nx-1,j,k)
      ENDDO
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_n2v
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  SUBROUTINE arakawa_c_t2n(datain,nx_t,ny_t,nz,dataout)

    ! Destaggers a WRF Arakawa C from the staggered thermodynamic
    ! points to the non-staggered points.   Note that this routine
    ! returns an array that is one element larger in each direction
    ! than the input array.
    !
    !  Example:  Input 3x3 "T" points, return 4x4 "N" points
    !
    !           N   N   N   N
    !             T   T   T
    !           N   N   N   N
    !             T   T   T
    !           N   N   N   N
    !             T   T   T
    !           N   N   N   N

    IMPLICIT NONE

    INTEGER, INTENT(IN)       :: nx_t
    INTEGER, INTENT(IN)       :: ny_t
    INTEGER, INTENT(IN)       :: nz
    REAL, INTENT(IN)          :: datain(nx_t,ny_t,nz)
    REAL, INTENT(OUT)         :: dataout(nx_t+1,ny_t+1,nz)

    INTEGER                   :: i,j,k

    vertical_loop:  DO k=1,nz

      ! First, compute all of the interior points

      DO j = 2, ny_t
        DO i = 1, nx_t

           dataout(i,j,k) = 0.25*( datain(i-1,j-1,k) + datain(i-1,j,k) + &
                                  datain(i,j,k) + datain(i,j-1,k) )

        ENDDO
      ENDDO

      ! Now, extrapolate upper and lower rows, except corner points

      DO i = 2, nx_t

        dataout(i,1,k) = 2.0* dataout(i,2,k)-dataout(i,3,k)
        dataout(i,ny_t+1,k) = 2.0*dataout(i,ny_t,k)-dataout(i,ny_t-1,k)

      ENDDO

      ! Extrapolate left and right columns, except corner points

      DO j = 2, ny_t

        dataout(1,j,k) = 2.0*dataout(2,j,k)-dataout(3,j,k)
        dataout(nx_t+1,j,k) = 2.0*dataout(nx_t,j,k)-dataout(nx_t-1,j,k)

      ENDDO

      ! Compute corner point values by solving for 4 point average

      dataout(1,1,k) = 4.0 * datain(1,1,k) - &
                             dataout(1,2,k) - &
                             dataout(2,2,k) - &
                             dataout(2,1,k)
      dataout(1,ny_t+1,k) = 4.0 * datain(1,ny_t,k) - &
                                  dataout(1,ny_t,k) - &
                                  dataout(2,ny_t+1,k) - &
                                  dataout(2,ny_t,k)
      dataout(nx_t+1,ny_t+1,k) = 4.0 * datain(nx_t,ny_t,k) - &
                                       dataout(nx_t,ny_t,k) - &
                                       dataout(nx_t,ny_t+1,k) - &
                                       dataout(nx_t+1,ny_t,k)
      dataout(nx_t+1,1,k) = 4.0 * datain(nx_t,1,k) - &
                                dataout(nx_t,1,k) - &
                                dataout(nx_t,2,k) - &
                                dataout(nx_t+1,2,k)

    ENDDO vertical_loop
    RETURN
  END SUBROUTINE arakawa_c_t2n
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  SUBROUTINE arakawa_c_u2n(datain,nx_u,ny_u,nz,dataout)

    ! Destaggers from the WRF "u" grid to the non-staggered grid.  The
    ! return array will be 1 element larger in the Y direction than
    ! the input array.  Example:
    !
    !   Input 4x3 "U" grid, return 4x4 "N" grid:
    !
    !            N   N   N   N
    !            U   U   U   U
    !            N   N   N   N
    !            U   U   U   U
    !            N   N   N   N
    !            U   U   U   U
    !            N   N   N   N

    IMPLICIT NONE

    INTEGER,  INTENT(IN)   :: nx_u
    INTEGER,  INTENT(IN)   :: ny_u
    INTEGER,  INTENT(IN)   :: nz
    REAL,     INTENT(IN)   :: datain(nx_u,ny_u,nz)
    REAL,     INTENT(OUT)  :: dataout(nx_u,ny_u+1,nz)

    INTEGER                :: i,k
    INTEGER                :: nx,ny

    nx = nx_u
    ny = ny_u+1

    DO k = 1, nz

      ! Linear interpolation along each column, except top/bottom rows
      DO i = 1, nx

        ! Average of points above and below to fill interior rows
        dataout(i,2:ny_u,k) = 0.5*(datain(i,1:ny_u-1,k)+datain(i,2:ny_u,k))

        ! Fill bottom row
        dataout(i,1,k) = 2.0 * dataout(i,2,k) - dataout(i,3,k)

        ! Fill top row
        dataout(i,ny,k) = 2.0 * dataout(i,ny-1,k) - dataout(i,ny-2,k)

      ENDDO
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_u2n
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 SUBROUTINE arakawa_c_v2n(datain,nx_v,ny_v,nz,dataout)

    ! Destaggers from the WRF "v" grid to the non-staggered grid.  The
    ! return array will be 1 element larger in the X direction than
    ! the input array.  Example:
    !
    !   Input 3x4 "V" grid, return 4x4 "N" grid:
    !
    !            N V N V N V N
    !
    !            N V N V N V N
    !
    !            N V N V N V N
    !
    !            N V N V N V N

    IMPLICIT NONE
    INTEGER,  INTENT(IN)   :: nx_v
    INTEGER,  INTENT(IN)   :: ny_v
    INTEGER,  INTENT(IN)   :: nz
    REAL,     INTENT(IN)   :: datain(nx_v,ny_v,nz)
    REAL,     INTENT(OUT)  :: dataout(nx_v+1,ny_v,nz)

    INTEGER                :: j,k
    INTEGER                :: nx,ny

    nx = nx_v + 1
    ny = ny_v

    DO k = 1, nz

      ! Linear interpolation along each row, except left/right columns
      DO j = 1, ny

        ! Average of points above and below to fill interior rows
        dataout(2:nx_v,j,k) = 0.5*(datain(1:nx_v-1,j,k)+datain(2:nx_v,j,k))

        ! Fill left column
        dataout(1,j,k) = 2.0 * dataout(2,j,k) - dataout(3,j,k)

        ! Fill top row
        dataout(nx,j,k) = 2.0 * dataout(nx-1,j,k) - dataout(nx-2,j,k)

      ENDDO
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_v2n
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 SUBROUTINE arakawa_c_v2t(datain,nx_v,ny_v,nz,dataout)

    ! Destaggers from the WRF "v" grid to the mass grid.  The
    ! return array will be 1 element smaller in the Y direction than
    ! the input array.  Example:
    !
    !   Input 3x4 "V" grid, return 3x3 "T" grid:
    !
    !              V   V   V
    !              T   T   T
    !              V   V   V
    !              T   T   T
    !              V   V   V
    !              T   T   T
    !              V   V   V

    IMPLICIT NONE
    INTEGER,  INTENT(IN)   :: nx_v
    INTEGER,  INTENT(IN)   :: ny_v
    INTEGER,  INTENT(IN)   :: nz
    REAL,     INTENT(IN)   :: datain(nx_v,ny_v,nz)
    REAL,     INTENT(OUT)  :: dataout(nx_v,ny_v-1,nz)

    INTEGER                :: j,k
    INTEGER                :: nx,ny

    nx = nx_v
    ny = ny_v - 1

    DO k = 1, nz

      ! Linear interpolation along each column
      DO j = 1, ny

        dataout(:,j,k) = 0.5*(datain(:,j,k) + datain(:,j+1,k))

      ENDDO
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_v2t
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 SUBROUTINE arakawa_c_u2t(datain,nx_u,ny_u,nz,dataout)

    ! Destaggers from the WRF "u" grid to the mass grid.  The
    ! return array will be 1 element smaller in the X direction than
    ! the input array.  Example:
    !
    !   Input 4x3 "U" grid, return 3x3 "T" grid:
    !
    !
    !            U T U T U T U
    !
    !            U T U T U T U
    !
    !            U T U T U T U
    !
    IMPLICIT NONE
    INTEGER,  INTENT(IN)   :: nx_u
    INTEGER,  INTENT(IN)   :: ny_u
    INTEGER,  INTENT(IN)   :: nz
    REAL,     INTENT(IN)   :: datain(nx_u,ny_u,nz)
    REAL,     INTENT(OUT)  :: dataout(nx_u-1,ny_u,nz)

    INTEGER                :: i,k
    INTEGER                :: nx,ny

    nx = nx_u - 1
    ny = ny_u

    DO k = 1, nz

      ! Linear interpolation along each row
      DO i = 1, nx

        dataout(i,:,k) = 0.5*(datain(i,:,k) + datain(i+1,:,k))

      ENDDO
    ENDDO
    RETURN
  END SUBROUTINE arakawa_c_u2t
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  SUBROUTINE wlevs2hlevs(datain,nx,ny,nz_in,dataout)

    ! Vertically destaggers an array from the full W levels to
    ! the half levels.  Output array is one less in the z dimension
    ! than the input array

    IMPLICIT NONE
    INTEGER, INTENT(IN)   :: nx,ny,nz_in
    REAL, INTENT(IN)      :: datain(nx,ny,nz_in)
    REAL, INTENT(OUT)     :: dataout(nx,ny,nz_in-1)

    INTEGER :: k

    DO k = 1,nz_in-1

      dataout(:,:,k) = 0.5*(datain(:,:,k)+datain(:,:,k+1))

    ENDDO
    RETURN
  END SUBROUTINE wlevs2hlevs
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
END MODULE grid_utils


SUBROUTINE DiffXEven(var,varX,order,Nx,Ny,Nz,DiffStencils,alpha)
!
! Explicit order differentiation in X-direction.
!
! No conditional loops.
!
! By Allan P. Engsig-Karup.
USE Precision
USE DataTypes
IMPLICIT NONE
TYPE (Diff_def) :: DiffStencils
INTEGER :: Nx, Ny, Nz, rank, order, alpha, i,j,k
REAL(KIND=long), DIMENSION(Nz,Nx,Ny) :: var, varX
REAL(KIND=long), DIMENSION(2*alpha+1) :: Stencil
rank = 2*alpha+1
DO i = 1, alpha
	Stencil = DiffStencils%StencilX(i,1:rank,order)
	DO j = 1, Ny
		DO k = 1, Nz
			varX(k,i,j) = DOT_PRODUCT(Stencil,var(k,1:rank,j))
		END DO
	END DO
END DO
DO i = alpha+1, Nx-alpha
	Stencil = DiffStencils%StencilX(i,1:rank,order)
	DO j = 1, Ny
		DO k = 1, Nz
			varX(k,i,j) = DOT_PRODUCT(Stencil,var(k,i-alpha:i+alpha,j))
		END DO
	END DO
END DO
DO i = Nx-alpha+1, Nx
	Stencil = DiffStencils%StencilX(i,1:rank,order)
	DO j = 1, Ny
		DO k = 1, Nz
			varX(k,i,j) = DOT_PRODUCT(Stencil,var(k,Nx+1-rank:Nx,j))
		END DO
	END DO
END DO
END SUBROUTINE DiffXEven


! DART software - Copyright UCAR. This open source software is provided
! by UCAR, "as is", without charge, subject to all terms of use at
! http://www.image.ucar.edu/DAReS/DART/DART_download
!
! $Id$

program sys_sim2

! This one is described in notes from 14 Dec. or thereabout, 2001. Sample the
! uncertainty in the correlation coefficient due to sample size and approximate
! the resulting distribution in terms of a Gaussian (mean and variance). Probably
! want to evaluate how close to Gaussian this is, too.

! See notes from first two weeks of December, 2001. This program begins attempts
! to analyze the value of particular observations. Here, we begin by trying to
! determine the value of observations with a given correlation to a state variable
! using an N member ensemble to compute the correlations.

use random_seq_mod, only : random_seq_type, init_random_seq, random_gaussian, &
   twod_gaussians, random_uniform

implicit none

! version controlled file description for error handling, do not edit
character(len=256), parameter :: source   = &
   "$URL$"
character(len=32 ), parameter :: revision = "$Revision$"
character(len=128), parameter :: revdate  = "$Date$"

type (random_seq_type) :: r
double precision, allocatable :: rnum(:, :)
double precision :: c(2, 2), sum_err_var, sigma_y_p, sigma_y_o, sigma_x_p
double precision :: y_truth, y_o, sample_correl, reg_coef, sample_reg_coef
double precision :: sigma_y_u, y_u, sigma_x_u, x_u, sample_x_u, x_error
double precision :: x_error_var, total_err_var, mean(2), correl, est_err_var
double precision :: temp_sum, pe_2_sum, xx, s_xx, s_xx_2, xx_mean, xx_var
double precision, allocatable :: x_mean(:), x_var(:)
integer :: n, i, j, n_samples, index

! Initialize repeatable random sequence
call init_random_seq(r)

! Initialize error accumulation for averaging
sum_err_var = 0.0
mean = 0.0; temp_sum = 0.0; pe_2_sum = 0.0;

! For now have a set of free parameters that may be too large
write(*, *) 'Input prior variance of observation variable'
read(*, *) sigma_y_p
write(*, *) 'Input variance of observing instrument'
read(*, *) sigma_y_o
write(*, *) 'Input prior variance of state variable'
read(*, *) sigma_x_p
write(*, *) 'Input ensemble size'
read(*, *) n
write(*, *) 'input an expected correlation'
read(*, *) correl

! Set up covariance matrix for computing the sample correlation
c(1, 1) = 1.0; c(2, 2) = 1.0; c(1, 2) = correl; c(2, 1) = correl

! Allocate storage for doing the correlation
allocate(rnum(2, n))

write(*, *) 'Input number of samples for statistics'
read(*, *) n_samples
allocate(x_mean(n_samples), x_var(n_samples))

! Loop through the number of samples
do i = 1, n_samples

! Produce a sample of an observation yo by getting truth from prior
! and adding on sample from observatoinal error distribution
   y_truth = random_gaussian(r, dble(0.0), sqrt(sigma_y_p))
   y_o = y_truth + random_gaussian(r, dble(0.0), sqrt(sigma_y_o))

y_o = 0.4

! Use random sample to generate erroneous sample correlation
! Generate n pairs of numbers from this distribution
   do j = 1, n
      call twod_gaussians(r, mean, c, rnum(:, j))
   end do

! Compute the sample correlation
   call comp_correl(rnum, n, sample_correl)

! Compute regression coefficient, erroneous regression coefficient
   reg_coef = correl * sqrt(sigma_x_p) / sqrt(sigma_y_p)
   sample_reg_coef = sample_correl * sqrt(sigma_x_p) / sqrt(sigma_y_p)

! Compute the updated covariance and mean for y given this observation
   sigma_y_u = 1.0 / (1.0 / sigma_y_p + 1.0 / sigma_y_o)
   y_u = sigma_y_u * (0.0 + y_o / sigma_y_o)

! Compute updated covariance for x
! See notes from 11 Dec. for sigma_x_u computation
   sigma_x_u = sigma_x_p - correl**2 * sigma_x_p * sigma_y_p &
      / (sigma_y_o + sigma_y_p)

! Compute correct x_u and sample x_u
   x_u = reg_coef * y_u
   sample_x_u = sample_reg_coef * y_u

! Compute the mean and variance for this particular update
   x_mean(i) = sample_x_u
   x_var(i) = sigma_x_p - sample_correl**2 * sigma_y_p &
      / (sigma_y_o + sigma_y_p)

! Compute temporary sum to evaluate partial analytic solution
   temp_sum = temp_sum + (sample_correl - correl)**2 * y_o**2
   pe_2_sum = pe_2_sum + (sample_correl - correl)**2

! Compute error in x
   x_error = sample_x_u - x_u
   x_error_var = x_error **2

! Total expected variance for this one is x_error_var plus sigma_x_u
   total_err_var = x_error_var + sigma_x_u

! Accumulate this for averaging
   sum_err_var = sum_err_var + total_err_var

end do

! Output the expected value of the mean x
write(*, *) 'Expected updated mean is ', sum(x_mean) / n_samples

! Now compute the expected value of the updated variance by doing sampling
! The distribution is a sum of Gaussians with means and variance given
! by x_mean and x_var; each is equally likely. So, do a sample, randomly
! select a 'kernel', then randomly select a value from it.
s_xx = 0.0
s_xx_2 = 0.0
do i = 1, 10 * n_samples
   index = random_uniform(r) * n_samples + 1
   if(index < 1 .or. index > n_samples) then
      write(*, *) 'bad index ', i, index
      stop
   end if
   if(i / 500000 * 500000 == i) then
      write(*, *) 'mean, var ', i, index, real(x_mean(index)), real(x_var(index))
   endif
   xx = random_gaussian(r, x_mean(index), sqrt(x_var(index)))
   s_xx = s_xx + xx
   s_xx_2 = s_xx_2 + xx**2
end do

xx_mean = s_xx / (10.0 * n_samples)
write(*, *) 'Sampled mean is ', xx_mean
write(*, *) 'Expected mean is ', x_u
xx_var = (s_xx_2 - (10.0 * n_samples) * xx_mean**2) / &
   (10.0 * n_samples - 1.0)
write(*, *) 'Sampled variance is ', xx_var
write(*, *) 'Expected raw var is ', sigma_x_u
write(*, *) 'Mean variance is    ', s_xx_2 / (10.0 * n_samples)

if(1 == 1) stop

! Output the mean total_err_var
write(*, *) '----------'
write(*, *) 'Large N Mean total_err_var is ', sigma_x_u
write(*, *) 'Expected sample x variance is ', sum_err_var / n_samples
write(*, *) '----------'

! Using sample for product of correlation and y_0_2
write(*, *) 'Expected error variance is                  ', &
   (temp_sum / n_samples )*(sigma_x_p / sigma_y_p) * (sigma_y_u / sigma_y_o)**2

! Estimate using sample only for correlation
write(*, *) 'Expected error variance, only sample correl ', &
   sigma_x_p * sigma_y_p * (pe_2_sum / n_samples) / (sigma_y_p + sigma_y_o)

est_err_var = ((1.0 - correl**2)**2 / (n - 1.)) * sigma_x_p * sigma_y_p / &
   (sigma_y_p + sigma_y_o)
write(*, *) 'Estimated correl expected variance is       ', est_err_var
write(*, *) '----------'

write(*, *) 'pe_2_mean                   ', pe_2_sum / n_samples
write(*, *) 'expected value of pe_2_mean ', (1.0 - correl**2)**2 / (n - 1.)

write(*, *) '----------'
write(*, *) 'net improvement ', (sigma_x_p - sum_err_var / n_samples) / &
   (sigma_x_p - sigma_x_u)
write(*, *) 'Computed net improvement ', 1. - (pe_2_sum / n_samples) / correl**2

end program sys_sim2

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

subroutine comp_correl(ens, n, correl)

implicit none

integer, intent(in) :: n
double precision, intent(in) :: ens(2, n)
double precision, intent(out) :: correl
double precision :: sum_x, sum_y, sum_xy, sum_x2, sum_y2

sum_x = sum(ens(2, :))
sum_y = sum(ens(1, :))
sum_xy = sum(ens(2, :) * ens(1, :))
sum_x2 = sum(ens(2, :) * ens(2, :))

! Computation of correlation
sum_y2 = sum(ens(1, :) * ens(1, :))

correl = (n * sum_xy - sum_x * sum_y) / &
   sqrt((n * sum_x2 - sum_x**2) * (n * sum_y2 - sum_y**2))

end subroutine comp_correl