program fft_cbpm2

 use gain_fit_mod
 use bmad
 use tbt_mod
 use nr
 use nrutil, only: dpc

 implicit none

 interface
   subroutine clip_data(co,i)
    use bmad
    implicit none
    type (coord_struct) co(:)
    integer i,j,k
   end subroutine
 end interface

 interface
   subroutine plot_fft(co, fty,ftx, range, bpm, just_one, name_number)
    use bmad
    implicit none
    type (coord_struct) co(:)
    real(rp) ftx(:), fty(:)
    real(rp) range
!    character*(*) bpm_name
   character*20 name_number
   logical just_one
   integer bpm
   end subroutine
 end interface

interface
 subroutine plot_xy_vs_turn(co,nn, bpm, just_one, name_number)
  use bmad
  implicit none
  type (coord_struct) co(:)
  logical just_one
  character*20 name_number
  integer bpm
  integer nn
 end subroutine
end interface

 type (coord_struct) co(300000)

 character*120 file_name
 character*120 string
 character*5 answer/'N'/
 character*30 bpm_name
 character*5 prefix/'DET_0'/
 character*20 name_number

 integer ix, ios, nn, j, i
 integer isign, turn,  data_type, skip, start
 integer, parameter :: lost_particle$ = 1, phase_space$ = 2, analyzer$ = 3, measurement$ = 4
 integer ix_name
 integer lun
 integer but(4)
 integer m

 logical bpm_data(-1:120)

 parameter nmax=262144
 real(rp) ftx(nmax), fty(nmax), ftz(nmax)
 real(rp) range
 real(rp) x, y
 real(rp) hscale/22./, vscale/27./
 real(rp) pie

 complex(dpc) cdata(1,nmax)

 logical first_time/.true./, just_one/.false./

   pie=atan(1.)*4

do
   call get_turn_by_turn

   print '(a,$)',' Which BPM ? '
   read(5,*)bpm
   if(bpm == -1)just_one = .false.
   if(bpm > 0 )just_one = .true.
   
  bpm_data(bpm) = .true.
  do m=0,100
    if(.not. bpm_data(bpm))cycle
    if(just_one == .false.)bpm = m
      bpm_data(bpm) = .true.
      if(all(orbit_data(1)%detector(bpm)%button(1:4) == 0))then
        print '(a,1x,i5,1x,a)',' no data for bpm ', bpm, bpm_names(bpm)
        bpm_data(bpm) = .false.
        do j=0,100
         if(all(orbit_data(1)%detector(j)%button(1:4) == 0 ))cycle
          print '(a,1x,i5,1x,a)',' Good data for BPM ',j,bpm_names(j)
        end do
       cycle ! no data for this bpm
      endif

   do i=1,norbits
    but(1:4) = orbit_data(i)%detector(bpm)%button(1:4)
!    x = (but(4)-but(3)+but(2)-but(1)) * hscale/sum(but(1:4))
!    y = (but(4)+but(3)-but(2)-but(1)) * vscale/sum(but(1:4))
     x = orbit_data(i)%detector(bpm)%x
     y = orbit_data(i)%detector(bpm)%y
    co(i)%vec(1)= x
    co(i)%vec(3) = y

   end do

  if(just_one)print *,' There are ', norbits, ' data points'

!    call clip_data(co,i)
    print *,' There are ', i, ' data points after clip data' 
    if(i < 10)then
      print*, ' There is insufficient data at', bpm_name(1:ix_name),' for an FFT '
      open(unit=lun, file=file_name, status='old', iostat=ios)
      cycle
    endif


  if(first_time)then
!    print '(a,$)', ' Transform data beginning with turn ?'
!    read (*,'(i)') start 
!    print '(a,$)', ' How many turns (power of 2, 2**18 = 262144) ?'
!    read (*,'(i)') nn
     start=1
     nn = log(float(norbits+1))/log(2.)
     nn = 2**nn
  endif

    if(nn-start > nmax)then
      print *,' More than nmax points. Array too small '
      stop
    else
      print *, nn, ' points for fft'
    endif

 data_type = measurement$
    if(data_type == measurement$)then

      range = 390.1/2
      do j = 1,nn
       if(j+start-1 > i)then
         print *, ' No data for i > ', i
         stop
       endif

        ftx(j) = co(j+start-1)%vec(1) *(2*(sin((pie*j)/nn))*(sin((pie*j)/nn)))
        fty(j) = co(j+start-1)%vec(3) *(2*(sin((pie*j)/nn))*(sin((pie*j)/nn)))
