program make_cube
  use gildas_def
  use image_def
  use gkernel_interfaces
  use gbl_format
  !---------------------------------------------------------------------
  ! GILDAS
  !	Makes a filled cube from a possibly non-filled one,
  !	preserving sampling by duplicating pixels and then
  !	smoothing each plane 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 ...
  !---------------------------------------------------------------------
  ! Local
  character(len=filename_length) :: namex,namey
  logical :: error
  integer :: i,j,n, niter, nexp, ier
  real :: p,valbi
  integer :: nx,ny,mx,my
  type(gildas) :: x,y
  real, allocatable, dimension(:,:) :: desc, grad, v, w, work
  !
  call gildas_open
  call gildas_real('P$',p,1)
  call gildas_inte('NITER$',niter,1)
  call gildas_inte('EXPANSION$',nexp,1)
  call gildas_char('IN$',namey)
  call gildas_char('OUT$',namex)
  call gildas_real('GUESS$',valbi,1)
  call gildas_close
  !
  if (nexp.lt.2. .or. nexp.gt.5) then
    write(6,*) 'E-MAKE_CUBE,  Invalid expansion ',nexp
    goto 100
  endif
  !
  ! Input file
  n = len_trim(namey)
  if (n.eq.0) goto 100
  call gildas_null(y)
  call gdf_read_gildas(y,namey(1:n),'.gdf', error, data=.false.)
  if (error) then
    write(6,*) 'F-MAKE_CUBE,  Cannot read input file'
    goto 100
  endif
  !
  call gildas_null(x)
  call gdf_copy_header(y,x,error)
  !
  x%gil%dim(1) = nexp*y%gil%dim(1)
  x%gil%dim(2) = nexp*y%gil%dim(2)
  !
  ! Transform coordinates (1,1) ---> (1,1)
  x%gil%inc(1:2) = y%gil%inc(1:2)/nexp
  x%gil%ref(1:2) = 1.0d0
  x%gil%val(1:2) = (1.0d0-y%gil%ref(1:2))*y%gil%inc(1:2)+y%gil%val(1:2)
  !
  ! Create output image
  n = len_trim(namex)
  call sic_parsef(namex(1:n),x%file,' ','.gdf')
  call gdf_create_image(x,error)
  if (error) then
    write(6,*) 'F-MAKE_CUBE,  Cannot output image'
    goto 100
  endif
  !
  mx = y%gil%dim(1)
  my = y%gil%dim(2)
  nx = x%gil%dim(1)
  ny = x%gil%dim(2)
  allocate (work(mx,my), v(nx,ny), desc(nx,ny), grad(nx,ny), w(nx,ny), &
      & stat=ier)
  if (ier.ne.0) then
    call gagout('E-MAKE_CUBE,  Memory allocation error')
    goto 100
  endif
  !
  ! Now loop over planes
  do j = 1,x%gil%dim(4)
    x%trc(4) = j
    x%blc(4) = j
    do i = 1,x%gil%dim(3)
      x%trc(3) = i
      x%blc(3) = i
      !
      y%blc = x%blc
      y%trc = x%trc
      call gdf_read_data(y,y%r2d,error)
      if (error) goto 100
      !
      !
      ! Fill first work space
      call fill(mx,my,y%r2d,work,  &
     &        y%gil%bval,y%gil%eval,nexp,valbi,   &
     &        nx,ny,           &
     &        v,               & ! V
     &        x%r2d )            ! A
      !
      desc = 0
      grad = 0
      w = 0
      !
      call dgsm002(v,   &    ! V
     &        nx,ny,    &
     &        x%r2d,    &    ! A
     &        desc, grad, w,  &
     &        p,niter,y%gil%bval,y%gil%eval)
      call gdf_write_data(x, x%r2d, error)
    enddo
  enddo
  !
  call gagout('S-MAKE_CUBE,  Successful completion')
  call sysexi(1)
100 call sysexi (fatale)
end program make_cube
!
subroutine fill (nx,ny,in,work,bval,eval,nexp,valbi,   &
     &    mx,my,v,a)
  integer :: nx                     !
  integer :: ny                     !
  real :: in(nx,ny)                 !
  real :: work(nx,ny)               !
  real :: bval                      !
  real :: eval                      !
  integer :: nexp                   !
  real :: valbi                     !
  integer :: mx                     !
  integer :: my                     !
  real :: v(mx,my)                  !
  real :: a(mx,my)                  !
  ! Local
  integer :: i,j,k,ip,im,jp,jm,ii,jj,jj0
  real :: w,z,good
  !
  ! First, fill in work array, trying to fill small gaps by crude interpolation
  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
  !
  ! Now fill guess and Vput arrays from work array
  good = valbi
  jj = 0
  do j=1,ny
    jj = jj+1
    ii = 0
    do i=1,nx
      ii = ii+1
      v(ii,jj) = in(i,j)
      a(ii,jj) = work(i,j)
      if (abs(a(ii,jj)-bval).gt.eval) then
        good = a(ii,jj)
      endif
      do k = 2,nexp
        ii = ii+1
        v(ii,jj) = bval
        a(ii,jj) = good
      enddo
    enddo
    jj0 = jj
    do k =2,nexp
      jj = jj+1
      do ii=1,mx
        v(ii,jj) = bval
        a(ii,jj) = a(ii,jj0)
      enddo
    enddo
  enddo
end subroutine fill
!
subroutine dgsm002(v,mi,mj,a,desc,grad,w,p,itermax,bval,eval)
  !---------------------------------------------------------------------
  ! D. Girard  smoothing technique
  !---------------------------------------------------------------------
  real*4 :: v(*)                    !
  integer :: mi                     !
  integer :: mj                     !
  real*4 :: a(*)                    !
  real*4 :: desc(*)                 !
  real*4 :: grad(*)                 !
  real*4 :: w(*)                    !
  real :: p                         !
  integer :: itermax                !
  real :: bval                      !
  real :: eval                      !
  ! Global
  real*4 :: dot
  ! Local
  ! Resolution iterative par gradient conjugue
  !
  real*4 :: vij,norm2grad,norm2grad_1,beta,lambda
  integer :: i,n,iter
  !
  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)
  integer :: mi                     !
  integer :: 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.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)
  real*4 :: dot                     !
  real*4 :: z(*)                    !
  real*4 :: s(*)                    !
  integer :: n                      !
  ! Local
  integer :: i
  real(8) :: ddot
  ddot=0.0
  do i=1,n
    ddot = ddot + s(i)*z(i)
  enddo
  dot = ddot
end function dot
