MODULE variables
  INTEGER(4), PARAMETER             :: nx1=100, nx2=100
  INTEGER(4)                        :: ndivx1=20, ndivx2=20
  REAL(8)                           :: dx1, dx2
  REAL(8)                           :: rho=1000.0d0, nu=0.1d0
  REAL(8)                           :: dumax1, dumax2, pdmax
  REAL(8)                           :: alpha_u1=0.7d0, alpha_u2=0.7d0, alpha_p=0.3d0
  REAL(8)   , DIMENSION(0:nx1,0:nx2):: u1, u2, p, pd
  REAL(8)   , DIMENSION(0:nx1,0:nx2):: ap, ae, aw, an, as, app
END MODULE variables

PROGRAM cavity
  USE variables
  IMPLICIT NONE
  integer(4)::i,j,k
  
  CALL Settings
  dumax1=100.d0
  dumax2=100.d0
  pdmax =100.d0
  i=0
  k=0
  do while(pdmax>1.0e-6)
    i=i+1
    print *, i,pdmax
    do while(dumax1>1.0d-6 .or. dumax2>1.0d-6)
      k=k+1
      CALL Cal_u1
      CALL Cal_u2
    end do
    CALL Coupling
  end do
  
  CALL Output

END PROGRAM cavity

SUBROUTINE Settings
  USE variables
  IMPLICIT NONE
  INTEGER(4)                      :: i, j
  
  dx1=1.d0/real(ndivx1)
  dx2=1.d0/real(ndivx2)
  
  do i=1,ndivx1+1
    do j=0,ndivx2
      u1(i,j)=0.d0
      u2(i,j)=0.d0
    end do
  end do
  
  do i=1,ndivx1
    do j=1,ndivx2
      p(i,j)=0.d0
      pd(i,j)=0.d0
    end do
  end do
  
END SUBROUTINE Settings

SUBROUTINE Cal_u1
  USE variables
  IMPLICIT NONE
  INTEGER(4)                      :: i, j, k
  REAL(8)                         :: U1e, U1w, U2n, U2s
  REAL(8)                         :: du
  
  do k=1,3
    dumax1=0.d0
    do i=2,ndivx1
      do j=1, ndivx2
        U1e=0.5d0*(u1(i  ,j  )+u1(i+1,j  ))
        U1w=0.5d0*(u1(i-1,j  )+u1(i  ,j  ))
        U2n=0.5d0*(u2(i-1,j  )+u2(i  ,j  ))
        U2s=0.5d0*(u2(i-1,j-1)+u2(i  ,j-1))
        ap(i,j)= 2.d0*nu*(dx2/dx1+dx1/dx2)
        aw(i,j)= nu*dx2/dx1
        ae(i,j)= nu*dx2/dx1
        as(i,j)= nu*dx1/dx2
        an(i,j)= nu*dx1/dx2
        app(i,j)=dx2/rho
          
        if(abs(U1e)>1.0d-10) then
          ap(i,j)= ap(i,j)+0.5d0*U1e*( U1e/abs(U1e)+1.d0)*dx2
          ae(i,j)= ae(i,j)-0.5d0*U1e*(-U1e/abs(U1e)+1.d0)*dx2
        end if
        if(abs(U1w)>1.0d-10) then
          ap(i,j)= ap(i,j)-0.5d0*U1w*(-U1w/abs(U1w)+1.d0)*dx2
          aw(i,j)= aw(i,j)+0.5d0*U1w*( U1w/abs(U1w)+1.d0)*dx2
        end if
        if(abs(U2n)>1.0d-10) then
          ap(i,j)= ap(i,j)+0.5d0*U2n*( U2n/abs(U2n)+1.d0)*dx1
          an(i,j)= an(i,j)-0.5d0*U2n*( U2n/abs(U2n)+1.d0)*dx2
        end if
        if(abs(U2s)>1.0d-9) then
          ap(i,j)= ap(i,j)-0.5d0*U2s*(-U2s/abs(U2s)+1.d0)*dx1
          as(i,j)= as(i,j)+0.5d0*U2s*( U2s/abs(U2s)+1.d0)*dx1
        end if
        if(j==1) then
          du=(aw(i,j)*u1(i-1,j)+ae(i,j)*u1(i+1,j) &
 &           -as(i,j)*u1(i,j  )+an(i,j)*u1(i,j+1) &
 &           +app(i,j)*(p(i-1,j)-p(i,j)))/ap(i,j) &
 &           -u1(i,j)
        else if(j==ndivx2) then
          du=(aw(i,j)*u1(i-1,j)+ae(i,j)*u1(i+1,j) &
 &           +as(i,j)*u1(i,j-1)+an(i,j)*(2.d0-u1(i,j)) &
 &           +app(i,j)*(p(i-1,j)-p(i,j)))/ap(i,j) &
 &           -u1(i,j)
        else
          du=(aw(i,j)*u1(i-1,j)+ae(i,j)*u1(i+1,j) &
 &           +as(i,j)*u1(i,j-1)+an(i,j)*u1(i,j+1) &
 &           +app(i,j)*(p(i-1,j)-p(i,j)))/ap(i,j) &
 &           -u1(i,j)
        end if
        
        if(abs(du) > dumax1) then
          dumax1=abs(du)
        end if
        u1(i,j)=u1(i,j)+alpha_u1*du
      end do
    end do
  end do
