! 2011.07.06 - j. shanks
! program to simulate ac_eta, phase, and coupling data 

program test

  use bmad
  use random_mod
  use dr_misalign_mod
  use correct_ring_mod2
  use radiation_mod
  use cesr_basic_mod
  use sim_utils
  use nonlin_bpm_mod
  use sim_bpm_mod
  use mode3_mod
  use tune_tracker_mod
  use sim_ac_and_cbar_mod
  

 implicit none

    type (lat_struct) ring
    type (cesr_struct) cesr
    type (coord_struct), allocatable :: orb(:)

    logical err, everything_ok
    integer i, j, ix, jx, kx, dummy
    real(rp) harvest(7)
    integer :: seed = 0.
    ! Related to phase/coupling simulation
    real(rp), allocatable :: phi(:,:,:), delta_phi_ax(:), delta_phi_by(:)
    real(rp) :: dphi_ax_avg = 0, dphi_by_avg = 0
    real(rp), allocatable :: cbar_meas(:,:,:), cbar_bmad(:,:,:) ! (:,2,2) = across all BPMs
    real(rp), allocatable :: A(:,:,:) ! = (n_bpms,2,2) = (i_bpm, (Aa,Ab),(Ba,Bb))
    real(rp) :: rms_cbar(2,2)=0, rms_phia = 0, rms_phib = 0, rms_ac_x = 0, rms_ac_y = 0


    real(rp) :: current = 1000000, skq02w_kick = 0.
    integer :: n_bpms = 0, nIter = 1
    character(40) :: bpm_mask = "^DET\_[0-9]{2}[ewEW]$" ! default to using CESR naming convention

    ! parameters for misalignment; to be read in and loaded into bpm_error_sigmas and ma structures
    type(det_error_struct) :: bpm_error_sigmas
    type(det_struct), allocatable :: bpm(:)
    type(ma_struct) ma(20)
    logical :: misalign_magnets = .false.
    real(rp) :: shear_x = 0., gain_sigma = 0., timing_sigma = 0.
    real(rp) :: bpm_offset = 0., bpm_rotation = 0., bpm_noise = 0.


    ! File handling:
    integer ios, version, arg_num, iargc, readstatus
    integer :: lun_list(20) ! for recording available luns from lunget
    character(20) :: output='cesrv'
    character(100) lat_file, file_name, lat_file_name, path, phase_file_name, ac_file_name, rms_file_name

    ! intelligent bookkeeping (default to old):
    logical :: auto_bookkeeper = .true.

    real(rp) v(6,6), betaz, alphaz, x_amp, y_amp, num, den, sum_square
    real(rp)      sinpy_px,sinpy,cospx,cospy_px,cospy,sinpx
    real(rp) v0(6,6), simp(6,6)/36*0/, symp(6,6), vt(6,6)
    real(rp) nm(6,6)
    real(rp) vinv(6,6)

    integer k

  namelist /parameters/ lat_file

  do i=0,4,2
    simp(1+i,2+i)=1
    simp(2+i,1+i)=-1
  end do
  print *,'simp'
  do i=1,6
   print '(6es12.4)',simp(i,1:6)
  end do

