!---------------------------------------------------------------------
!
! plait.f90:
! Elaborated on original ideas from Ch. Nieten and
! the NOD2 package.
!
! P. Hily-Blant feb-2005
!
!---------------------------------------------------------------------
!
module plait_common
  integer(kind=4), parameter :: sp=4
  integer(kind=4), parameter :: dp=8
  character(len=*), parameter :: rname = 'PLAIT'
  real(kind=sp), parameter :: wfunc_thres = 1.e-3_sp
end module plait_common
!
module  plait_interface
  interface
     subroutine plait_check(next,prev,error)
       use plait_common
       use image_def
       type(gildas), intent(in) :: prev , next
       logical, intent(out) :: error
     end subroutine plait_check
     !
     function plait_wfunc(r,width)
       use plait_common
       use phys_const
       real(kind=sp) :: plait_wfunc
       real(kind=sp), intent(in) :: r, width
     end function plait_wfunc
     !
     subroutine plait_cutoff(a,lower,upper,bval)
       use plait_common
       real(kind=sp), intent(in) :: lower , upper , bval
       real(kind=sp), intent(inout) :: a(:,:,:)
     end subroutine plait_cutoff
     !
     subroutine plait_replace(a,b,bval,eval)
       use plait_common
       real(kind=4), intent(in)    :: b(:,:,:)
       real(kind=4), intent(inout) :: a(:,:,:)
       real(kind=sp), intent(in) :: bval,eval
     end subroutine plait_replace
     !
     subroutine plait_add(aa,ww,head,a1,a2)
       use plait_common
       use image_def
       type(gildas), intent(in) :: head
       real(kind=4), intent(in)  :: a1(:,:,:),a2(:,:,:)
       real(kind=4), intent(out) :: aa(:,:,:)
       integer(kind=4), intent(inout) :: ww(:,:,:)
     end subroutine plait_add
     !
     subroutine plait_mean(w,a,bval,eval)
       use plait_common
       real(kind=sp), intent(in) :: bval , eval
       real(kind=sp), intent(inout) :: a(:,:,:)
       integer(kind=4), intent(in) :: w(:,:,:)
     end subroutine plait_mean
     !
     subroutine plait_weight(w,theta,scale,head)
       use plait_common
       use image_def
       use phys_const
!       use plait_interface, only : plait_wfunc
       type(gildas), intent(in) :: head
       real(kind=sp), intent(in) :: theta , scale
       real(kind=4), intent(inout) :: w(:,:)
     end subroutine plait_weight
     !
     subroutine plait_norm(a,w,bval,eval)
       use plait_common
       complex(kind=4), intent(inout) :: a(:,:,:)
       real(kind=sp), intent(in) :: w(:,:)
       real(kind=4), intent(in) :: bval, eval
     end subroutine plait_norm
  end interface
end module plait_interface
!
subroutine plait_check(next,prev,error)
  use plait_common
  use image_def
  !---------------------------------------------------------------------
  ! CHECK INPUT CUBES HAVE IDENTICAL DIMENSIONS
  !---------------------------------------------------------------------
  type(gildas), intent(in)  :: prev,next
  logical,      intent(out) :: error
  !
  error = .false.
  if (next%gil%ndim.ne.prev%gil%ndim) then
     error = .true.
     return
  endif
  if (.not.all(next%gil%dim.eq.prev%gil%dim)) then
     error = .true.
  endif
end subroutine plait_check
!
function plait_wfunc(r,width)
  use plait_common
  use phys_const
  !---------------------------------------------------------------------
  ! WEIGHTING FUNCTION
  !---------------------------------------------------------------------
  real(kind=sp) :: plait_wfunc
  real(kind=sp), intent(in) :: r , width
  ! Local
  real(kind=sp) :: zsc
  !
  plait_wfunc = 1.0_sp
  if (width.eq.0.0) return
  zsc = r*width
  if (abs(zsc).ge.1.0_sp) return
  zsc = zsc*pi*0.5_sp
  plait_wfunc = max(wfunc_thres,(sin(zsc))**2)
