!------------------------------------------------------------------------------|
!------------------------------------------------------------------------------|
!                                                                              |
!              ||===\\                                                         | 
!              ||    \\                                                        |
!              ||     ||   //==\\   ||  ||   //==||  ||/==\\                   |
!              ||     ||  ||    ||  ||  ||  ||   ||  ||    ||                  |
!              ||    //   ||    ||  ||  ||  ||   ||  ||                        |
!              ||===//     \\==//    \\==\\  \\==\\  ||                        |
!                                                                              |
!------------------------------------------------------------------------------|
!------------------------------------------------------------------------------|
!                                                                              |
!              CALCULATE_LSF    Apr. 2007                                      |
!                                                                              |
!------------------------------------------------------------------------------|
!------------------------------------------------------------------------------|

subroutine calculate_lsf (lsf,octree_lsf,noctree_lsf,icon_lsf,nleaves_lsf,     &
                         na_lsf,xl,yl,zl,xln,yln,zln,icon,nelem,na,            &
                         levelmax_oct,normaladvect)

!------------------------------------------------------------------------------|
!(((((((((((((((( Purpose of the routine  ))))))))))))))))))))))))))))))))))))))
!------------------------------------------------------------------------------|
! this subroutine calculates the lsf (level set function) on an octree from
! the position of a series of particles connected by a 2D triangulation

! the algorithm goes first through the triangles
! to calculate a 3D normal at the "centre" of each triangle (from the position of the
! corresponding particles). From that normal, the levelset function is
! estimated at the nodes of the leaf of the levelset octree containing the
! centre of the particles (this algorithm assumes that the density of the
! particles is larger than the density of the octree)

! in a second pass, the algorithm propagates the sign of the level set function
! by successive pass through the leaves of the octree. The algorithm converges
! very rapidly because each successive path is in reverse order from the previous
 
! lsf is the values of the lsf at the nodes (main output of the routine)
! octree_lsf is the octree on which the lsf is to be built known
! noctree_lsf is its size
! icon_lsf is the node-leaf connectivity of the octree
! nleaves_lsf is the number of leaves in the octree
! na_lsf is the number of nodes in the octree
! xl,yl,zl are the coordinates of the particles on the surface
! icon is the connectivity matrix defining the triangulation
! nelem is the number of triangles
! na is the number of particles defining the surface
! levelmax_oct is the maximum level of refinement of the octree

!------------------------------------------------------------------------------|
!((((((((((((((((  declaration of the subroutine arguments  ))))))))))))))))))))
!------------------------------------------------------------------------------|

!use mpi

implicit none

include 'mpif.h'