END SUBROUTINE Cal_u1


SUBROUTINE Cal_u2
  USE variables
  IMPLICIT NONE
  INTEGER(4)                      :: i, j, k
  REAL(8)                         :: U1e, U1w, U2n, U2s
  REAL(8)                         :: du
  
  do k=1,3
    dumax2=0.d0
    do i=1,ndivx1
      do j=1, ndivx2-1
        U1e=0.5d0*(u1(i+1,j  )+u1(i+1,j+1))
        U1w=0.5d0*(u1(i  ,j  )+u1(i  ,j+1))
        U2n=0.5d0*(u2(i  ,j  )+u2(i  ,j+1))
        U2s=0.5d0*(u2(i  ,j-1)+u2(i  ,j  ))
        ap(i,j)= 2.d0*nu*(dx2/dx1+dx1/dx2)
        aw(i,j)= nu*dx2/dx1
        ae(i,j)= nu*dx2/dx1
        as(i,j)= nu*dx1/dx2
        an(i,j)= nu*dx1/dx2
        app(i,j)=dx1/rho
          
        if(abs(U1e)>1.0d-10) then
          ap(i,j)= ap(i,j)+0.5d0*U1e*( U1e/abs(U1e)+1.d0)*dx2
          ae(i,j)= ae(i,j)-0.5d0*U1e*(-U1e/abs(U1e)+1.d0)*dx2
        end if
        if(abs(U1w)>1.0d-10) then
          ap(i,j)= ap(i,j)-0.5d0*U1w*(-U1w/abs(U1w)+1.d0)*dx2
          aw(i,j)= aw(i,j)+0.5d0*U1w*( U1w/abs(U1w)+1.d0)*dx2
        end if
        if(abs(U2n)>1.0d-9) then
          ap(i,j)= ap(i,j)+0.5d0*U2n*( U2n/abs(U2n)+1.d0)*dx1
          an(i,j)= an(i,j)-0.5d0*U2n*( U2n/abs(U2n)+1.d0)*dx2
        end if
        if(abs(U2s)>1.0d-9) then
          ap(i,j)= ap(i,j)-0.5d0*U2s*(-U2s/abs(U2s)+1.d0)*dx1
          as(i,j)= as(i,j)+0.5d0*U2s*( U2s/abs(U2s)+1.d0)*dx1
        end if
        if(i==1) then
          du=(-aw(i,j)*u2(i,j)+ae(i,j)*u2(i+1,j) &
 &           +as(i,j)*u2(i,j-1)+an(i,j)*u2(i,j+1) &
 &           +app(i,j)*(p(i,j)-p(i,j+1)))/ap(i,j) &
 &           -u2(i,j)
        else if(i==ndivx1) then
          du=(aw(i,j)*u2(i-1,j)-ae(i,j)*u2(i,j) &
 &           +as(i,j)*u2(i,j-1)+an(i,j)*u2(i,j+1) &
 &           +app(i,j)*(p(i,j)-p(i,j+1)))/ap(i,j) &
 &           -u2(i,j)
        else
          du=(aw(i,j)*u2(i-1,j)+ae(i,j)*u2(i+1,j) &
 &           +as(i,j)*u2(i,j-1)+an(i,j)*u2(i,j+1) &
 &           +app(i,j)*(p(i,j)-p(i,j+1)))/ap(i,j) &
 &           -u2(i,j)
        end if
        
        if(abs(du) > dumax2) then
          dumax2=abs(du)
        end if
        u2(i,j)=u2(i,j)+alpha_u2*du
      end do
    end do
  end do
