! subroutine get_g2_fields(x,B,b1,b2, out_of_range)
! 
! Routine to get magnetic field value of fringe field and inflector field maps
!
! Modules needed:
!  use magfield
!  use magfield_interface
!  use parameters_bmad
!
! Input:
!   x -- Real: Dimension(3), coordinates of trajectory in cm with respect to the axis through the center of the old inflector r=718.9 cm, z=0.
!
!  Output:
!   B -- Real: Dimension(3), magnetc field at x
!   b1 -- Real: Dim(3,3), gradient of each component of magnetic field at x
!   b2 -- Real: Dim(3,3), second derivative of each component of magnetic field at x, (no cross terms)
!   out_of_range: Logical, default false, if true x is out of range of the map
!  
 subroutine get_g2_fields(x,B,b1,b2, out_of_range)
   use magfield
   use magfield_interface, dummy => get_g2_fields
   use parameters_bmad
   implicit none


 logical first/.true./
 logical out_of_range

   type(magfield_struct), allocatable,save :: map_inj(:,:,:),map_inf(:,:,:)
   type(magfield_struct), allocatable,save :: map(:,:,:)
   type(magfield_struct), save :: xmin_inj, xmin_inf, xmin


   real(rp) x(3), B(3) ,b1(3,3),b2(3,3)
   real(rp) B_inj(3) ,b1_inj(3,3),b2_inj(3,3),B_inf(3) ,b1_inf(3,3),b2_inf(3,3)
   real(rp) y(3)
   real(rp) deltax(3)
   real(rp) theta   ! /0.00235/  !theta = map_tilt, map_tilt is in parameters_bmad
   real(rp) zlength/0./
   real(rp), save :: cost,sint
   real(rp) xoffset/718.9/, yoffset/0.0/, zoffset/-430./ !xoffset [cm] of centerline of old inflector, zoffset to end of inflector
   integer ind(3)
   integer multiplier, file_index, fringe/1/, inflector/2/

B_inj = 0.
B_inf = 0.
b1_inj = 0.
b1_inf = 0.
b2_inj = 0.
b2_inf = 0.

 if(first)then
  if(index(field_file(1)%name,'empty')/=0 .or. index(field_file(2)%name,'empty')/=0)then
     print '(a)',' You cannot initialize get_g2_field with the <empty> file'
     stop
  endif
!  print '(2a)',' get_g2_fields: field_file(1)%name = ', field_file(1)%name
   call read_field(fringe,map_inj,xmin_inj)
   call read_field(inflector,map_inf,xmin_inf)
   call compute_derivatives(fringe,map_inj)
   call compute_derivatives(inflector,map_inf)
   theta = map_tilt - tilt
   cost=cos(theta)
   sint=sin(theta)
print *,'xmin_inf =',xmin_inf%x
print *,'xmin_inj =',xmin_inj%x
print *,'xmin_inj-xmin_inf =',xmin_inj%x-xmin_inf%x
print '(a,3es12.4)',' map_inj(1,1,1)%x = ', map_inj(1,1,1)%x
print '(a,3es12.4)',' map_inf(1,1,1)%x = ', map_inf(1,1,1)%x
print '(a,3es12.4)',' map_tilt, tilt, theta',map_tilt,tilt,theta
print '(a,es12.5,a,es12.5)',' Inflector Peak By field = ', inflector_field * Bmagic/1.4698,' Bmagic = ',Bmagic
   first = .false.
 endif


