# 1 "<stdin>"
# 1 "<built-in>"
# 1 "<command line>"
# 1 "<stdin>"
module interp_filter_mod
  use nhelix
  use sep
  implicit none
  integer, private, save :: n1,n2,noff
  real, private, pointer, save :: dips(:,:),func(:,:) ,angles(:,:)
  integer, private, pointer, save :: lags(:,:)
  logical, private, pointer, save :: have(:,:)
  real, private, save :: dip_range,ang_range
  integer, private, save :: nparts
  real, private, save :: ddip,daz
  interface interp_filter
    module procedure interp3d_filter,interp2d_filter
  end interface
  interface create_steer_integrate_op
    module procedure create_steer_integrate_op_2d
    module procedure create_steer_integrate_op_3d
  end interface
  contains
  logical function init_filt_interp(n_1,n_off,diprange,angrange,n_2&
    &,n_parts,d_dip,d_az,is_2d)
    integer :: n_1
    integer :: i1,i2,n2s
    real :: angrange,diprange,d_dip,d_az
    integer :: n_2,n_parts,n_off
    logical :: is_2d
    init_filt_interp=.false.
    n1=n_1
    n2=n_2
    ddip=d_dip
    daz=d_az
    nparts=n_parts
    noff=n_off
    if (.not. is_2d) then
      n2s=noff*2+2
    else
      n2s=1
    end if
    ang_range=angrange
    dip_range=diprange
    dip_range=dip_range*atan(1.)/45.*3.
    ang_range=ang_range*atan(1.)/45.*3.
    allocate(lags(noff*2+2,n2s))
    allocate(func(noff*2+2,n2s))
    allocate(dips(noff*2+2,n2s))
    allocate(angles(noff*2+2,n2s))
    allocate(have(noff*2+2,n2s))
!DO THE DIPS BOTH CASES
    do i2=1,noff*2+1
      dips(i2,:)=atan(real(i2-noff-1))
    end do
    dips(noff*2+2,:)=atan(1.)*2.
    if (.not. is_2d) then
!FIRST AXIS DIPS SECOND AXIS AZIMUTH
      do i2=1,noff*2+1
        angles(:,i2)=atan(real(i2-noff-1))
        do i1=1,noff*2+1
          lags(i1,i2)=(n1*n2)+(i1-noff-1)+(i2-noff-1)*n1
        end do
      end do
      angles(:,noff*2+2)=atan(1.)*2
      do i1=1,noff*2+1
        lags(noff*2+2,i1)=1 + n1*n2+(i1-noff-1)*n1
        lags(i1,noff*2+2)=1 + n1+ (i1-noff-1)
      end do
      lags(noff*2+2,noff*2+2)=1
    else
!2-D case
      do i1=1,noff*2+1
        lags(i1,1)=n1+i1-noff-1
      end do
      lags(noff*2+2,1)=1
    end if
    init_filt_interp=.true.
  end function
  logical function clean_filt_interp()
    deallocate(lags,func,dips,angles,have)
    clean_filt_interp=.true.
  end function
  logical function create_steer_integrate_op_3d(filt,p1,p2)
    type(nfilter) :: filt
    real :: p1(:,:,:),p2(:,:,:)
    real, pointer :: mydip(:,:,:),myaz(:,:,:)
    integer :: ndip,naz,i2,i3,i1
    create_steer_integrate_op_3d=.false.
    allocate(mydip(size(p1,1),size(p1,2),size(p1,3)))
    allocate(myaz(size(p1,1),size(p1,2),size(p1,3)))
    if (.not. convert_p_dip_az(p1,p2,mydip,myaz)) then
      write(0,*) "trouble converting p to dip and azimuth"
      return
    end if
