!........................................................................
!
! Subroutine : SEXTUPOLE_RESONANCE_CALC (ELE, RES)
!
! Description: Subroutine to calculate the resonance strengths for a sextupole.
!              Note: The subroutine uses the Twiss parameters at the sextupole.
!
! Arguments  :   
!    Input:
!      ELE -- Ele_struct: Sextupole element. The sextupole must not be tilted.
!
!    Output:
!      RES -- Sex_sb_res_struct: Structure holding the resonance info.
!             See NONLIN.INC for more details       
!
! Mod/Commons:    
!
! Calls      :      
!
! Author     : 
!
! Modified   :     
!   
!
! error check
! Resonance terms
! a cos
! a sin
! b cos
! b sin
!          
!........................................................................
!
!
! $Log$
! Revision 1.7  2007/01/30 16:15:14  dcs
! merged with branch_bmad_1.
!
! Revision 1.3  2003/06/07 20:44:30  cesrulib
! conform to the lahey f95 standard
!
! Revision 1.2  2003/04/30 17:14:55  cesrulib
! dlr's changes since last import
!
! Revision 1.1.1.1  2002/12/13 19:23:30  cesrulib
! import bmadz
!
!
!........................................................................
!

subroutine sextupole_resonance_calc (ele, res)

  use bmad


  use nonlin_mod

  implicit none

  type (ele_struct) ele
  type (sextupole_res_struct) res

  real(rp) k11, k13, k14, k31, k32, k33, q14, q22, q24, q32, q42, q44, ex, ey
  real(rp) a_cos(-2:3, -2:3, -2:2), a_sin(-2:3, -2:3, -2:2)
  real(rp) b_cos(-2:3, -2:3, -2:2), b_sin(-2:3, -2:3, -2:2)
  real(rp) a_cos_100_300, a_cos_100_120, a_cos_100_102
  real(rp) b_cos_010_030, b_cos_010_210, b_cos_010_012
  real(rp) a_sin_100_300, a_sin_100_120, a_sin_100_102
  real(rp) b_sin_010_030, b_sin_010_210, b_sin_010_012
  real(rp) sqrt_beta_a, sqrt_beta_b, sqrt_beta_z
  real(rp) v_mat(4,4), v_inv_mat(4,4), g_mat(4,4), g_inv_mat(4,4)
  real(rp) vbar_mat(4,4), vbar_inv_mat(4,4)
  real(rp) eta_a_vec(4), eta_x_vec(4), cbar(2,2)
  real(rp) gamma_c, factor, phi_vec(3)
  real(rp) theta

  integer ia, ib, iz, ix, i

!

  res%n_res = 18

  res%a_map = 0
  res%a_map(1, 0, 0) = -1
  res%a_map(3, 0, 0) =  4
  res%a_map(2, 1, 0) =  5
  res%a_map(2,-1, 0) =  6
  res%a_map(2, 0, 1) =  7
  res%a_map(2, 0,-1) =  8
  res%a_map(1, 2, 0) =  9
  res%a_map(1,-2, 0) = 10
  res%a_map(1, 1, 1) = 11
  res%a_map(1, 1,-1) = 12
  res%a_map(1,-1, 1) = 13
  res%a_map(1,-1,-1) = 14
  res%a_map(1, 0, 2) = 15
  res%a_map(1, 0,-2) = 16
  res%a_map(0, 1, 0) = 17
  res%a_map(0, 0, 1) = 18

  res%b_map = 0
  res%b_map( 0, 1, 0) = -1
  res%b_map( 0, 3, 0) =  4
  res%b_map( 1, 2, 0) =  5
  res%b_map(-1, 2, 0) =  6
  res%b_map( 0, 2, 1) =  7
  res%b_map( 0, 2,-1) =  8
  res%b_map( 2, 1, 0) =  9
  res%b_map(-2, 1, 0) = 10
  res%b_map( 1, 1, 1) = 11
  res%b_map( 1, 1,-1) = 12
  res%b_map(-1, 1, 1) = 13
  res%b_map(-1, 1,-1) = 14
  res%b_map( 0, 1, 2) = 15
  res%b_map( 0, 1,-2) = 16
  res%b_map( 1, 0, 0) = 17
  res%b_map( 0, 0, 1) = 18