end function plait_wfunc
!
subroutine plait_weight(w,theta,scale,head)
  use plait_common
  use image_def
  use phys_const
  use plait_interface, only : plait_wfunc
  !---------------------------------------------------------------------
  ! COMPUTES WEIGHTING IMAGE
  !---------------------------------------------------------------------
  real(kind=4),  intent(inout) :: w(:,:)
  real(kind=sp), intent(in)    :: theta,scale
  type(gildas),  intent(in)    :: head
  ! Local
  integer(kind=4) :: i,j,m,n
  real(kind=sp) :: phi,cphi,sphi
  real(kind=sp) :: x(head%gil%dim(1)),y(head%gil%dim(2))
  real(kind=sp) :: d
  real(kind=sp) :: dx,dy,xinc,yinc
  !
  n = head%gil%dim(1)
  m = head%gil%dim(2)
  xinc = head%gil%convert(3,1)
  yinc = head%gil%convert(3,2)
  ! orientation of weighting in FT space
  phi  = theta * pi / 180._sp
  cphi = cos(phi)
  sphi = sin(phi)
  ! discretization of axes in FT space
  ! X and Y arrays are filled in with FORALL
  dx   = 1.0_sp / (xinc*(n-1))
  dy   = 1.0_sp / (yinc*(m-1))
  forall(i=1:n)
     x(i) = (i-1-(i/(n/2+1))*(n-1))
  end forall
  forall(j=1:m)
     y(j) = (j-1-(j/(m/2+1))*(m-1))
  end forall
  x    = x * dx * cphi
  y    = y * dy * sphi
  ! DX and DY are redefined in the loops
  do i=1,n
     dx = x(i)
     do j=1,m
        dy = y(j)
        d  = dx + dy
        w(i,j) = plait_wfunc(d,scale)
     enddo
  enddo
end subroutine plait_weight
!
subroutine plait_cutoff(a,lower,upper,bval)
  use plait_common
  !---------------------------------------------------------------------
  ! BLANK PIXELS OUTSIDE [LOWER:UPPER] INTERVAL
  !---------------------------------------------------------------------
  real(kind=sp), intent(in)    :: lower,upper,bval
  real(kind=sp), intent(inout) :: a(:,:,:)
  !
  if ((lower.eq.0._sp).and.(upper.eq.0._sp)) return
  where (a.lt.lower.or.a.gt.upper) a = bval
  !
end subroutine plait_cutoff
!
subroutine plait_replace(a,b,bval,eval)
  use plait_common
  !---------------------------------------------------------------------
  ! WHERE A IS BLANKED REPLACE WITH CORRESPONDING B VALUE
  !---------------------------------------------------------------------
  real(kind=4),  intent(in)    :: b(:,:,:)
  real(kind=4),  intent(inout) :: a(:,:,:)
  real(kind=sp), intent(in)    :: bval,eval
  !
!!$  integer :: I
  ! Check efficiency of either methods
!!$  do i=1,size(a,3)
!!$     where (abs(a(:,:,i)-bval).ge.eval) a(:,:,i) = b(:,:,i)
!!$  enddo
  !
  where (abs(a-bval).le.eval) a = b
  !
end subroutine plait_replace
!
subroutine plait_norm(a,w,bval,eval)
  use plait_common
  !---------------------------------------------------------------------
  ! NORMALIZE THE OUTPUT OF PLAITED FFTs
  !---------------------------------------------------------------------
  complex(kind=4), intent(inout) :: a(:,:,:)
  real(kind=sp),   intent(in)    :: w(:,:)
  real(kind=4),    intent(in)    :: bval,eval
  ! Local
  integer(kind=4) :: i,dim3
  !
  dim3 = size(a,3)
  do i=1,dim3
     where (w.ne.0._sp.and.abs(a(:,:,i)-bval).gt.eval)&
          a(:,:,i) = a(:,:,i) / w
  enddo