!
!call srite("p1.H",p1,size(myaz)*4)
!call srite("p2.H",p2,size(mydip)*4)
!call srite("mya.H",myaz,size(myaz)*4)
!call srite("myd.H",mydip,size(mydip)*4)
    if (.not. create_filt_database_3d(filt,mydip,myaz,ndip,naz)) then
      write(0,*) "trouble creating filt database"
      return
    end if
    if (.not. assign_filt_database_3d(filt,mydip,myaz,ndip,naz,p1,p2))&
      & then
      write(0,*) "trouble assigning filters"
      return
    end if
    deallocate(mydip,myaz)
    create_steer_integrate_op_3d=.true.
  end function
  logical function assign_filt_database_2d(filt,mydip,ndip)
    type(nfilter) :: filt
    real :: mydip(:,:,:)
    integer :: idip,iaz,i,i1,i2,i3,ndip
    real :: odip,oaz
    assign_filt_database_2d=.false.
    odip=minval(mydip)
    allocate(filt%pch(size(mydip)))
    i=0
    do i3=1,size(mydip,3)
      do i2=1,size(mydip,2)
        do i1=1,size(mydip,1)
          i= i+1
          idip=min(ndip,max(1,nint( (mydip(i1,i2,i3) - odip)/ddip) +1)&
            &)
          filt%pch(i)=idip
        end do
      end do
    end do
    assign_filt_database_2d=.true.
  end function
  logical function assign_filt_database_3d(filt,mydip,myaz,ndip,naz,p1&
    &,p2)
    type(nfilter) :: filt
    real :: mydip(:,:,:), myaz(:,:,:),p1(:,:,:),p2(:,:,:)
    integer :: idip,iaz,i,i1,i2,i3,ndip,naz
    real :: odip,oaz
    assign_filt_database_3d=.false.
    odip=minval(mydip)
    oaz =minval(myaz)
    allocate(filt%pch(size(mydip)))
!mydip=0
!myaz=0
    i=0
    do i3=1,size(mydip,3)
      do i2=1,size(mydip,2)
        do i1=1,size(mydip,1)
          i= i+1
          idip=min(ndip,max(1,nint( (mydip(i1,i2,i3) - odip)/ddip) +1)&
            &)
          iaz =min(naz ,max(1,nint( (myaz(i1,i2,i3) - oaz )/daz ) +1)&
            &)
          filt%pch(i)=idip + (iaz -1)* ndip
        end do
      end do
    end do
    assign_filt_database_3d=.true.
  end function
  logical function convert_p_dip_az(p1,p2,myd,mya)
    real :: p1(:,:,:),p2(:,:,:)
    real, pointer :: myd(:,:,:),mya(:,:,:)
    integer :: i1,i2,i3
    convert_p_dip_az=.false.
    mya=0
!p2=0
    myd=sqrt(p1**2+p2**2)
    mya=acos((p1+.00001)/(myd+.00001))*45/atan(1.)
    myd=atan(myd)*45/atan(1.)
!where(p2<0.) myd=-myd
    where(p2<0.)
      myd=-myd
end where
!where(p1<0.) myd=-myd
    where(mya > 90.)
      mya=mya-180.
end where
!where(mya>45.) mya=45.
!where(mya<-45.) mya=-45.
    convert_p_dip_az=.true.
  end function
  logical function create_filt_database_3d(filt,mydip,myaz,ndip,naz)
    type(nfilter) :: filt
    real :: mydip(:,:,:),myaz(:,:,:)
    real :: oaz,odip,mym
    integer :: naz,ndip,iaz,idip,i
    real :: a,d
    create_filt_database_3d=.false.
    oaz =minval(myaz)
    odip=minval(mydip)
    naz = ceiling(( maxval(myaz) - oaz)/ daz)+1
    ndip= ceiling(( maxval(mydip)-odip)/ddip)+1
    allocate(filt%hlx(naz*ndip))
    nullify(filt%mis)
    i=0
    do iaz=1,naz
      a=oaz + daz * ( iaz-1)
      do idip=1,ndip
        i= i+1
        d=odip+ ddip* (idip-1)
        if (.not. interp3d_filter(d,a,filt%hlx(i))) then
          write(0,*) "trouble creating filter ",iaz,a,idip,d
          return
        end if
      end do
    end do
