program analyze_harp
use precision_def
use nr
use bmad
use naff_mod
implicit none

interface
subroutine find_peak_freq(datac,nsize,freq)
 use precision_def
 implicit none
 real(rp) freq(:)
 complex(rp) datac(:)
 integer nsize
end subroutine


subroutine find_freq_near(f0,fh,fv,freq_in, freq)

 use precision_def
 implicit none
 real(rp) freq_in(:), freq(:), f0, fh, fv
 end subroutine
end interface

 integer, parameter:: n_time_bins=600000

 type harp_struct
  integer fiber(-3:3), ntotal
  real(rp) average, rms, freq(20), third
  complex(rp) amp(20), fft_fiber(-3:3)
 end type

 type (harp_struct) harp(4,0:n_time_bins)
 type (harp_struct) tot_hits(4), FR(4), tune(4), width(4), fiber_tune(4,-3:3)

 type (coord_struct) start_orb
 integer fiberNum
 integer tbin, tbin_max, lun, lun1
 integer i, n, lines/0/
 integer isign/1/
 integer m
 integer k, ix
 integer opt_dump_spectra/79/
 integer delta_bin
 integer iy
integer tbin_m

 real(rp) xfiber(-3:3)/-0.039,-0.026,-0.013,0.,0.013,0.026,0.039/
 real(rp) yfiber(-3:3)/-0.039,-0.026,-0.013,0.,0.013,0.026,0.039/
 real(rp) two/2./
! real(rp), allocatable :: data_cos(:,:), data_ave_cos(:,:), data_rms_cos(:,:)
! real(rp), allocatable :: data_sin(:,:), data_ave_sin(:,:), data_rms_sin(:,:)
 real(rp) time_bin/1.e-8/, radius/0.00025/
 real(rp) ave_last
 real(rp) closed_orbit
 real(rp) freq(3), f0/6.7/,fh/0.65/,fv/2.68/
real(rp) cross_talk/0/

 complex(rp), allocatable :: datac(:,:), datac_ave(:,:), datac_rms(:,:), datac_third(:,:)
 integer ix_hit, iy_hit
 integer ix_timebin, ix_radius, ix_turns
 integer i_abs
 integer start_turns/0/, end_turns/0/
 integer ios
 integer num_fib/0/
 integer fiber_tmp(-3:3)
integer j

 character*160 string, file(100)/100*''/
 character*6 wbin 
 character*2 wct
 character*100 out_name
 logical active_fib(-3:3)/7*.false./
 namelist/input/time_bin,radius,start_turns,end_turns,file, active_fib, cross_talk

 ! initialize harp positions


!   open(unit=lun, file = 'harp_plane_hits.dat')
   forall(i=-3:3)harp(:,:)%fiber(i) = 0
   
   harp(:,:)%ntotal = 0
   harp(:,:)%average = 0
   harp(:,:)%rms = 0
   harp(:,:)%third = 0
   forall(i=1:4)tot_hits(i)%fiber(-3:3) = 0
   tbin_max = 0
ix=0
k=0
lun=lunget()

 open(unit=lun,file='list.dat', STATUS='old', IOSTAT=ios)
    READ (lun, NML=input, IOSTAT=ios)
    WRITE(6,NML=input)
  print *, 'ios=', ios
   rewind(unit=lun)
!    READ (lun, NML=input)
  CLOSE(lun)

   lun1 = lunget()
   open(unit=lun1, file = 'harp_plane_hits_all.dat')

write(wct,'(i2)')int(cross_talk*100)
if(wct(1:1) == ' ')wct(1:1) = '0'
print *,' wct = ', wct

do k=1,size(file)
 if(trim(file(k)) /= '')print *,' file = ',trim(file(k))
end do

    do i=-3,3,1
     if(active_fib(i))num_fib=num_fib+1
    end do


do k=1,size(file)
   if(trim(file(k)) == '')cycle  
   lun=lunget()
   open(unit=lun, file = file(k))
   print *,' file = ',trim(file(k))
   do while(.true.)
     read(lun,'(a)', end=99)string
     write(lun1,'(a)')string
     if(index(string,'harp')/= 0)cycle
     read(string,'(i5,8es18.8)',end=99)fiberNum, start_orb%t, start_orb%s, start_orb%vec
!     write(lun1,'(i5,8es18.8)')fiberNum, start_orb%t, start_orb%s, start_orb%vec
     if(start_orb%t < start_turns*149.e-9 .or. (start_orb%t > end_turns * 149.e-9 .and. end_turns /= 0))cycle
