module calibrate_sextupole_mod

contains

!+                                              
! Subroutine calibrate_sextupole
!
! Setup: 
!  1) Put the tunes where tune changes will not dump the beam.
!  2) Measure the betaqtron phase to get a more accurate analysis.
!
! Note: This routine modified from [cesr.dcs.sext]sex_meas.f90
!-

subroutine calibrate_sextupole (i_sex_var, cu_sex, u, err_flag)

use cesrv_struct
use cesrv_interface
use find_slaves_for_lord_mod
use cesr_db_mod
use cesr_group_mod
use phase_mod
use cesr_read_data_mod

implicit none

type (universe_struct), target :: u
type (var_struct), pointer :: sex_var
type (ele_struct), pointer :: sex_ele
type (v1_var_struct), pointer :: v1_hbump
type (cesr_freq_struct) freq, freq0, freq1, freq2, dfreq
type (butns_struct) butns

type this_bump_struct
  real(rp) x_max
  real(rp) x_sex
  real(rp) merit
  integer ix_slave(10)
  integer cu_range
  integer cu_wanted
end type
                    
type (this_bump_struct) hbump(98)

integer i, k, i_det1, i_det2, j, cu_wanted, n_butns
integer bump_num, cu_bump0, cu_bump, ix_slave(10), nr, ix1, ix_ele
integer i_sex, sex_set1_cu, sex_set2_cu, istat, file_num, i_sex_var
integer del_cu_bump, idummy, real_del, raw(4,120), iu
integer ix_hb, garbage(98), tot_cu_bump, cu1_bump, cu2_bump
integer cu_low_lim, cu_high_lim, cu_sex(2)
integer ixs, vnumbr, ib, ib0, d_cu5, d_cu6, cu, dcu
integer ix_qt5, ix_qt6, cu0_qt5, cu0_qt6, ix, n_bump_set

real(rp) x_max, x_at_sex, merit_max, x_sex, dQ, factor, max_beta
real(rp) phi_x0, phi_y0, denom
real(rp) dQx_qt5, dQy_qt5, dQx_qt6, dQy_qt6
real(rp), parameter :: delta = 100

character fmt*80, header1*80, header2*80
character data_file*80, ans*16
character(80) bmad_file_name
character(200) orb_file, digested_file
character(20) date, line

logical loc_mask(100), h_tracker_locked, v_tracker_locked
logical ok, err, err_flag

! init

sex_var => u%var(i_sex_var)
i_sex = sex_var%ix_db
sex_ele => u%ring%ele(sex_var%ix_ele)

line = 'GROUP HBUMP  '
call match_var_type (line, v1_hbump, u, err_flag)
if (err_flag) return

freq%x%reflection = .true.
freq%y%reflection = .true.
freq%rev = 390.14

freq0 = freq; freq1 = freq; freq2 = freq

! get initial tunes:

do
  if (logic%debug) exit
  call get_tracker_lock_status (h_tracker_locked, v_tracker_locked)
  if (h_tracker_locked .and. v_tracker_locked) exit
  print *,        'ERROR: A TRACKER (OR TWO) IS NOT LOCKED'
  print '(a, $)', '       LOCK & HIT <CR> TO RECHECK, "E" TO EXIT: '
  accept '(a)', ans

  if (ans(1:1) == 'e' .or. ans(1:1) == 'E') then
    if (logic%command_file_open) then
      close (logic%iu_command_file)
      logic%command_file_open = .false.
    endif
    return
  endif

enddo

call get_freq (freq0, 5)

print '(a, 2f10.2)', ' Initial Tunes:', freq0%x%tune, freq0%y%tune

! find strengths of Qtune knobs

call db_group_to_bmad_group ('CSR QTUNEING', 5, 0, 0, u%ring, u%db, ix_qt5, ok, .true.)
call db_group_to_bmad_group ('CSR QTUNEING', 6, 0, 0, u%ring, u%db, ix_qt6, ok, .true.)


