module immersedbc
use types,only:rprec
use param,only:ld,ny,nz
implicit none
private
public n_bldg,bldg_pts,fx,fy,fz,u_des,v_des,w_des,bldg_stress,bcuint
integer::n_bldg
integer,allocatable::bldg_pts(:,:)
real(kind=rprec),dimension(ld,ny,nz)::fx,fy,fz,u_des,v_des,w_des
logical,parameter::callag=.false.

contains

subroutine bldg_stress(txx,txy,txz,tyy,tyz,tzz)
implicit none
real(kind=rprec),dimension(ld,ny,nz),intent(inout)::txx,txy,txz,tyy,tyz,tzz
call bldg_stress_extrapol(txx)
call bldg_stress_extrapol(txy)
call bldg_stress_extrapol(txz)
call bldg_stress_extrapol(tyy)
call bldg_stress_extrapol(tyz)
call bldg_stress_extrapol(tzz)
end subroutine bldg_stress

subroutine bldg_stress_extrapol(tij)
implicit none

integer::jx,jy,jz
real(kind=rprec),dimension(ld,ny,nz):: tij
real(kind=rprec)::avg,npoints
integer :: i, px, py, lx, ly, lz,lz_1,lz_2

do i=1,n_bldg
   px=bldg_pts(1,i)
   py=bldg_pts(2,i)
   lx=bldg_pts(3,i)
   ly=bldg_pts(4,i)
   lz=bldg_pts(5,i)
   lz_1=lz+1
   lz_2=lz+2
   if(lz_1.ge.nz)lz_1=nz-1
   if(lz_2.ge.nz)lz_2=nz-1
! faces
   tij(px-1,py:py+ly,1:1+lz)=tij(px-2,py:py+ly,1:1+lz)
   tij(px,py:py+ly,1:1+lz)=tij(px-2,py:py+ly,1:1+lz)

   tij(px+lx,py:py+ly,1:1+lz)=tij(px+lx+2,py:py+ly,1:1+lz)
   tij(px+lx+1,py:py+ly,1:1+lz)=tij(px+lx+2,py:py+ly,1:1+lz)

   tij(px:px+lx,py-1,1:1+lz)=tij(px:px+lx,py-2,1:1+lz)
   tij(px:px+lx,py,1:1+lz)=tij(px:px+lx,py-2,1:1+lz)

   tij(px:px+lx,py+ly,1:1+lz)=tij(px:px+lx,py+ly+2,1:1+lz)
   tij(px:px+lx,py+ly+1,1:1+lz)=tij(px:px+lx,py+ly+2,1:1+lz)
!TS
!Change lz to lz_p to avoid overbound
   tij(px:px+lx,py:py+ly,1+lz) =tij(px:px+lx,py:py+ly,1+lz_2)
   tij(px:px+lx,py:py+ly,1+lz_1) =tij(px:px+lx,py:py+ly,1+lz_2)

! overwrite edges
   tij(px,py,1:1+lz-1) = 0.5_rprec*(tij(px-1,py,1:1+lz-1)+&
        tij(px,py-1,1:1+lz-1))
   tij(px-1,py-1,1:1+lz-1) = tij(px,py,1:1+lz-1)

   tij(px+lx,py,1:1+lz-1) = 0.5_rprec*(tij(px+lx+1,py,1:1+lz-1)+&
        tij(px+lx,py-1,1:1+lz-1))
   tij(px+lx+1,py-1,1:1+lz-1) = tij(px+lx,py,1:1+lz-1)

   tij(px+lx,py+ly,1:1+lz-1) = 0.5_rprec*(tij(px+lx+1,py+ly,1:1+lz-1)+&
        tij(px+lx,py+ly+1,1:1+lz-1))
   tij(px+lx+1,py+ly+1,1:1+lz-1) = tij(px+lx,py+ly,1:1+lz-1)

   tij(px,py+ly,1:1+lz-1) = 0.5_rprec*(tij(px-1,py+ly,1:1+lz-1)+&
        tij(px,py+ly+1,1:1+lz-1))
   tij(px-1,py+ly+1,1:1+lz-1) = tij(px,py+ly,1:1+lz-1)

