subroutine track_through_inj_line
 use bmad
 implicit none
 real(rp) x(3), B(3),z, b1(3,3),b2(3,3),y
 real(rp) xdum 
 character*120 file, file_inf
 integer iz, iy,ix
   real(rp) xoffset/0.085/, yoffset/0.015/ 
 logical out_of_range

 file = "/Users/dlr10/development/bmad_dist/g-2/magneticfield/injec_fld.dat"
 file_inf = "/Users/dlr10/development/bmad_dist/g-2/magneticfield/inf_field_alone.dat"
! file = "/home/dlr/development9_linux/g2_magneticfield/injec_fld.dat"
! file_inf = "/home/dlr/development9_linux/g2_magneticfield/inf_field_alone.dat"


write(11,'(/,a,i4,/)')'#' , ix
do iz = 1, 400000

  z = iz*0.1 + 0.
  x(1:3)=0
  x(1) = 8.5
  x(2) = 1.5
  x(3)=z 

!  print *,file, x,B

  call get_g2_fields(file,file_inf, x, B, b1,b2, out_of_range)
  if(out_of_range)then
!   print *,' Outside of field volume '
   cycle
  else
   write(11, '(3es12.4,6x,3es12.4)')x,B

  endif
 end do

 return
 end subroutine

  subroutine em_field_custom(ele, param, s_rel,t_rel, orb, local_ref,field,calcd, err_flag)

use bmad_struct
use bmad_interface, except_dummy => em_field_custom

   implicit none
   type(ele_struct) :: ele
   type(lat_param_struct) param
   type(coord_struct) orb
   type(em_field_struct) field
   real(rp), intent(in) :: s_rel, t_rel
   real(rp) xx(3), BB(3),zz, b1(3,3),b2(3,3)
   real(rp) xoffset/0.085/, yoffset/0.015/ !   real(rp) xoffset/0.05/, yoffset/0.0125/
   logical  out_of_range,local_ref
   logical, optional :: calcd, err_flag
   character(32) :: r_name = 'em_field_custom'
   character*120 file_inj, file_inf
   file_inj = "/Users/dlr10/lepp/g-2/magneticfield/injec_fld.dat"
   file_inf = "/Users/dlr10/lepp/g-2/magneticfield/inf_field_alone.dat"
! file_inj = "/home/dlr/development9_linux/g2_magneticfield/injec_fld.dat"
! file_inf = "/home/dlr/development9_linux/g2_magneticfield/inf_field_alone.dat"

!   if(present(err_flag))err_flag = .false.
!   if(present(calcd))calcd = .false.
 ! meters to cm
   xx(1) = (orb%vec(1)+xoffset)*100. 
   xx(2) = (orb%vec(3)+yoffset)*100. 
   xx(3) = s_rel*100.                
   call get_g2_fields(file_inj, file_inf,xx, BB,b1,b2,out_of_range)
   field%B(1:3) = BB(1:3) * 1.e-4  ! gauss to tesla

  write(12, '(2(a,3(ES14.6)))')' xx ', xx, ' field%B', field%B

   return
   end subroutine
   
   
  subroutine interpolate_field(x,B,b1,b2)
   use bmad
   implicit none
   real(rp) x(3), B(3),z, b1(3,3),b2(3,3)
   real(rp) xgrid(3), dx(3)
   integer ind(3),i

   ind(1:3) = (x(1:3))*2+1
   xgrid(1:3) = (ind(1:3)-1)*0.5
   dx(1:3) = x(1:3) - xgrid(1:3)
!  print '(a,6es12.4)',' dx, B ', dx,B
   forall(i=1:3) B(i) = B(i) + dot_product(b1(i,:),dx(:))    
!  print '(a,3es12.4)', 'B(1:3) ', B(1:3)
   return
  end subroutine


! READ magnetic field file

 subroutine get_g2_fields(field_file_inj, field_file_inf, &
                                         x,B,b1,b2, out_of_range)
   use magfield
   use magfield_interface
   implicit none


 logical first/.true./
 logical out_of_range
 character(*) field_file_inj, field_file_inf

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

   real(rp) x(3), B(3) ,b1(3,3),b2(3,3)
   integer ind(3)

 if(first)then
   call read_field(field_file_inj,map_inj,xmin_inj)
   call read_field(field_file_inf,map_inf,xmin_inf)
   call sum_fields(map_inj,map_inf, xmin_inj, xmin_inf, map, xmin)
   call compute_derivatives(map)
   first = .false.
 endif

 ind(1:3) = (x(1:3))*2+1
 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.

  call interpolate_field(x,B,b1,b2)

 else
   out_of_range  =.true.
 endif