!i=ndip ;
!filt%hlx(i)%lag=0 ; filt%hlx(i)%flt=0.
!call allocatehelix(filt%hlx(i),1)
    create_filt_database_3d=.true.
  end function
  logical function create_steer_integrate_op_2d(filt,p1)
    type(nfilter) :: filt
    integer :: ndip
    real :: p1(:,:,:)
    real, pointer :: mydip(:,:,:)
    create_steer_integrate_op_2d=.false.
    allocate(mydip(size(p1,1),size(p1,2),size(p1,3)))
    mydip=atan(p1)*45./atan(1.)
    if (.not. create_filt_database_2d(filt,mydip,ndip)) then
      write(0,*) "trouble creating filt database"
      return
    end if
    if (.not. assign_filt_database_2d(filt,mydip,ndip)) then
      write(0,*) "trouble assigning filters"
      return
    end if
    deallocate(mydip)
    create_steer_integrate_op_2d=.true.
  end function
  logical function create_filt_database_2d(filt,mydip,ndip)
    type(nfilter) :: filt
    real :: mydip(:,:,:)
    integer :: ndip
    real :: odip,d
    integer :: idip,i
    create_filt_database_2d=.false.
    odip=minval(mydip)
    ndip=ceiling((maxval(mydip)-odip)/ddip)+1
    allocate(filt%hlx(ndip))
    nullify(filt%mis)
    do idip=1,ndip
      d=odip+(idip-1)*ddip
      if (.not. interp2d_filter(d,filt%hlx(idip))) then
        write(0,*) "trouble creating filter", idip,d
        return
      end if
    end do
!i=ndip ;
!filt%hlx(i)%lag=0 ; filt%hlx(i)%flt=0.
!call allocatehelix(filt%hlx(i),1)
    create_filt_database_2d=.true.
  end function
  logical function interp3d_filter(dip,angle,filt)
    type(filter) :: filt
    real :: dip,angle,ang2,dip2
    interp3d_filter=.false.
    dip2=dip*atan(1.)/45.
    ang2=angle*atan(1.)/45.
    if (.not. calc_3d_func(dip2,ang2)) then
      write(0,*) "trouble calculating 3d filter"
      return
    end if
!if(.not. calc_3d_lagrange(dip2,ang2)){
! write(0,*) "trouble calculating 3d filter"
! return
!}
    if (.not. make_filter(filt)) then
      write(0,*) "trouble making filter"
      return
    end if
    interp3d_filter=.true.
  end function
  logical function interp2d_filter(dip,filt)
    real :: dip,dip2
    type(filter) :: filt
    interp2d_filter=.false.
    dip2=dip*atan(1.)/45.
    if (.not. calc_2d_func(dip2)) then
      write(0,*) "trouble calculating 2d filter"
      return
    end if
!if(.not. calc_2d_lagrange(dip2)){
! write(0,*) "trouble calculating 2d filter"
! return
!}
    if (.not. make_filter(filt)) then
      write(0,*) "trouble making filter"
      return
    end if
    interp2d_filter=.true.
  end function
  logical function calc_3d_lagrange(dip,ang)
    real :: dip,ang
    real :: odip,myd,delta_dip,ddip
    real :: oang,mya,delta_ang,dang
    real :: val,f1,f2,vala
    integer :: i,i1,i2,j,bd,ed,ba,ea
    real , allocatable :: tab(:,:),d1(:),a1(:)
    calc_3d_lagrange=.false.