! roof edges
   tij(px+1:px+lx-1,py,1+lz) = 0.5_rprec*(tij(px+1:px+lx-1,py-1,1+lz)+&
        tij(px+1:px+lx-1,py,1+lz_1))
   tij(px+1:px+lx-1,py-1,1+lz_1) = tij(px+1:px+lx-1,py,1+lz)

   tij(px+lx,py+1:py+ly-1,1+lz) =0.5_rprec*(tij(px+lx+1,py+1:py+ly-1,1+lz)+&
        tij(px+lx,py+1:py+ly-1,1+lz_1))
   tij(px+lx+1,py+1:py+ly-1,1+lz_1) = tij(px+lx,py+1:py+ly-1,1+lz) 

   tij(px+1:px+lx-1,py+ly,1+lz) =0.5_rprec*(tij(px+1:px+lx-1,py+ly+1,1+lz)+&
        tij(px+1:px+lx-1,py+ly,1+lz_1))
   tij(px+1:px+lx-1,py+ly+1,1+lz_1) = tij(px+1:px+lx-1,py+ly,1+lz)

   tij(px,py+1:py+ly-1,1+lz) =0.5_rprec*(tij(px-1,py+1:py+ly-1,1+lz)+&
        tij(px,py+1:py+ly-1,1+lz_1))
   tij(px-1,py+1:py+ly-1,1+lz_1) = tij(px,py+1:py+ly-1,1+lz) 

!  the corners
   tij(px-1,py,1+lz) = tij(px-2,py,1+lz)
   tij(px,py-1,1+lz) = tij(px,py-2,1+lz)
   tij(px,py,1+lz_1) = tij(px,py,1+lz_2)
   tij(px,py,1+lz)= (1._rprec/3._rprec)*(tij(px-1,py,1+lz)&
        +tij(px,py-1,1+lz)+tij(px,py,1+lz_1))

   tij(px+lx+1,py,1+lz) = tij(px+lx+2,py,1+lz)
   tij(px+lx,py-1,1+lz) = tij(px+lx,py-2,1+lz)
   tij(px+lx,py,1+lz_1) = tij(px+lx,py,1+lz_2)
   tij(px+lx,py,1+lz)= (1._rprec/3._rprec)*(tij(px+lx+1,py,1+lz)&
        +tij(px+lx,py-1,1+lz)+tij(px+lx,py,1+lz_1))

   tij(px+lx+1,py+ly,1+lz) = tij(px+lx+2,py+ly,1+lz)
   tij(px+lx,py+ly+1,1+lz) = tij(px+lx,py+ly+2,1+lz)
   tij(px+lx,py+ly,1+lz_1) = tij(px+lx,py+ly,1+lz_2)
   tij(px+lx,py+ly,1+lz)= (1._rprec/3._rprec)*(tij(px+lx+1,py+ly,1+lz)&
        +tij(px+lx,py+ly+1,1+lz)+tij(px+lx,py+ly,1+lz_1))

   tij(px-1,py+ly,1+lz) = tij(px-2,py+ly,1+lz)
   tij(px,py+ly+1,1+lz) = tij(px,py+ly+2,1+lz)
   tij(px,py+ly,1+lz_1) = tij(px,py+ly,1+lz_2)
   tij(px,py+ly,1+lz)= (1._rprec/3._rprec)*(tij(px-1,py+ly,1+lz)&
        +tij(px,py+ly+1,1+lz)+tij(px,py+ly,1+lz_1))

