!+
! Subroutine track1_custom (orbit, ele, param, err_flag, finished, track)
!
! Dummy routine for custom tracking. 
! If called, this routine will generate an error message and quit.
! This routine needs to be replaced for a custom calculation.
!
! Modules Needed:
!   use bmad
!
! Input:
!   orbit   -- Coord_struct: Starting position.
!   ele     -- Ele_struct: Element.
!   param   -- lat_param_struct: Lattice parameters.
!
! Output:
!   orbit   -- Coord_struct: End position.
!   param   -- lat_param_struct: Lattice parameters.
!     %lost   -- Logical. Set to true if a particle is lost.
!   track   -- track_struct, optional: Structure holding the track information if the 
!               tracking method does tracking step-by-step.
!-

subroutine track1_custom (orbit, ele, param, err_flag, finished, track)

  use bmad

  implicit none

  type (coord_struct) :: orbit, start
  type (ele_struct) , target :: ele
  type (ele_struct) , pointer :: ele2
  type (lat_param_struct) :: param
  type (track_struct), optional :: track

  integer ios, i, j, idx, ix

  real(rp) kickx,kicky,sfactor
  real(rp) s1, s2
  real(rp) ele_len, und_len, gfr

  logical err_flag, finished

!
  err_flag = .false.
  finished = .false.

!
  call set_custom_attribute_name('USE_FIELD_INT', err_flag, 3)

  start = orbit
  orbit%s = ele%s
  ele_len = value_of_attribute(ele,'L', err_flag)
  und_len = value_of_attribute(ele,'L', err_flag)

!read data for tracking   ele%r
  if(ele%slave_status .eq. super_slave$ .or. ele%slave_status .eq. slice_slave$)  then
     do i=1,ele%n_lord
        ele2 => pointer_to_lord(ele, i, ix_slave=ix)
        if(associated(ele2%descrip)) then
           und_len=value_of_attribute(ele2,'L',err_flag)    ! read in undulator length
           exit
        endif
     end do
  else
     ele2 => ele
  endif

  sfactor = c_light / ele2%value(E_TOT$)  

!Generate the kick data

! first half kick
  call offset_particle (ele, param, set$, orbit)  ! lab frame -> element frame
  
! set the boundaries
! Before Jan18,2013, the boundary was set to [-30,30]
! After set to [-22,20]
! For KYMA undulator, the boundary was set to [-35,35]

  if( start%vec(1)>35.1e-3 .or. start%vec(1)<-35.1e-3 .or. start%vec(3)>2.5e-3 .or. start%vec(3)<-2.5e-3 .or. value_of_attribute(ele,'USE_FIELD_INT',err_flag) .eq. 0.) then
	kickx=0
	kicky=0
  else 
 	call kick_integral(start%vec(1),start%vec(3),sfactor,kickx,kicky)
  end if

  orbit%vec(2) = orbit%vec(2) + kickx/2*ele_len/und_len ! scaled with undulator length
  orbit%vec(4) = orbit%vec(4) + kicky/2*ele_len/und_len

!  call offset_particle (ele, param, unset$, orbit)    ! element frame -> lab frame
 
! symplectic cal
  gfr = 35.e-3
  if(orbit%vec(1)<gfr .and. orbit%vec(1)>-gfr) then
     call symp_lie_bmad (ele, param, orbit, orbit, make_matrix=.false., track=track) 
     !call track1_taylor (orbit, ele, param, orbit) 
  end if

! second half kick

  call offset_particle (ele,param, set$, orbit)

  if( orbit%vec(1)>35e-3 .or. orbit%vec(1)<-35e-3 .or. orbit%vec(3)>2.5e-3 .or. orbit%vec(3)<-2.5e-3 .or. value_of_attribute(ele,'USE_FIELD_INT',err_flag) .eq. 0.) then        	
     kickx=0
     kicky=0
  else 
 	call kick_integral(orbit%vec(1),orbit%vec(3),sfactor,kickx,kicky)
  end if

  orbit%vec(2) = orbit%vec(2) + kickx/2*ele_len/und_len ! scaled with undulator length
  orbit%vec(4) = orbit%vec(4) + kicky/2*ele_len/und_len

  call offset_particle (ele,param, unset$, orbit)