! func=0.
! have=.false.
! i2=1
! if(ang >= angles(1,size(angles,2))) i2=size(angles,2)
! else{
! while(angles(1,i2) < ang) i2+=1
! i2-=1
! }
! if(i2==0){ ba=1;ea=3}
! else if(i2==size(angles,2)){ ba=size(angles,2)-2;ea=size(angles,2)}
! else{
! if(ang>0){ ba=max(1,i2-2);ea=min(size(angles,2),i2+2)}
! else { ba=max(1,i2-1);ea=min(size(angles,2),i2+3)}
! }
!
! if(dip >= dips(size(angles,1),1)) i1=size(dips,1)
! else{
! i1=1
! while(dips(i1,1) < dip) i1+=1
! i1-=1
! }
! if(i1==0) {bd=1;ed=3}
! else if(i1==size(angles,1)){bd=size(dips,1)-2; ed=size(dips,1)}
! else{
! if(dip>0) {bd=max(1,i1-2); ed=min(size(dips,1),i1+2);}
! else {bd=max(1,i1-1); ed=min(size(dips,1),i1+3);}
! }
!
! allocate(d1(ed-bd+1),a1(ed-bd+1),tab(size(d1),size(a1)))
! d1=dips(bd:ed,1); a1=angles(1,ba:ea)
! if(.not. lagrange_2d_tab(dip,ang,d1,a1,tab)){
! write(0,*) "trouble doing the 2-D lagrange interpolation"
! return
! }
! func(bd:ed,ba:ea)=-tab*.9
! deallocate(d1,a1,tab)
!write(0,*) "whre b"
    calc_3d_lagrange=.true.
  end function
  logical function calc_3d_func(dip,ang)
    real :: dip,ang
    real :: odip,myd,delta_dip,ddip
    real :: oang,mya,delta_ang,dang
    real :: val,f1,f2,vala
    integer :: i,i1,i2,j
    calc_3d_func=.false.
    odip=dip-dip_range/2.
    ddip=dip_range/(nparts-1)
    delta_dip=3.1415926536/dip_range
    oang=ang-ang_range/2.
    dang=ang_range/(nparts-1)
    delta_ang=3.1415926536/ang_range
    func=0.
    have=.false.
    do j=1,nparts
      mya=(j-1)*dang+oang
!FIND THE LARGEST ANG ARRAY VALUE LESS THEN THE CURRENT DIP
      i2=1
      if (mya >= angles(1,size(angles,2))) then
        i2=size(angles,2)
      else
        do while (angles(1,i2) < mya)
          i2= i2+1
        end do
        i2= i2-1
        vala=(sin(delta_ang*(mya-oang))**2)
        f2=(mya-angles(1,i2))/(angles(1,i2+1)-angles(1,i2))
      end if
      if (i2 > 0 .and. i2 < size(angles,2)) then
        do i=1,nparts
          myd=(i-1)*ddip+odip
          i1=1
!FIND THE LARGEST DIPS ARRAY VALUE LESS THEN THE CURRENT DIP
          if (myd >= dips(size(dips,1),1)) then
            i1=size(dips)
          else
            do while (dips(i1,1) < myd)
              i1= i1+1
            end do
            i1= i1-1
          end if
          if (i1 > 0 .and. i1 < size(dips,1)) then
            val=(sin(delta_dip*(myd-odip))**2)*vala
            f1=(myd-dips(i1,1))/(dips(i1+1,1)-dips(i1,1))
            func(i1,i2)= func(i1,i2)+(1.-f1)*(1.-f2)*val
            func(i1+1,i2) = func(i1+1,i2) +f1 *(1.-f2)*val
            func(i1,i2+1)= func(i1,i2+1)+(1.-f1)*(f2)*val
            func(i1+1,i2+1) = func(i1+1,i2+1) +f1 *(f2)*val
            have(i1:i1+1,i2:i2+1)=.true.
          end if
        end do
      end if
    end do
! func=-.96*func/sum(func)
    calc_3d_func=.true.
  end function
  logical function calc_2d_lagrange(dip)
    real :: dip
    real :: odip,myd,delta_dip,ddip
    real :: val,f
    real, allocatable :: tab(:),diped(:)
    integer :: i,i1,nc
    calc_2d_lagrange=.false.