!     if(start_orb%t < start_turns*149.e-9)cycle
     lines = lines + 1
     if((lines/1000000)*1000000 == lines)print *, lines
    ix_hit = -99
    iy_hit = -99

    do i=-3,3,1
     if(active_fib(i))then
     if((fiberNum == 1 .or. fiberNum == 3) .and. (abs(start_orb%vec(3) - yfiber(i))< radius))iy_hit=i
     if((fiberNum == 2 .or. fiberNum == 4) .and. (abs(start_orb%vec(1) - xfiber(i))< radius))ix_hit=i
        if((iy_hit > -99 .and. (fiberNum == 1 .or. fiberNum == 3)) .or. (ix_hit>-99 .and.(fiberNum == 2 .or. fiberNum== 4)))then
         tbin = (start_orb%t - start_turns*149.e-9)/time_bin+.5
        if(tbin > n_time_bins)then
          print *,' tbin = ',tbin ,'> n_time_bins = ', n_time_bins
          stop
        endif
         tbin_max = max(tbin, tbin_max)
         harp(fiberNum,tbin)%fiber(i) = harp(fiberNum,tbin)%fiber(i) + 1
         harp(fiberNum,tbin)%ntotal = harp(fiberNum,tbin)%ntotal +1
         n= harp(fiberNum,tbin)%ntotal
         tot_hits(fiberNum)%fiber(i) = tot_hits(fiberNum)%fiber(i) + 1
         exit
        endif
      endif
    end do
  end do
99 continue
 close(lun)
print*, 'tbin_max = ', tbin_max
end do !files    

! cross talk
do tbin = 1,tbin_max
  do i=-3,3,1
   do fiberNum=1,4
     if(active_fib(i))then
      fiber_tmp(i) = harp(fiberNum,tbin)%fiber(i)
      do j = -3,3,1
       if(.not. active_fib(j) .or. j == i)cycle
        harp(fiberNum,tbin)%fiber(j) = harp(fiberNum,tbin)%fiber(j)+ cross_talk * fiber_tmp(i)  !all fibers have same cross talk with all other fibers
        harp(fiberNum,tbin)%ntotal = harp(fiberNum,tbin)%ntotal +  cross_talk * fiber_tmp(i)
         tot_hits(fiberNum)%fiber(j) = tot_hits(fiberNum)%fiber(j) + cross_talk * fiber_tmp(i)
      end do
    endif
   end do
 end do
end do

         
     do tbin = 1, tbin_max    
       do fiberNum = 1,4
         n= harp(fiberNum,tbin)%ntotal
         if(n>0)then
         if(fiberNum == 2 .or. fiberNum == 4)then
             harp(fiberNum,tbin)%average = sum(harp(fiberNum,tbin)%fiber(:) * xfiber(:))/float(n) 
             harp(fiberNum,tbin)%rms =   sum (harp(fiberNum,tbin)%fiber(:)*yfiber(:)**2) /float(n) - harp(fiberNum,tbin)%average**2 
         endif
         if(fiberNum == 1 .or. fiberNum == 3)then
             harp(fiberNum,tbin)%average = sum(harp(fiberNum,tbin)%fiber(:) * yfiber(:))/float(n) 
             harp(fiberNum,tbin)%rms =   sum (harp(fiberNum,tbin)%fiber(:)*yfiber(:)**2) /float(n) - harp(fiberNum,tbin)%average**2 
         endif
       endif
     end do
   end do
 
! 99 continue
!   close(lun)
!print *, 'tbin_max = ' ,tbin_max
!end do ! files

! fourier transform of Number vs time
  n = log(float(tbin_max))/log(two)
  m = two**n 
 print *,'log(tbin_max)/log(two)=',n
 print *,'two**n =',m

  allocate(datac(1:4,1:m), datac_ave(1:4,1:m), datac_rms(1:4,1:m), datac_third(1:4,1:m))
  
  do fiberNum=1,4
   if(fiberNum == 1 .or. fiberNum == 3) closed_orbit=sum(tot_hits(fiberNum)%fiber(:)*yfiber(:))/sum(tot_hits(fiberNum)%fiber)  
   if(fiberNum == 2 .or. fiberNum == 4) closed_orbit=sum(tot_hits(fiberNum)%fiber(:)*xfiber(:))/sum(tot_hits(fiberNum)%fiber)  
   do i=1,m
    i_abs = abs(i)
    datac(fiberNum,i) = cmplx(float(harp(fiberNum,i_abs)%ntotal),0.)
    datac_ave(fiberNum,i) = cmplx(harp(fiberNum,i_abs)%average - closed_orbit,0.)
    datac_rms(fiberNum,i) = cmplx(harp(fiberNum,i_abs)%rms,0.)
    harp(fiberNum,i)%fft_fiber(:) = cmplx(harp(fiberNum, i)%fiber(:),0.)
   end do
   print *,' tbin_max = ', tbin_max,' log(two) = ', log(two), ' n= ',n,' m = ',m,' size(data_cos(fiberNum,:) = ', size(datac(fiberNum,:))
 ! call naff(datac(fiberNum,1:m),tot_hits(fiberNum)%freq,tot_hits(fiberNum)%amp,opt_dump_spectra)
  call four1(datac(fiberNum,1:m),isign)
