program dg_smooth
  use gildas_def
  use image_def
  use gbl_format
  use gkernel_interfaces
  !---------------------------------------------------------------------
  ! GILDAS
  !	Smooth an image using the CONJUGATE GRADIENT ALGORITHM :
  !	The smoothed image is the equilibrium state of a thin flexible plate
  !	constrained to pass near each height datum by a spring attached
  !	between it and the plate.
  !	the smoothing parameter p is the ratio :
  !	     p = ( plate stiffness) / ( springs stiffness)
  !	The convergence is obtained with about 10 iterations ...
  !---------------------------------------------------------------------
  character(len=*), parameter :: pname = 'DG_SMOOTH'
  ! Local
  character(len=filename_length) :: namex,namey,namez
  logical :: error,start
  integer(kind=4) :: i,j,kz
  real(kind=4) :: p,valbi
  type(gildas) :: x,y,z
  integer(kind=4) :: niter, ier
  !
  call gildas_open
  call gildas_logi('RESTART$',start,1)
  call gildas_real('P$',p,1)
  call gildas_inte('NITER$',niter,1)
  call gildas_char('Z_NAME$',namez)
  call gildas_char('Y_NAME$',namey)
  call gildas_char('X_NAME$',namex)
  call gildas_real('GUESS$',valbi,1)
  call gildas_close
  start = .not.start
  !
  !
  call gildas_null(y)
  call gdf_read_gildas(y,namez,'.gdf',error,data=.false.)
  if (error) then
    call gagout ('F-'//pname//',  Cannot read input file')
    goto 100
  endif
  !
  call gildas_null(x)
  call gdf_copy_header(y,x,error)
  !
  ! Create or Read output image
  if (start) then
    call gdf_copy_header(y,x,error)
    call sic_parsef(namey,x%file,' ','.gdf')
    x%gil%extr_words = 0
    call gdf_create_image (x,error)
    if (error) then
      call gagout ('F-'//pname//',  Cannot create smoothed image')
      goto 100
    endif
  else
    x%loca%read = .false.  ! Open for write
    call gdf_read_gildas(y,namex,'.gdf',error,data=.false.)
    if (error) then
      call gagout ('F-'//pname//',  Cannot read smoothed image')
      goto 100
    endif
  endif
  !
  ! Work file, in Z
  call gildas_null(z)
  if (start) then
    call gdf_copy_header(y,z,error)
    call sic_parsef(namex,z%file,' ','.gdf')
    z%gil%ndim   = 4
    z%gil%dim(3) = 3*y%gil%dim(3)*y%gil%dim(4)
    z%gil%dim(4) = 1
    z%loca%size   = 3*y%loca%size
    call gdf_create_image (z,error)
    if (error) then
      call gagout ('F-'//pname//',  Cannot create work file')
      goto 100
    endif
  else
    z%loca%read = .false.  ! Open for write
    call gdf_read_gildas(z,namex,'.gdf',error,data=.false.)
    if (error) then
      call gagout ('F-'//pname//',  Cannot read work file')
      goto 100
    endif
  endif
  !
  if (z%loca%size.ne.3*y%loca%size) then
    call gagout('E-'//pname//',  Output and '//   &
     &      'Work Images do not match')
    goto 100
  endif
  if (y%loca%size.ne.x%loca%size) then
    call gagout('E-'//pname//',  Input and '//   &
     &      'Output Images do not match')
    goto 100
  endif
  if (y%gil%eval.ge.0) y%gil%eval = max(y%gil%eval,abs(y%gil%bval*1e-7))
  !
  !	v_addr = y%loca%addr		! oRIGINAL IMAGE
  !	w_addr = z%loca%addr		! wORK IMAGE
  !	a_addr = x%loca%addr		! sMOOTH IMAGE
  !
  allocate (x%r2d(x%gil%dim(1),x%gil%dim(2)), & 
    & y%r2d(y%gil%dim(1),y%gil%dim(2)),  & 
    & z%r3d(z%gil%dim(1),z%gil%dim(2),3), stat=ier)
  if (ier.ne.0) then
    call gagout ('F-'//pname//',  Memory allocation failure')
    goto 100
  endif
  !
  kz = 1
  do j=1,y%gil%dim(4)
    do i=1,y%gil%dim(3)
      x%blc(3) = i
      x%trc(3) = i
      x%blc(4) = j
      x%trc(4) = j
      y%blc = x%blc
      y%trc = x%trc
      call gdf_read_data(y,y%r2d,error)
      z%blc(3) = kz
      z%trc(3) = kz+2
      z%blc(4) = 1
      z%trc(4) = 1
      kz = kz+3
      if (start) then
        call gdf_read_data(x,x%r2d,error)
        call gdf_read_data(z,z%r3d,error)
        call dgsm001(y%r2d,x%gil%dim(1),x%gil%dim(2),   &
     &          x%r2d,y%gil%bval,y%gil%eval,            &
     &          z%r3d(:,:,1),   &
     &          z%r3d(:,:,2),   &
     &          z%r3d(:,:,3),   &
     &          valbi)
      endif
      !
      call dgsm002(y%r2d,x%gil%dim(1),x%gil%dim(2),   &
     &          x%r2d,          &
     &          z%r3d(:,:,1),   &
     &          z%r3d(:,:,2),   &
     &          z%r3d(:,:,3),   &
     &          p,niter,y%gil%bval,y%gil%eval)
      call gdf_write_data (z,z%r3d,error)
      call gdf_write_data (x,x%r2d,error)
    enddo
  enddo
  !
  call gagout('S-'//pname//',  Successful completion')
  call sysexi (1)
  !
100 call sysexi (fatale)
end program dg_smooth
!
subroutine dgsm001(v,mi,mj,a,bval,eval,ap,ap2,ap3,valbi)
  use gildas_def
  !---------------------------------------------------------------------
  !	 Init
  !---------------------------------------------------------------------
  real(kind=4),               intent(in)  :: v(*)
  integer(kind=index_length), intent(in)  :: mi
  integer(kind=index_length), intent(in)  :: mj
  real(kind=4),               intent(out) :: a(*)
  real(kind=4),               intent(in)  :: bval
  real(kind=4),               intent(in)  :: eval
  real(kind=4),               intent(out) :: ap(mi*mj)
  real(kind=4),               intent(out) :: ap2(mi*mj)
  real(kind=4),               intent(out) :: ap3(mi*mj)
  real(kind=4),               intent(in)  :: valbi
  ! Local
  integer(kind=size_length) :: i,n
  !
  n = mi*mj
  ap = 0.0
  ap2 = 0.0
  ap3 = 0.0
  if (eval.lt.0.0) then
    do i=1,n
      a(i) = v(i)
    enddo
  else
    do i=1,n
      if (abs(v(i)-bval).gt.eval) then
        a(i) = v(i)
      else
        a(i) = valbi
      endif
    enddo
  endif
end subroutine dgsm001
!
subroutine dgsm002(v,mi,mj,a,desc,grad,w,p,itermax,bval,eval)
  use gildas_def
  !---------------------------------------------------------------------
  ! D. Girard  smoothing technique
  !---------------------------------------------------------------------
  real(kind=4),               intent(in)    :: v(*)
  integer(kind=index_length), intent(in)    :: mi
  integer(kind=index_length), intent(in)    :: mj
  real(kind=4),               intent(inout) :: a(*)
  real(kind=4),               intent(out)   :: desc(*)
  real(kind=4),               intent(out)   :: grad(*)
  real(kind=4),               intent(out)   :: w(*)
  real(kind=4),               intent(in)    :: p
  integer(kind=4),            intent(in)    :: itermax
  real(kind=4),               intent(in)    :: bval
  real(kind=4),               intent(in)    :: eval
  ! Global
  real(kind=4), external :: dot
  ! Local
  ! Resolution iterative par gradient conjugue
  !
  real(kind=4) :: vij,norm2grad,norm2grad_1,beta,lambda
  integer(kind=4) :: iter
  integer(kind=size_length) :: i,n
  !
  if (itermax.le.0) return
  n = mi*mj
  !
  ! Gradient
  call prodtd(v,mi,mj,p,a,grad,bval,eval)
  do i=1,n
    vij = v(i)
    if (abs(vij-bval).gt.eval) then
      grad(i)=grad(i)-vij
    endif
  enddo
  norm2grad = dot (grad,grad,n)
  !
  call prodtd(v,mi,mj,p,grad,w,bval,eval)
  lambda = norm2grad / dot(grad,w,n)
  !
  do i=1,n
    desc(i) = - grad(i)
  enddo
  do i=1,n
    a(i) = a(i) - lambda * grad(i)
  enddo
  norm2grad_1 = norm2grad
  do i=1,n
    grad(i)= grad(i) - lambda*w(i)
  enddo
  !
  ! Iterations
  do iter = 2, itermax
    norm2grad = dot(grad,grad,n)
    beta = norm2grad/norm2grad_1
    do i=1,n
      desc(i) = -grad(i) + beta * desc(i)
    enddo
    call prodtd(v,mi,mj,p,desc,w,bval,eval)
    lambda = norm2grad / dot(desc,w,n)
    do i=1,n
      a(i) = a(i) + lambda * desc(i)
    enddo
    norm2grad_1 = norm2grad
    do i=1,n
      grad(i) = grad(i) + lambda * w(i)
    enddo
  enddo
end subroutine dgsm002
!
subroutine prodtd(v,mi,mj,p,z,s,bval,eval)
  use gildas_def
  !---------------------------------------------------------------------
  !  Init
  !---------------------------------------------------------------------
  integer(kind=index_length), intent(in)  :: mi
  integer(kind=index_length), intent(in)  :: mj
  real(kind=4),               intent(in)  :: v(mi,mj)
  real(kind=4),               intent(in)  :: p
  real(kind=4),               intent(in)  :: z(mi,mj)
  real(kind=4),               intent(out) :: s(mi,mj)
  real(kind=4),               intent(in)  :: bval
  real(kind=4),               intent(in)  :: eval
  ! Local
  integer(kind=index_length) :: i,j
  real(kind=4) :: w
  !
  do j=1,mj
    do i=1,mi
      if (abs(v(i,j)-bval).gt.eval) then
        s(i,j) = z(i,j)
      else
        s(i,j) = 0.0
      endif
    enddo
  enddo
  do j=2,mj-1
    do i=2,mi-1
      w = p*(2.0*z(i,j)-z(i-1,j)-z(i+1,j))
      s(i,j)= s(i,j) +2.0*w
      s(i-1,j)=s(i-1,j) - w
      s(i+1,j) = s(i+1,j) -w
      w = p*(2.0*z(i,j)-z(i,j+1)-z(i,j-1))
      s(i,j)= s(i,j) +2.0*w
      s(i,j-1)=s(i,j-1) - w
      s(i,j+1) = s(i,j+1) -w
      w = 0.5*p*(z(i+1,j+1)+z(i-1,j-1)-z(i+1,j-1)-z(i-1,j+1))/4.0
      s(i+1,j+1)= s(i+1,j+1) +w
      s(i-1,j-1)= s(i-1,j-1) +w
      s(i+1,j-1)= s(i+1,j-1) -w
      s(i-1,j+1)= s(i-1,j+1) -w
    enddo
  enddo
end subroutine prodtd
!
function dot (z,s,n)
  use gildas_def
  real(kind=4) :: dot
  real(kind=4),              intent(in) :: z(*)
  real(kind=4),              intent(in) :: s(*)
  integer(kind=size_length), intent(in) :: n
  ! Local
  integer(kind=size_length) :: i
  dot=0.0
  do i=1,n
    dot = dot + s(i)*z(i)
  enddo
end function dot