! error check

  if (ele%key /= sextupole$) then
    print *, 'ERROR IN SEXTUPOLE_RESONANCE_CALC: ELEMENT NOT A SEXTUPOLE'
    call err_exit
  endif

  if (ele%value(tilt$) /= 0) then
    print *, 'ERROR IN SEXTUPOLE_RESONANCE_CALC: SEXTUPOLE HAS A TILT.'
    print *, '      I DO NOT KNOW HOW TO HANDLE THIS!'
    print *, '      ', trim(ele%name), ele%value(tilt$)
    call err_exit
  endif

! Resonance terms

  call make_v_mats (ele, v_mat, v_inv_mat)
  call make_g_mats (ele, g_mat, g_inv_mat)
  vbar_mat = matmul(matmul (g_mat, v_mat), g_inv_mat)
  vbar_inv_mat = matmul(matmul (g_mat, v_inv_mat), g_inv_mat)

  sqrt_beta_a = sqrt(ele%a%beta)
  sqrt_beta_b = sqrt(ele%b%beta)
  sqrt_beta_z = sqrt(ele%z%beta)
  if (sqrt_beta_z == 0) sqrt_beta_z = 1

  eta_a_vec(1) = ele%a%eta
  eta_a_vec(2) = ele%a%etap
  eta_a_vec(3) = ele%b%eta
  eta_a_vec(4) = ele%b%etap

  eta_x_vec = matmul(v_mat, eta_a_vec) / sqrt_beta_z
  ex = eta_x_vec(1)
  ey = eta_x_vec(3)

  call c_to_cbar (ele, cbar)
  gamma_c = ele%gamma_c

  k11 = sqrt_beta_a * gamma_c
  k13 = sqrt_beta_a * cbar(1,1)
  k14 = sqrt_beta_a * cbar(1,2)
  k31 = sqrt_beta_b * cbar(2,2)
  k32 = sqrt_beta_b * cbar(1,2)
  k33 = sqrt_beta_b * gamma_c

  q14 = sqrt_beta_b * cbar(1,2)
  q22 = sqrt_beta_a * gamma_c
  q24 = sqrt_beta_b * cbar(2,2)
  q32 = sqrt_beta_a * cbar(1,2)
  q42 = sqrt_beta_a * cbar(1,1)
  q44 = sqrt_beta_b * gamma_c

! a cos

  a_cos_100_300 = 3*k11*k31*q14 - k31*k32*q22 - k11*k32*q24
  a_cos_100_102 = -2*ex*ey*q14
  a_cos_100_120 = -2*k13*k33*q14

  a_cos(3, 0, 0) =  k11*k31*q14 + k31*k32*q22 + k11*k32*q24
  a_cos(1,-2, 0) = -k13*k33*q14 - k13*k14*q22 + k14*k33*q24
  a_cos(2,-1, 0) =  k13*k31*q14 - k14*k32*q14 - k11*k33*q14 - &
                     k11*k14*q22 - k32*k33*q22 + k14*k31*q24 + k13*k32*q24
  a_cos(0, 1, 0) = 2*(k13*k31*q14 - k11*k33*q14 + k32*k33*q22 - k13*k32*q24)
  a_cos(2, 1, 0) =  k13*k31*q14 + k14*k32*q14 - k11*k33*q14 + &
                     k11*k14*q22 - k32*k33*q22 - k14*k31*q24 + k13*k32*q24
  a_cos(1, 2, 0) = -k13*k33*q14 + k13*k14*q22 + k14*k33*q24
  a_cos(1, 0,-2) =  ex*ey*q14
  a_cos(2, 0,-1) = -ex*k32*q14 - ex*k11*q22 - ey*k31*q22 - ey*k11*q24 + &
                     ex*k31*q24
  a_cos(1,-1,-1) =  ey*k14*q14 - ex*k13*q22 + ey*k33*q22 - ey*k13*q24 - &
                     ex*k33*q24
  a_cos(1, 1,-1) = -ey*k14*q14 - ex*k13*q22 + ey*k33*q22 - ey*k13*q24 - &
                     ex*k33*q24
  a_cos(2, 0, 1) = -a_cos(2,0,-1)
  a_cos(1,-1, 1) = -a_cos(1,1,-1)
  a_cos(1, 1, 1) = -a_cos(1,1,-1)
  a_cos(1, 0, 2) =  a_cos(1,0,-2)

