program main
   ! Stand-alone test program for implicit stationary wave solver
   ! Based on wave enrgy balance for directionally spread waves, single representative frequency
   ! 4-sweep implicit method
   ! This test program is for structured 2D grids such as XBeach, but subroutines find_upwind_neighbours and 
   ! solve_energy_balance2Dstat are for unstructured grids. At the main level arrays are two-dimensional in 
   ! space but at lower level they are 1D.
   !
   ! (c) 2020 Dano Roelvink, IHE Delft
   !
   ! Local arrays
   real*8,  dimension(:,:),   allocatable    :: x,y                     ! x,y coordinates of grid
   real*8,  dimension(:,:),   allocatable    :: zb,hh                   ! bed level, water depth
   real*8,  dimension(:,:),   allocatable    :: dhdx,dhdy               ! water depth gradients
   real*8,  dimension(:,:),   allocatable    :: kwav,nwav               ! wave number, ratio Cg/C
   real*8,  dimension(:,:),   allocatable    :: C,Cg                    ! wave celerity, group velocity
   real*8,  dimension(:,:),   allocatable    :: fw                      ! friction coefficient
   real*8,  dimension(:,:),   allocatable    :: H                       ! rms wave height
   real*8,  dimension(:,:),   allocatable    :: Dw,Df                   ! dissipation due to breaking, bed friction
   real*8,  dimension(:,:),   allocatable    :: thetam                  ! mean wave direction
   real*8,  dimension(:,:),   allocatable    :: uorb                    ! orbital velocity
   integer, dimension(:,:),   allocatable    :: kp                      ! surrounding points for each grid point (unstructured)
   logical, dimension(:,:),   allocatable    :: inner                   ! mask for inner points (not on any boundary)
   integer, dimension(:),     allocatable    :: neumannconnected        ! neumann boundary indices connected to inner points
   integer, dimension(:),     allocatable    :: seapts                  ! grid numbers on incoming wave boundary
   integer*4                                 :: noseapts,noneumannpts   ! number of sea points, neumann boundary points
   real*8,  dimension(:),     allocatable    :: theta,sinth,costh       ! wave angles,sine and cosine of wave angles
   real*8,  dimension(:),     allocatable    :: dist,ee0                ! input directional distribution, wave energy distribution at sea 
   integer, dimension(:,:,:), allocatable    :: prev                    ! two upwind grid points per grid point and wave direction
   real*8,  dimension(:,:,:), allocatable    :: w                       ! weights of upwind grid points, 2 per grid point and per wave direction
   real*8,  dimension(:,:),   allocatable    :: ds                      ! distance to interpolated upwind point, per grid point and direction
   real*8,  dimension(:,:,:), allocatable    :: ctheta                  ! refraction speed, per grid point and direction
   
   ! Local input variables
   integer                                   :: nx,ny           ! number of grid cells in two directions
   integer                                   :: m,n             ! number of grid points in two directions
   real*8                                    :: dx,dy           ! x and y grid sizes (only for regular grid)
   integer                                   :: testcase=1      ! test case number
   real*8                                    :: Tp              ! peak wave period
   real*8                                    :: Hrms            ! incident rms wave height
   real*8                                    :: dir0            ! incident mean direction (deg, cartesian definition)
   real*8                                    :: thetamean       ! incident mean direction (rad, cartesian definition)
   integer                                   :: ms              ! power in cos^ms distribution function
   real*8                                    :: zs0             ! mean water level
   real*8                                    :: fw0             ! uniform wave friction factor 
   real*8                                    :: fwcutoff        ! depth below which to apply space-varying fw
   real*8                                    :: alfa,gamma      ! coefficients in Baldock breaking dissipation model
   character*232                             :: gridfile        ! name of gridfile (Delft3D .grd format)
   character*232                             :: depfile         ! name of bathymetry file (Delft3D .dep format)
   character*232                             :: fwfile          ! name of bed friction factor file (Delft3D .dep format)
   real*8                                    :: dt              ! time step (no limitation)

   ! Local constants
   real*8                                    :: rho             ! water density
   real*8                                    :: g               ! acceleration of gravity
   real*8                                    :: sigm            ! angular frequency
   real*8                                    :: t0,t1,t2,t3,t4  ! timers

   pi=4d0*atan(1.d0)
   g=9.81d0
   rho=1025.d0
   alfa=1.d0
   gamma=0.75d0
   dt=1000.d0
   hmin=0.1d0

   ! Case-specific inputs
   if (testcase==1) then
      gridfile='buck008.grd'
      depfile='buck008.dep'
      fwfile='fw008.dep'
      fwcutoff=5.d0
      fw0=0.01
      call read_d3d_grid_dims(gridfile,m,n)
      nx=m-1
      ny=n-1
      mn=m*n
      dir0=-100
      Hrms=1.67
      Tp=12
      zs0=0.d0
      ms=10
   else
      nx=200
      ny=200
      dx=5.d0
      dy=5.d0
      m=nx+1
      n=ny+1
      mn=m*n
      slope=0.02d0
      h0=20.d0      
      dir0=45.d0
      Hrms=2.d0
      Tp=8.d0
      zs0=0.d0
      fw0=0.01d0
      ms=10
   endif 

   ! Definition of directional grid
   thetamean=dir0
   ntheta=18

   allocate(theta(ntheta))
   allocate(sinth(ntheta))
   allocate(costh(ntheta))
   allocate(dist(ntheta))
   allocate(ee0(ntheta))

   thetamin=thetamean-90.d0
   thetamax=thetamean+90.d0
   dtheta=(thetamax-thetamin)/dble(ntheta)
   do itheta=1,ntheta
      theta(itheta)=thetamin+.5d0*dtheta+(itheta-1)*dtheta
   enddo

   theta=theta*pi/180.d0
   dtheta=dtheta*pi/180.d0
   thetamean=thetamean*pi/180.d0
   
   ! Offshore wave energy distribution
   E0=0.125d0*rho*g*Hrms**2
   dist=(cos(theta-thetamean))**ms;
   where (abs(theta-thetamean)>pi/2.d0) dist=0.d0
   ee0=dist/sum(dist)*E0/dtheta

   ! Allocation of spatial arrays
   allocate(x(m,n))
   allocate(y(m,n))
   allocate(zb(m,n))
   allocate(hh(m,n))
   allocate(inner(m,n))
   allocate(dhdx(m,n))
   allocate(dhdy(m,n))
   allocate(kwav(m,n))
   allocate(nwav(m,n))
   allocate(C(m,n))
   allocate(Cg(m,n))
   allocate(fw(m,n))
   allocate(H(m,n))
   allocate(Dw(m,n))
   allocate(Df(m,n))
   allocate(thetam(m,n))
   allocate(uorb(m,n))
   allocate(ctheta(m,n,ntheta))
   allocate(kp(mn,9))
   allocate(w(mn,ntheta,2))
   allocate(prev(mn,ntheta,2))
   allocate(ds(mn,ntheta))
   allocate(neumannconnected(mn))
   allocate(seapts(n))

   ! Case-dependent definition of grid-based variables
   if (testcase==1) then
      call read_d3d_grid(gridfile,x,y,m,n)
      call read_d3d_dep(depfile,m,n,zb)
      call read_d3d_dep(fwfile,m,n,fw)
      where(zs0-zb>fwcutoff) fw=fw0
      hh=zs0-zb
   else      
      x0=nx/2*dx
      y0=ny/2*dy
      do j=1,n
         do i=1,m
            x(i,j)=(i-1)*dx
            y(i,j)=(j-1)*dy
            hh(i,j)=zs0-max(-h0+(x(i,j))*slope,-h0)
         enddo
      enddo
      fw=fw0
   endif
   hh=max(hh,hmin)

   ! Definition of inner points (not on boundary)
   inner=.false.
   inner(2:nx,2:ny)=.true.
   
   ! Definition of Neumann  boundary points, according to XBeach definition
   neumannconnected=0
   do i=2,m
       neumannconnected(i+m)=i
   enddo
   do i=2,m
      neumannconnected((n-2)*m+i)=(n-1)*m+i
   enddo
   

   ! Definition of sea boundary points, according to XBeach definition
   noseapts=n
   do j=1,n
      seapts(j)=(j-1)*m+1
   enddo

   ! Initialization of reference tables
   kp=0
   w=0.d0
   prev=0
   ds=0.d0

   call timer(t0) 

   ! Define surrounding grid points for each grid point
   call create_surrounding_points(m,n,kp)
   call timer(t1)
   write(*,*)'create_surrounding_points: ',t1-t0,' seconds'

   ! Find upwind neighbours for each grid point and wave direction
   call find_upwind_neighbours(x,y,mn,theta,ntheta,kp,w,prev,ds)
   call timer(t2)
   write(*,*)'find_upwind_neighbours:    ',t2-t1,' seconds'

   ! Compute water depth gradients
   call slopes(x,y,hh,m,n,dhdx,dhdy)

   ! Compute celerities and refraction speed
   call disper_approx(hh,Tp,kwav,nwav,C,Cg,mn)
   sinth=sin(theta)
   costh=cos(theta)
   sigm=2.d0*pi/Tp
   do itheta=1,ntheta
      ctheta(:,:,itheta)= sigm/sinh(2.d0*kwav*hh)*(dhdx*sinth(itheta)-dhdy*costh(itheta))
   enddo

   ! Solve the directional wave energy balance on an unstructured grid
   call solve_energy_balance2Dstat (x,y,mn,w,ds,inner,prev,seapts,noseapts,neumannconnected,       &
                                    ee0,theta,ntheta,thetamean,                                    &
                                    hh,kwav,cg,ctheta,fw,Tp,dt,rho,alfa,gamma,                     &
                                    H,Dw,Df,thetam,uorb)
   call timer(t3)
   write(*,*)'computation:               ',t3-t2,' seconds'
   
   ! Simple output as ASCII blocks
   call writeblock('x.txt',x,m,n)
   call writeblock('y.txt',y,m,n)
   call writeblock('zb.txt',zb,m,n)
   call writeblock('hh.txt',hh,m,n)
   call writeblock('cg.txt',cg,m,n)
   call writeblock('H.txt',H,m,n)
   call writeblock('thetam.txt',thetam*180.d0/pi,m,n)
   call writeblock('Dw.txt',Dw,m,n)
   call writeblock('Df.txt',Df,m,n)

   call timer(t4)
   write(*,*)'output:                    ',t4-t3,' seconds'
   write(*,*)'total:                     ',t4-t0,' seconds'
   pause
   