return
end subroutine

subroutine read_field(field_file, map,xmin)

   use magfield
   use magfield_interface

   implicit none
   type(magfield_struct), allocatable :: position(:), map(:,:,:), temp_map(:,:,:)
   type(magfield_struct) x0, xmax, xmin, dxyz(-1:1)
   integer iu, istat/0/, nlines, ind(3),i,j,k,l, ntot(3), ix,jx
   integer k1, k2
   integer delta
   character(*)  field_file
   character*140 string
   real(rp)  b1(3), b2(3)

   iu = lunget()
   open(unit=iu, file = field_file, status = 'old', action='read')
   nlines = 0
    xmin%x=1000.
   xmax%x=-1000.
   do while(.true.)
      string(1:140) = ' '
      read(iu, '(a140)', IOSTAT = istat ) string
      if(istat < 0) exit
     if(index(string,'#') /= 0)cycle
     if(string(1:10) == '          ')cycle
     if(index(field_file,'inj')/=0)read(string, *)x0%x(1:3), x0%B(1:3), x0%H
     if(index(field_file,'inj')==0)read(string, *)x0%x(1:3), x0%B(1:3)
     xmin%x(1:3) = min(xmin%x(1:3),x0%x(1:3))
     xmax%x(1:3) = max(xmax%x(1:3),x0%x(1:3))

     nlines = nlines + 1
   end do
   ntot(1:3) = (xmax%x(1:3)-xmin%x(1:3))*2 + 1

   allocate(map(ntot(1),ntot(2),ntot(3)))
   allocate(temp_map(ntot(1),ntot(2),ntot(3)))
   rewind(unit=iu)

   do while(.true.)
      string(1:140) = '                                                                                                                                                                       '
     read(iu,'(a140)',IOSTAT = istat) string
     if(istat < 0)exit
     if(index(string,'#') /= 0)cycle
     if(string(1:10) == '          ')cycle
     if(index(field_file,'inj')/=0)read(string, *)x0%x(1:3), x0%B(1:3), x0%H
     if(index(field_file,'inj')==0)read(string, *)x0%x(1:3), x0%B(1:3)

    ind(1:3) = (x0%x(1:3) - xmin%x(1:3))*2 + 1
    map(ind(1),ind(2),ind(3))%B(1:3) = x0%B(1:3)
    map(ind(1),ind(2),ind(3))%x(1:3) = x0%x(1:3)
  end do
! correct the discontinuity at z=259.5. Set the field at this point to be average of 259 and 260

  k1 = 259.5*2+1
  k2 = 430.5*2+1
  do i = 1,ntot(1)
   do j = 1,ntot(2)
!    print *,'map(i,j,k)%x(3)',map(i,j,k1)%x(3), map(i,j,k2)%x(3)
    map(i,j,k1)%B(1:3) = (map(i,j,k1-1)%B(1:3) + map(i,j,k1+1)%B(1:3))*0.5
    map(i,j,k2)%B(1:3) = (map(i,j,k2-1)%B(1:3) + map(i,j,k2+1)%B(1:3))*0.5
   delta = 2
   end do
 end do

return
end

subroutine compute_derivatives(map)

   use magfield
   use magfield_interface

   implicit none
   type(magfield_struct), allocatable :: map(:,:,:)
   type(magfield_struct) x0, xmax, xmin, dxyz(-1:1)
   integer i,j,k,l, ntot(3), ix,jx
   real(rp)  b1(3), b2(3)


! next compute dB(1:3)/dxyz(1:3)

!print *
ntot(1) = size(map,1)
ntot(2) = size(map,2)
ntot(3) = size(map,3)