phi_x0 = u%ring%a%tune
phi_y0 = u%ring%b%tune

u%ring%ele(ix_qt5)%control_var(1)%value = delta
call lat_make_mat6 (u%ring, ix_qt5)
call twiss_at_start (u%ring)
dQx_qt5 = 390.1 * (u%ring%a%tune - phi_x0) / (twopi * delta)
dQy_qt5 = 390.1 * (u%ring%b%tune - phi_y0) / (twopi * delta)
u%ring%ele(ix_qt5)%control_var(1)%value = 0
call lat_make_mat6 (u%ring, ix_qt5)

u%ring%ele(ix_qt6)%control_var(1)%value = delta
call lat_make_mat6 (u%ring, ix_qt6)
call twiss_at_start (u%ring)
dQx_qt6 = 390.1 * (u%ring%a%tune - phi_x0) / (twopi * delta)
dQy_qt6 = 390.1 * (u%ring%b%tune - phi_y0) / (twopi * delta)
u%ring%ele(ix_qt5)%control_var(1)%value = 0
call lat_make_mat6 (u%ring, ix_qt5)

print *
print '(15x, a)', 'dQ_x      dQ_y'
print '(15x, a)', '(kHz / CU)'
print '(a, 3p2f10.2)', ' Qtune_5:', dQx_qt5, dQy_qt5
print '(a, 3p2f10.2)', ' Qtune_6:', dQx_qt6, dQy_qt6
         
!          

do i = 1, vnumbr('CSR HBUMPING')
  if (.not. v1_hbump%v(i)%exists) cycle
  ix_hb = v1_hbump%v(i)%ix_ele
  call find_slaves_for_lord (u%ring, ix_hb, hbump(i)%ix_slave)
  u%ring%ele(ix_hb)%control_var(1)%value = delta
  call control_bookkeeper (u%ring, u%ring%ele(ix_hb))
  call closed_orbit_calc (u%ring, u%orb, 4)
  hbump(i)%x_max = maxval(abs(u%orb(1:u%ring%n_ele_track)%vec(1))) / delta
  hbump(i)%x_sex = abs(u%orb(sex_ele%ix_ele)%vec(1)) / delta
  u%ring%ele(ix_hb)%control_var(1)%value = 0
  call control_bookkeeper (u%ring, u%ring%ele(ix_hb))
  call group_limits ('CSR HBUMPING', i, cu_low_lim, cu_high_lim)
  hbump(i)%cu_range = cu_high_lim - cu_low_lim
enddo

! pick dets on either side

do i = 1, 99
  if (u%db%detector(i)%ix_lat == 0) cycle
  if (u%db%detector(i)%ix_lat > sex_ele%ix_ele) then
    i_det2 = i 
    exit
  else
    i_det1 = i
  endif
enddo

loc_mask = .false.
loc_mask(i_det1) = .true.
loc_mask(i_det2) = .true.
print *, 'Detectors used: ', i_det1, i_det2
                                             
! choose limits

sex_set1_cu = max(-logic%sex_calib_delta_cu/2, sex_var%cu_low_lim)
sex_set2_cu = min(sex_set1_cu + logic%sex_calib_delta_cu, sex_var%cu_high_lim)

! Fix the bump to use

ixs = sex_ele%ix_ele
max_beta = max(u%ring%ele(ixs)%a%beta, u%ring%ele(ixs)%b%beta)
factor = abs(u%ring%ele(ixs)%value(l$) * logic%sex_calib_delta_cu * &
                                 sex_var%dvar_dcu * max_beta) / (4 * pi)

print *
print *, 'Bump     X_max     X@Sex  CU_Range CU_wanted     Merit'
print *, '      (mm @ Bump / CU)'