end program


subroutine create_surrounding_points(m,n,kp)
   integer,intent(in)                     :: m,n
   integer, dimension(m*n,9),intent(out)  :: kp
   
   integer, dimension(9)                  :: ii,jj,dk
   integer                                :: i,j,k
   
   ii=(/-1,-1, 0, 1, 1, 1, 0,-1,-1/)
   jj=(/ 0,-1,-1,-1, 0, 1, 1, 1, 0/)
   dk=ii+jj*m

   do j=2,n-1
      do i=2,m-1
         k=i+(j-1)*m
         kp(k,:)=k+dk
      enddo
   enddo
end subroutine create_surrounding_points

subroutine find_upwind_neighbours(x,y,mn,theta,ntheta,kp,w,prev,ds)
   integer, intent(in)                                :: mn,ntheta           ! length of x,y; length of theta
   real*8,  dimension(mn), intent(in)                 :: x,y                 ! x, y coordinates of grid
   integer, dimension(mn,9), intent(in)               :: kp                  ! grid indices of surrounfing points per grid point
   real*8,  dimension(ntheta), intent(in)             :: theta               ! array of wave angles 
   real*8,  dimension(mn,ntheta,2), intent(out)       :: w                   ! per grid point and direction, weight of upwind points
   integer, dimension(mn,ntheta,2), intent(out)       :: prev                ! per grid point and direction, indices of upwind points
   real*8,  dimension(mn,ntheta), intent(out)         :: ds                  ! upwind distance to intersection point for each direction

   real*8,  dimension(2)                              :: xsect,ysect,ww
   real*8                                             :: pi,dss,xi,yi
   integer                                            :: ind1,ind2
   integer                                            :: ip
   integer                                            :: k, itheta
   ! find upwind neighbours for each cell in an unstructured grid x,y (1d
   ! vectors) given vector of directions theta

   pi=4d0*atan(1.d0)

   np=size(kp,dim=2)
   do k=1,mn
      if (kp(k,1)/=0) then
         do itheta=1,ntheta
            do ip=1,np-1
               ind1=kp(k,ip)
               ind2=kp(k,ip+1)
               xsect=(/x(ind1),x(ind2)/)
               ysect=(/y(ind1),y(ind2)/)
               call intersect_angle(x(k),y(k),theta(itheta)+pi,xsect,ysect,ww,dss,xi,yi)
               if (dss/=0) then
                  w(k,itheta,1)=ww(1)
                  w(k,itheta,2)=ww(2)
                  ds(k,itheta)=dss
                  prev(k,itheta,1)=kp(k,ip)
                  prev(k,itheta,2)=kp(k,ip+1)
                  exit
               endif
            enddo
            if (dss==0) then
               prev(k,itheta,1)=1
               prev(k,itheta,2)=1
               w(k,itheta,1)=0
               w(k,itheta,2)=0
            endif
         enddo
      endif
   enddo