double precision lsf(na_lsf)
integer octree_lsf(noctree_lsf) 
integer noctree_lsf
integer icon_lsf(8,nleaves_lsf)
integer nleaves_lsf
integer na_lsf
double precision xl(na),yl(na),zl(na)
double precision xln(na),yln(na),zln(na)
integer icon(3,nelem)
integer nelem
integer na
integer levelmax_oct
logical normaladvect
!------------------------------------------------------------------------------|
!(((((((((((((((( declaration of the subroutine internal variables )))))))))))))
!------------------------------------------------------------------------------|

integer ierr,iproc,nproc,ii,jj,kk,iii,istart,ijk,iend,itest,i,j,k,leaf_lsf
integer i1,i2,j1,j2,k1,k2,it,inode,niter,iinc,num_lsf,level,loc,nleaf_test,err
integer,dimension(:),allocatable::leaf_test
integer,dimension(:),allocatable::done_el,done_nd,norm
double precision x0,y0,z0,dxyz,sum_lsf,test
double precision x0_loc,y0_loc,z0_loc
double precision x,y,z,x1,x2,x3,y1,y2,y3,z1,z2,z3
double precision x1n,x2n,x3n,y1n,y2n,y3n,z1n,z2n,z3n
double precision lsf8(8)
double precision,dimension(:),allocatable::distmin
double precision,dimension(:),allocatable::xxln,yyln,zzln

!------------------------------------------------------------------------------|
!------------------------------------------------------------------------------|

call mpi_comm_size (mpi_comm_world,nproc,ierr)
call mpi_comm_rank (mpi_comm_world,iproc,ierr)

! first pass using the centres of the triangles connecting
! the particles of the surface
! (note that we also take the opportunity to update the normal of
!  the particles)

lsf=0.d0

allocate (done_el(nleaves_lsf),stat=err) ; if (err.ne.0) call stop_run ('Error alloc done_el in calculate_lsf$')
allocate (done_nd(na_lsf),stat=err)      ; if (err.ne.0) call stop_run ('Error alloc done_nd in calculate_lsf$')
allocate (distmin(na_lsf),stat=err)      ; if (err.ne.0) call stop_run ('Error alloc distmin in calculate_lsf$')
allocate (norm(na),stat=err)             ; if (err.ne.0) call stop_run ('Error alloc norm in calculate_lsf$')
allocate (xxln(na),stat=err)             ; if (err.ne.0) call stop_run ('Error alloc xxln in calculate_lsf$')
allocate (yyln(na),stat=err)             ; if (err.ne.0) call stop_run ('Error alloc yyln in calculate_lsf$')
allocate (zzln(na),stat=err)             ; if (err.ne.0) call stop_run ('Error alloc zzln in calculate_lsf$')

done_el=1
done_nd=1
distmin=1.d6

! NOTE by JEAN
! here we should not be recalculating the normals but using those that have been advected	
! But I have checked for the ball that the normals are OK during the first step for the ball

if (normaladvect) then
xxln=xln
yyln=yln
zzln=zln
else
call compute_normals (na,xl,yl,zl,nelem,icon,xxln,yyln,zzln)
endif

! estimates signed distance to surface

do it=1,nelem
   x1=xl(icon(1,it))
   x2=xl(icon(2,it))
   x3=xl(icon(3,it))
   y1=yl(icon(1,it))
   y2=yl(icon(2,it))
   y3=yl(icon(3,it))
   z1=zl(icon(1,it))
   z2=zl(icon(2,it))
   z3=zl(icon(3,it))
   x1n=xxln(icon(1,it))
   x2n=xxln(icon(2,it))
   x3n=xxln(icon(3,it))
   y1n=yyln(icon(1,it))
   y2n=yyln(icon(2,it))
   y3n=yyln(icon(3,it))
   z1n=zzln(icon(1,it))
   z2n=zzln(icon(2,it))
   z3n=zzln(icon(3,it))
   i1=int(minval(xl(icon(:,it)))/.5d0**levelmax_oct)
   i2=int(maxval(xl(icon(:,it)))/.5d0**levelmax_oct)+1
   j1=int(minval(yl(icon(:,it)))/.5d0**levelmax_oct)
   j2=int(maxval(yl(icon(:,it)))/.5d0**levelmax_oct)+1
   k1=int(minval(zl(icon(:,it)))/.5d0**levelmax_oct)
   k2=int(maxval(zl(icon(:,it)))/.5d0**levelmax_oct)+1
   allocate (leaf_test((k2-k1+1)*(j2-j1+1)*(i2-i1+1)),stat=err)
   if (err.ne.0) call stop_run ('Error alloc leaf_test in calculate_lsf$')
   nleaf_test=0
!   do k=k1,k2-1
!      z=(real(k)+.5d0)*(.5d0**levelmax_oct)
!      do j=j1,j2-1
!         y=(real(j)+.5d0)*(.5d0**levelmax_oct)
!         do i=i1,i2-1
!            x=(real(i)+.5d0)*(.5d0**levelmax_oct)
   do k=k1,k2
      z=dfloat(k)*(.5d0**levelmax_oct)-.25d0**levelmax_oct
      do j=j1,j2
         y=dfloat(j)*(.5d0**levelmax_oct)-.25d0**levelmax_oct
         do i=i1,i2
            x=dfloat(i)*(.5d0**levelmax_oct)-.25d0**levelmax_oct
            if (x*(x-1.d0).le.0.d0 .and. &
                y*(y-1.d0).le.0.d0 .and. &
                z*(z-1.d0).le.0.d0) then
               ! find octree leaf
               call octree_find_leaf (octree_lsf,noctree_lsf,x,y,z,leaf_lsf,level,loc,x0,y0,z0,dxyz)
               do itest=1,nleaf_test
                  if (leaf_lsf.eq.leaf_test(itest)) goto 1111
               enddo
               nleaf_test=nleaf_test+1
               leaf_test(nleaf_test)=leaf_lsf
               ! compute potential lsf's
               ijk=0
               do kk=0,1
                  do jj=0,1
                     do ii=0,1
                        ijk=ijk+1
                        x0_loc=x0+ii*dxyz
                        y0_loc=y0+jj*dxyz
                        z0_loc=z0+kk*dxyz
                        inode=icon_lsf(ijk,leaf_lsf)
                        call distance_to_triangle &
                          (x1,y1,z1,x2,y2,z2,x3,y3,z3, &
                           x1n,y1n,z1n,x2n,y2n,z2n,x3n,y3n,z3n, &
                           x0_loc,y0_loc,z0_loc,test)
                        lsf8(ijk)=test
                     enddo
                  enddo
               enddo
               do ijk=1,8
               inode=icon_lsf(ijk,leaf_lsf)
                 if (abs(lsf8(ijk)).lt.distmin(inode)) then
                 lsf(inode)=lsf8(ijk)
                 distmin(inode)=abs(lsf8(ijk))
                 done_nd(inode)=0
                 endif
               enddo
1111           continue
            endif
         enddo
      enddo
   enddo
   deallocate (leaf_test)
enddo

! now propagates the lsf into elements that are not cut by the surface

istart=1
iend=nleaves_lsf
iinc=1
niter=0
do while (sum(done_nd).ne.0)
   do i=istart,iend,iinc
      if (done_el(i).eq.1) then
         do k=1,8
            if (done_nd(icon_lsf(k,i)).eq.0) then
               done_el(i)=0
! note that we average the value of the lsf around the nodes of the leaf
! to find the sign of the nodes that have not yet been assigned
! the reason for this is that there might be leaves where the surface is
! passing through but where no particle resides; these leaes would have
! lsf values of different sign on different nodes from the previous two passes
! this should never happen...
               sum_lsf=0.d0
               num_lsf=0
               do kk=1,8
                  if (done_nd(icon_lsf(kk,i)).eq.0) then
                     num_lsf=num_lsf+1
                     sum_lsf=sum_lsf+lsf(icon_lsf(kk,i))
                  endif
               enddo
               sum_lsf=sum_lsf/num_lsf
               do kk=1,8
                  if (done_nd(icon_lsf(kk,i)).eq.1) then
                      lsf(icon_lsf(kk,i))=sign(1.d0,sum_lsf)
                     done_nd(icon_lsf(kk,i))=0
                  endif
               enddo
               goto 1
            endif
         enddo
      endif
1     continue
   enddo
   iii=istart
   istart=iend
   iend=iii
   iinc=-iinc
   niter=niter+1
   if (niter.eq.100) then
      print*,'Too many iterations'
      print*,'in algoritm propagating lsf function'
      print*,'number of nodes remaining in lsf build ',sum(done_nd)
      call stop_run ('program terminated in calculate_lsf$')
   endif
enddo

deallocate (done_el)
deallocate (done_nd)
deallocate (distmin)
deallocate (norm)
deallocate (xxln)
deallocate (yyln)
deallocate (zzln)

return
end
!------------------------------------------------------------------------------|
!------------------------------------------------------------------------------|

subroutine distance_to_triangle (x1,y1,z1,x2,y2,z2,x3,y3,z3,                   &
                                 x1n,y1n,z1n,x2n,y2n,z2n,x3n,y3n,z3n,          &
                                 x0,y0,z0,dist)

implicit none

double precision x0,x1,x2,x3
double precision y0,y1,y2,y3
double precision z0,z1,z2,z3
double precision x1n,y1n,z1n
double precision x2n,y2n,z2n
double precision x3n,y3n,z3n
double precision dist

double precision x(4),y(4),z(4)
double precision xxn(4),yyn(4),zzn(4)
double precision xn,yn,zn,xyzn
double precision xc,yc,zc
double precision alpha,beta
double precision xnp(3),ynp(3),znp(3),alphap(3),dd(3)
double precision xp,yp,zp,xpp,ypp,zpp
double precision xxxn,yyyn,zzzn
integer k

x(1)=x1;x(2)=x2;x(3)=x3;x(4)=x1
y(1)=y1;y(2)=y2;y(3)=y3;y(4)=y1
z(1)=z1;z(2)=z2;z(3)=z3;z(4)=z1
xxn(1)=x1n;xxn(2)=x2n;xxn(3)=x3n;xxn(4)=x1n
yyn(1)=y1n;yyn(2)=y2n;yyn(3)=y3n;yyn(4)=y1n
zzn(1)=z1n;zzn(2)=z2n;zzn(3)=z3n;zzn(4)=z1n

xc=(x1+x2+x3)/3.d0
yc=(y1+y2+y3)/3.d0
zc=(z1+z2+z3)/3.d0
xn=(y2-y1)*(z3-z1)-(y3-y1)*(z2-z1)
yn=(z2-z1)*(x3-x1)-(z3-z1)*(x2-x1)
zn=(x2-x1)*(y3-y1)-(x3-x1)*(y2-y1)
xyzn=sqrt(xn**2+yn**2+zn**2)
xn=xn/xyzn
yn=yn/xyzn
zn=zn/xyzn
alpha=xn*(x0-xc)+yn*(y0-yc)+zn*(z0-zc)

do k=1,3
   xnp(k)=(y(k+1)-y(k))*zn-(z(k+1)-z(k))*yn
   ynp(k)=(z(k+1)-z(k))*xn-(x(k+1)-x(k))*zn
   znp(k)=(x(k+1)-x(k))*yn-(y(k+1)-y(k))*xn
   xyzn=sqrt(xnp(k)**2+ynp(k)**2+znp(k)**2)
   xnp(k)=xnp(k)/xyzn
   ynp(k)=ynp(k)/xyzn
   znp(k)=znp(k)/xyzn
   alphap(k)=(x0-x(k))*xnp(k)+(y0-y(k))*ynp(k)+(z0-z(k))*znp(k)
enddo

if (alphap(1).le.0.d0 .and. alphap(2).le.0.d0 .and. alphap(3).le.0.d0) then
   dist=alpha
   return
endif

do k=1,3
   dd(k)=1.d6
   if (alphap(k).ge.0.d0) then
      xp=x0-alpha*xn
      yp=y0-alpha*yn
      zp=z0-alpha*zn
      xpp=xp-alphap(k)*xnp(k)
      ypp=yp-alphap(k)*ynp(k)
      zpp=zp-alphap(k)*znp(k)
      beta=((xpp-x(k))*(x(k+1)-x(k))+(ypp-y(k))*(y(k+1)-y(k))+(zpp-z(k))*(z(k+1)-z(k)))/ &
           ((x(k+1)-x(k))*(x(k+1)-x(k))+(y(k+1)-y(k))*(y(k+1)-y(k))+(z(k+1)-z(k))*(z(k+1)-z(k)))
      if (beta.lt.0.d0) then
         dd(k)=sqrt((x0-x(k))**2+(y0-y(k))**2+(z0-z(k))**2)
         alpha=xxn(k)*(x0-x(k))+yyn(k)*(y0-y(k))+zzn(k)*(z0-z(k))
         dd(k)=sign(dd(k),alpha)
      elseif (beta.gt.1.d0) then
         dd(k)=sqrt((x0-x(k+1))**2+(y0-y(k+1))**2+(z0-z(k+1))**2)
         alpha=xxn(k+1)*(x0-x(k+1))+yyn(k+1)*(y0-y(k+1))+zzn(k+1)*(z0-z(k+1))
         dd(k)=sign(dd(k),alpha)
      else
         xxxn=xxn(k)+beta*(xxn(k+1)-xxn(k))
         yyyn=yyn(k)+beta*(yyn(k+1)-yyn(k))
         zzzn=zzn(k)+beta*(zzn(k+1)-zzn(k))
         dd(k)=sqrt((x0-xpp)**2+(y0-ypp)**2+(z0-zpp)**2)
         alpha=xxxn*(x0-xpp)+yyyn*(y0-ypp)+zzzn*(z0-zpp)
         dd(k)=sign(dd(k),alpha)
      endif
   endif
enddo

dist=dd(1)
if (abs(dd(2)).lt.abs(dist)) dist=dd(2)
if (abs(dd(3)).lt.abs(dist)) dist=dd(3)

return
end

