program Ultimatch_weight
  
  use sep
  use helix
  use nhelix
  use nhelicon
	use nchconest
  use nchelicon
  use lapfac
  use cartesian
  use findmatch_mod
  use verbosity_mod
  use irls_priv

!  use build_filter_mod

  implicit none

  integer, external                    :: getch
  real,          pointer, dimension(:) :: data1,data2,data3,data4,out,dummy_data1
  type (filter)                        :: match,laplac
  type (filter), pointer, dimension(:) :: nmatch
  type (nfilter)                       :: nsmatch

  integer :: mode               ! 1=single filter, 2=patches, 3=NS filtering
  integer :: nsmode             ! 1=regularized, 2=preconditioned 3=no-reg-no-prec
  integer :: wmode              ! 1=noweight 2=L1 3=user-provided weight
  integer :: invmode            ! 1=normal 2=iteratively reweighted
  logical :: cg                 ! whether to use cg to find matched-filters
  integer :: ndim               ! dimension of dataset
  integer :: niter
  integer :: nfreq              ! set >0 for l1 via nonlinear CG
  integer :: nmem
  integer :: ntimes
  real    :: eps,rbar
  logical :: twodim,interleave 
  character (len=20) :: targetname
  integer, dimension(:), pointer :: nfilt  ! no. of coefficients in each filter
  integer, dimension(:), pointer :: n      ! data-size, dimension(ndim)
  real,    dimension(:), pointer :: d, o
  integer, dimension(:), pointer :: npatch ! no. of patches, dimension(ndim)
  integer, dimension(:), pointer :: nwind  ! overlapping patch-size
  integer, dimension(:), pointer :: idx    ! index counter (ndim)

  integer, dimension(:), pointer :: nh    ! length of each filter in the table
  integer, dimension(:), pointer :: pch   ! ns filter indexing
  integer, dimension(:), pointer :: psize ! non-overlapping patch-size

  integer :: i,stat,ilag,ipatch,idim,ipanels,npanels ! junk variables
  
  integer                                :: j,j_dat(2),jj,nouter,nhmax,ii
  integer                                :: ncoef_data
  real, pointer, dimension(:)            :: temp_array
  logical                                :: rad,ste
	real								:: min_mask,max_mask
  
  call sep_init(SOURCE)
  ndim=sep_dimension()

  allocate(n(ndim), o(ndim), d(ndim))
  call from_history('n',n)
  call from_history('o',o)
  call from_history('d',d)
  interleave=.false.
  if (getch('target','s',targetname) == 0) interleave=.true.

  call from_param('2D',twodim,.false.)
  write(0,*) '2D',twodim
  npanels=1
  if (twodim) then
     allocate(nfilt(2))
     nfilt=n(1:2)
     npanels=1
     if (ndim>2) npanels=n(3)
     if (interleave) then 
        npanels=1
        if (ndim>3) npanels=n(4)
        call to_history('n3',npanels)
        call to_history('n4',1)
     end if
     deallocate(n)
     ndim=2
     allocate(n(2))
     n=nfilt
     deallocate(nfilt)
  end if

  allocate(nfilt(ndim),idx(ndim))
  nfilt=1; nfilt(1)=21

  call from_param('mode',mode,1)
  call from_param('nsmode',nsmode,1)
  call from_param('wmode',wmode,1)
  call from_param('invmode',invmode,1)
  call from_param('ntimes',ntimes,1)
  call from_param('cg',cg,.false.)
  call from_param('verb',verb,10)
  call from_param('nfilt',nfilt,nfilt)
  call from_param('nfreq',nfreq,-1)
  call from_param('nmem',nmem,5)
  call from_param('rbar',rbar,1.)
  call from_param('nouter',nouter,1)
  stat=l1_priv_init(rbar)

  call from_param('niter',niter,product(nfilt)-1)
  call from_param('eps',eps,1.) ! for the adaptive subtraction

  ! Patching parameters
  allocate(npatch(ndim),nwind(ndim))
  npatch=1
  call from_param('npatch',npatch,npatch)
  do i=1,ndim
     if (npatch(i)==1) then 
        nwind(i)=n(i)
     else 
        nwind(i)=2*n(i)/npatch(i)
     end if
  end do

  call from_param('nwind',nwind,nwind)

  allocate(data1(product(n)),data2(product(n)),data3(product(n)),out(product(n)))
	if (nouter>1) allocate(dummy_data1(product(n)))

  ! Create a single filter
  call allocatehelix(match,product(nfilt))
  do ilag=1,product(nfilt)
     call line2cart(nfilt,ilag,idx)
     match%lag(ilag)=idx(1)-nfilt(1)/2-1
     do idim=2,ndim
        match%lag(ilag)=match%lag(ilag)+(idx(idim)-nfilt(idim)/2-1)*product(n(1:idim-1))
     end do
  end do

  select case(mode)

  case(1)
     if (verb>0) write(0,*) '############################'
     if (verb>0) write(0,*) '# Single matched-filter...  '
     if (verb>0) write(0,*) '#---------------------------'
     if (verb>1) write(0,*) '#    nfilt  = ',nfilt
     if (verb>1) write(0,*) '#    nfreq = ',nfreq
     if (verb>1) write(0,*) '#    nmem  = ',nmem
     if (verb>0) write(0,*) '############################'
     

  case(2)
     if (verb>0) write(0,*) '#####################################'
     if (verb>0) write(0,*) '# Non-stationary matched-filtering...'
     if (verb>0) write(0,*) '#------------------------------------'
     if (verb>1) write(0,*) '#    ndim   = ',ndim
     if (verb>1) write(0,*) '#    n      = ',n
     if (verb>1) write(0,*) '#    npatch = ',npatch
     if (verb>1) write(0,*) '#    nfilt  = ',nfilt
     if (verb>1) write(0,*) '#    nfreq  = ',nfreq
     if (verb>1) write(0,*) '#    nmem   = ',nmem

     !! Create a non-stationary filter
     allocate(nh(product(npatch)))      !! nh is the length of each filter in table
     nh=product(nfilt)                  !! Set them all constant
     call psize_init(n,npatch,psize)    !! psize(ndim) is the patch-size
     
     if (verb>0) write(0,*) '#    patch-size = ',psize
     if (verb>0) write(0,*) '#####################################'
     
     call pch_init(n,npatch,psize,pch)  !! pch(ndata) is the index
     call nallocate(nsmatch,nh,pch)

     do ipatch=1,product(npatch)
        nsmatch%hlx(ipatch)%lag=match%lag
     end do
		
	nhmax=size(nsmatch%hlx(1)%lag)

     call laplac_init(laplac,nsmatch,npatch) !! laplac is the regularizer

  case default
     call seperr('mode not implemented')
  end select
  
  panels:do ipanels=1,npanels

     if (verb>1) write(0,*) '###################################'
     if (verb>1) write(0,*) '   Panel = ',ipanels,' out of ',npanels
     if (verb>1) write(0,*) '-----------------------------------'

     out=0.
     
     ! Read in data
     call sep_read(data1)
     if (interleave) then
        call sep_read(data2)
     else
        call sep_read(data2,'target')
     end if
     if ((maxval(data1).eq.minval(data1)).or.(maxval(data2).eq.minval(data2))) then
        call sep_write(out)
        cycle
     end if

	if (wmode==3) then
		allocate(data4(product(n)))
        call sep_read(data4,'weight')
		max_mask=-9999; min_mask=99999
		do ii=1,product(n)
			if (data4(ii)>max_mask) max_mask=data4(ii)
			if (data4(ii)<max_mask) min_mask=data4(ii)
		end do
		if (max_mask==0. .OR. min_mask<0) then
			write(0,*)'ERROR:mask is all zeros or contain negative numbers'
			stop
		end if
		data4=data4/max_mask
	end if

     select case(mode)
        
     case(1) ! One filter
        
        select case(wmode)
           
        case(1) ! No weight
           
           if (verb>2) write(0,*) ' -----------------------------------'
           if (verb>2) write(0,*) ' Calling findmatch with no weight...'
           if (verb>2) write(0,*) ' -----------------------------------'

           call findmatch(data1,data2,match,niter)
        
        case(2) ! IRLS L1

           if (verb>2) write(0,*) ' ----------------------------'
           if (verb>2) write(0,*) ' Calling findmatch with L1...'
           if (verb>2) write(0,*) ' ----------------------------'

           call findmatch(data1,data2,match,niter,nmem_in=nmem, &
           & nfreq_in=nfreq)
        
		case(3) ! User-supplied weight

           if (verb>2) write(0,*) ' -----------------------------'
           if (verb>2) write(0,*) ' Calling findmatch with user-supplied weight...'
           if (verb>2) write(0,*) ' -----------------------------'
			

			!! outer cycle (non-linear iterations)
			do jj=1,nouter
				call findmatch(data1,data2,match,niter,wght=data4)
				if (nouter>1) then
					dummy_data1=data1
					call nchconest_init(dummy_data1,match)
					stat=nchconest_lop(.FALSE.,.FALSE.,dummy_data1,match%flt)
					data4=dummy_data1-data2
					data4=abs(1.-data4)
				end if
			end do

        case default
           call seperr('Weight mode not implemented')
        end select
        
		!! write out the filter and the multiples
        call dumpfilter(match)
        call applymatch(data1,out,match)
		if (wmode==3) out=out*data4 !! apply the mask to the multiples
        
     case(2) ! NS filters
        
        select case(wmode)
           
        case(1) ! No weight
           
           if (verb>2) write(0,*) ' -------------------------------------'
           if (verb>2) write(0,*) ' Calling findnsmatch with no weight...'
           if (verb>2) write(0,*) ' -------------------------------------'
           
           call findnsmatch(data1,data2,nsmatch,laplac,nsmode,niter,eps)
           
        case(2) ! IRLS L1
           
           if (verb>2) write(0,*) ' -------------------------------'
           if (verb>2) write(0,*) ' Calling findnsmatch with L1 ...'
           if (verb>2) write(0,*) ' -------------------------------'
           
           call findnsmatch(data1,data2,nsmatch,laplac,nsmode,niter, &
           & eps,nfreq_in=nfreq,nmem_in=nmem) 
           
		case(3) ! User-supplied weight

           if (verb>2) write(0,*) ' -----------------------------'
           if (verb>2) write(0,*) ' Calling findnsmatch with user-supplied weight...'
           if (verb>2) write(0,*) ' -----------------------------'
			
			call findnsmatch(data1,data2,nsmatch,laplac,nsmode,niter,eps,wght=data4)

        case default
           call seperr('Weight mode not implemented')
        end select
        
        if (invmode.ne.2) then
           if (verb>2) write(0,*) ' Calling applynsmatch...'
           call applynsmatch(data1,out,nsmatch) 
        end if
        
     case default
        call seperr('Mode not implemented')  
     end select
     
     call sep_write(out)
     
  end do panels
  
  write(0,*) '---- The End ----'
  
  call sep_close()
  