end subroutine find_upwind_neighbours

subroutine intersect_angle(x0,y0,phi,x,y,W,ds,xi,yi)
   real*8, intent(in)               :: x0,y0,phi
   real*8, dimension(2),intent(in)  :: x,y
   real*8, dimension(2),intent(out) :: W
   real*8, intent(out)              :: ds,xi,yi
   real*8                           :: eps,m,a,b,n,L,d1,d2
   eps=1d-3
   if (abs(x(2)-x(1))>eps) then
      m=(y(2)-y(1))/(x(2)-x(1))
      a=y(1)-m*x(1)
      n=tan(phi)
      b=y0-n*x0
      xi=(b-a)/(m-n)
      yi=a+m*xi
   else
      yi=(x(1)-x0)*tan(phi)+y0
      xi=x(1)
   endif

   L=hypot(x(2)-x(1),y(2)-y(1))
   d1=hypot(xi-x(1),yi-y(1))
   d2=hypot(xi-x(2),yi-y(2))
   ds=hypot(xi-x0,yi-y0)
   err=hypot((x0-xi)+ds*cos(phi),(y0-yi)+ds*sin(phi))
   if (abs(L-d1-d2)<eps .and. err<eps) then
      W(1)=d2/L
      W(2)=d1/L
   else
      W(1)=0.d0
      W(2)=0.d0
      ds=0.d0
   endif