! a sin

  a_sin_100_300 = k11*k32*q14 + k11*k11*q22/2 - k31*k31*q22/2 - &
                  3*k32*k32*q22/2 + (- k11*k31*q24)
  a_sin_100_120 = k13*k13*q22 + k14*k14*q22 - k33*k33*q22 + 2*k13*k33*q24
  a_sin_100_102 = ex*ex*q22 - ey*ey*q22 + 2*ex*ey*q24

  a_sin(3, 0, 0) =  k11*k32*q14 + k11*k11*q22/2 - k31*k31*q22/2 + &
                     k32*k32*q22/2 - k11*k31*q24
  a_sin(1,-2, 0) = -k14*k33*q14 + k13*k13*q22/2 - k14*k14*q22/2 - &
                     k33*k33*q22/2 + k13*k33*q24
  a_sin(2,-1, 0) =  k14*k31*q14 + k13*k32*q14 + k11*k13*q22 + k31*k33*q22 - &
                     k13*k31*q24 + k14*k32*q24 + k11*k33*q24
  a_sin(0, 1, 0) = -2*k14*k31*q14 + 2*k14*k32*q24
  a_sin(2, 1, 0) = -k14*k31*q14 + k13*k32*q14 + k11*k13*q22 + k31*k33*q22 - &
                     k13*k31*q24 - k14*k32*q24 + k11*k33*q24
  a_sin(1, 2, 0) =  k14*k33*q14 + k13*k13*q22/2 - k14*k14*q22/2 - &
                     k33*k33*q22/2 + k13*k33*q24
  a_sin(1, 0,-2) = -ex*ex*q22/2 + ey*ey*q22/2 - ex*ey*q24
  a_sin(2, 0,-1) = -ey*k11*q14 + ex*k31*q14 - ey*k32*q22 + ex*k32*q24
  a_sin(1,-1,-1) = -ey*k13*q14 - ex*k33*q14 - ex*k14*q22 - ey*k14*q24
  a_sin(1, 1,-1) = -ey*k13*q14 - ex*k33*q14 + ex*k14*q22 + ey*k14*q24
  a_sin(0, 0, 1) = 2*(ey*k11*q14 - ex*k31*q14 - ey*k32*q22 + ex*k32*q24)
  a_sin(2, 0, 1) = -a_sin(2,0,-1)
  a_sin(1,-1, 1) = -a_sin(1,-1,-1)
  a_sin(1, 1, 1) = -a_sin(1,1,-1)
  a_sin(1, 0, 2) =  a_sin(1,0,-2)

