program make_back
  use gildas_def
  use image_def
  use gkernel_interfaces
  use gbl_format
  !---------------------------------------------------------------------
  ! GILDAS
  !	Computes the background of an image. Thresholding is used
  !	to define the background, then the image is smoothed
  !	and blanked pixels are filled 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 ...
  !---------------------------------------------------------------------
  ! Global
  include 'gbl_memory.inc'
  ! Local
  character(len=filename_length) :: namex,namey
  logical :: error
  integer(size_length) :: i
  integer :: n,niter
  real :: p,thre(3)
  real(4), allocatable :: ipw(:,:,:), ipt(:,:)
  type(gildas) :: x,y
  integer(kind=size_length) :: sdim(gdf_maxdims)
  integer :: ndim, ier
  !
  call gildas_open
  call gildas_real('P$',p,1)
  call gildas_inte('NITER$',niter,1)
  call gildas_char('IN$',namey)
  call gildas_char('OUT$',namex)
  call gildas_real('THRESHOLD$',thre,3)
  call gildas_close
  !
  ! Input file
  call gildas_null(y)
  call gdf_read_gildas(y,namey,'.gdf',error, data=.false.)
  if (error) then
    write(6,*) 'F-MAKE_BACK,  Cannot read input file'
    goto 100
  endif
  !
  ! Create output image
  call gildas_null(x)
  call gdf_copy_header(y,x,error)
  n = lenc(namex)
  call sic_parsef(namex(1:n),x%file,' ','.gdf')
  call gdf_create_image(x,error)
  if (error) then
    write(6,*) 'F-MAKE_BACK,  Cannot output image'
    goto 100
  endif
  !
  ! Re-shape to 3 - dim
  ndim = y%gil%ndim
  sdim = y%gil%dim
  y%gil%ndim = 3
  y%gil%dim(3) = y%loca%size/(y%gil%dim(1)*y%gil%dim(2))
  x%gil%ndim = 3
  x%gil%dim = y%gil%dim
  !
  ! Get work space
  allocate (ipt(x%gil%dim(1), x%gil%dim(2)), ipw(x%gil%dim(1), x%gil%dim(2), 4), stat=ier)
  if (ier.ne.0) goto 100
  allocate (x%r2d(x%gil%dim(1), x%gil%dim(2)), y%r2d(x%gil%dim(1), x%gil%dim(2)), stat=ier)
  if (ier.ne.0) goto 100
  !
  ! Now loop over planes
  do i=1,x%gil%dim(3)
    x%blc(3) = i
    x%trc(3) = i
    y%blc = x%blc
    y%trc = x%trc
    call gdf_read_data(x,x%r2d,error)
    call gdf_read_data(y,y%r2d,error)
    !
    ! Fill first work space
    call fill(y%gil%dim(1),y%gil%dim(2),   & !NX,NY,
    &        y%r2d,   & !IN
    &        ipt,     & !WORK
    &        y%gil%bval,y%gil%eval,thre,   &  !bval,eval,thre
    &        ipw(:,:,1),x%r2d) !out,guess
    call dgsm001(x%gil%dim(1),x%gil%dim(2),   &  !mi,mj
    &        ipw(:,:,2) ,   &   !AP
    &        ipw(:,:,3) ,   & !AP2
    &        ipw(:,:,4) ,   & !AP3
    &        thre(3))         !first_guess
    call dgsm002(ipw(:,:,1),x%gil%dim(1),x%gil%dim(2),   &  !V,MI,MJ
    &        x%r2d,x%gil%bval,x%gil%eval,   &   !A,BVAL,EVAL
    &        ipw(:,:,2), &   !DESC
    &        ipw(:,:,3), &   !grad
    &        ipw(:,:,4),  & !W
    &        p,niter)         !P,ITERMAX
    call gdf_write_data(x,x%r2d,error) 
  enddo
  call gagout('S-MAKE_BACK,  Successful completion')
  call sysexi (1)
  !