end subroutine intersect_angle

subroutine solve_energy_balance2Dstat(x,y,mn,w,ds,inner,prev,seapts,noseapts,neumannconnected,       &
                                      ee0,theta,ntheta,thetamean,                                    &
                                      hh,kwav,cg,ctheta,fw,T,dt,rho,alfa,gamma,                      &
                                      H,Dw,Df,thetam,uorb)
   
   ! In/output variables and arrays
   real*8, dimension(mn),intent(in)           :: x,y                    ! x,y coordinates of grid
   integer, intent(in)                        :: mn,ntheta              ! number of grid points, number of directions
   real*8,  dimension(mn,ntheta,2),intent(in) :: w                      ! weights of upwind grid points, 2 per grid point and per wave direction
   real*8, dimension(mn,ntheta), intent(in)   :: ds                     ! distance to interpolated upwind point, per grid point and direction
   real*8, intent(in)                         :: thetamean              ! mean offshore wave direction (rad)
   real*8, dimension(ntheta), intent(in)      :: ee0,theta              ! distribution of wave angles and offshore wave energy density
   logical, dimension(mn), intent(in)         :: inner                  ! mask of inner grid points (not on boundary)
   integer, dimension(mn,ntheta,2),intent(in) :: prev                   ! two upwind grid points per grid point and wave direction
   integer, intent(in)                        :: noseapts               ! number of offshore wave boundary points
   integer, dimension(noseapts)               :: seapts                 ! indices of offshore wave boundary points
   integer, dimension(mn),intent(in)          :: neumannconnected       ! number of neumann boundary point if connected to inner point
   real*8, dimension(mn), intent(in)          :: hh                     ! water depth
   real*8, dimension(mn), intent(in)          :: kwav                   ! wave number
   real*8, dimension(mn), intent(in)          :: cg                     ! group velocity
   real*8, dimension(mn,ntheta), intent(in)   :: ctheta                 ! refractioon speed
   real*8, dimension(mn), intent(in)          :: fw                     ! wave friction factor
   real*8, intent(in)                         :: T                      ! wave period
   real*8, intent(in)                         :: dt                     ! time step (s)
   real*8, intent(in)                         :: rho                    ! water density
   real*8, intent(in)                         :: alfa,gamma             ! coefficients in Baldock wave breaking dissipation
   real*8, dimension(mn), intent(out)         :: H                      ! wave height
   real*8, dimension(mn), intent(out)         :: Dw                     ! wave breaking dissipation
   real*8, dimension(mn), intent(out)         :: Df                     ! wave friction dissipation
   real*8, dimension(mn), intent(out)         :: thetam                 ! mean wave direction
   real*8, dimension(mn), intent(out)         :: uorb                   ! orbital velocity

   ! Local variables and arrays
   integer, dimension(:), allocatable         :: ok                     ! mask for fully iterated points
   real*8                                     :: eemax,dtheta           ! maximum wave energy density, directional resolution
   integer                                    :: sweep,niter            ! sweep number, number of iterations
   integer                                    :: k,k1,k2,i,ind(1)       ! counters (k is grid index)
   integer, dimension(:,:), allocatable       :: indx                   ! index for grid sorted per sweep direction
   real*8, dimension(:,:), allocatable        :: ee,eeold               ! wave energy density, energy density previous iteration
   real*8, dimension(:,:), allocatable        :: dee                    ! difference with energy previous iteration
   real*8, dimension(:), allocatable          :: eeprev, cgprev         ! energy density and group velocity at upwind intersection point
   real*8, dimension(:,:),allocatable         :: A,B,C,R                ! coefficients in the tridiagonal matrix solved per point
   real*8, dimension(:), allocatable          :: DoverE                 ! ratio of mean wave dissipation over mean wave energy
   real*8, dimension(:), allocatable          :: E                      ! mean wave energy
   real*8, dimension(:), allocatable          :: diff                   ! maximum difference of wave energy relative to previous iteration
   real*8, dimension(:), allocatable          :: ra                     ! coordinate in sweep direction
   integer, dimension(:), allocatable         :: tempneu                ! work array
   real*8                                     :: pi=4.d0*atan(1.d0)
   real*8                                     :: g=9.81d0
   real*8                                     :: hmin=0.1d0             ! minimum water depth
   real*8                                     :: fac=0.25d0             ! underrelaxation factor for DoverE
   real*8                                     :: crit=0.0001            ! relative accuracy

   ! Allocate local arrays
   allocate(ok(mn))
   allocate(indx(mn,4))
   allocate(ee(mn,ntheta))
   allocate(eeold(mn,ntheta))
   allocate(dee(mn,ntheta))
   allocate(eeprev(ntheta))
   allocate(cgprev(ntheta))
   allocate(A(mn,ntheta))
   allocate(B(mn,ntheta))
   allocate(C(mn,ntheta))
   allocate(R(mn,ntheta))
   allocate(DoverE(mn))
   allocate(E(mn))
   allocate(diff(mn))
   allocate(ra(mn))
   allocate(tempneu(noneumannpts))

   ok=0
   indx=0
   eemax=1.d0;
   dtheta=theta(2)-theta(1)
   niter=400
   
   ! Sort coordinates in sweep directions
   do sweep=1,4
      ra=x*cos(thetamean+(sweep-1)*pi/2.d0)+y*sin(thetamean+(sweep-1)*pi/2.d0)
      call hpsort_eps_epw (mn, ra , indx(:,sweep), 1.d-6)
   enddo

   ! Boundary condition at sea side (uniform)
   do i=1,noseapts
      k=seapts(i)
      ee(k,:)=ee0
   enddo
   
   ! Start iteration
   do iter=1,niter
      sweep=mod(iter,4)
      if (sweep==0) then
         sweep=4;
      endif
      if (sweep==1) then
         eeold=ee;
      endif
      !  Loop over all points depending on sweep direction
      do count=1,mn
         k=indx(count,sweep)
         if (inner(k)) then
            if (hh(k)>1.1*hmin) then
               if (.not. ok(k)) then
                  ! Only perform computations on wet inner points that are not yet converged (ok)
                  do itheta=1,ntheta
                     k1=prev(k,itheta,1)
                     k2=prev(k,itheta,2)
                     eeprev(itheta)=w(k,itheta,1)*ee(k1,itheta)+w(k,itheta,2)*ee(k2,itheta)
                     cgprev(itheta)=w(k,itheta,1)*cg(k1)+w(k,itheta,2)*cg(k2)
                  enddo
                  do itheta=2,ntheta-1
                     A(k,itheta)=-ctheta(k,itheta-1)/2/dtheta
                     B(k,itheta)=1/dt+cg(k)/ds(k,itheta)+DoverE(k)
                     C(k,itheta)=ctheta(k,itheta+1)/2/dtheta
                     R(k,itheta)=ee(k,itheta)/dt+cgprev(itheta)*eeprev(itheta)/ds(k,itheta)
                  enddo
                  if (ctheta(k,1)<0) then
                     A(k,1)=0.d0
                     B(k,1)=1/dt-ctheta(k,1)/dtheta+cg(k)/ds(k,1)+DoverE(k)
                     C(k,1)=ctheta(k,2)/dtheta
                     R(k,1)=ee(k,1)/dt+cgprev(itheta)*eeprev(1)/ds(k,1)
                  else
                     A(k,1)=0.d0
                     B(k,1)=1.d0/dt
                     C(k,1)=0.d0
                     R(k,1)=0.d0
                  endif
                  if (ctheta(k,ntheta)>0) then
                     A(k,ntheta)=-ctheta(k,ntheta-1)/dtheta
                     B(k,ntheta)=1/dt+ctheta(k,ntheta)/dtheta+cg(k)/ds(k,ntheta)+DoverE(k)
                     C(k,ntheta)=0
                     R(k,ntheta)=ee(k,ntheta)/dt+cgprev(itheta)*eeprev(ntheta)/ds(k,ntheta)
                  else
                     A(k,ntheta)=0.d0
                     B(k,ntheta)=1.d0/dt
                     C(k,ntheta)=0.d0
                     R(k,ntheta)=0.d0
                  endif
                  ! Solve tridiagonal system per point
                  call solve_tridiag(A(k,:),B(k,:),C(k,:),R(k,:),ee(k,:),ntheta)
                  ee(k,:)=max(ee(k,:),0.d0)
                  E(k)=sum(ee(k,:))*dtheta
                  H(k)=sqrt(8*E(k)/rho/g)
                  H(k)=min(H(k),gamma*hh(k))
                  E(k)=0.125d0*rho*g*H(k)**2
                  call baldock(g,rho,alfa,gamma,kwav(k),hh(k),H(k),T,1,Dw(k))
                  uorb(k)=pi*H(k)/T/sinh(kwav(k)*hh(k))
                  Df(k)=0.28d0*rho*fw(k)*uorb(k)**3
                  DoverE(k)=(1.d0-fac)*DoverE(k)+fac*(Dw(k)+Df(k))/max(E(k),1.d-6)
                  do itheta=1,ntheta
                     B(k,itheta)=1/dt+cg(k)/ds(k,itheta)+DoverE(k)
                  enddo
                  ! Solve tridiagonal system per point
                  call solve_tridiag(A(k,:),B(k,:),C(k,:),R(k,:),ee(k,:),ntheta)
                  ee(k,:)=max(ee(k,:),0.d0)
               endif
            else
               ee(k,:)=0
            endif
            if (neumannconnected(k)/=0) then
                ee(neumannconnected(k),:)=ee(k,:)
            endif
            
         endif

      enddo
      do k=1,mn
         ! Compute directionally integrated paraneters
         ee(k,:)=max(ee(k,:),0.d0)
         E(k)=sum(ee(k,:))*dtheta
         H(k)=sqrt(8*E(k)/rho/g)
         call baldock(g,rho,alfa,gamma,kwav(k),hh(k),H(k),T,1,Dw(k))
         uorb(k)=pi*H(k)/T/sinh(kwav(k)*hh(k))
         Df(k)=0.28d0*rho*fw(k)*uorb(k)**3
         DoverE(k)=(1.d0-fac)*DoverE(k)+fac*(Dw(k)+Df(k))/max(E(k),1.d-6)
         thetam(k)=atan2(sum(ee(k,:)*sin(theta)),sum(ee(k,:)*cos(theta)))
      enddo
      if (sweep==4) then
         ! Check convergence after all 4 sweeps
         do k=1,mn
            dee(k,:)=ee(k,:)-eeold(k,:)
            diff(k)=maxval(abs(dee(k,:)))
            if (diff(k)/eemax<crit) ok(k)=1
         enddo
         ! Percentage of converged points
         percok=sum(ok)/dble(mn)*100.d0;
         eemax=maxval(ee)
         ! Relative maximum error
         error=maxval(diff)/eemax
         write(*,'(a,i6,a,f10.5,a,f7.2)')'iteration ',iter/4 ,' error = ',error,'   %ok = ',percok
         if (error<crit .and. iter>4) then
            write(*,*) 'Stop'
            exit
         endif
      endif
   enddo