contains
     
  !! Calculate non-overlapping patch-size given a certain number of patches
  subroutine psize_init(n,npatch,psize)
    integer, dimension(:), intent(in)  :: n,npatch
    integer, dimension(:), pointer     :: psize
    allocate(psize(ndim))
    do i=1,ndim
       psize(i)=1+(n(i)-1)/npatch(i)
    end do
  end subroutine psize_init

  !! Divides data-space between rectangular patches
  subroutine pch_init(n,npatch,psize,pch)
    integer, dimension(:), intent(in)  :: n,npatch,psize
    integer, dimension(:), pointer     :: pch,ii
    integer :: i,j

    allocate(pch(product(n)),ii(size(n)))
    do i=1,product(n)
       pch(i)=0
       call line2cart(n,i,ii)
       do j=1,size(n)
          ii(j)=(1+(ii(j)-1)/psize(j))
       end do
       call cart2line(npatch,ii,pch(i))
    end do
    deallocate(ii)
  end subroutine pch_init
  
  !! Create a roughener to dampen NS filter estimation
  subroutine laplac_init(laplac,nsfilt,npatch)
    type  (filter)        :: laplac
    type (nfilter)        :: nsfilt
    integer, dimension(:) :: npatch

    real    :: e1,e2
    integer :: nhmax
    nhmax=size(nsfilt%hlx(1)%lag)

    write(0,*) 'size',size(npatch)
    select case(size(npatch))
    case (1)
       call allocatehelix(laplac,1)  
       laplac%lag(1)=nhmax;   laplac%flt(1)=-.99
    case (2)
       !! 1-D smoothing only

       ! crap 2-D smoothing
       call allocatehelix(laplac,3)  
       laplac%lag=(/ nhmax, npatch(1)*nhmax, nhmax*(npatch(1)+1)  /)
       e1=0.2; e2=0.1
       laplac%flt=(/ e1-1.,      e2-1.,     (1.-e1)*(1.-e2)  /)

       if (verb>5) then
          write(0,*) '  Roughening filter...'
          call dumpfilter(laplac)
       end if

    case default
       write(0,*) 'Testmatch'
       call seperr('dimensionality not implemented')
    end select

  end subroutine laplac_init

end program Ultimatch_weight