end subroutine track1_custom


subroutine kick_integral(x,y,sfactor,kx,ky)

  use bmad
  implicit none
  real(rp) x,y,sfactor,kx,ky
  real(rp) Ix1,Ix2
  real(rp) Iy1,Iy2
  real(rp) a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15,a16,a17,a18,a19,a20,a21,a22,a23,a24,c
  real(rp) b1,b2,b3,b4,b5,b6,b7,b8,b9,b10,b11,b12,b13,b14,b15,b16,b17,b18,b19,b20,b21,b22,b23,b24 
  logical :: symp =.true.

! from fitting I1y
  c=1000
  a1 =      0.1725
  a2 =      0.3229 *c
  a3 =     -1.555
  a4 =      2.719
  a5 =      0.1806 *c
  a6 =      1.629 
  a7 =      3.735
  a8 =      0.6427 *c
  a9 =     -4.948 
  a10 =     2.557
  a11 =     0.1425*c
  a12 =     1.491
  a13 =     0.1582
  a14 =     0.4417*c
  a15 =    -0.1346
  a16 =     3.798
  a17 =     0.6415*c
  a18 =    -1.841
  a19 =     0.06116
  a20 =     0.9737*c
  a21 =     2.959
  a22 =     4.995
  a23 =     0.1606*c
  a24 =    -1.573

! from fitting I1x
  b1 =      0.4254 
  b2 =      0.5454 *c
  b3 =      -1.437 
  b4 =       108.5 
  b5 =      0.2973 *c
  b6 =       1.799
  b7 =      0.7358
  b8 =        0.24*c
  b9 =       1.553
  b10 =       0.134
  b11 =     0.04744*c
  b12 =      0.6705 
  b13 =      0.1718  
  b14 =      0.7918 *c
  b15 =       1.814 
  b16 =     0.04194 
  b17 =      0.1087 *c
  b18 =     -0.6761 
  b19 =      0.1124 
  b20 =      0.7055*c
  b21 =       1.037
  b22 =       108.8 
  b23 =       0.297*c
  b24 =      -1.346