end subroutine solve_energy_balance2Dstat


subroutine solve_tridiag(a,b,c,d,x,n)
   implicit none
   !	 a - sub-diagonal (means it is the diagonal below the main diagonal)
   !	 b - the main diagonal
   !	 c - sup-diagonal (means it is the diagonal above the main diagonal)
   !	 d - right part
   !	 x - the answer
   !	 n - number of equations

   integer,parameter :: r8 = kind(1.d0)

   integer,intent(in) :: n
   real(r8),dimension(n),intent(in) :: a,b,c,d
   real(r8),dimension(n),intent(out) :: x
   real(r8),dimension(n) :: cp,dp
   real(r8) :: m
   integer i

   ! initialize c-prime and d-prime
   cp(1) = c(1)/b(1)
   dp(1) = d(1)/b(1)
   ! solve for vectors c-prime and d-prime
   do i = 2,n
      m = b(i)-cp(i-1)*a(i)
      cp(i) = c(i)/m
      dp(i) = (d(i)-dp(i-1)*a(i))/m
   end do
   ! initialize x
   x(n) = dp(n)
   ! solve for x from the vectors c-prime and d-prime
   do i = n-1, 1, -1
      x(i) = dp(i)-cp(i)*x(i+1)
   end do

end subroutine solve_tridiag

