!+
! 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, except_dummy => track1_custom

  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

  if (trim(ele2%descrip) .ne. 'ccw') then
     if(.not. associated(ele2%r)) then
        allocate(ele2%r(32,3,1))
        if (.not. associated(ele2%descrip)) then
           print *, 'ele%descrip not associated: ', ele%name
           call err_exit
        endif
        open(unit=2, file=ele2%descrip, status = 'old', iostat = ios)
        if (ios == 0) then
           do i=1,32
              read(2,*) (ele2%r(i,j,1), j=1,3)
           end do
        else
           print *, 'ERROR: CANNOT OPEN FILE: ', trim(ele2%descrip)
        endif
        close (unit=2)
        ele2%r(:,1,1)=ele2%r(:,1,1)*1.0e-3
        ele2%r(:,2,1)=ele2%r(:,2,1)*1.0e-4*(-0.06)  !convert the field integral to kick (kickx=-0.06*Iy)
        ele2%r(:,3,1)=ele2%r(:,3,1)*1.0e-4*0.06     !convert the field integral to kick (kicky=0.06*Ix)
     end if
     sfactor=ele2%r(32,1,1)*1e3                     !kick conversion factor
     und_len=ele2%r(32,2,1)*1e4/(-0.06)            !Another method to read in KYMA undlator length
  endif

!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, .false., 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)
  endif

  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

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


  Iy1=a1*sin(a2*1e3*x+a3)*cosh(a2*1e3*y)+a4*sin(a5*1e3*x+a6)*cosh(a5*1e3*y)+a7*sin(a8*1e3*x+a9)*cosh(a8*1e3*y)+ &
           a10*sin(a11*1e3*x+a12)*cosh(a11*1e3*y)+a13*sin(a14*1e3*x+a15)*cosh(a14*1e3*y)+a16*sin(a17*1e3*x+a18)*cosh(a17*1e3*y)+ &
           a19*sin(a20*1e3*x+a21)*cosh(a20*1e3*y)+a22*sin(a23*1e3*x+a24)*cosh(a23*1e3*y)

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

  Iy1=Iy1*1e-4
  Ix1=Ix1*1e-4


! from fitting I1x
 
  a1 =      0.4254 
  a2 =      0.5454 
  a3 =      -1.437 
  a4 =       108.5 
  a5 =      0.2973 
  a6 =       1.799
  a7 =      0.7358
  a8 =        0.24
  a9 =       1.553
  a10 =       0.134
  a11 =     0.04744
  a12 =      0.6705 
  a13 =      0.1718  
  a14 =      0.7918 
  a15 =       1.814 
  a16 =     0.04194 
  a17 =      0.1087 
  a18 =     -0.6761 
  a19 =      0.1124 
  a20 =      0.7055
  a21 =       1.037
  a22 =       108.8 
  a23 =       0.297
  a24 =      -1.346


  Ix2=a1*sin(a2*1e3*x+a3)*cosh(a2*1e3*y)+a4*sin(a5*1e3*x+a6)*cosh(a5*1e3*y)+a7*sin(a8*1e3*x+a9)*cosh(a8*1e3*y)+ &
           a10*sin(a11*1e3*x+a12)*cosh(a11*1e3*y)+a13*sin(a14*1e3*x+a15)*cosh(a14*1e3*y)+a16*sin(a17*1e3*x+a18)*cosh(a17*1e3*y)+ &
           a19*sin(a20*1e3*x+a21)*cosh(a20*1e3*y)+a22*sin(a23*1e3*x+a24)*cosh(a23*1e3*y)
  Iy2=a1*cos(a2*1e3*x+a3)*sinh(a2*1e3*y)+a4*cos(a5*1e3*x+a6)*sinh(a5*1e3*y)+a7*cos(a8*1e3*x+a9)*sinh(a8*1e3*y)+ &
           a10*cos(a11*1e3*x+a12)*sinh(a11*1e3*y)+a13*cos(a14*1e3*x+a15)*sinh(a14*1e3*y)+a16*cos(a17*1e3*x+a18)*sinh(a17*1e3*y)+ &
           a19*cos(a20*1e3*x+a21)*sinh(a20*1e3*y)+a22*cos(a23*1e3*x+a24)*sinh(a23*1e3*y)

  Ix2=Ix2*1e-4
  Iy2=Iy2*1e-4

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

end subroutine kick_integral