100 call sysexi (fatale)
end program make_back
!
subroutine fill (nx,ny,in,work,bval,eval,thre,   &
     &    out,guess)
  use gildas_def
  integer(kind=index_length) :: nx                     !
  integer(kind=index_length) :: ny                     !
  real :: in(nx,ny)                 !
  real :: work(nx,ny)               !
  real :: bval                      !
  real :: eval                      !
  real :: thre(3)                   !
  real :: out(nx,ny)                !
  real :: guess(nx,ny)              !
  ! Local
  integer :: i,j,ip,im,jp,jm
  real :: z,w
  !
  do j = 1,ny
    do i = 1,nx
      if (abs(in(i,j)-bval).gt.eval) then
        work(i,j) = in(i,j)
      else
        ip = max(i-1,1)
        im = min(i+1,nx)
        jp = max(j-1,1)
        jm = min(j+1,ny)
        w = 0.0
        z = 0.0
        if (abs(in(ip,j)-bval).gt.eval) then
          z = z+in(ip,j)
          w = w+1.0
        endif
        if (abs(in(im,j)-bval).gt.eval) then
          z = z+in(im,j)
          w = w+1.0
        endif
        if (abs(in(i,jp)-bval).gt.eval) then
          z = z+in(i,jp)
          w = w+1.0
        endif
        if (abs(in(i,jm)-bval).gt.eval) then
          z = z+in(i,jm)
          w = w+1.0
        endif
        if (w.gt.0) then
          work(i,j) = z/w
        else
          work(i,j) = bval
        endif
      endif
    enddo
  enddo
  !
  do j = 1,ny
    do i = 1,nx
      if (abs(in(i,j)-bval).le.eval) then
        if (abs(work(i,j)-bval).le.eval) then
          guess(i,j) = thre(3)
        else
          guess(i,j) = work(i,j)
        endif
        out(i,j) = bval
      elseif (in(i,j).gt.thre(2)) then
        work(i,j) = thre(2)
        guess(i,j) = thre(2)
        out(i,j) = bval
      elseif (in(i,j).lt.thre(1)) then
        work(i,j) = thre(1)
        guess(i,j) = thre(1)
        out(i,j) = bval
      else
        work(i,j) = in(i,j)
        guess(i,j) = in(i,j)
        out(i,j) = in(i,j)
      endif
    enddo
  enddo
end subroutine fill
!
subroutine dgsm001(mi,mj,ap,ap2,ap3,first_guess)
  use gildas_def
  !---------------------------------------------------------------------
  !	 Init
  !---------------------------------------------------------------------
  integer(kind=index_length) :: mi                  !
  integer(kind=index_length) :: mj                  !
  real :: ap(mi,mj)                 !
  real :: ap2(mi,mj)                !
  real :: ap3(mi,mj)                !
  real :: first_guess               !
  ! Local
  ap = 0.0
  ap2 = 0.0
  ap3 = 0.0