! b cos

  b_cos(1, 0, 0) = 2*k11*k13*q32 + 2*k31*k33*q32 - 2*k11*k14*q42 - &
                                       2*k14*k31*q44
  b_cos(-1, 2, 0) = k11*k13*q32 + k31*k33*q32 + k11*k14*q42 + k32*k33*q42 + &
                                       k14*k31*q44 + k13*k32*q44
  b_cos(-2,1,0) = k11**2*q32/2 - k31**2*q32/2 + k32**2*q32/2 - &
                                       k31*k32*q42 + k11*k32*q44

  b_cos_010_030 = 3*k13**2*q32/2 + k14**2*q32/2 - 3*k33**2*q32/2 - &
                                            k13*k14*q42 + k14*k33*q44
  b_cos_010_210 = k11**2*q32 - k31**2*q32 - k32**2*q32
  b_cos_010_012 = ex**2*q32 - ey**2*q32


  b_cos(0, 3, 0) = k13**2*q32/2 - k14**2*q32/2 - k33**2*q32/2 + k13*k14*q42 &
                                       - k14*k33*q44
  b_cos(2, 1, 0) = k11**2*q32/2 - k31**2*q32/2 + k32**2*q32/2 + k31*k32*q42 &
                                       - k11*k32*q44
  b_cos(1, 2, 0) = k11*k13*q32 + k31*k33*q32 + k11*k14*q42 - k32*k33*q42 + &
                                       k14*k31*q44 - k13*k32*q44
  b_cos(0, 1, -2) = -ex**2*q32/2 + ey**2*q32/2
  b_cos(-1, 1, 1) = -ey*k32*q32 + ex*k11*q42 + ey*k31*q42 - &
                                       ey*k11*q44 + ex*k31*q44
  b_cos(1, 1, -1) = -ey*k32*q32 - ex*k11*q42 - ey*k31*q42 + &
                                       ey*k11*q44 - ex*k31*q44
  b_cos(0, 2, -1) = ex*k14*q32 - ex*k13*q42 + ey*k33*q42 + &
                                       ey*k13*q44 +  ex*k33*q44
  b_cos(-1, 1, -1) = ey*k32*q32 - ex*k11*q42 - ey*k31*q42 + &
                                       ey*k11*q44 -  ex*k31*q44
  b_cos(1, 1, 1) = ey*k32*q32 + ex*k11*q42 + ey*k31*q42 - &
                                       ey*k11*q44 +  ex*k31*q44
  b_cos(0, 2, 1) = -ex*k14*q32 + ex*k13*q42 - ey*k33*q42 - &
                                       ey*k13*q44 -  ex*k33*q44
  b_cos(0, 1, 2) = -ex**2*q32/2 + ey**2*q32/2

! b sin

  b_sin(1, 0, 0) = 2*k32*k33*q32 - 2*k14*k32*q44
  b_sin(-1, 2, 0) = -(k11*k14*q32 + k32*k33*q32 - k11*k13*q42 - k31*k33*q42 - &
                      k13*k31*q44 + k14*k32*q44 + k11*k33*q44)
  b_sin(-2, 1, 0) = -(-k31*k32*q32 - k11**2*q42/2 + k31**2*q42/2 - &
                                       k32**2*q42/2 -  k11*k31*q44)

  b_sin_010_030 = -k13*k14*q32 + k13**2*q42/2 + 3*k14**2*q42/2 - &
                   k33**2*q42/2 - k13*k33*q44
  b_sin_010_210 = k11**2*q42 - k31**2*q42 - k32**2*q42 + 2*k11*k31*q44
  b_sin_010_012 = ex**2*q42 - ey**2*q42 - 2*ex*ey*q44

  b_sin(0, 3, 0) = -k13*k14*q32 + k13**2*q42/2 - k14**2*q42/2 - &
                                       k33**2*q42/2 - k13*k33*q44
  b_sin(2, 1, 0) = -k31*k32*q32 + k11**2*q42/2 - k31**2*q42/2 + &
                                       k32**2*q42/2 +  k11*k31*q44
  b_sin(1, 2, 0) = -k11*k14*q32 + k32*k33*q32 + k11*k13*q42 + k31*k33*q42 +  &
                                       k13*k31*q44 + k14*k32*q44 - k11*k33*q44
  b_sin(0, 1, -2) = -ex**2*q42/2 + ey**2*q42/2 + ex*ey*q44
  b_sin(-1, 1, 1) = -(ex*k11*q32 + ey*k31*q32 + ey*k32*q42 + ex*k32*q44)
  b_sin(1, 1, -1) = ex*k11*q32 + ey*k31*q32 - ey*k32*q42 - ex*k32*q44
  b_sin(0, 2, -1) = ex*k13*q32 - ey*k33*q32 + ex*k14*q42 - ey*k14*q44
  b_sin(0, 0, 1) = -2*ex*k13*q32 + 2*ey*k33*q32 + 2*ex*k14*q42 - &
                    2*ey*k14*q44
  b_sin(-1, 1, -1) = -(-ex*k11*q32 - ey*k31*q32 - ey*k32*q42 - ex*k32*q44)
  b_sin(1, 1, 1) = -ex*k11*q32 - ey*k31*q32 + ey*k32*q42 + ex*k32*q44
  b_sin(0, 2, 1) = -ex*k13*q32 + ey*k33*q32 - ex*k14*q42 + ey*k14*q44
  b_sin(0, 1, 2) = -ex**2*q42/2 + ey**2*q42/2 + ex*ey*q44