!        ftz(j) = 1.
      end do
      call realft(ftx(1:nn),1)     
      call realft(fty(1:nn),1)     
    else

    range = 390.1
    do j = 1,nn
     if(j+start-1 > i)then
       print *, ' No data for i > ', i
       stop
     endif


     cdata(1,j) = cmplx(co(j+start-1)%vec(1), co(j+start-1)%vec(2))*(2*(sin((pie*j)/nn))*(sin((pie*j)/nn)))
    end do
    call fourrow(cdata(:,1:nn),1)
    forall(i=1:nn) ftx(i)=sqrt(cdata(1,i)* conjg(cdata(1,i)))

    isign=1
    do j = 1,nn
     cdata(1,j) = cmplx(co(j+start-1)%vec(3),co(j+start-1)%vec(4))*(2*(sin((pie*j)/nn))*(sin((pie*j)/nn)))
    end do
    call fourrow(cdata(:,1:nn), isign)
     forall(i=1:nn)fty(i)=sqrt(cdata(1,i)*conjg(cdata(1,i)))
    isign=1

!    do j = 1,nn
!     cdata(1,j) = cmplx(co(j+start-1)%vec(5),co(j+start-1)%vec(6))*(2*(sin((pie*j)/nn))*(sin((pie*j)/nn)))
!    end do
!    call fourrow(cdata(:,1:nn), isign)
!     forall(i=1:nn)ftz(i)=sqrt(cdata(1,i)*conjg(cdata(1,i)))

   endif