print '(a,3i4)',' ntot ', ntot(1:3)
do j=1,ntot(1)
  do k=1,ntot(2)
    do l=1,ntot(3)
     do ix=1,3
      do jx=-1,1,1

        if(ix == 1 .and. j+jx >= 1 .and. j+jx <= ntot(ix))then
          dxyz(jx)%B(1:3) = map(j+jx,k,l)%B(1:3)
         elseif(ix == 2 .and. k+jx >= 1 .and. k+jx <= ntot(ix))then
          dxyz(jx)%B(1:3) = map(j,k+jx,l)%B(1:3)
         elseif(ix == 3 .and. l+jx >= 1 .and. l+jx <= ntot(ix))then
          dxyz(jx)%B(1:3) = map(j,k,l+jx)%B(1:3)
         else
          cycle
        endif
      end do


!      print '(a,i,3es12.4)','2  ix, dxyz', ix,  dxyz(-1:1)%B(2)
      call quadratic_fit(dxyz, b1,b2)
!      print '(a,3es12.4)','b1(1:3) ', b1
      map(j,k,l)%dB(1:3,ix) = b1(1:3) !first derivative at dB_1:3/dx_1:3
      map(j,k,l)%d2B(1:3,ix) = b2(1:3) !second derivative at d2B_1:3/d2x_1:3 /2
     end do
!      write(11,'(6F12.4)') map(j,k,l)%x(1:3),map(j,k,l)%B(1:3)
   end do
  end do
end do

return
end subroutine

   subroutine sum_fields(map_inj,map_inf, xmin_inj, xmin_inf,map, xmin)
    use magfield
    implicit none
    type(magfield_struct), allocatable :: map_inj(:,:,:), map_inf(:,:,:)
    type(magfield_struct), allocatable :: map(:,:,:), Boff(:)
    type(magfield_struct) x0, xmax, xmin, dxyz(-1:1), xmin_inj, xmin_inf, diffx
    integer ntot(3), ntot_inf(3), idif(3)
    integer i,j,k,l
    real(rp), allocatable :: by(:), bx(:)

    forall(i=1:3)ntot(i) = size(map_inj,i)
    forall(i=1:3)ntot_inf(i) = size(map_inf,i)

         idif(1:3) = 2*(xmin_inf%x(1:3) - xmin_inj%x(1:3))
    allocate(map(1:ntot(1),1:ntot(2),1:ntot(3)))
    map(:,:,:) = map_inj(:,:,:)
    allocate(Boff(1:ntot(3)))
    forall(i=1:ntot(3))boff(i)%B=0
    boff(520:ntot(3))%B(1)=155
    boff(520:ntot(3))%B(2)=14700
    xmin%x = xmin_inj%x

    do i=1+idif(1),ntot(1)
!     write(13,'(/,a,/)')'#'
     do j=1+idif(2),ntot(2)
!     write(13,'(/,a,/)')'#'
      do k=1+idif(3),ntot(3)

       map(i,j,k)%B(1:3) = map_inj(i,j,k)%B(1:3)  + Boff(k)%B(1:3)

    diffx%x(1:3) = map_inj(i,j,k)%x(1:3) - map_inf(i-idif(1),j-idif(2),k-idif(3))%x(1:3)
    if(any(abs(diffx%x(1:3)) > 0.00001))then
       
       write(13,'(4(3es12.4))')map(i,j,k)%B(2), map_inj(i,j,k)%B(2), map_inf(i-idif(1),j-idif(2),k-idif(3))%B(2), &
              map_inf(i-idif(1),j-idif(2),k-idif(3))%x(1:3),map_inj(i,j,k)%x(1:3),diffx%x
    endif
      end do
     end do
    end do
    return
   end

      subroutine quadratic_fit(dxyz,b1,b2)
       use magfield
       use nr
       implicit none
       real(rp) dx/0.5/
       type(magfield_struct) dxyz(-1:1)
       real(rp) b0(3), b1(3), b2(3)
       integer ix, i 

         b0(1:3) = dxyz(0)%B(1:3)
         b1(1:3) = (dxyz(1)%B(1:3) - dxyz(-1)%B(1:3))/2/dx ! derivative of B_ix wr x,y,z
         b1(1:3) = (dxyz(1)%B(1:3) - dxyz(0)%B(1:3))/dx ! derivative of B_ix wr x,y,z
         b2(1:3) = (dxyz(1)%B(1:3) + dxyz(-1)%B(1:3) - 2*b0(1:3))/2/dx**2 ! derivative of B_ix wr x,y,z


      return
      end subroutine quadratic_fit