merit_max = -100
do i = 1, vnumbr('CSR HBUMPING')
  if (.not. v1_hbump%v(i)%exists) cycle

  nr = u%ring%n_ele_track
  ix1 = hbump(i)%ix_slave(1)
  do j = 1, size(ix_slave)
    if (hbump(i)%ix_slave(j) == -1) then
      ix_slave(j) = ix1
    else
      ix_slave(j) = modulo2(hbump(i)%ix_slave(j) - ix1, nr/2) + ix1
    endif
  enddo
  ix_ele = modulo2(ixs-ix1, nr/2) + ix1
  if (ix_ele < minval(ix_slave)) cycle
  if (ix_ele > maxval(ix_slave)) cycle

  x_at_sex = hbump(i)%x_sex
  if (x_at_sex < 2e-7) cycle
  dQ = factor * x_at_sex
  hbump(i)%cu_wanted = logic%dQ_max_sex_calib / dQ
  x_max = hbump(i)%x_max
  hbump(i)%merit = max (x_at_sex - (x_max - x_at_sex)/2, x_at_sex/3)
  if (hbump(i)%cu_range < hbump(i)%cu_wanted) hbump(i)%merit = &
              hbump(i)%merit * hbump(i)%cu_range / hbump(i)%cu_wanted
  if (hbump(i)%merit > merit_max) then
    merit_max = hbump(i)%merit
    bump_num = i
  endif

  print '(i5, 6p 2f10.3, 0p2i10, 6pf10.3)', i, hbump(i)%x_max, &
        hbump(i)%x_sex, hbump(i)%cu_range, hbump(i)%cu_wanted, hbump(i)%merit

enddo

if (logic%auto_measurement) then
  print *, 'Bump Used:', bump_num
else
  istat = in4get1 ('Bump Number to Use @ ', bump_num)
  if (istat == -1) then
    close (logic%iu_command_file)
    logic%command_file_open = .false.
    return
  endif                                                  
endif
                                    
! Fix the bump limits
! We try to use nice round numbers.

dQ = factor * hbump(bump_num)%x_sex
n_bump_set = logic%sex_calib_n_bump_set

print *
print *, 'frequency swing (kHz) @ 1000 CU Bump: ', 390.1 * 1000 * dQ

call vmgetn ('CSR HBUMPING', bump_num, bump_num, cu_bump0) 
cu_bump = cu_bump0

call group_limits ('CSR HBUMPING', bump_num, cu_low_lim, cu_high_lim)

print *, 'Bump now at:', cu_bump0
print *, 'Bump Limits:', cu_low_lim, cu_high_lim

cu_low_lim  = 50 * ceiling(cu_low_lim/50.0)   ! round up
cu_high_lim = 50 * floor(cu_high_lim/50.0)    ! round down

cu = 50 * (n_bump_set - 1)
tot_cu_bump = cu * (min(cu_high_lim-cu_low_lim, hbump(bump_num)%cu_wanted) / cu)

cu1_bump = 50 * floor((cu_bump0 - 0.5*tot_cu_bump) / 50)
cu1_bump = min(50*floor(cu_high_lim/50.0)-tot_cu_bump, cu1_bump)
cu1_bump = max(50*ceiling(cu_low_lim/50.0), cu1_bump)

del_cu_bump = tot_cu_bump / (n_bump_set - 1) 
cu2_bump = cu1_bump + del_cu_bump * (n_bump_set - 1)


if (logic%auto_measurement) then
  print *, 'Bump: Initial_set, Final_set, Delta:', cu1_bump, cu2_bump, del_cu_bump
else
  istat = in4get3 ('For the Bump: Initial_set, Final_set, Delta: @', cu1_bump, cu2_bump, del_cu_bump)
  if (istat == -1) then
    close (logic%iu_command_file)
    logic%command_file_open = .false.
    return
  endif
  if (del_cu_bump /= 0) n_bump_set = 1 + abs(nint(real(cu2_bump - cu1_bump) / del_cu_bump))
endif

! get QT values

