program Ultimatch
  
  use sep
  use npef_pat
  use helix
  use nhelix
  use nhelicon
  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
  type (nfilter), pointer              :: sig_pef
  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=PEF 4=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,noise,rbar,eps2
  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, dimension(:), pointer :: nfpef ! no. of coefficients for PEFs

  !nonstationary PEF parameters
 
  integer                        :: na(2),center(2),gap(2) ! filter size
  integer                        :: niter_d      ! number of itterations to predict data
  real                           :: eps_d        ! epsilon(amount of smoothing when predict)
  integer                        :: radial,steer ! type of smoother
  real                           :: width,amp    !width when doing radial
  real                           :: dtau,doff,dtheta  !FILTER FREQUENCY

  integer :: i,stat,ilag,ipatch,idim,ipanels,npanels ! junk variables
  
  integer                                :: j,j_dat(2)
  type(nfilter), pointer,save            :: noiz_pef
  type(nfilter), pointer,save            :: noiz_rough,data_rough
  integer                                :: ncoef_data
  real, pointer, dimension(:)            :: temp_array
  logical                                :: rad,ste
  
  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),nfpef(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.)
  stat=l1_priv_init(rbar)

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

  ! Patching parameters
  allocate(npatch(ndim),nwind(ndim))
  npatch=1
  call from_param('npatch',npatch,npatch)
  call from_param('noise',noise,0.)
  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)))

  ! 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

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

     select case(wmode)
        
     case(3)

        ! Read ns PEF parameters
        allocate(sig_pef)
        call read_filter_pars("data",na,center,gap,niter_d,&
        & eps_d,radial,steer,width,amp,dtau,doff,dtheta)

        ! ATTEMPT TO SHORTCUT COMPUTEMOD

        if (doff==0.) then
           j_dat(2)=3
        else
           j_dat(2)=doff/d(2)
        endif

        j_dat(1)=dtau/d(1)

        rad=(radial==1) 
        ste=(steer==1)

        ! Build filters (first attempt)
        call init_build_filters(n,j_dat,center,na,o,d,width, &
        & amp,rad,ste,dtau,dtheta)

        allocate(temp_array(product(n)))

        if (verb>0) write(0,*) '#####################################'
        if (verb>0) write(0,*) '#       Weighting with NS PEF       #'
        if (verb>0) write(0,*) '#-----------------------------------#'
        if (verb>0) write(0,*) '# radial =',radial
        if (verb>0) write(0,*) '#  steer =',steer
        if (verb>0) write(0,*) '#     na =',na
        if (verb>0) write(0,*) '#  niter =',niter_d
        if (verb>0) write(0,*) '#    eps =',eps_d
        if (verb>0) write(0,*) '#     j1 =',j_dat(1)
        if (verb>0) write(0,*) '#     j2 =',j_dat(2)
        if (verb>0) write(0,*) '#####################################'

        select case(invmode)
           
        case(1)
           
           if (verb>0) write(0,*) '#-----------------------------------#'
           if (verb>0) write(0,*) '#   BUILD REGULARIZATION OPERATORS  #'
           if (verb>0) write(0,*) '#   FOR THE PEF ESTIMATION          #'
           if (verb>0) write(0,*) '#####################################'

           allocate(noiz_pef,data_rough,noiz_rough)
           call build_2d_filter(noiz_pef,noiz_rough,ncoef_data)
           call build_2d_filter (sig_pef,data_rough,ncoef_data)
           
        case(2)

           if (verb>0) write(0,*) '#-----------------------------------#'
           if (verb>0) write(0,*) '#   BUILD REGULARIZATION OPERATORS  #'
           if (verb>0) write(0,*) '#   FOR THE PEF ESTIMATION          #'
           if (verb>0) write(0,*) '#####################################'

           allocate(data_rough)
           call build_2d_filter(sig_pef,data_rough,ncoef_data)
          
        case default
           call seperr('mode not implemented')
        end select
        
        
     end select

  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==4) then
		allocate(data4(product(n)))
        call sep_read(data4,'weight')
	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) ! PEF

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

           call from_param('nfpef',nfpef)
           call findmatchPEF(data1,data2,match,niter,nfpef,ndim,n)
        
		case(4) ! User-supplied weight

           if (verb>2) write(0,*) ' -----------------------------'
           if (verb>2) write(0,*) ' Calling findmatch with user-supplied weight...'
           if (verb>2) write(0,*) ' -----------------------------'
			
           call findmatch(data1,data2,match,niter,wght=data4)

        case default
           call seperr('Weight mode not implemented')
        end select
        
        call dumpfilter(match)
        call applymatch(data1,out,match)
        
     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) ! PEF
           
           select case(invmode)
              
           case(1)
              
              if (verb>2) write(0,*) ' INVMODE =',invmode
              if (verb>2) write(0,*) ' ------------------------------'
              if (verb>2) write(0,*) ' Compute ns PEF for the signal '
              if (verb>2) write(0,*) '           with Nd             '
              if (verb>2) write(0,*) ' ------------------------------'
 
              ! Compute N
              do i=1,size(noiz_pef%hlx)
                 noiz_pef%hlx(i)%flt=0.
              end do
              
              temp_array=data1
              call find_pef(temp_array,noiz_pef,noiz_rough,niter_d,eps_d,&
              & ncoef_data,verb=.true.)
    
              ! Compute Nd
              call nhelicon_init(noiz_pef)
              stat=nhelicon_lop(.false.,.false.,data2,data3)
              call nhelicon_close()

              do i=1,size(sig_pef%hlx)
                 sig_pef%hlx(i)%flt=0.
              end do
              
              ! Compute S
              temp_array=data3              
              call find_pef(temp_array,sig_pef,data_rough,niter_d,eps_d,&
              & ncoef_data,verb=.true.)
              
              if (verb>2)  write(0,*) '--------------------------------'
              if (verb>2)  write(0,*) ' Calling findnsmatch with PEF...'
              if (verb>2)  write(0,*) '--------------------------------'
              
              call findnsmatchPEF(data1,data2,nsmatch,sig_pef,laplac,nsmode,&
              & niter,eps)

           case(2)
              
              if (verb>2) write(0,*) ' INVMODE =',invmode
              if (verb>2) write(0,*) ' ------------------------------'
              if (verb>2) write(0,*) ' Signal PEF estimated from the '
              if (verb>2) write(0,*) ' adaptive subtraction result.  '
              if (verb>2) write(0,*) ' ------------------------------'
              
              data3=0.
              
              if (verb>2) write(0,*) '----------------'
              if (verb>2) write(0,*) 'Iteration # -- 1'
              if (verb>2) write(0,*) '----------------'
              if (verb>2) write(0,*) ' Calling findnsmatch with no weight...'

              call findnsmatch(data1,data2,nsmatch,laplac,nsmode,niter,eps)
              call applynsmatch(data1,out,nsmatch)
              data3=data2-out
              
              do i=2,ntimes
                 
                 if (verb>2) write(0,*) '-------------------------'
                 if (verb>2) write(0,*) 'Iteration # --',i,'/',ntimes
                 if (verb>2) write(0,*) '-------------------------'   
                 if (verb>2) write(0,*) '    ------------------------------'
                 if (verb>2) write(0,*) '    Compute ns PEF for the signal '
                 if (verb>2) write(0,*) '    ------------------------------'
                  
                 do j=1,size(sig_pef%hlx)
                    sig_pef%hlx(j)%flt=0.
                 end do
                 
                 temp_array=data3
                 call find_pef(temp_array,sig_pef,data_rough,niter_d,eps_d,&
                 & ncoef_data,verb=.true.)

                 data3=0
                
                 if (verb>2)  write(0,*) '   --------------------------------'
                 if (verb>2)  write(0,*) '   Calling findnsmatch with PEF...'
                 if (verb>2)  write(0,*) '   --------------------------------'

                 call findnsmatchPEF(data1,data2,nsmatch,sig_pef,laplac,nsmode,&
                 & niter,eps2)
                 call applynsmatch(data1,out,nsmatch)
                 data3=data2-out

              end do
              
           case default
              call seperr('Inversion mode not implemented')
           end select
           
		case(4) ! 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
  
  subroutine read_filter_pars(string,na,center,gap,niter,eps,&
  & radial,steer,width,amp,dtau,doff,dtheta)
    
    use sep
    
    character (len=*)    :: string
    integer              :: na(2),center(2),gap(2),niter
    real                 :: eps
    integer              :: radial,steer,ierr
    real                 :: width,amp,dtau,doff,dtheta
    character (len=1024) ::  temp
    integer,external     :: getch
    
    temp="na1_"//string;call from_param(temp,na(1),7)
    temp="na2_"//string;call from_param(temp,na(2),3)
    temp="gap1_"//string;call from_param(temp,gap(1),0)
    temp="gap2_"//string;call from_param(temp,gap(2),1)
    temp="center1_"//string;call from_param(temp,center(1),floor(real(na(1))/2.+.5))
    temp="center2_"//string;call from_param(temp,center(2),1)
    
    temp="niter_"//string;call from_param(temp,niter,75)
    temp="eps_"//string;call from_param(temp,eps,1.)
    temp="radial_"//string;call from_param(temp,radial,1)
    temp="steer_"//string;call from_param(temp,steer,1)
    temp="width_"//string;call from_param(temp,width,1.5)
    temp="amp_"//string;call from_param(temp,amp,.98)
    temp="dtau_"//string;call from_param(temp,dtau,.04)
    temp="doff_"//string;call from_param(temp,doff,0.)
    temp="dtheta_"//string;call from_param(temp,dtheta,2.)
    
  end subroutine read_filter_pars

  !! 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