subroutine baldock (rho,g,alfa,gamma,k,hh,H,T,opt,Dw)
   real*8, intent(in)                :: rho
   real*8, intent(in)                :: g
   real*8, intent(in)                :: alfa
   real*8, intent(in)                :: gamma
   real*8, intent(in)                :: k
   real*8, intent(in)                :: hh
   real*8, intent(in)                :: H
   real*8, intent(in)                :: T
   integer, intent(in)               :: opt
   real*8, intent(out)               :: Dw

   ! Compute dissipation according to Baldock
   Hmax=0.88/k*tanh(gamma*k*hh/0.88d0)
   if (opt==1) then
      Dw=0.25*alfa*rho*g/T*exp(-(Hmax/H)**2)*(Hmax**2+H**2)
   else
      Dw=0.25*alfa*rho*g/T*exp(-(Hmax/H)**2)*(Hmax**3+H**3)/gamma/hh
   endif
end subroutine baldock

!
! Copyright (C) 2010-2016 Samuel Ponce', Roxana Margine, Carla Verdi, Feliciano Giustino
! Copyright (C) 2007-2009 Jesse Noffsinger, Brad Malone, Feliciano Giustino
!
! This file is distributed under the terms of the GNU General Public
! License. See the file `LICENSE' in the root directory of the
! present distribution, or http://www.gnu.org/copyleft.gpl.txt .
!
! Adapted from flib/hpsort_eps
!---------------------------------------------------------------------
subroutine hpsort_eps_epw (n, ra, ind, eps)
   !---------------------------------------------------------------------
   ! sort an array ra(1:n) into ascending order using heapsort algorithm,
   ! and considering two elements being equal if their values differ
   ! for less than "eps".
   ! n is input, ra is replaced on output by its sorted rearrangement.
   ! create an index table (ind) by making an exchange in the index array
   ! whenever an exchange is made on the sorted data array (ra).
   ! in case of equal values in the data array (ra) the values in the
   ! index array (ind) are used to order the entries.
   ! if on input ind(1)  = 0 then indices are initialized in the routine,
   ! if on input ind(1) != 0 then indices are assumed to have been
   !                initialized before entering the routine and these
   !                indices are carried around during the sorting process
   !
   ! no work space needed !
   ! free us from machine-dependent sorting-routines !
   !
   ! adapted from Numerical Recipes pg. 329 (new edition)
   !
   implicit none
   !-input/output variables
   integer, intent(in)   :: n
   real*8, intent(in)  :: eps
   integer :: ind (n)
   real*8 :: ra (n)
   !-local variables
   integer :: i, ir, j, l, iind
   real*8 :: rra
   !
   ! initialize index array
   IF (ind (1) .eq.0) then
      DO i = 1, n
         ind (i) = i
      ENDDO
   ENDIF
   ! nothing to order
   IF (n.lt.2) return
   ! initialize indices for hiring and retirement-promotion phase
   l = n / 2 + 1

   ir = n

   sorting: do

      ! still in hiring phase
      IF ( l .gt. 1 ) then
         l    = l - 1
         rra  = ra (l)
         iind = ind (l)
         ! in retirement-promotion phase.
      ELSE
         ! clear a space at the end of the array
         rra  = ra (ir)
         !
         iind = ind (ir)
         ! retire the top of the heap into it
         ra (ir) = ra (1)
         !
         ind (ir) = ind (1)
         ! decrease the size of the corporation
         ir = ir - 1
         ! done with the last promotion
         IF ( ir .eq. 1 ) then
            ! the least competent worker at all !
            ra (1)  = rra
            !
            ind (1) = iind
            exit sorting
         ENDIF
      ENDIF
      ! wheter in hiring or promotion phase, we
      i = l
      ! set up to place rra in its proper level
      j = l + l
      !
      DO while ( j .le. ir )
         IF ( j .lt. ir ) then
            ! compare to better underling
            IF ( hslt( ra (j),  ra (j + 1) ) ) then
               j = j + 1
               !else if ( .not. hslt( ra (j+1),  ra (j) ) ) then
               ! this means ra(j) == ra(j+1) within tolerance
               !  if (ind (j) .lt.ind (j + 1) ) j = j + 1
            ENDIF
         ENDIF
         ! demote rra
         IF ( hslt( rra, ra (j) ) ) then
            ra (i) = ra (j)
            ind (i) = ind (j)
            i = j
            j = j + j
            !else if ( .not. hslt ( ra(j) , rra ) ) then
            !this means rra == ra(j) within tolerance
            ! demote rra
            ! if (iind.lt.ind (j) ) then
            !    ra (i) = ra (j)
            !    ind (i) = ind (j)
            !    i = j
            !    j = j + j
            ! else
            ! set j to terminate do-while loop
            !    j = ir + 1
            ! endif
            ! this is the right place for rra
         ELSE
            ! set j to terminate do-while loop
            j = ir + 1
         ENDIF
      ENDDO
      ra (i) = rra
      ind (i) = iind

   END DO sorting
contains

   !  internal function
   !  compare two real number and return the result

   logical function hslt( a, b )
      REAL*8 :: a, b
      IF( abs(a-b) <  eps ) then
         hslt = .false.
      ELSE
         hslt = ( a < b )
      end if
   end function hslt

   !
end subroutine hpsort_eps_epw

subroutine slopes(x,y,h,m,n,dhdx,dhdy)
   integer, intent(in)                    :: m,n
   real*8, dimension(m,n), intent(in)     :: x,y,h
   real*8, dimension(m,n), intent(out)    :: dhdx,dhdy

   real*8                                 :: deltas,deltan,dhds,dhdn,cosald,sinalf
   integer                                :: i,j

   ! Compute slopes in water depth (note: for structured grid only)
   dhdx=0.d0
   dhdy=0.d0
   do j=2,n-1
      do i=2,m-1
         deltas=hypot(x(i+1,j)-x(i-1,j),y(i+1,j)-y(i-1,j))
         deltan=hypot(x(i,j+1)-x(i,j-1),y(i,j+1)-y(i,j-1))
         dhds=(h(i+1,j)-h(i-1,j))/deltas
         dhdn=(h(i,j+1)-h(i,j-1))/deltan
         cosalf=(x(i+1,j)-x(i-1,j))/deltas
         sinalf=(y(i+1,j)-y(i-1,j))/deltas
         dhdx(i,j)=dhds*cosalf-dhdn*sinalf
         dhdy(i,j)=dhds*sinalf+dhdn*cosalf
      enddo
   enddo
end subroutine slopes

subroutine disper_approx(h,T,k,n,C,Cg,mn)
   integer, intent(in)                :: mn
   real*8, dimension(mn), intent(in)  :: h
   real*8, intent(in)                 :: T
   real*8, dimension(mn), intent(out) :: k,n,C,Cg

   real*8                             :: sigma,g,pi
   g=9.81d0
   pi=4.d0*atan(1.d0)
   sigma=2.d0*pi/T
   k = sigma**2/g*(1-exp(-(sigma*sqrt(h/g))**(2.5)))**(-0.4)
   C= sigma/k
   n = 0.5d0+k*h/sinh(2*k*h)
   Cg=n*C

end subroutine disper_approx

subroutine read_d3d_grid_dims(gridfile,m,n)
   character*232,intent(in)           :: gridfile
   integer, intent(out)               :: m,n

   integer                            :: ier,ier2
   character*232                      :: line
   open(31,file=gridfile,status='old',iostat=ier)
   if (ier .ne. 0) then
      stop('error reading gridfile')
   endif
   do
      read(31,'(a)',iostat=ier)line
      if (line(1:1)/='*') then
         exit
      endif
   enddo
   read(31,*,iostat=ier2) m,n
   ! new grid format now specifies missing value, so catch this error
   if (ier2 .ne. 0) then
      read(31,*,iostat=ier2) m,n
   endif
   close(31)
end subroutine read_d3d_grid_dims

subroutine read_d3d_grid(gridfile,x,y,m,n)
   character*232,intent(in)           :: gridfile
   integer, intent(in)                :: m,n
   real*8, dimension(m,n),intent(out) :: x,y

   integer                            :: ier,ier2,ier3,idum,i,j
   character*232                      :: line
   !
   ! Gridfile
   !
   open(31,file=gridfile,status='old',iostat=ier)
   if (ier .ne. 0) then
      stop('error reading gridfile')
   endif
   do
      read(31,'(a)',iostat=ier)line
      if (line(1:1)/='*') then
         exit
      endif
   enddo
   read(31,*,iostat=ier2) idum,idum
   ! new grid format now specifies missing value, so catch this error
   if (ier2 .ne. 0) then
      read(31,*,iostat=ier2) idum,idum
   endif

   read(31,*,iostat=ier3) idum,idum,idum
   ! if any iostat is still /= 0 then there is an error reading the file
   if (ier+ier2+ier3 .ne. 0) then
      stop('error reading grid file')
   endif

   read(31,*,iostat=ier) &
   (line,dum,(x(i,j),i=1,m),j=1,n), &
   (line,dum,(y(i,j),i=1,m),j=1,n)
   if (ier .ne. 0) then
      stop('error reading grid file')
   endif

   close(31)
end subroutine read_d3d_grid

subroutine read_d3d_dep(depfile,m,n,zb)
   character*232,intent(in)           :: depfile
   integer, intent(in)                :: m,n
   real*8, dimension(m,n),intent(out) :: zb

   integer                            :: ier,i,j
   character*232                      :: line
   open(33,file=depfile,status='old')
   do j=1,n
      read(33,*,iostat=ier)(zb(i,j),i=1,m)
      if (ier .ne. 0) then
         stop('error reading depth')
      endif
   enddo
   close(33)
end subroutine read_d3d_dep

subroutine writeblock(filename,var,m,n)
   character(*), intent(in)           :: filename
   integer, intent(in)                :: m,n
   real*8, dimension(m,n), intent(in) :: var
    
   integer                            :: i,j
    
   open(11,file=filename)
   do j=1,n
      write(11,'(1000f12.3)')(var(i,j),i=1,m)
   enddo
   close(11)
end subroutine writeblock

subroutine timer(t)
   real*8,intent(out)               :: t
   integer*4                        :: count,count_rate,count_max
   call system_clock (count,count_rate,count_max)
   t = dble(count)/count_rate
end subroutine timer