call vxgetn ('CSR QTUNEING', 5, 5, cu0_qt5)
call vxgetn ('CSR QTUNEING', 6, 6, cu0_qt6)

! Init file

call date_and_time_stamp (date, .true.)
data_file = 'calibrate_sextupole.dat.' // date(1:10)

iu = lunget()
open (iu, file = data_file, access = 'append')
print *, 'Opened data file: ', trim(data_file)

write (iu, *) '&lat'
write (iu, *) 'Lattice = "', trim(logic%lattice), '"'
write (iu, *) '/end'

header1 = '         |    Delta    |   @Sex_set1   |          Position'
header2 = ' Cu_bump |  f_x    f_y |   f_x     f_y |   x1     y1     x2     y2'
if (.not. logic%debug) then
  call vxputn('CSR SCA RSET', 3, 3, 1)   ! Set Tune tracker sample rate to 10hz
  call vxputn('CSR SCA RSET', 7, 7, 1)   ! This is max resolution
endif

call vxgetn ('CSR SEXT CUR', i_sex, i_sex, sex_var%cu_saved)

! write header

write (iu, *)
write (iu, *) '&Params'
write (iu, *) '   i_Sex          =', sex_var%ix_db
write (iu, *) '   i_det1         =', i_det1
write (iu, *) '   i_det2         =', i_det2
write (iu, *) '   cu_sex_set1    =', sex_set1_cu 
write (iu, *) '   cu_sex_set2    =', sex_set2_cu 
write (iu, *) '   Bump_name      = "', trim('CSR HBUMPING'), '"'
write (iu, *) '   Bump_num       =', bump_num
write (iu, *) '   date           = "', trim(date), '"'
write (iu, *) '/end'
write (iu, '(a)') trim(header1)
write (iu, '(a)') trim(header2)

! loop over all bump amplitudes

if (.not. logic%debug) then
  call group_set ('CSR HBUMPING', bump_num, cu1_bump-cu_bump, cu_bump, real_del)
endif