end subroutine plait_norm
!
subroutine plait_add(aa,ww,head,a1,a2)
  use plait_common
  use image_def
  !---------------------------------------------------------------------
  ! ADD TWO DATA CUBES TAKING INTO ACCOUNT BLANKING VALUE
  !---------------------------------------------------------------------
  real(kind=4),    intent(inout) :: aa(:,:,:)
  integer(kind=4), intent(inout) :: ww(:,:,:)
  type(gildas),    intent(in)    :: head
  real(kind=4),    intent(in)    :: a1(:,:,:),a2(:,:,:)
  ! Local
  integer(kind=4) :: mask(head%gil%dim(1),head%gil%dim(2))
  real(kind=sp) :: bval , eval
  integer(kind=4) :: i

  bval = head%gil%bval
  eval = head%gil%eval
  do i=1,head%gil%dim(3)
     !
     mask = 0
     where (abs(a1(:,:,i)-bval).gt.eval) mask = 1
     ww(:,:,i) = ww(:,:,i) + mask
     aa(:,:,i) = aa(:,:,i) + a1(:,:,i) * mask
     !
     mask = 0
     where (abs(a2(:,:,i)-bval).gt.eval) mask = 1
     ww(:,:,i) = ww(:,:,i) + mask
     aa(:,:,i) = aa(:,:,i) + a2(:,:,i) * mask
     !
  enddo
  !
