! Subroutine to apply gains from bpm(:)%gain to the 
! phase and cbar matrix in ring%ele(bpm(:)%ix_lat);
! returns phase and cbar.

subroutine gain_cbar(ring, bpm, phi, cbar)
  use bmad
  use nonlin_bpm_mod
  use sim_bpm_mod
  use sim_ac_and_cbar_mod
  
  implicit none
  
  type(lat_struct), target :: ring
  type(ele_struct), pointer :: ele
  type(det_struct) bpm(:)
  real(rp) :: cbar_bmad(2,2)
  real(rp) :: beta_rel, Arel, Brel, phi_a_rel, phi_b_rel
  real(rp) :: rel_amp, rel_phase
  real(rp) :: phi_mat(2,2)
  real(rp) :: current = 1.e8, in_phase_amp = 1.e-5 ! arbitrary scale factors

  real(rp), allocatable :: delta_phi_ax(:), delta_phi_by(:)
  real(rp) :: dphi_ax_avg, dphi_by_avg
  
  ! amplitudes from Bmad-computed cbar matrix
  real(rp) :: A_total(3) = 0, Are(3) = 0, Aim(3) = 0
  real(rp) :: B_total(3) = 0, Bre(3) = 0, Bim(3) = 0
  
  ! button values from bmad-computed cbar matrix, 
  ! then passed through nonlin_bpm_interpolate and 
  ! affected by gains
  real(rp) :: d_are(4,0:2,0:2), d_aim(4,0:2,0:2)
  real(rp) :: d_bre(4,0:2,0:2), d_bim(4,0:2,0:2)
  
  ! for use with nonlin_bpm:
  type(cesr_det_plane_struct) :: horz(0:n_det_maxx), vert(0:n_det_maxx)
  !type (cesr_det_dc_position_struct) :: dc(0:n_det_maxx)
  real(rp) :: d(4,0:2,0:2), co(3), x(3)
  real(rp) :: Shorz(4,2), Svert(4,2)
  real(rp) :: Ax(2), Ay(2), Bx(2), By(2)

  ! data to be returned:
  real(rp), allocatable :: phi(:,:,:), cbar(:,:,:)

  integer :: ix, jx, ix_lat, ix_db, n_bpms
  
  n_bpms = size(bpm)

  x(1:2) = 0
  x(3)   = current

  if (.not. allocated(phi)) allocate(phi(size(bpm),2,2), cbar(size(bpm),2,2))
  
  ! only use nonlin map for BPM 1, to simplify things
  call nonlin_bpm_set_pointers(1)
    
  do ix = 1, size(bpm)
     if (bpm(ix)%ix_lat .eq. 0) cycle ! ignore elements that aren't populated

     ix_lat = bpm(ix)%ix_lat
     ix_db = bpm(ix)%ix_db
     
     ele => ring%ele(ix_lat)

     
     beta_rel = ele%a%beta / ele%b%beta
     
     call c_to_cbar(ele, cbar_bmad)
     
     phi_a_rel = atan2(cbar_bmad(1,2),cbar_bmad(2,2))
     phi_b_rel = atan2(cbar_bmad(1,2),cbar_bmad(1,1))
     
     ! Definitions: 
     ! phi(1,:) = phi_ax, phi_ay
     ! phi(2,:) = phi_bx, phi_by
     ! phi_a_rel = phi_ay - phi_ax
     ! phi_b_rel = phi_bx - phi_by
     
     phi_mat(1,1) = ele%a%phi
     phi_mat(1,2) = phi_a_rel + ele%a%phi
     phi_mat(2,1) = phi_b_rel + ele%b%phi
     phi_mat(2,2) = ele%b%phi
     
     ! define total amplitudes of a- and b-mode motion:
     A_total(1) = in_phase_amp
     A_total(2) = in_phase_amp * (-1./ele%gamma_c)*sqrt(1./beta_rel)*sqrt(cbar_bmad(1,2)**2 + cbar_bmad(2,2)**2)
     A_total(3) = current
     
     B_total(1) = in_phase_amp * ( 1./ele%gamma_c)*sqrt(   beta_rel)*sqrt(cbar_bmad(1,2)**2 + cbar_bmad(1,1)**2)
     B_total(2) = in_phase_amp
     B_total(3) = current
 
     Are(1) = A_total(1) * cos(phi_mat(1,1)) ! in-phase (real)
     Are(2) = A_total(2) * cos(phi_mat(1,2)) ! in-phase
     Are(3) = current
     
     Aim(1) = A_total(1) * sin(phi_mat(1,1)) ! out-of-phase (im)
     Aim(2) = A_total(2) * sin(phi_mat(1,2)) ! out-of-phase
     Aim(3) = current
     
     Bre(1) = B_total(1) * cos(phi_mat(2,1)) ! in-phase
     Bre(2) = B_total(2) * cos(phi_mat(2,2)) ! in-phase
     Bre(3) = current
     
     Bim(1) = B_total(1) * sin(phi_mat(2,1)) ! out-of-phase
     Bim(2) = B_total(2) * sin(phi_mat(2,2)) ! out-of-phase
     Bim(3) = current
     
     ! Now run each of these amplitude sets through nonlin_bpm_interpolate
     ! to generate simulated button signals
     
     call nonlin_bpm_interpolate(Are,d_are)
     call nonlin_bpm_interpolate(Aim,d_aim)
     call nonlin_bpm_interpolate(Bre,d_bre)
     call nonlin_bpm_interpolate(Bim,d_bim)
     
     
 
     ! Model this block of code off of nonlin_xy_shake_components
     ! Apply gain errors and load into S-structures for nonlin_phase
     do jx = 1, 4
        Shorz(jx,1) = bpm(ix)%gain(jx) * d_are(jx,0,0)
        Shorz(jx,2) = bpm(ix)%gain(jx) * d_aim(jx,0,0)
        Svert(jx,1) = bpm(ix)%gain(jx) * d_bre(jx,0,0)
        Svert(jx,2) = bpm(ix)%gain(jx) * d_bim(jx,0,0)
     enddo 

     ! again, only using nonlin map for BPM 1
     call nonlin_phase(1,Shorz,x,Ax,Ay)
     call nonlin_phase(1,Svert,x,Bx,By)
     
     ! now load back into horz, vert for final processing:
     horz(ix_db)%x%amp = sqrt(sum(Ax*Ax))
     horz(ix_db)%y%amp = sqrt(sum(Ay*Ay))

     vert(ix_db)%x%amp = sqrt(sum(Bx*Bx))
     vert(ix_db)%y%amp = sqrt(sum(By*By))

     horz(ix_db)%x%phase = atan2 (Ax(2),Ax(1)) * 180 / pi
     horz(ix_db)%y%phase = atan2 (Ay(2),Ay(1)) * 180 / pi

     vert(ix_db)%x%phase = atan2 (Bx(2),Bx(1)) * 180 / pi
     vert(ix_db)%y%phase = atan2 (By(2),By(1)) * 180 / pi

     if (horz(ix_db)%x%amp /= 0) then
        rel_amp = horz(ix_db)%y%amp / horz(ix_db)%x%amp
        rel_phase = (horz(ix_db)%y%phase - horz(ix_db)%x%phase)*pi/180.
        horz(ix_db)%cbar22 = -rel_amp * cos(rel_phase)
        horz(ix_db)%cbar12 = rel_amp * sin(rel_phase)
     else
        horz(ix_db)%cbar22 = 0
        horz(ix_db)%cbar12 = 0
     endif
     
     if (vert(ix_db)%y%amp /= 0) then
        rel_amp = vert(ix_db)%x%amp / vert(ix_db)%y%amp
        rel_phase = (vert(ix_db)%x%phase - vert(ix_db)%y%phase)*pi/180.
        vert(ix_db)%cbar11 = rel_amp * cos(rel_phase)
        vert(ix_db)%cbar12 = rel_amp * sin(rel_phase)
     else
        vert(ix_db)%cbar11 = 0
        vert(ix_db)%cbar12 = 0
     endif
     
     vert(ix_db)%cbar12 = vert(ix_db)%cbar12 / sqrt(ring%ele(ix_lat)%a%beta / ring%ele(ix_lat)%b%beta)
     horz(ix_db)%cbar22 = horz(ix_db)%cbar22 * sqrt(ring%ele(ix_lat)%a%beta / ring%ele(ix_lat)%b%beta)
     vert(ix_db)%cbar11 = vert(ix_db)%cbar11 / sqrt(ring%ele(ix_lat)%a%beta / ring%ele(ix_lat)%b%beta)
     
     ! transfer to final matrix, for return
     cbar(ix,1,2) = vert(ix_db)%cbar12 
     cbar(ix,2,2) = horz(ix_db)%cbar22 
     cbar(ix,1,1) = vert(ix_db)%cbar11 

     ! straight from cesrv...
     call button_phase (horz(ix_db), 0.0_rp, x_plane$)
     call button_phase (vert(ix_db), 0.0_rp, y_plane$)
 

  ! correct the phase advance for when atan2 'wraps around' from pi --> -pi
     horz(ix_db)%phase_design = ring%ele(ix_lat)%a%phi * 180./pi
     vert(ix_db)%phase_design = ring%ele(ix_lat)%b%phi * 180./pi
     horz(ix_db)%phase_meas   = horz(ix_db)%x%phase 
     vert(ix_db)%phase_meas   = vert(ix_db)%y%phase 
     
     horz(ix_db)%ok = .true.
     vert(ix_db)%ok = .true.

  enddo ! loop over BPMs

  call phase_shift_near_design(horz)
  call phase_shift_near_design(vert)


  phi(:,1,1) = horz(bpm(:)%ix_db)%phase_meas * pi/180.
  phi(:,1,2) = horz(bpm(:)%ix_db)%y%phase    * pi/180.
  phi(:,2,1) = vert(bpm(:)%ix_db)%x%phase    * pi/180.
  phi(:,2,2) = vert(bpm(:)%ix_db)%phase_meas * pi/180.

  if (.not. allocated(delta_phi_ax)) allocate(delta_phi_ax(n_bpms),delta_phi_by(n_bpms))
  delta_phi_ax(:) = 0
  delta_phi_by(:) = 0
      
  delta_phi_ax(:) = ring%ele(bpm(:)%ix_lat)%a%phi - phi(:,1,1)
  delta_phi_by(:) = ring%ele(bpm(:)%ix_lat)%b%phi - phi(:,2,2)
        
  dphi_ax_avg = (sum(delta_phi_ax(:))) / (size(delta_phi_ax))
  dphi_by_avg = (sum(delta_phi_by(:))) / (size(delta_phi_by))

  phi(:,1,1)  = phi(:,1,1) + dphi_ax_avg
  phi(:,2,2)  = phi(:,2,2) + dphi_by_avg

       
  end subroutine gain_cbar