! interior
   if (callag) then
      do jz = 1,lz
         npoints=2._rprec*(lx+ly+2)
         avg=(1._rprec/npoints)*(SUM(tij(px:px+lx,py,jz))&
              +SUM(tij(px:px+lx,py+ly,jz))&
              +SUM(tij(px,py:py+ly,jz))&
              +SUM(tij(px+lx,py:py+ly,jz)))
         tij(px+1:px+lx-1,py+1:py+ly-1,jz) = avg
      end do
   else
      tij(px+1:px+lx-1,py+1:py+ly-1,1:1+lz-1) = 0._rprec
   end if
end do
end subroutine bldg_stress_extrapol

!TS SUBROUTINES FOR CUBIC SPLINE INTERPOLATION
SUBROUTINE bcuint(y,y1,y2,y12,x1dif,x2dif,M,N,x1,x2,ansy,jz)
implicit none
INTEGER::M,N,jz
REAL(KIND=rprec),INTENT(IN)::x1dif,x2dif
REAL(KIND=rprec),DIMENSION(M),INTENT(IN)::x1
REAL(KIND=rprec),DIMENSION(N),INTENT(IN)::x2
REAL(KIND=rprec),DIMENSION(M,N),INTENT(INOUT)::ansy
REAL(KIND=rprec),INTENT(IN)::y(4),y1(4),y12(4),y2(4)
INTEGER::i,j,k
REAL(KIND=rprec)::t,u
REAL(KIND=rprec),DIMENSION(4,4)::c
call bcucof(y,y1,y2,y12,x1dif,x2dif,c)
!if(jz.eq.16)then
!   print*,'y1',real(y),x1dif,x2dif
!   print*,'x12',x1,x2
!endif
if(x1dif.eq.0._rprec.or.x2dif.eq.0._rprec)stop 'bad input in bcuint'
do j=2,N-1
do i=2,M-1
t=x1(i)/x1dif
u=x2(j)/x2dif
ansy(i,j)=0._rprec
do k=4,1,-1
   ansy(i,j)=t*ansy(i,j)+((c(k,4)*u+c(k,3))*u+c(k,2))*u+c(k,1)
enddo
enddo
enddo
END SUBROUTINE bcuint

SUBROUTINE bcucof(y,y1,y2,y12,d1,d2,c)
IMPLICIT NONE
REAL(KIND=rprec), INTENT(IN) :: d1,d2
REAL(KIND=rprec), DIMENSION(4), INTENT(IN) :: y,y1,y2,y12
REAL(KIND=rprec), DIMENSION(4,4), INTENT(OUT) :: c
REAL(KIND=rprec), DIMENSION(16) :: x
REAL(KIND=rprec), DIMENSION(16,16) :: wt
DATA wt /1,0,-3,2,4*0,-3,0,9,-6,2,0,-6,4,&
     8*0,3,0,-9,6,-2,0,6,-4,10*0,9,-6,2*0,-6,4,2*0,3,-2,6*0,-9,6,&
     2*0,6,-4,4*0,1,0,-3,2,-2,0,6,-4,1,0,-3,2,8*0,-1,0,3,-2,1,0,-3,&
     2,10*0,-3,2,2*0,3,-2,6*0,3,-2,2*0,-6,4,2*0,3,-2,0,1,-2,1,5*0,&
     -3,6,-3,0,2,-4,2,9*0,3,-6,3,0,-2,4,-2,10*0,-3,3,2*0,2,-2,2*0,&
     -1,1,6*0,3,-3,2*0,-2,2,5*0,1,-2,1,0,-2,4,-2,0,1,-2,1,9*0,-1,2,&
     -1,0,1,-2,1,10*0,1,-1,2*0,-1,1,6*0,-1,1,2*0,2,-2,2*0,-1,1/
x(1:4)=y
x(5:8)=y1*d1
x(9:12)=y2*d2
x(13:16)=y12*d1*d2
x=matmul(wt,x)
c=reshape(x,(/4,4/),order=(/2,1/))
END SUBROUTINE bcucof
end module immersedbc