end subroutine dgsm001
!
subroutine dgsm002(v,mi,mj,a,bval,eval,desc,grad,w,p,itermax)
  use gildas_def
  !---------------------------------------------------------------------
  ! D. Girard  smoothing technique
  !---------------------------------------------------------------------
  integer(kind=index_length) :: mi                   !
  integer(kind=index_length) :: mj                   !
  real(4) :: v(mi,mj)                !
  real(4) :: a(mi,mj)                !
  real(4) :: bval                    !
  real(4) :: eval                    !
  real(4) :: desc(mi,mj)             !
  real(4) :: grad(mi,mj)             !
  real(4) :: w(mi,mj)                !
  real(4) :: p                       !
  integer :: itermax                !
  ! Global
  real(4) :: dot,norme2
  ! Local
  real(4) :: beta,lambda
  real(4) :: norm2grad,norm2grad_1
  !
  ! Resolution iterative par gradient conjugue
  ! Pas de splines pour l'instant
  ! Compromis fidelite aux donnees (moindre carre)
  ! et terme de flexion de symmetrie correcte.
  ! BUT: Etude problemes de convergence.
  !						D. Girard. 20-dec-1986
  !
  ! Ici P n'a pas la signification de C. de Borr
  !-------------------------------------------
  !
  integer i0,j0,i,j,iter
  !
  i0 = max(1,mi/2)
  j0 = max(1,mj/2)
  if (itermax.le.0) return
  !
  call iprotd (v,mi,mj,p,a,grad,bval,eval) ! Calcul du gradient(=residu
  do j=1,mj
    do i=1,mi
      if (abs(v(i,j)-bval).gt.eval) then
        grad(i,j)=grad(i,j)-v(i,j)
      endif
    enddo
  enddo
  norm2grad = norme2(mi,mj,grad)
  !
  call iprotd (v,mi,mj,p,grad,w,bval,eval)
  lambda = norm2grad/dot(mi,mj,grad,w)
  !
  do j=1,mj
    do i=1,mi
      desc(i,j) = - grad(i,j)
    enddo
  enddo
  !
  do j=1,mj
    do i=1,mi
      a(i,j) = a(i,j) - lambda*grad(i,j)
    enddo
  enddo
  norm2grad_1 = norm2grad
  do j=1,mj
    do i=1,mi
      grad(i,j) = grad(i,j) - lambda*w(i,j)
    enddo
  enddo
  !
  ! ITERATIONS
  do iter = 2,itermax
    norm2grad = norme2(mi,mj,grad)
    beta = norm2grad/norm2grad_1
    do j=1,mj
      do i=1,mi
        desc(i,j) = -grad(i,j) + beta*desc(i,j)
      enddo
    enddo
    call iprotd(v,mi,mj,p,desc,w,bval,eval)
    !
    lambda = norm2grad / dot(mi,mj,desc,w)
    !
    do j=1,mj
      do i=1,mi
        a(i,j) = a(i,j) + lambda*desc(i,j)
      enddo
    enddo
    !
    norm2grad_1 = norm2grad
    !
    do j=1,mj
      do i=1,mi
        grad(i,j) = grad(i,j) + lambda*w(i,j)
      enddo
    enddo
  enddo
end subroutine dgsm002
!
subroutine iprotd(v,mi,mj,p,z,s,bval,eval)
  use gildas_def
  integer(kind=index_length) :: mi                   !
  integer(kind=index_length) :: mj                   !
  real(4) :: v(mi,mj)                !
  real(4) :: p                       !
  real(4) :: z(mi,mj)                !
  real(4) :: s(mi,mj)                !
  real(4) :: bval                    !
  real(4) :: eval                    !
  ! Local
  integer :: i,j
  real(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
      endif
    enddo
  enddo
  do j=2,mj-1
    do i=2,mi-1
      w = p*(2*z(i,j)-z(i-1,j)-z(i+1,j))
      s(i,j) = s(i,j) +2*w
      s(i-1,j) = s(i-1,j) -w
      s(i+1,j) = s(i+1,j) -w
      w = p*(2*z(i,j)-z(i,j+1)-z(i,j-1))
      s(i,j) = s(i,j) +2*w
      s(i,j-1) = s(i,j-1) -w
      s(i,j+1) = s(i,j+1) -w
      w = 2*p*(z(i+1,j+1)+z(i-1,j-1)-z(i+1,j-1)-z(i-1,j+1))/4
      s(i+1,j+1) = s(i+1,j+1) +w/4
      s(i-1,j-1) = s(i-1,j-1) +w/4
      s(i+1,j-1) = s(i+1,j-1) -w/4
      s(i-1,j+1) = s(i-1,j+1) -w/4
    enddo
  enddo
end subroutine iprotd
!
function dot (mi,mj,z,s)
  use gildas_def
  real(4) :: dot                     !
  integer(kind=index_length) :: mi                   !
  integer(kind=index_length) :: mj                   !
  real(4) :: z(mi,mj)                !
  real(4) :: s(mi,mj)                !
  ! Local
  integer :: i,j
  !
  dot = 0.
  do j=1,mj
    do i=1,mi
      dot = dot + s(i,j)*z(i,j)
    enddo
  enddo
end function dot
!
function norme2(mi,mj,z)
  use gildas_def
  real(4) :: norme2                  !
  integer(kind=index_length) :: mi                     !
  integer(kind=index_length) :: mj                     !
  real(4) :: z(mi,mj)                !
  ! Local
  integer :: i,j
  !
  norme2 = 0.
  do j=1,mj
    do i=1,mi
      norme2 = norme2 + z(i,j)*z(i,j)
    enddo
  enddo
end function norme2