!    open(unit=15, file=bpm_name(1:ix_name)//'.dat')
    write(15,'(a6,4a12)')' index','     x      ','     y      ','   fftx     ','   ffty     '
    do j=1,nn
     write(15,'(i6,4e12.4)')j, co(j+start-1)%vec(1),co(j+start-1)%vec(3), ftx(j) , fty(j)
     write(16,'(i6,2e12.4)')j, log(ftx(j)**2), log(fty(j)**2)
    end do
  if(first_time) &
    call read_string('Plot fft ? ','Y',answer)

    write(name_number,'(i)')rd_number
    call string_trim(name_number,name_number,ix)
    print *,' number = ',rd_number,' name_number = ',name_number(1:ix)

     if(answer(1:1) == 'Y' .or. answer(1:1) == 'y') &
      call plot_fft(co(start:start+nn-1),fty(1:nn), ftx(1:nn), range, bpm, just_one,name_number)

    call read_string('Plot reflection ?','Y',answer)
     if(answer(1:1) == 'Y' .or. answer(1:1) == 'y') &
      call plot_fft(co(start:start+nn-1),fty(1:nn), ftx(1:nn), -range, bpm, just_one,name_number)


       print '(a,$)',' Plot phase space (Y/N) ?  '
        read (*,'(a)') answer
        call str_upcase(answer, answer)
       if(index(answer,'Y') /= 0) &
         call plot_phase_space(data_type)

       print '(a,$)',' Plot x-y vs turn number (Y/N) ?  '
        read (*,'(a)') answer
        call str_upcase(answer, answer)
       if(index(answer,'Y') /= 0) &
         call plot_xy_vs_turn(co(start:start+nn-1),nn, bpm, just_one,name_number(1:ix))

    if(just_one)exit
    first_time = .false.
   end do ! m=0,100
  end do
    end

 subroutine plot_fft(co, fty,ftx, range, bpm, just_one, name_number)
  use bmad
  

  implicit none

  type (coord_struct) co(:)
  real(rp) ftx(:), fty(:)
  real(rp) ra(size(fty),2)
  real(rp) d,b,c,A,tune(10,2)
  real(rp) range
  real*4 rftx(size(fty)), rfty(size(fty)), xmax, ymax, xmin, ymin
  real*4 x(size(fty)), xpos(size(fty)), ypos(size(fty)), turn(size(fty))
  real*4 minz, maxz
  real*4 srange
  real*4 f
  real*4 int_tune(10,2)
  real*4 low_end_range/0/, high_end_range/1./

!  character*(*) bpm_name
  character*20 device_type
  character*20 answer
  character*40 title
  character*20 name_number

  logical already_open /.false./
  logical just_one

  integer iostat, istat2, icall, pgopen, i, n, imx(1), j, ixy
  integer bpm
  integer ix

  low_end_range = 0.5
  high_end_range = 1
  f=390.1 !f=1 for fractional tune
  srange = range
  n  = size(fty)
  do i = 1,n
    if(ftx(i) /= 0.)rftx(i) = log(ftx(i)**2)
    if(fty(i) /= 0.)rfty(i) = log(fty(i)**2)
    x(i) = i/float(n)
!    xpos(i) = co(i)%vec(1)
    ypos(i) = co(i)%vec(3)
    turn(i) = i
    ra(i,1) = abs(ftx(i))
    ra(i,2) = abs(fty(i))
  end do


  do ixy = 1,2
   do i = 1,10
    imx = maxloc(ra(5:n-5,ixy)) + 4 

    d= ra(imx(1),ixy)
    b=ra(imx(1)+1,ixy)
    if(d /= 0. .or. b /= 0.)then
    c=cos(twopi/n)
    A= (-(d+b*c)*(d-b)+b*sqrt(c*c*(d+b)*(d+b)-2*d*b*(2*c*c-c-1)))/(d*d+b*b+2*d*b*c)
    endif
    int_tune(i,ixy) = imx(1)/float(n)
    tune(i,ixy)= imx(1)/float(n)+(1/twopi)*asin(A*sin(twopi/float(n)))
!    print '(1x,3i5,2e12.4)',i,ixy,imx(1),ra(imx(1),ixy), tune(i,ixy)
    do j = max(1,imx(1)-20),min(n,imx(1)+20)
     ra(j,ixy)=0.
    end do
   end do
  end do

  print '(1x,2a34)','  vertical(kHz)  ','  horizontal(kHz) ' 
  print '(1x,4a17)','peak bin','interpolated max','peak bin','interpolated max' 
  do i = 1,10
   if(range >0)then
    print '(1x,4(4x,f10.3,4x))',(2.-int_tune(i,2))*range, (2.-tune(i,2))*range,(2.-tune(i,1))*range,(2.-int_tune(i,1))*range
   else   
    print '(1x,4(4x,f10.3,4x))',(1.+int_tune(i,2))*abs(range), (1.+tune(i,2))*abs(range),(1.+tune(i,1))*abs(range),(1.+int_tune(i,1))*abs(range)
   endif
  end do
print *
  do i = 1,10
    print '(1x,4(4x,f10.3,4x))',int_tune(i,2), tune(i,2),int_tune(i,1),int_tune(i,1)
  end do

    write(31,'(i,10es14.6)') bpm, ((2.-tune(i,1))*range/390.1, i=1,10)
    write(32,'(i,10es14.6)') bpm, ((2.-tune(i,2))*range/390.1, i=1,10)

if(just_one)then
 do while(.true.)
!  if(.not. already_open)then
     istat2 =  pgopen('/XSERVE')
     if(istat2 >= 0)already_open = .true.
     icall = 1
!  endif
    device_type = '/XSERVE'
99  if(device_type(1:7) /= '/XSERVE')istat2 = pgopen(device_type)

  call pgslct(istat2)

  call pgpap(10.,1.)
  call pgsubp(1,2)
  call pgscr(0, 1., 1., 1.)
  call pgscr(1,0.,0.,0.)
  call pgscr(2, 1., 0., 0.)
  call pgscr(3,0.,0.,0.,1.0)
  call pgsch(2.)     
  call pgask(.false.)
  call pgsci(icall)

  minz = minval(ypos(1:n))
  maxz = maxval(ypos(1:n))
  call pgenv(low_end_range*f,high_end_range*f,minval(rftx),maxval(rftx), 0,0)
  title = ' fft horizontal - '//'RD'//trim(name_number)
  call pglab('kHz',' ',title)
  if(range >0)call pgline(n, (2-x)*390.1/2., rftx)
  if(range <0)call pgline(n, (1+x)*390.1/2., rftx)

  call pgenv(low_end_range*f,high_end_range*f,minval(rfty), maxval(rfty), 0,0)
  title = ' fft vertical '
  call pglab('kHz',' ',title)
  if(range<0)call pglab('kHz',' ',title//' (reflection)')
  if(range>0)call pgline(n, (2-x)*f/2., rfty)
  if(range<0)call pgline(n, (1+x)*f/2., rfty)
!
!  minz = minval(xpos(1:n))
!  maxz = maxval(xpos(1:n))
!  call pgenv(0,float(n),minz, maxz,0,0)
!  title = ' horizontal position at '//bpm_name
!  call pglab('turn','mm',title)
!  call pgline(n, turn, xpos)

!  call pgenv(0,srange, xmin, xmax,0,0)
!  title = ' horizontal fft at '//bpm_name
!  call pglab('kHz',' ',title)
!  call pgline(n, x, rftx)


  if(device_type(1:7) == '/XSERVE')then
   answer = ' '
   print '(a,$)', ' Hardcopy?, Postscript("PS") or Gif ("Gif") '
   read(5,'(a)', err=30) answer
   call str_upcase(answer, answer)
   if(index(answer, 'PS') /= 0 .or. index(answer,'GIF') /= 0)then
     if(index(answer, 'PS') /= 0)device_type = 'fft.ps/VCPS' 
     if(index(answer, 'GIF') /= 0)device_type = 'fft.gif/VGIF' 
     goto 99
   endif
  endif

30  if(device_type(1:7) /= '/XSERVE') then
   print *,' write ',device_type(1:index(device_type,'/')-1)
   if(istat2 > 0)call pgslct(istat2)
   call pgclos
   already_open = .false.
  endif

   print '(a,$)',' Change plot range ?'
   read(5,*)answer
   if(index(answer,'y') == 0 .and. index(answer,'Y') == 0) exit
   print '(a,$)',' Min and max frequency to plot (kHz)  '
   read(5,*)low_end_range,high_end_range
            low_end_range= low_end_range/390.1
            high_end_range = high_end_range/390.1
  end do
  endif !just_one
  return

     end subroutine

   subroutine plot_phase_space( data_type)

  use gain_fit_mod
  use bmad

  implicit none

  type (coord_struct) co

  real*4 x(10000,2), y(10000,2), turn, max1, max2, x_avg(2), y_avg(2)
  real*4 x_sum(2), y_sum(2)

!  character*(*) file_name
  character*16 detector_(2)
  character*5 detector/'DET_0'/
  character*20 answer, device_type
  character*40 title
  character*120 string
  character*30 bpm_name(2)

  logical already_open /.false./

  integer  data_type
  integer, parameter :: lost_particle$ = 1, phase_space$ = 2, analyzer$ = 3, measurement$ = 4

  integer iostat, istat2, icall, pgopen, i, n, ios
  integer ix_(2), ix 
  integer ix_name
  integer ii
  integer j,nn, number(2)
  integer lun

30   print '(a,$)', ' Pair of detectors ? (i.e. 2W,8W) '
   read(5,'(a)', err=30) answer
   call str_upcase(answer, answer)
   call string_trim(answer,answer,ix)

   read(answer,*,err=30)detector_(1), detector_(2)
   print *, ' Detectors:       ', detector_(1:2)
 
! lun=lunget()
! open (unit= lun, file = file_name, status = 'old', iostat = ios)

     do j=1,2
       x_sum(j)=0.
       y_sum(j)=0.
     end do
!     do while(.true.)

!     read(lun,'(a)',end=10)string

!     if(data_type == analyzer$)then
!        do j = 1,2
!         call string_trim(detector_(j), detector_(j),ix_(j))
!         bpm_name(j) = detector(1:7-ix_(j))//detector_(j)(1:ix_(j))
!         if(index(string, bpm_name(j)(1:7)) == 0)cycle 
!         call string_trim(string, string, ix)
!         read(string(1:ix),*)turn
!         call string_trim(string(index(string,bpm_name(j)(1:7))+7+1:),string,ix)
!         read(string,*)co%vec(1:4)
!         x(turn,j)=co%vec(1)
!         y(turn,j)=co%vec(3)
!         x_sum(j) = x_sum(j)+x(turn,j)
!         y_sum(j) = y_sum(j)+y(turn,j)
!         number(j) = turn
!        end do
!     endif

     if(data_type == measurement$)then
        do j = 1,2
         call bpm_chara_to_int(detector_(j),bpm) 
         print '(i,1x,a,i)',j,detector_(j),bpm
         x(1:norbits,j) = orbit_data(1:norbits)%detector(bpm)%x
         y(1:norbits,j) = orbit_data(1:norbits)%detector(bpm)%y
         call string_trim(string(ix+1:),string,ix)
         x_sum(j) = sum(x(:,j))
         y_sum(j) = sum(y(:,j))
         number(j) = norbits
        end do
     endif
!     if(data_type == measurement$)then
!        do j = 1,2
!         call string_trim(detector_(j), detector_(j),ix_(j))
!         bpm_name(j) = 'CESR BPM '//detector_(j)(1:ix_(j))
!         ix_name = 4+5+ix_(j)
!         if(index(string, bpm_name(j)(1:ix_name)) == 0)cycle 
!         call string_trim(string, string, ix)
!         read(string(1:ix),*)turn
!         call string_trim(string(index(string,bpm_name(j)(1:ix_name))+ix_name+1:),string,ix)
!         read(string(1:ix),*)x(turn,j)
!         call string_trim(string(ix+1:),string,ix)
!         read(string(1:ix),*)y(turn,j)
!         x_sum(j) = x_sum(j)+x(turn,j)
!         y_sum(j) = y_sum(j)+y(turn,j)
!         number(j) = turn
!        end do
!     endif

!  end do

10 continue
   do j=1,2
     if(number(j) < 10)then
        print *,' No data for ', bpm_name(j)
        close(unit=lun)
        goto 30
     endif
   end do
   turn = norbits
   nn = turn
   x_avg(1:2)=x_sum(1:2)/turn
   y_avg(1:2)=y_sum(1:2)/turn

  do i = 1,nn
    do j=1,2
      x(i,j)=x(i,j)-x_avg(j)
      y(i,j)=y(i,j)-y_avg(j)
    end do
  end do


  if(.not. already_open)then
     istat2 =  pgopen('/XSERVE')
     if(istat2 >= 0)already_open = .true.
     icall = 1
  endif
    device_type = '/XSERVE'
99  if(device_type(1:7) /= '/XSERVE')istat2 = pgopen(device_type)

  call pgslct(istat2)

  call pgpap(8.,1.)
  call pgsubp(1,2)
  call pgscr(0, 1., 1., 1.)
  call pgscr(1,0.,0.,0.)
  call pgscr(2, 1., 0., 0.)
  call pgscr(3,0.,0.,0.,1.0)
  call pgsch(2.)     
  call pgask(.false.)
  call pgsci(icall)

  max1 = maxval(abs(x(1:nn,1)))
  max2 = maxval(abs(x(1:nn,2)))
  call pgenv(-max1,max1,-max2,max2,0,0)
  title = ' horizontal phase space '
  call pglab(detector_(1)(1:ix_(1))//'(mm)',detector_(2)(1:ix_(2))//'(mm)',title)
  call pgpt(nn, x(:,1), x(:,2),18)

  max1 = maxval(abs(y(1:nn,1)))
  max2 = maxval(abs(y(1:nn,2)))
  call pgenv(-max1,max1,-max2,max2,0,0)
  title = ' vertical phase space '
  call pglab(detector_(1)(1:ix_(1))//'(mm)',detector_(2)(1:ix_(2))//'(mm)' ,title)
  call pgpt(nn,y(:,1),y(:,2),18)


  if(device_type(1:7) == '/XSERVE')then
   answer = ' '
   print '(a,$)', ' Hardcopy?, Postscript("PS") or Gif ("Gif") '
   read(5,'(a)', err=30) answer
   call str_upcase(answer, answer)
   if(index(answer, 'PS') /= 0 .or. index(answer,'GIF') /= 0)then
     if(index(answer, 'PS') /= 0)device_type = 'phase_space.ps/VCPS' 
     if(index(answer, 'GIF') /= 0)device_type = 'phase_space.gif/VGIF' 
     goto 99
   endif
  endif

  if(device_type(1:7) /= '/XSERVE') then
   print *,' write ',device_type(1:index(device_type,'/')-1)
   if(istat2 > 0)call pgslct(istat2)
   call pgclos
   already_open = .false.
  endif

  return

    end subroutine


     subroutine clip_data(co,i)
  use bmad
  

  implicit none

  type (coord_struct) co(:)
  integer i,j,k
  real(rp) sum,average,y

  sum=0.
  do j=1,i
   sum = sum +co(j)%vec(3)
  end do

  average = sum/i
  k=0
  do j=1,i
   y = co(j)%vec(3) - average

   if(abs(y) > 500.) cycle
    k=k+1 
    co(k)%vec(3) = y
  end do
   i=k
  return
end subroutine

subroutine bpm_chara_to_int(detect,bpm)
implicit none
character*(*) detect
integer bpm, ix

 call string_trim(detect,detect,ix) 
 if(ix == 2)then
  read(detect(1:1),'(i1)')bpm
 elseif(ix == 3)then
  read(detect(1:2),'(i2)')bpm
 else
  print *,' detector name is not right ',detect
 endif
 if(detect(ix:ix) == 'E')bpm = 99-bpm
return
end



 subroutine plot_xy_vs_turn(co,nn, bpm, just_one, name_number)
  use bmad
  

  implicit none

  type (coord_struct) co(:)
  real*4 x(nn), xpos(nn), ypos(nn), turn(nn)
  real*4 minz, maxz

  character*20 device_type
  character*20 answer
  character*40 title
  character*20 name_number

  logical already_open /.false./
  logical just_one

  integer iostat, istat2, icall, pgopen, i, n, imx(1), j, ixy
  integer bpm
  integer nn

  do i = 1,nn
    x(i) = float(i)
    xpos(i) = co(i)%vec(1)*1000
    ypos(i) = co(i)%vec(3)*1000
    turn(i) = i
  end do

n=nn

if(just_one)then
!  if(.not. already_open)then
     istat2 =  pgopen('/XSERVE')
     if(istat2 >= 0)already_open = .true.
     icall =1
!  endif
    device_type = '/XSERVE'
99  if(device_type(1:7) /= '/XSERVE')istat2 = pgopen(device_type)

  call pgslct(istat2)

  call pgpap(10.,1.)
  call pgsubp(1,2)
  call pgscr(0, 1., 1., 1.)
  call pgscr(1,0.,0.,0.)
  call pgscr(2, 1., 0., 0.)
  call pgscr(3,0.,0.,0.,1.0)
  call pgsch(2.)     
  call pgask(.false.)
  call pgsci(icall)

  minz = minval(xpos(1:nn-1))
  maxz = maxval(xpos(1:nn-1))
 print *,' minz= ',minz,' maxz = ',maxz
  call pgenv(0,float(nn),minz,maxz, 0,0)
  title = ' horizontal position '//'RD'//trim(name_number)
  call pglab('turn','mm',title)
  call pgsci(icall+1)
  call pgline(n, x, xpos)

 print *,' minz= ',minval(ypos(1:nn-1)),' maxz = ',maxval(ypos(1:nn-1))
  call pgsci(icall)
  call pgenv(0,float(nn),minval(ypos(1:nn-1)), maxval(ypos(1:nn-1)), 0,0)
  title = ' vertical position '
  call pglab('turn','mm',title)
  call pgsci(icall+3)
  call pgline(n, x, ypos)

  if(device_type(1:7) == '/XSERVE')then
   answer = ' '
   print '(a,$)', ' Hardcopy?, Postscript("PS") or Gif ("Gif") '
   read(5,'(a)', err=30) answer
   call str_upcase(answer, answer)
   if(index(answer, 'PS') /= 0 .or. index(answer,'GIF') /= 0)then
     if(index(answer, 'PS') /= 0)device_type = 'xyvsturn.ps/VCPS' 
     if(index(answer, 'GIF') /= 0)device_type = 'xyvsturn.gif/VGIF' 
     goto 99
   endif
  endif

30  if(device_type(1:7) /= '/XSERVE') then
   print *,' write ',device_type(1:index(device_type,'/')-1)
   if(istat2 > 0)call pgslct(istat2)
   call pgclos
   already_open = .false.
  endif
  endif !just_one
  return

     end subroutine