!

  factor = sqrt(2.0) / 2

  do ia = -2, 3
    do ib = -2, 3
      do iz = -2, 2
        ix = res%a_map(ia, ib, iz)
        if (ix > 0) then
          res%a(ix)%dj_cos = a_cos(ia, ib, iz) * factor
          res%a(ix)%dj_sin = a_sin(ia, ib, iz) * factor
          res%a(ix)%iq = (/ ia, ib, iz /)
          res%a(ix)%ij = abs(res%a(ix)%iq)
          if (sum(res%a(ix)%ij) == 1) res%a(ix)%ij(1) = 2
        endif
        ix = res%b_map(ia, ib, iz)
        if (ix > 0) then
          res%b(ix)%dj_cos = b_cos(ia, ib, iz) * factor
          res%b(ix)%dj_sin = b_sin(ia, ib, iz) * factor
          res%b(ix)%iq = (/ ia, ib, iz /)
          res%b(ix)%ij = abs(res%b(ix)%iq)
          if (sum(res%b(ix)%ij) == 1) res%b(ix)%ij(2) = 2
        endif
      enddo
    enddo
  enddo

  res%a(1)%dj_cos = a_cos_100_300 * factor
  res%a(1)%dj_sin = a_sin_100_300 * factor
  res%a(1)%iq  = (/ 1, 0, 0 /)
  res%a(1)%ij  = (/ 3, 0, 0 /)

  res%a(2)%dj_cos = a_cos_100_120 * factor
  res%a(2)%dj_sin = a_sin_100_120 * factor
  res%a(2)%iq  = (/ 1, 0, 0 /)
  res%a(2)%ij  = (/ 1, 2, 0 /)

  res%a(3)%dj_cos = a_cos_100_102 * factor
  res%a(3)%dj_sin = a_sin_100_102 * factor
  res%a(3)%iq  = (/ 1, 0, 0 /)
  res%a(3)%ij  = (/ 1, 0, 2 /)

  res%b(1)%dj_cos = b_cos_010_030 * factor
  res%b(1)%dj_sin = b_sin_010_030 * factor
  res%b(1)%iq  = (/ 0, 1, 0 /)
  res%b(1)%ij  = (/ 0, 3, 0 /)

  res%b(2)%dj_cos = b_cos_010_210 * factor
  res%b(2)%dj_sin = b_sin_010_210 * factor
  res%b(2)%iq  = (/ 0, 1, 0 /)
  res%b(2)%ij  = (/ 2, 1, 0 /)

  res%b(3)%dj_cos = b_cos_010_012 * factor
  res%b(3)%dj_sin = b_sin_010_012 * factor
  res%b(3)%iq  = (/ 0, 1, 0 /)
  res%b(3)%ij  = (/ 0, 1, 2 /)

  phi_vec = (/ ele%a%phi, ele%b%phi, ele%z%phi /)

  do i = 1, res%n_res

    res%a(i)%amp = sqrt(res%a(i)%dj_cos**2 + res%a(i)%dj_sin**2)
    theta = dot_product(res%a(i)%iq, phi_vec)
    res%a(i)%sin = res%a(i)%amp * sin(theta)
    res%a(i)%cos = res%a(i)%amp * cos(theta)

    res%b(i)%amp = sqrt(res%b(i)%dj_cos**2 + res%b(i)%dj_sin**2)
    theta = dot_product(res%b(i)%iq, phi_vec)
    res%b(i)%sin = res%b(i)%amp * sin(theta)
    res%b(i)%cos = res%b(i)%amp * cos(theta)

  end do

  if (ele%mode_flip) then
    res%a = res%b
    res%b = res%a
  else
    res%a = res%a
    res%b = res%b
  endif

  res%x_map = res%a_map
  res%y_map = res%b_map

  do i = 1, res%n_res
    res%x(i)%iq = res%a(i)%iq
    res%y(i)%iq = res%b(i)%iq
    res%x(i)%ij = res%a(i)%ij
    res%y(i)%ij = res%b(i)%ij
  enddo

end subroutine