pause

  arg_num=iargc()
  if(arg_num==0) then
     file_name='test.in'
  else
     call getarg(1, file_name)
  end if
  call string_trim (file_name, file_name, ix)
  open (unit= 1, file = file_name, status = 'old', iostat = ios)

  if(ios.ne.0)then
     write(*,*)
     write(*,*) 'ERROR: CANNOT OPEN FILE: ', trim(file_name)
  endif

  ! read in the parameters
  rewind(1)
  read(1, nml = parameters,iostat=readstatus)
  if(readstatus > 0) then
     print *,"CAN NOT READ FROM ",file_name, " IOSTAT = ", readstatus
     stop
  end if

  bmad_com%auto_bookkeeper = auto_bookkeeper

  output = trim(output)


  ! init lattice
  if(lat_file(1:8) == 'digested')then
     call read_digested_bmad_file (lat_file, ring, version)
  else
     call fullfilename(lat_file,lat_file)
     call bmad_parser (lat_file, ring)
  endif

  call set_on_off(rfcavity$, ring, on$)

  ! Locate BPMs we wish to study:
  call find_bpms(ring, bpm_mask, bpm)
  n_bpms = ubound(bpm,1)

  write(*,*) "n_bpms = ", n_bpms



     call closed_orbit_calc(ring, orb, 6)
     call lat_make_mat6(ring, -1, orb)
     call twiss_at_start(ring)
     call twiss_propagate_all(ring)
     call twiss3_at_start(ring, err)
     call twiss3_propagate_all(ring)
     
     ! populate cbar_bmad structure:
     do ix = 1, n_bpms
        call c_to_cbar(ring%ele(bpm(ix)%ix_lat),cbar_bmad(ix,:,:))
     enddo

  print '(/)'
  print '(a)',' full turn at start '
  print '(/)'
  print '(6es12.4)', ring%param%t1_with_RF(1:6,1:6)     
  print '(/)'
  print '(6es12.4)', ring%ele(0)%mode3%v(1:6,1:6)
     v(:,:) = ring%ele(0)%mode3%v(:,:)
     do k=1,6
      do j=1,6
       vt(k,j) = v(j,k)
      end do
     end do

     symp = matmul(vt,matmul(simp,v))
     print '(/,a,i)','symp for bpm =', bpm(i)%ix_db
     do k=1,6
      print '(6es12.4)',symp(k,1:6)
     end do

     vinv = -matmul(simp,matmul(vt,simp))
     nm = matmul(vinv,matmul(ring%param%t1_with_RF,v))     
     print '(/,a)','vinv*t*v'
     do k=1,6
      print '(6es12.4)',nm(k,1:6)
     end do

  lun_list(9) = lunget()
  open(unit=lun_list(9),file='test.dat')

  lun_list(11) = lunget()
  open(unit=lun_list(11),file='v.dat')
pause

  do i=1,n_bpms
     write(lun_list(9),'(/,i7)') bpm(i)%ix_db
     WRITE(lun_list(9),'(6es12.4)')ring%ele(bpm(i)%ix_lat)%mat6(1:6,1:6)
     WRITE(lun_list(9),'(6es12.4)')ring%ele(bpm(i)%ix_lat)%mode3%v(1:6,1:6)

     v(:,:) = ring%ele(bpm(i)%ix_lat)%mode3%v(:,:)
     do k=1,6
      do j=1,6
       vt(k,j) = v(j,k)
      end do
     end do

     symp = matmul(vt,matmul(simp,v))
     print '(/,a,i)','symp for bpm =', bpm(i)%ix_db
     do k=1,6
      print '(6es12.4)',symp(k,1:6)
     end do

     betaz = ring%ele(bpm(i)%ix_lat)%mode3%c%beta
     alphaz= ring%ele(bpm(i)%ix_lat)%mode3%c%alpha
     x_amp = sqrt((v(1,5)*betaz-v(1,6)*alphaz)**2 + V(1,6)**2)
     y_amp = sqrt((v(3,5)*betaz-v(3,6)*alphaz)**2 + V(3,6)**2)
     num = -v(1,6)*(v(3,5)*betaz+v(3,6)*alphaz) +v(3,6)*(v(1,5)*betaz+v(1,6)*alphaz)
     den = v(1,6)*v(3,6)+(v(1,5)*betaz+v(1,6)*alphaz)*(v(3,5)*betaz+v(3,6)*alphaz)
     sum_square = sqrt(num**2+den**2)

     sinpx = (v(1,5)*betaz-v(1,6)*alphaz)/sqrt((v(1,5)*betaz-v(1,6)*alphaz)**2+v(1,6)**2)
     cospx = v(1,6)/sqrt((v(1,5)*betaz-v(1,6)*alphaz)**2+v(1,6)**2)

     sinpy = (v(3,5)*betaz-v(3,6)*alphaz)/sqrt((v(3,5)*betaz-v(3,6)*alphaz)**2+v(3,6)**2)
     cospy = v(3,6)/sqrt((v(3,5)*betaz-v(3,6)*alphaz)**2+v(3,6)**2)

     sinpy_px = sinpy*cospx-cospy*sinpx
     cospy_px = cospy*cospx+sinpy*sinpx
    
     write(lun_list(11),'(i7,12es18.4)') bpm(i)%ix_db,v(1,3),v(1,4), v(1,5), v(1,6), v(2,3),v(2,4),v(2,5),v(2,6), v(3,5),v(3,6),v(4,5),v(4,6)
  enddo
  

end program test