end subroutine plait_add
!
subroutine plait_mean(w,a,bval,eval)
  use gkernel_interfaces
  use plait_common
  !---------------------------------------------------------------------
  ! COMPUTES THE MEAN OF INPUT CUBES
  ! IF THE MEAN CONTAINS BLANKED PIXEL(S) RETURN
  !---------------------------------------------------------------------
  real(kind=sp),   intent(in)    :: bval,eval
  real(kind=sp),   intent(inout) :: a(:,:,:)
  integer(kind=4), intent(in)    :: w(:,:,:)
  !
  ! First, check if any zero in weights
  if (minval(w).eq.0) then
     call gagout('F-'//rname//',  BLANKED PIXEL(S) IN SUM OF IMAGES')
     return
  endif
  ! No zero in w : computes the MEAN
  where (abs(a-bval).gt.eval) a = a / w
  !
end subroutine plait_mean
!
program plait_program
  use image_def
  use gkernel_interfaces
  use plait_common
  use plait_interface
  !---------------------------------------------------------------------
  !
  !---------------------------------------------------------------------
  integer(kind=4), parameter :: nmap=5
  integer(kind=4) :: nfile,i,j,ier
  integer(kind=4) :: dim(3),maxdim
  character(len=256) :: message
  type(gildas) :: cin(nmap)
  type(gildas) :: cout , prev
  complex(kind=4), allocatable :: b(:,:,:),res(:,:,:)
  real(kind=4),    allocatable :: a(:,:,:),som(:,:,:)
  integer(kind=4), allocatable :: w3d(:,:,:)
  real(kind=sp),   allocatable :: w2d(:,:),norm(:,:)
  real(kind=4),    allocatable :: wfft(:)
  real(kind=sp) :: theta(nmap) , length(nmap)
  real(kind=sp) :: thres(2),blan(2)
  logical :: error
  !
  error = .false.
  do i=1,nmap
     call gildas_null(cin(i))
  enddo
  call gildas_null(cout)
  call gildas_null(prev)
  !
!!$  write(*,*) "READING INPUT PARAMETERS"
  call gildas_open
  call gildas_char('NAME_1$',cin(1)%file)
  call gildas_char('NAME_2$',cin(2)%file)
  call gildas_char('NAME_3$',cin(3)%file)
  call gildas_char('NAME_4$',cin(4)%file)
  call gildas_char('NAME_5$',cin(5)%file)
  call gildas_char('O_NAME$',cout%file)
  call gildas_inte('NFILE$',nfile,1)
  call gildas_real('THETA$',theta,nmap)
  call gildas_real('LENGTH$',length,nmap)
  call gildas_real('CUTOFF$',thres,2)
  call gildas_real('BLANK$',blan,2)
  call gildas_close
!!$  write(*,*) "PARAMETERS RED",nfile
  !
  ! Check that input files exist
  !
  do i=1,nfile
     ier = gag_inquire(cin(i)%file,lenc(cin(i)%file))
     if (ier.ne.0) then
        message = 'Input cube '//char(48+i)//' ' &
                  //cin(i)%file(1:len_trim(cin(i)%file))//' not found'
        call gagout('F-'//rname//',  '//message)
        call sysexi(fatale)
     endif
  enddo
  !
  ! Check that input cubes match
  !
  i = 1
  call gdf_read_header(cin(i),error)
!!$  write(*,*) "HEADER RED",i,cin(i)%file
  if (gildas_error(cin(i),rname,error)) call sysexi(fatale)
  prev = cin(i)
  do i=2,nfile
     call gdf_read_header(cin(i),error)
     if (gildas_error(cin(i),rname,error)) call sysexi(fatale)
     call plait_check(cin(i),prev,error)
     if (error) then
        call gagout('F-'//rname//',  Input cubes dimensions do not match')
        call sysexi(fatale)
     endif
     prev = cin(i)
  enddo
  dim(1:3) = cin(1)%gil%dim(1:3)
  maxdim = 2*maxval(dim)
  !
  ! Allocating memory
  !
  allocate(&
       w2d (dim(1),dim(2)),&
       norm(dim(1),dim(2)),&
       w3d (dim(1),dim(2),dim(3)),&
       a   (dim(1),dim(2),dim(3)), &
       b   (dim(1),dim(2),dim(3)), &
       res (dim(1),dim(2),dim(3)),&
       som (dim(1),dim(2),dim(3)),&
       wfft(maxdim),&
       stat=ier)
  if (failed_allocate(rname,'plait buffers',ier,error))  call sysexi(fatale)
  !
  ! Initialize SOM with first input cube
  !
  i = 1
  call gdf_read_data(cin(i),som,error)
  if (gildas_error(cin(i),rname,error)) call sysexi(fatale)
  !
  ! Compute the mean of input cubes
  !
  w3d = 0.
  som = 0.
  do i=1,nfile
     call gdf_read_data(cin(i),a,error)
     if (gildas_error(cin(i),rname,error)) call sysexi(fatale)
     call plait_cutoff(a,thres(1),thres(2),blan(1))
     call plait_add(som,w3d,cin(i),a,som)
  enddo
  call plait_mean(w3d,som,blan(1),blan(2))
  !
  ! Perform the plait algorithm
  !
  res  = cmplx(0.,0.,kind(0.))
  do i=1,nfile
     wfft = cmplx(0.,kind(0.))
     a    = 0.
     call gdf_read_data(cin(i),a,error)
     if (gildas_error(cin(i),rname,error)) call sysexi(fatale)
     call plait_cutoff(a,thres(1),thres(2),blan(1))
     call plait_replace(a,som,blan(1),blan(2))
     b(:,:,:) = cmplx(a,0.,kind(a))
     call plait_weight(w2d,theta(i),length(i),cin(i))
     do j=1,dim(3)
        call fourt(b(:,:,j),dim(1:2),2,1,1,wfft)
     enddo
     !-!$$     call fourt(b,dim(1:2),cin(1)%gil%ndim,1,0,wfft)
     forall(j=1:dim(3)) res(:,:,j) = res(:,:,j) + b(:,:,j)*w2d
     norm(:,:) = norm + w2d
  enddo
  call plait_norm(res,norm,blan(1),blan(2))
  wfft = cmplx(0.,kind(0.))
  do j=1,dim(3)
     call fourt(res(:,:,j),dim(1:2),2,-1,1,wfft)
  enddo
  a(:,:,:) = real(res)
  where (abs(a-blan(1)).gt.blan(2)) 
     a = a/real(dim(1)*dim(2))
  end where
  !
!-!$$  a = aimag(res)/real(dim(1)*dim(2))
!-!!$  call gildas_null(gsom)
!-!!$  gsom%file = "mean.lmv"
!-!!$  call gdf_copy_header(cin(1),gsom)
!-!!$  call gdf_write_image(gsom,som,error)
  !
  ! Write result in output image
  !
  call gdf_copy_header(cin(1),cout,error)
  call gdf_write_image(cout,a,error)
  if (gildas_error(cout,rname,error)) call sysexi(fatale)
  deallocate(w2d,norm,w3d,a,b,res,som,wfft,stat=ier)
  if (ier.ne.0) then
     call gagout('F-'//rname//',  Deallocation error')
     call sysexi(fatale)
  endif
end program plait_program