!  call find_peak_freq(datac(fiberNum,1:m),m,FR(fiberNum)%freq)
  call four1(datac_ave(fiberNum,1:m),isign)
!  call find_peak_freq(datac_ave(fiberNum,1:m),m,Tune(fiberNum)%freq)
  call four1(datac_rms(fiberNum,1:m),isign)
!  call find_peak_freq(datac_rms(fiberNum,1:m),m,Width(fiberNum)%freq)
  do i=-3,3
   if(active_fib(i))then
   call four1(harp(fiberNum,1:m)%fft_fiber(i),isign)
!   call find_peak_freq(harp(fiberNum,1:m)%fft_fiber(i),m,fiber_tune(fiberNum,i)%freq)
   endif !active_fib
  end do
 end do
  write(wbin,'(f6.2)')time_bin*1.e9
if(wbin(1:1) == ' ')wbin(1:1) = '0'
if(wbin(2:2) == ' ')wbin(2:2) = '0'
 write(wct,'(i2)')int(cross_talk*100)
if(wct(1:1) == ' ')wct(1:1) = '0'
print *,' wct = ', wct

   lun = lunget()
if(num_fib > 1) out_name = 'FiberHarp-'//wbin//'ns-'//wct//'ct.dat'
if(num_fib == 1) out_name = 'FiberHarp-'//wbin//'ns-'//wct//'ct.dat_central_fib'
  open(unit = lun, file = trim(out_name))
 print *, trim(out_name)
 write(lun,'(a)')'#! Data from files '

  do k=1,ix
   print *,' k = ',k,' file(k) = ', trim(file(k))
   write(lun,'(a2,1x,a)')'#!', trim(file(k))
  end do
  write(lun, *)'# tbin_max = ', tbin_max,' log(two) = ', log(two), ' n= ',n,' m = ',m,' size(datac(fiberNum,:) = ', size(datac(fiberNum,:))
 write(lun,'(a1,1x,a,es12.4)')'#',' Time bin = ',time_bin
 write(lun,'(a1,1x,a,es12.4)')'#',' fiber_radius = ',radius
 write(lun,'(a1,1x,a,1x,i10,a,1x,i10)')'#','Start turns = ', start_turns, 'End turns = ',end_turns
  do fiberNum = 1,4
    if(fiberNum == 1 .or. fiberNum == 3)then
       write(lun,'(a7,1x,i1,1x,a,es12.4)')'# Harp ', fiberNum, 'average_positon = ', sum(tot_hits(fiberNum)%fiber(:)*xfiber(:))/sum(tot_hits(fiberNum)%fiber)  
     else
       write(lun,'(a7,1x,i1,1x,a, es12.4)')'# Harp ', fiberNum, 'average_positon = ', sum(tot_hits(fiberNum)%fiber(:)*yfiber(:))/sum(tot_hits(fiberNum)%fiber)    
    endif
       write(lun,'(a,1x,i1,1x,a,10es12.4)')'# Harp ',fiberNum, ' FR frequencies ', FR(fiberNum)%freq(1:10)/m/time_bin/1.e6
       write(lun,'(a,1x,i1,1x,a,10es12.4)')'# Harp ',fiberNum, ' Tune frequencies ', Tune(fiberNum)%freq(1:10)/m/time_bin/1.e6
       write(lun,'(a,1x,i1,1x,a,10es12.4)')'# Harp ',fiberNum, ' Width frequencies ', Width(fiberNum)%freq(1:10)/m/time_bin/1.e6
  end do

  write(lun,'(a1,1x,2a18,4a18,3a36)')'#','Harp','Time bin','Average position','RMS position','Total Hits','Freq bin','FFT cmplx tot hits','FFT cmplx average','FFT complex rms'
  do fiberNum =1,4
   do tbin = 1, tbin_max
      tbin_m = min(m, tbin)
!    if(harp(fiberNum,tbin)%ntotal /= 0)write(lun, '(2i12,2es12.4,i10)')fiberNum, tbin, harp(fiberNum,tbin)%average, harp(fiberNum,tbin)%rms, harp(fiberNum,tbin)%ntotal
     write(lun, '(2x,2i18,2es18.4,i18,1x,es18.4, 6es18.4)')fiberNum, tbin,  harp(fiberNum,abs(tbin))%average, harp(fiberNum,abs(tbin))%rms, &
                   harp(fiberNum,abs(tbin))%ntotal, abs(tbin)/time_bin/m/1.e6,&
                   datac(fiberNum,tbin_m), datac_ave(fiberNum, tbin_m), datac_rms(fiberNum,tbin_m)
   end do

  end do
 close(lun)
 close(lun1)