! KYMA undulator field integrals on 9/8/2014

  if (.not. symp) then
     ! from fitting I1y
     Iy1=a1*sin(a2*x+a3)*cosh(a2*y)+a4*sin(a5*x+a6)*cosh(a5*y)+a7*sin(a8*x+a9)*cosh(a8*y)+ &
          a10*sin(a11*x+a12)*cosh(a11*y)+a13*sin(a14*x+a15)*cosh(a14*y)+a16*sin(a17*x+a18)*cosh(a17*y)+ &
          a19*sin(a20*x+a21)*cosh(a20*y)+a22*sin(a23*x+a24)*cosh(a23*y)

     Ix1=a1*cos(a2*x+a3)*sinh(a2*y)+a4*cos(a5*x+a6)*sinh(a5*y)+a7*cos(a8*x+a9)*sinh(a8*y)+ &
          a10*cos(a11*x+a12)*sinh(a11*y)+a13*cos(a14*x+a15)*sinh(a14*y)+a16*cos(a17*x+a18)*sinh(a17*y)+ &
          a19*cos(a20*x+a21)*sinh(a20*y)+a22*cos(a23*x+a24)*sinh(a23*y)
     
     Iy1=Iy1*1e-4
     Ix1=Ix1*1e-4
     

     ! from fitting I1x
     Ix2=b1*sin(b2*x+b3)*cosh(b2*y)+b4*sin(b5*x+b6)*cosh(b5*y)+b7*sin(b8*x+b9)*cosh(b8*y)+ &
          b10*sin(b11*x+b12)*cosh(b11*y)+b13*sin(b14*x+b15)*cosh(b14*y)+b16*sin(b17*x+b18)*cosh(b17*y)+ &
          b19*sin(b20*x+b21)*cosh(b20*y)+b22*sin(b23*x+b24)*cosh(b23*y)
     Iy2=b1*cos(b2*x+b3)*sinh(b2*y)+b4*cos(b5*x+b6)*sinh(b5*y)+b7*cos(b8*x+b9)*sinh(b8*y)+ &
          b10*cos(b11*x+b12)*sinh(b11*y)+b13*cos(b14*x+b15)*sinh(b14*y)+b16*cos(b17*x+b18)*sinh(b17*y)+ &
          b19*cos(b20*x+b21)*sinh(b20*y)+b22*cos(b23*x+b24)*sinh(b23*y)
     
     Ix2=Ix2*1e-4
     Iy2=Iy2*1e-4

     kx=(-Iy1+Iy2)*sfactor
     ky=(-Ix1+Ix2)*sfactor

  else

     kx=b13*sinh(b14*y)*cos(b14*x)*cos(b15)-b22*sinh(b23*y)*sin(b23*x)*sin(b24)-b16*sinh(b17*y)*sin(b17*x)*sin(b18)-b19*sinh(b20*y)*sin(b20*x)*sin(b21)+ &
          b7*sinh(b8*y)*cos(b8*x)*cos(b9)-b13*sinh(b14*y)*sin(b14*x)*sin(b15)+b16*sinh(b17*y)*cos(b17*x)*cos(b18)+ &
          b19*sinh(b20*y)*cos(b20*x)*cos(b21)+b4*sinh(b5*y)*cos(b5*x)*cos(b6)+b1*sinh(b2*y)*cos(b2*x)*cos(b3)-b10*sinh(b11*y)*sin(b11*x)*sin(b12)- &
          b7*sinh(b8*y)*sin(b8*x)*sin(b9)+b22*sinh(b23*y)*cos(b23*x)*cos(b24)+b10*sinh(b11*y)*cos(b11*x)*cos(b12)-b4*sinh(b5*y)*sin(b5*x)*sin(b6)- &
          a10*sin(a11*x)*cos(a12)-a22*sin(a23*x)*cos(a24)-a7*sin(a8*x)*cos(a9)-a16*sin(a17*x)*cos(a18)-a13*sin(a14*x)*cos(a15)- &
          a19*sin(a20*x)*cos(a21)-a1*sin(a2*x)*cos(a3)-a4*sin(a5*x)*cos(a6)-b1*sinh(b2*y)*sin(b2*x)*sin(b3)-a1*cos(a2*x)*sin(a3)- &
          a4*cos(a5*x)*sin(a6)-a7*cos(a8*x)*sin(a9)-a10*cos(a11*x)*sin(a12)-a13*cos(a14*x)*sin(a15)-a16*cos(a17*x)*sin(a18)-a19*cos(a20*x)*sin(a21)-a22*cos(a23*x)*sin(a24)

     ky=-a1*cos(a3)*sinh(a2*y)-a4*cos(a6)*sinh(a5*y)-a7*cos(a9)*sinh(a8*y)-a10*cos(a12)*sinh(a11*y)-a13*cos(a15)*sinh(a14*y)-a16*cos(a18)*sinh(a17*y)- &
          a19*cos(a21)*sinh(a20*y)-a22*cos(a24)*sinh(a23*y)+b1*cosh(b2*y)*sin(b2*x)*cos(b3)+b1*cosh(b2*y)*cos(b2*x)*sin(b3)+ &
          b4*cosh(b5*y)*sin(b5*x)*cos(b6)+b4*cosh(b5*y)*cos(b5*x)*sin(b6)+b7*cosh(b8*y)*sin(b8*x)*cos(b9)+b7*cosh(b8*y)*cos(b8*x)*sin(b9)+ &
          b10*cosh(b11*y)*sin(b11*x)*cos(b12)+b10*cosh(b11*y)*cos(b11*x)*sin(b12)+b13*cosh(b14*y)*sin(b14*x)*cos(b15)+b13*cosh(b14*y)*cos(b14*x)*sin(b15)+ &
          b16*cosh(b17*y)*sin(b17*x)*cos(b18)+b16*cosh(b17*y)*cos(b17*x)*sin(b18)+b19*cosh(b20*y)*sin(b20*x)*cos(b21)+ &
          b19*cosh(b20*y)*cos(b20*x)*sin(b21)+b22*cosh(b23*y)*sin(b23*x)*cos(b24)+b22*cosh(b23*y)*cos(b23*x)*sin(b24)

     kx=kx*1.e-4*sfactor
     ky=ky*1.e-4*sfactor

  endif

end subroutine kick_integral