END SUBROUTINE Cal_u2

SUBROUTINE Coupling
  USE variables
  IMPLICIT NONE
  INTEGER(4)                        :: i, j
  REAL(8)                           :: U1e, U1w, U2n, U2s, du
  REAL(8)                           :: dpmax, dp, pref
  REAL(8)   , DIMENSION(0:nx1,0:nx2):: d1, d2, cp, cw, ce, cs, cn, cc

    do i=1,ndivx1+1
      do j=1, ndivx2
        if(i==1 .or. i==ndivx1+1) then
          d1(i,j)=0.d0
        else
          U1e=0.5d0*(u1(i+1,j  )+u1(i+1,j+1))
          U1w=0.5d0*(u1(i  ,j  )+u1(i  ,j+1))
          U2n=0.5d0*(u2(i  ,j  )+u2(i  ,j+1))
          U2s=0.5d0*(u2(i  ,j-1)+u2(i  ,j  ))
          ap(i,j)= 2.d0*nu*(dx2/dx1+dx1/dx2)
          app(i,j)=dx1/rho
          
          if(abs(U1e)>1.0d-10) then
            ap(i,j)= ap(i,j)+0.5d0*U1e*( U1e/abs(U1e)+1.d0)*dx2
          end if
          if(abs(U1w)>1.0d-10) then
            ap(i,j)= ap(i,j)-0.5d0*U1w*(-U1w/abs(U1w)+1.d0)*dx2
          end if
          if(abs(U2n)>1.0d-9) then
            ap(i,j)= ap(i,j)+0.5d0*U2n*( U2n/abs(U2n)+1.d0)*dx1
          end if
          if(abs(U2s)>1.0d-9) then
            ap(i,j)= ap(i,j)-0.5d0*U2s*(-U2s/abs(U2s)+1.d0)*dx1
          end if
          d1(i,j)=app(i,j)/ap(i,j)
        end if
      end do
    end do
    

    do i=1,ndivx1
      do j=0, ndivx2
        if(j==0 .or. j==ndivx2) then
          d2(i,j)=0.d0
        else
          U1e=0.5d0*(u1(i+1,j  )+u1(i+1,j+1))
          U1w=0.5d0*(u1(i  ,j  )+u1(i  ,j+1))
          U2n=0.5d0*(u2(i  ,j  )+u2(i  ,j+1))
          U2s=0.5d0*(u2(i  ,j-1)+u2(i  ,j  ))
          ap(i,j)= 2.d0*nu*(dx2/dx1+dx1/dx2)
          app(i,j)=dx1/rho
          
          if(abs(U1e)>1.0d-10) then
            ap(i,j)= ap(i,j)+0.5d0*U1e*( U1e/abs(U1e)+1.d0)*dx2
          end if
          if(abs(U1w)>1.0d-10) then
            ap(i,j)= ap(i,j)-0.5d0*U1w*(-U1w/abs(U1w)+1.d0)*dx2
          end if
          if(abs(U2n)>1.0d-9) then
            ap(i,j)= ap(i,j)+0.5d0*U2n*( U2n/abs(U2n)+1.d0)*dx1
          end if
          if(abs(U2s)>1.0d-9) then
            ap(i,j)= ap(i,j)-0.5d0*U2s*(-U2s/abs(U2s)+1.d0)*dx1
          end if
          d2(i,j)=app(i,j)/ap(i,j)
        end if
      end do
    end do
  
  do i=1,ndivx1
    do j=1,ndivx2
      pd(i,j)=0.d0
    end do
  end do
  
  dpmax=100.d0
  do while(dpmax>1.d-6)
    dpmax=0.d0
    do i=1,ndivx1
      do j=1,ndivx2
        cp(i,j)=(d1(i,j)+d1(i+1,j))*dx2+(d2(i,j)+d2(i,j-1))*dx1
        cw(i,j)=d1(i  ,j)*dx2
        ce(i,j)=d1(i+1,j)*dx2
        cs(i,j)=d2(i,j-1)*dx1
        cn(i,j)=d2(i,j  )*dx1
        cc(i,j)=-((u1(i+1,j)-u1(i,j))*dx2+(u2(i,j)-u2(i,j-1))*dx1)
        if(i==1 .and. j==1) then
          dp=(cw(i,j)*pd(i,j)+ce(i,j)*pd(i+1,j)  &
 &           +cs(i,j)*pd(i,j)+cn(i,j)*pd(i,j+1)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(i==1 .and. j==ndivx2) then
          dp=(cw(i,j)*pd(i,j)+ce(i,j)*pd(i+1,j)  &
 &           +cs(i,j)*pd(i,j-1)+cn(i,j)*pd(i,j)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(i==ndivx1 .and. j==1) then
          dp=(cw(i,j)*pd(i-1,j)+ce(i,j)*pd(i,j)  &
 &           +cs(i,j)*pd(i,j)+cn(i,j)*pd(i,j+1)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(i==ndivx1 .and. j==ndivx2) then
          dp=(cw(i,j)*pd(i-1,j)+ce(i,j)*pd(i,j)  &
 &           +cs(i,j)*pd(i,j-1)+cn(i,j)*pd(i,j)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(i==1) then
          dp=(cw(i,j)*pd(i,j)+ce(i,j)*pd(i+1,j)  &
 &           +cs(i,j)*pd(i,j-1)+cn(i,j)*pd(i,j+1)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(i==ndivx1) then
          dp=(cw(i,j)*pd(i-1,j)+ce(i,j)*pd(i,j)  &
 &           +cs(i,j)*pd(i,j-1)+cn(i,j)*pd(i,j+1)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(j==1) then
          dp=(cw(i,j)*pd(i-1,j)+ce(i,j)*pd(i+1,j)  &
 &           +cs(i,j)*pd(i,j)+cn(i,j)*pd(i,j+1)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else if(j==ndivx2) then
          dp=(cw(i,j)*pd(i-1,j)+ce(i,j)*pd(i+1,j)  &
 &           +cs(i,j)*pd(i,j-1)+cn(i,j)*pd(i,j)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        else
          dp=(cw(i,j)*pd(i-1,j)+ce(i,j)*pd(i+1,j)  &
 &           +cs(i,j)*pd(i,j-1)+cn(i,j)*pd(i,j+1)  &
 &           +cc(i,j))/cp(i,j)-pd(i,j)
        end if
        if(abs(dp) > dpmax) then
          dpmax=abs(dp)
        end if
        pd(i,j)=pd(i,j)+alpha_p*dp
      end do
    end do
  end do

  pdmax=0.d0
  do i=1,ndivx1
    do j=1,ndivx2
      if(abs(pd(i,j)) > pdmax) then
        pdmax=abs(pd(i,j))
      end if
      p(i,j)=p(i,j)+alpha_p*pd(i,j)
      u1(i,j)=u1(i,j)+alpha_u1*d1(i,j)*(pd(i-1,j)-pd(i,j  ))
      u2(i,j)=u2(i,j)+alpha_u1*d2(i,j)*(pd(i  ,j)-pd(i,j+1))
    end do
  end do
  
  pref=p(1,1)
  do i=1,ndivx1
    do j=1,ndivx2
     p(i,j)=p(i,j)-pref
    end do
  end do
  
END SUBROUTINE Coupling

SUBROUTINE Output
  USE variables
  IMPLICIT NONE
  INTEGER(4)                      :: i, j, k
  
  open(51, file='plot.dat')
  write(51,*)'TITLE = "Cavity"'
  write(51,*)'VARIABLES ="x1" "x2" "u1" "u2" "p" '
  write(51,'(a,i4,a,i4,a)') 'ZONE T="Time 0[sec]" I=',ndivx1,' J=',ndivx2,' F=POINT'
  do i=1,ndivx1
    do j=1, ndivx2
      write(51,'(5e15.5)') dx1*(real(i)-0.5d0), dx2*(real(j)-0.5d0), 0.5d0*(u1(i,j)+u1(i+1,j)), 0.5d0*(u2(i,j)+u2(i,j-1)), p(i,j)
    end do
  end do
  close(51)

END SUBROUTINE Output