! data file to plot phase space - match of average position at harps 1&3 in tbins off by 1/4 149 and same for 2&4
delta_bin = 149.e-9/time_bin/4
lun=lunget()
open(unit=lun,file='fiberPhaseSpace.dat')
print *,' Write fiberPhaseSpace.dat'
write(lun, '(2a,1x,i10)')' Phase -space: ', 'delta_bin = ',delta_bin
write(lun, '(4a16)')' harp','time bin',' pos_ave (harp)','pos_ave( +2, delta_bin)'
do fiberNum = 1,2
do tbin = 1,tbin_max  !tbin in time_bin increments. Delta_bin = 149.e-9/time_bin/4
  write(lun,'(i10,6x,i10,6x,2(es12.4,4x))') fiberNum, tbin, harp(fiberNum,abs(tbin))%average, harp(fiberNum+2, abs(tbin + delta_bin))%average 
end do
end do
close(lun)
 
 lun = lunget()
 open(unit = lun, file = 'Fiber-'//wbin//'ns-'//wct//'ct.dat')
 lun1 = lunget()
 open(unit = lun1, file = 'FiberChromaticity.dat')
 print *,' Writing Individual Fiber-'//wbin//'ns-'//wct//'ct.dat'
 write(lun,'(a)')'#! Data from files '

  write(lun, '(2x,a18,2x,a18,5a18)')'Harp','fiber','time bin','freq bin','total hits','real(fft)','imag(fft)'
  do fiberNum =1,4
   do iy=-3,3
    if(active_fib(iy))then
    write(lun,'(a1,2x,2i10,10es14.6)')'#',fiberNum,iy,fiber_tune(fiberNum,iy)%freq(1:10)/m/time_bin/1.e6
    do tbin = 1, m
!     write(6, *)fiberNum, iy, abs(tbin)/time_bin/m/1.e6, &
!                   harp(fiberNum,tbin)%fiber(iy), &
!                   harp(fiberNum,tbin)%fft_fiber(iy)

     write(lun, '(2x,i18,2x,i18,2x,i18,es18.4,2x,i18,2x,2es18.4)')fiberNum, iy,tbin, abs(tbin)/time_bin/m/1.e6, &
                   harp(fiberNum,tbin)%fiber(iy), &
                   harp(fiberNum,tbin)%fft_fiber(iy)
    end do
    call find_freq_near(f0,fh,fv,fiber_tune(fiberNum,i)%freq/m/time_bin/1.e6, freq)
    if(freq(1) > 0)then
     print '(2i10,3es12.4)',fibernum,i, freq(1),freq(2),freq(2)/freq(1)
     write(lun1, '(2i10,3es12.4)')fibernum,i, freq(1),freq(2),freq(2)/freq(1)
    endif

  endif  !active_fib
  end do
  end do

 
 end program


   
subroutine find_peak_freq(datac,nsize,freq)
 use precision_def
 implicit none
 real(rp)  freq(:)
 complex(rp)  datac(:)
 real(rp), allocatable :: temp(:)
 real(rp) x,a,b,c
 integer i, n,j, nsize

! nsize = size(datac)
 allocate(temp(0:nsize+1))

!print *, ' nsize = ',nsize
n=0
do i=1,nsize 
temp(i) = sqrt(real(datac(i))**2+imag(datac(i))**2)
end do
if(all(temp(:) == 0))return
 temp(1:5)=0
 temp(nsize-min(5,nsize):nsize)=0
 do while  (n<20)
  j=maxloc(temp,1)
  n=n+1
  freq(n) = j

  c = temp(j)
  a = 0.5*(temp(j+1)+temp(j-1)-2*temp(j))
  b = 0.5*(temp(j+1)-temp(j-1))
  if(a > 0)x = -b/2/a
 ! print '(i10,1x,es12.4,1x,i10,1x,5es12.4)',j,temp(j),n,freq(n), (x+j)/nsize*100.,a,b,c
  freq(n) =j+ x
  temp(j-5:j+5) = 0
end do

deallocate(temp)
return
end subroutine

 subroutine find_freq_near(f0,fh,fv,freq_in, freq)

 use precision_def
 implicit none
 real(rp) freq_in(:), freq(:), f0, fh, fv

 integer i

 i =  minloc(abs(freq_in(:)-f0),1)
 freq(1) =  freq_in(i) 
 i =  minloc(abs(freq_in(:) - fh),1)
 freq(2) =  freq_in(i) 
 i =  minloc(abs(freq_in(:) - fv),1)
 freq(3) =  freq_in(i) 

 return
 end subroutine