bump_loop: do ib = 1, n_bump_set

  ! Check tune tracker lock. bit 13: 1 => lock

  do
    if (logic%debug) exit
    call get_tracker_lock_status (h_tracker_locked, v_tracker_locked)
    if (h_tracker_locked .and. v_tracker_locked) exit
    print *,        'ERROR: A TRACKER (OR TWO) IS NOT LOCKED'
    print '(a, $)', '       LOCK & HIT <CR> TO RECHECK, "E" TO EXIT: '
    accept '(a)', ans

    if (ans(1:1) == 'e' .or. ans(1:1) == 'E') then
      if (logic%command_file_open) then
        close (logic%iu_command_file)
        logic%command_file_open = .false.
      endif
      return
    endif

  enddo   

  ! if have lock then Qtune to correct frequency

  do  
    call get_freq (freq, 5)
    if (abs(freq%x%tune-freq%y%tune) > 0.2) exit
    print *,         'ERROR: TRACKERS LOCKED TO THE SAME FREQUENCY!'
    print '(a, $)',   '       DETANGLE & HIT <CR> TO RECHECK, "E" TO EXIT: '
    accept '(a)', ans

    if (ans(1:1) == 'e' .or. ans(1:1) == 'E') then
      call vxputn ('CSR SEXT CUR', i_sex, i_sex, sex_var%cu_saved) ! reset
      if (logic%command_file_open) then
        close (logic%iu_command_file)
        logic%command_file_open = .false.
      endif
      exit bump_loop
    endif

  enddo

  dfreq%x%tune = freq%x%tune - freq0%x%tune
  dfreq%y%tune = freq%y%tune - freq0%y%tune

  denom = dQx_qt5 * dQy_qt6 - dQy_qt5 * dQx_qt6
  d_cu5 = (-dQy_qt6 * dfreq%x%tune + dQx_qt6 * dfreq%y%tune) / denom
  d_cu6 =  (dQy_qt5 * dfreq%x%tune - dQx_qt5 * dfreq%y%tune) / denom

  if (.not. logic%debug) then
    call group_set ('CSR QTUNEING', 5, d_cu5, cu, dcu)
    if (dcu /= d_cu5) then
      print *, 'ERROR: CSR QTUNEING #5 WILL NOT SET!'
      call err_exit
    endif

    call group_set ('CSR QTUNEING', 6, d_cu6, cu, dcu)
    if (dcu /= d_cu6) then
      print *, 'ERROR: CSR QTUNEING #6 WILL NOT SET!'
      call err_exit
    endif
  endif
  
  print *, 'Tune changes to recenter:', dfreq%x%tune, dfreq%y%tune
  print *, 'Recentering Tune with d_QT5, d_QT6:', d_cu5, d_cu6

  ! make the measurements
                                                   
  print *, 'Bump is at:', cu_bump

  if (.not. logic%debug) then
    do
      call set_and_meas_freq ('CSR SEXT CUR', i_sex, sex_set1_cu, freq1, err_flag)
      if (err_flag) cycle
      call set_and_meas_freq ('CSR SEXT CUR', i_sex, sex_set2_cu, freq2, err_flag)
      if (err_flag) cycle
      call vxputn ('CSR SEXT CUR', i_sex, i_sex, sex_var%cu_saved) ! reset

      call bear_str (raw, '')               ! get orbit
      call butout (raw, n_butns)                  ! write orbit
      call number_to_file_name (n_butns, 'orbit', orb_file, file_num, err_flag)
      if (err_flag) exit bump_loop
      call read_butns_file (orb_file, butns, u%db, err_flag, .true., &
                      logic%nonlinear_calc, logic%offset_correction, logic%gain_correction)
      if (err_flag) exit bump_loop
      exit
    enddo
  endif
        
  ! write the results

  fmt = '(i8, 2f7.2, 2f8.2, 4f7.2)'
  print '(a)', trim(header1)
  print '(a)', trim(header2)
  print fmt, cu_bump, freq2%x%tune-freq1%x%tune, freq2%y%tune-freq1%y%tune, &
                freq1%x%tune, freq1%y%tune, &
                1000*butns%det(i_det1)%x_orb, 1000*butns%det(i_det1)%y_orb, &
                1000*butns%det(i_det2)%x_orb, 1000*butns%det(i_det2)%y_orb
  print *

  write (iu, fmt) cu_bump, freq2%x%tune-freq1%x%tune, freq2%y%tune-freq1%y%tune, &
                freq1%x%tune, freq1%y%tune, &
                1000*butns%det(i_det1)%x_orb, 1000*butns%det(i_det1)%y_orb, &
                1000*butns%det(i_det2)%x_orb, 1000*butns%det(i_det2)%y_orb

  ! Change the bump

  if (ib == n_bump_set) exit

  if (.not. logic%debug) then
    call group_set ('CSR HBUMPING', bump_num, del_cu_bump, cu_bump, real_del)
    if (real_del /= del_cu_bump) then
      print *, 'ERROR: BUMP AT LIMIT.'
      if (abs(real_del) < 0.4 * abs(del_cu_bump)) exit
    endif
  endif

enddo bump_loop

! reset tunes and bumps

if (.not. logic%debug) then
  call group_set ('CSR HBUMPING', bump_num, cu_bump0-cu_bump, cu_bump, real_del)
  call vxgetn ('CSR QTUNEING', 5, 5, cu)
  dcu = cu0_qt5 - cu
  call group_set ('CSR QTUNEING', 5, dcu, cu, dcu)
  call vxgetn ('CSR QTUNEING', 6, 6, cu)
  dcu = cu0_qt6 - cu
  call group_set ('CSR QTUNEING', 6, dcu, cu, dcu)
  print *, 'Reset bump to value of', cu_bump
endif
write (iu, *) '---------------------------------------------------------'

close (iu)

end subroutine

end module