!
! This is how index ind(1:3) is defined when the map is read. 
!    ind(1:3) = (x0%x(1:3) - xmin%x(1:3))*multiplier + 1
! and this is the offset added in em_field_custom_inj. That is it assumes that the zero of the map is 710.
!   x(1) =8.9 ! boundary of inj map is 710. Centerline of inflector is 711.2 + 7.7. 718.9-710. = 8.9 
! So what we need to to is 
!  x(1) -> x(1) + 710. That is translate to map coordinate system.
!  then compute ind(1) = (x(1)-xmin%x(1)*multiplier +1 


  x(1) = x(1) + xoffset !xoffset = 718.9cm radius of center of old inflector
  x(3) = x(3) + zoffset

out_of_range = .true.

! get field from fringe field map
if(index(field_file(fringe)%name,'none') == 0)then 
 y=x
  if(field_file(fringe)%type == 3)then  ! extend 2D map to 3 dimensions
     y(1) = sqrt((zlength-x(3))**2+ x(1)**2)
     y(2) = x(2)
     y(3) = 0.
  endif

     call get_field(fringe,y,map_inj,B_inj,b1_inj,b2_inj,out_of_range)


endif

!get field from inflector map. Wuzeng inflector map is tilted cc about the downstream end by 2.35mrad = map_tilt. Total angle is map_tilt + tilt 
!  where tilt is however much we tilt in input file.
if(index(field_file(inflector)%name,'none') == 0 )then
 y=x

if(index(field_file(inflector)%name,'inf_field_alone')/= 0)then
! -x(3) is the distance from the exit end of the inflector
  deltax(1) =   (x(1)-xoffset)*cost + x(3)*sint  -(x(1)-xoffset)
!  deltax(1) =    x(3)*sint 
  deltax(2) = 0.
  deltax(3) =  -(x(1)-xoffset)*sint + x(3)*cost  - x(3) 
  y=x + deltax
! print '(a,3es12.4,a,3es12.4)',' x = ', x, ' deltax = ', deltax
endif

 if( abs(inflector_width-0.009) <= 0.0001 .and. field_file(inflector)%type /= 4) &
       call get_field(inflector,y,map_inf,B_inf,b1_inf,b2_inf,out_of_range) !use wuzeng field map
 if(field_file(inflector)%type == 4 .and. zlength-x(3) < 170.)then
     B_inf(2) =   14698. - 1.e4*field_file(inflector)%dGdx * (x(1)-xoffset) ! is the field of Wuzengs inflector map, alternatively  Bmagic * 1.e4, Bmagic = 1.45130, Bmagic/Wuzeng = 0.987413  
     B_inf(1) =   1.e4*field_file(inflector)%dGdx * x(2) ! is the field of Wuzengs inflector map, alternatively  Bmagic * 1.e4, Bmagic = 1.45130, Bmagic/Wuzeng = 0.987413  
 endif  
endif

! add fields from injection channel and inflector

  B = B_inj+B_inf * inflector_field * Bmagic/1.4698 !inflector field is a number near 1. If inflector_field=1, B_inf = B_wuzeng.
  b1=b1_inj+b1_inf * inflector_field * Bmagic/1.4698
  b2=b2_inj+b2_inf * inflector_field * Bmagic/1.4698

return
end subroutine get_g2_fields

! subroutine get_field(fringe_or_inflector,y,map,B,b1,b2,out_of_range)
!
! Input,   y(3) -- Real: position - map coordinates
!          file_index -- Integer", 1= fringe field, 2=inflector field
!          map -- magfield_struct: field map
! Output   B,b1,b2 -- Real, dimension(3): magnetic field at y, and first and second derivatives at nearest map grid point 
!          out_of_range -- logical: default false, true if y is out of range of the map
!                
subroutine get_field(fringe_or_inflector,y,map,B,b1,b2,out_of_range)

   use magfield
   use parameters_bmad
   implicit none

   logical out_of_range
   type(magfield_struct), allocatable :: map(:,:,:)
   real(rp) x(3), B(3) ,b1(3,3),b2(3,3)
   real(rp) y(3), xgrid_point(3)
   integer ind(3),i
   integer multiplier, fringe_or_inflector

 x=y
! determine map index
 multiplier = 1./field_file(fringe_or_inflector)%grid_spacing
 if(field_file(fringe_or_inflector)%type == 1 .or. field_file(fringe_or_inflector)%type == 2)y(2) = abs(y(2)) ! symmetric about x-z plane
! if(field_file(fringe_or_inflector)%type == 3) ind(1:3) = y(1:3)*multiplier + 1                                  ! 2D
 do i=1,3
 ind(i) = (y(i) - map(1,1,1)%x(i))*multiplier + 1       !ind(1:3) = (abs(y(1:3)))*multiplier+1   
 end do

 B=0.

 if(all(ind(1:3) > 0) .and. ind(1) .le. size(map,1) .and. ind(2) .le. size(map,2) .and. ind(3) .le. size(map,3))then
   B(1:3) = map(ind(1),ind(2),ind(3))%B(1:3)
   b1(1:3,1:3) = map(ind(1),ind(2),ind(3))%dB(1:3,1:3)
   b2(1:3,1:3) = map(ind(1),ind(2),ind(3))%d2B(1:3,1:3)
   out_of_range = .false.

  xgrid_point(1:3) = map(ind(1),ind(2),ind(3))%x(1:3)
  call interpolate_field(fringe_or_inflector,y,xgrid_point,B,b1,b2)

 endif

! if at the boundary of the map assume the value at the edge
   if(ind(3) > 0 .and. ind(3) <= size(map,3))then

     if(ind(1) * ind(2) < 0)then
      if(ind(1) < 0)    B(1:3) = map(1,ind(2),ind(3))%B(1:3) 
      if(ind(2) < 0)    B(1:3) = map(ind(1),2,ind(3))%B(1:3) 
      out_of_range = .false.
     endif

     if(ind(1) > size(map,1) .and. ind(2) < size(map,2) ) B(1:3) = map(size(map,1),ind(2),ind(3))%B(1:3) 
     if(ind(1) < size(map,1).and. ind(2) > size(map,2))B(1:3) = map(ind(1),size(map,2),ind(3))%B(1:3) 
     out_of_range = .false.
   endif
if(y(3) < -420. .and. B(2)< -6000)print '(a,1x,3es12.4,a,1x,3i10,1x,a,3es12.4,1x,a,3i10)','y',  y, ' ind =', ind, 'B = ',B,' size(map,1),size(map,2),size(map,3) ',size(map,1),size(map,2),size(map,3)
! if type 1 or 2 symmetric about y=0
if(field_file(fringe_or_inflector)%type == 1 .or. field_file(fringe_or_inflector)%type == 2)then
  if(x(2)<0)then
    B(1) = -B(1)
  endif
endif


return
end subroutine get_field

 subroutine Bfield_along_axis
   use precision_def
   use magfield
   use magfield_interface
   use parameters_bmad
   implicit none

 logical out_of_range
   real(rp) x(3), B(3) ,b1(3,3),b2(3,3)
   real(rp) z, deltaz, deltax, deltay
   real(rp) x0,xx
   integer lun
   character*100 filename(2)

  filename(1:2) = field_file(1:2)%name
  field_file(1)%name='empty' 
  field_file(2)%name='empty'
   
!  field_file_inj='none' !set field_file_inj = 'none' so that only inflector field will be written

  deltax = 0.1
  deltay=0.1
  deltaz=0.001
  lun = lunget()
  open(unit=lun,file=trim(directory)//'/'//'Bfield_along_injection_axis.dat')
  write(lun,'(a1,1x,6a12)')'#','x','y','z','Bx','By','Bz'
   x(2) = 0.0
   x(1) = 0.0 ! with respect to reference orbit. Offset added in get_g2_fields
   x0=0.
   xx=x0 -2.1 !-8.9-deltax +0.05

!do while (xx <= x0+2.0)
! xx = xx + deltax
 x(1) =x0
 write(lun,'(/,a1,a7,es12.4,/)')'#','x(1) = ',x(1)

  z=0.
  do while(z<4.5)
   x(1) = 0.
   x(2) = 1.
   z=z+deltaz
   x(3)=z*100
   call get_g2_fields(x,B,b1,b2, out_of_range)
   write(lun,'(3es12.4,1x,3es12.4)')x(1:3),B(1:3)
  end do
! end do

  close(unit=lun)
  field_file(1:2)%name = filename(1:2)
  print '(2a)',field_file(1:2)%name
 return
end