! odip=dip-dip_range/2.
! ddip=dip_range/(nparts-1)
! delta_dip=3.1415926536/dip_range
! func=0.
! have=.false.
!
! i1=1
! if(dip_range >= dips(size(dips,1),1)) i1=size(dips,1)
! else{
! i1=1
! while(dips(i1,1) < dip) i1+=1
! i1-=1
! }
! if(i1==0) have(1:3,1)=.true.
! else if(i1==size(dips,1)) have(size(dips,1)-2:)=.true.
! else{
! if(dip>0)
! have(max(1,i1-1):min(size(dips,1),i1+3))=.true.
! else
! have(max(1,i1-1):min(size(dips,1),i1+3))=.true.
! }
!
! nc=0
! do i1=1,size(have,1){
! if(have(i1,1)) nc+=1
! }
! allocate(tab(nc))
! allocate(diped(nc))
!
! nc=0
! do i1=1,size(have,1){
! if(have(i1,1)){
! nc+=1
! diped(nc)=dips(i1,1)
! }
! }
!
! if(.not. lagrange_1d_tab(dip,diped,tab)){
! write(0,*) "trouble getting 1-D tabulated function"
! return
! }
! nc=0
! do i1=1,size(have,1){
! if(have(i1,1)){
! nc+=1
! func(i1,1)=tab(nc)
! }
! }
!
!func=-func/sum(abs(func))
!
! deallocate(tab,diped)
    calc_2d_lagrange=.true.
  end function
  logical function calc_2d_func(dip)
    real :: dip
    real :: odip,myd,delta_dip,ddip
    real :: val,f
    integer :: i,i1
    calc_2d_func=.false.
    odip=dip-dip_range/2.
    ddip=dip_range/(nparts-1)
    delta_dip=3.1415926536/dip_range
    func=0.
    have=.false.
    do i=1,nparts
      myd=(i-1)*ddip+odip
      i1=1
!FIND THE LARGEST DIPS ARRAY VALUE LESS THEN THE CURRENT DIP
      if (myd >= dips(size(dips,1),1)) then
        i1=size(dips,1)
      else
        do while (dips(i1,1) < myd)
          i1= i1+1
        end do
        i1= i1-1
      end if
      if (i1 > 0 .and. i1 < size(dips,1)) then
        val=sin(delta_dip*(myd-odip))**2
        f=(myd-dips(i1,1))/(dips(i1+1,1)-dips(i1,1))
        func(i1,1)= func(i1,1)+(1.-f)*val
        func(i1+1,1) = func(i1+1,1) +f *val
        have(i1:i1+1,1)=.true.
      end if
    end do
    func=-func/sum(func)
    calc_2d_func=.true.
  end function
  logical function make_filter(filt)
    type(filter) :: filt
    integer :: i1,i2,nc
    make_filter=.false.
    nc=0
    do i2=1,size(have,2)
      do i1=1,size(have,1)
        if (abs(func(i1,i2))>.0001) then
          nc= nc+1
        end if
      end do
    end do
    call allocatehelix(filt,nc)
    nc=0
    do i2=1,size(have,2)
      do i1=1,size(have,1)
        if (abs(func(i1,i2)) > .0001) then
          nc= nc+1
          filt%flt(nc)=func(i1,i2)
          filt%lag(nc)=lags(i1,i2)
        end if
      end do
    end do
    filt%flt=-1.*filt%flt/sum(filt%flt)
    make_filter=.true.
  end function
  logical function lagrange_2d_tab(x_out,y_out,x_in,y_in,tab)
    real :: x_out,y_out,x_in(:),y_in(:),tab(:,:)
    integer :: i1,i2
    real :: num,den
    real :: tabx(size(x_in)),taby(size(y_in))
    lagrange_2d_tab=.false.
    if (size(y_in).ne.size(tab,2) .or. size(x_in).ne.size(tab,1)) then
      write(0,*) "expecting size(x_in==tab(1,:)) and size(y_in(tab(:,1&
        &)))"
      return
    end if
    tabx=0
    taby=0
    if (.not. lagrange_1d_tab(x_out,x_in,tabx)) then
      write(0,*) "trouble interpolating x"
      return
    end if
    if (.not. lagrange_1d_tab(y_out,y_in,taby)) then
      write(0,*) "trouble interpolating y"
      return
    end if
    do i2=1,size(tab,2)
      do i1=1,size(tab,1)
        tab(i1,i2)=tabx(i1)*taby(i2)
      end do
    end do
    lagrange_2d_tab=.true.
  end function
  logical function lagrange_1d_tab(x_out,x_in,tab)
    real :: x_out,x_in(:),tab(:)
    integer :: i1,i2
    real :: num,den
! lagrange_1d_tab=.false.
! tab=1
!
! do i2=1,size(x_in){
! do i1=1,size(x_in){
! if(i1!=i2) tab(i2)=tab(i2)*(x_out-x_in(i1))/(x_in(i2)-x_in(i1))
! }
! }
    lagrange_1d_tab=.true.
  end function
end module
